Dense map inference with user-defined priors: from priorlets to scan eigenvariations Paloma de la Puente Andrea Censi Universidad Politécnica de Madrid California Institute of Technology INDUSTRIAL ES ETSII | UPM
Introduction • SLAM = Simultaneous Localization and Mapping • Important for efficient navigation: The robot needs a map Th b t d I t t f ffi i t i ti of its environment and it needs to know its position to build the map the map chicken and egg problem • Sensors : sonars, laser scanners, cameras … • Methods : feature based state estimation, scan matching… (probabilistic approaches) • Bayesian inference requires a prior. It is not clear how to deal with this. What can we do? 2
Introduction • PRIOR = Assumptions on the Rectangular environment environment With proper prior, better results • • Previous approaches : Previous approaches : Circular prior = representation • • Questions: Questions: • Is it possible to define a framework in which priors are framework in which priors are specified by the user as Polygonal parameters? parameters? • Is it possible to decouple a prior from a particular from a particular Spline Spline representation? 3
Problem definition and setup • Sensor model: ρ = r ( m, q ) + ² noise i robot’s readings map pose α i • Surface normals: angles α I ρ i ρ • Problem definition: y t max m log p (˜ ρ | m ) + p ( m ) θ x measurements likelihood 4
5
What’s next? 1. Modeling structured priors with priorlets 2 2. Inference with structured priors I f ith t t d i 3. DOF Extraction 6
Modeling structured priors: topology of the environment • Environment divided into Surfaces Surfaces divided into Regions di id d i t S f R i Different regions of Different the same surfaces f surface • Two consecutive points may be either • On the same region • On different regions of the same surface • On different surfaces • Priors are defined by constraints on the three kinds y of neighbors 7
Modeling structured priors: our solution • Challenge: definition of the prior p ( m ) ( ) Ch ll d fi iti f th i • Solution: expressing priors as functions of ρ , α p ( ρ , α ) • Consequences : - We don’t need to deal with an infinite- dimensional m - We define shape priors only by their 0 th and 1 st order expansion (could be extended) 8
Modeling structured priors: examples name: Polygonal prior order: 2 max_curvature : 0 # cartesian coordinates p_1 = [cos(phi_1);sin(phi_1)] * rho_1; p_2 = [cos(phi_2);sin(phi_2)] * rho_2; p_ [ (p _ ); (p _ )] _ ; priorlet same_region: alpha_1 == alpha_2 (p 2 ‐ p 1)’ * [cos(alpha 1); sin(alpha 1)] == 0 (p_2 p_1) [cos(alpha_1); sin(alpha_1)] 0 name: Rectangular prior specializes: Polygonal prior priorlet different_region: (alpha_2 == alpha_1 ‐ pi/2) || … (alpha_2 == alpha_1 + pi/2) 9
Modeling structured priors: examples name: Circular prior order: 3 # 3 points to define a circle max_curvature: 10 # min radius = 0.1 m # given two (oriented) points, find the radius r12 = sin((alpha_2 ‐ alpha_1)/2) / norm(p_1 ‐ p_2); (( p _ p _ ) ) (p_ p_ ); r23 = sin((alpha_3 ‐ alpha_2)/2) / norm(p_3 ‐ p_2); r13 = sin((alpha_3 ‐ alpha_1)/2) / norm(p_3 ‐ p_1); priorlet same region: priorlet same_region: # the three points are on the same circle r12 == r23 r23 == r13 name: Circular prior (with prior on radius) specializes: Circular prior priorlet same_region: r ≈ 2 # it is likely that the radius is around 2.0 model_likelihood (r13 ‐ 2.0)^2 10
Modeling structured priors • A prior is defined by 3 priorlets • A “same region” priorlet A “ i ” i l t • A “different region” priorlet • A “different surface” priorlet • A priorlet defines part of an optimization problem x = ( ρ , α ) x ( ρ , α ) p priorlet same region g min P h(x)+ ... P ( ) Equalities q f ( x ) = 0 s.t. f ( x ) = 0 g ( x ) ≤ 0 Inequalities and g ( x ) ≤ 0 g ( ) h ( x ) h ( x ) Energies 11
Inference with structured priors • Final problem: max ( log ( p (˜ ρ | ρ )) + h T ( x ) - ) s.t. f T ( x ) = 0 structure constraints depend on topology d d l and g T ( x ) ≤ 0 ( ) geometric constraints and x ≤ x ≤ x • Two groups of variables: Discrete,topology T (division in regions and surfaces, • outliers) Continuous, x • • Strategy: nested optimization Outer loop, greedy on T with relaxation • • Inner loop, homotopy method with penalty functions 12
Inference with structured priors • Homotopy method with penalty functions S l i Solving a constrained minimization problem by iteratively t i d i i i ti bl b it ti l solving an unconstrained minimization problem Constrained problem Unconstrained problem min h ( x ) min h ( x ) + λ ( f ( x ) 2 + max (0 , g ( x )) 2 ) ( s.t. f ( x ) = 0 and g ( x ) ≤ 0 and g ( x ) ≤ 0 • Convergence under proper conditions as λ → ∞ • We also use a log-barrier method 13
Inference with structured priors Final estimate Initial estimate Initial estimate x x 0 0 Model Covariance Covariance < m < n f ( x ) = 0 ( ) m << n 14
15
16
17
18
19
20
21
22
23
24
25
26
Covariance shrinkage Projected Final estimate covariance Initial estimate ker ∇ f ( x ) x x 0 Useful to recover Model structure from local Covariance constraints constraints < m < n f ( x ) = 0 m << n << 27
DOF extraction • Previous approaches: DOF intrinsic in the representation representation r ( x c , y c ) • Our approach: recover the DOF from ∇ f ( x ) 28
DOF extraction DOF extraction: recovering the structure of the environment • We obtain a basis for the total degrees of freedom of the free space: f ( ) f ( x ) = 0 → FREE = Ker ( ∇ f ) ( f ) FREE = EXTRINSIC + INTRINSIC STRUCTURE span δ x δ q DOF due to the possible motion of the robot 29
Conclusions and future work • Conclusions • Yes, it is possible to define a framework in which priors Y i i ibl d fi f k i hi h i are specified by the user as parameters • Yes it is possible to decouple a prior from a particular • Yes, it is possible to decouple a prior from a particular representation • Future work • Backtracking for inference of the topology • Automatic selection of prior Automatic selection of prior • More experiments • Integration with SLAM • Integration with SLAM 30
Recommend
More recommend
