Dense map inference with user-defined priors: from priorlets to - - PowerPoint PPT Presentation

▶
dense map inference with user defined priors from
SMART_READER_LITE
LIVE PREVIEW

Dense map inference with user-defined priors: from priorlets to - - PowerPoint PPT Presentation

Jul 22, 2023 146 likes •456 views

Dense map inference with user-defined priors: from priorlets to scan eigenvariations Paloma de la Puente Andrea Censi Universidad Politcnica de Madrid California Institute of Technology INDUSTRIAL ES ETSII | UPM Introduction SLAM =

slide-1
SLIDE 1

Dense map inference with user-defined priors: from priorlets to scan eigenvariations

Paloma de la Puente Universidad Politécnica de Madrid

INDUSTRIALES ETSII | UPM

Andrea Censi California Institute of Technology

slide-2
SLIDE 2

Introduction

  • SLAM = Simultaneous Localization and Mapping

I t t f ffi i t i ti Th b t d

  • Important for efficient navigation: The robot needs a map
  • f 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

slide-3
SLIDE 3

Introduction

  • PRIOR = Assumptions on the

environment

Rectangular

environment With proper prior, better results

  • Previous approaches:
  • Previous approaches:

prior = representation

  • Questions:

Circular

  • Questions:
  • Is it possible to define a

framework in which priors are framework in which priors are specified by the user as parameters?

Polygonal

parameters?

  • Is it possible to decouple a prior

from a particular

Spline

3

from a particular representation?

Spline

slide-4
SLIDE 4

Problem definition and setup

  • Sensor model:

ρ = r(m, q) + ²

i readings map robot’s pose noise

  • Surface normals: angles αI

αi ρ

  • Problem definition:

ρi y t θ maxmlog p(˜ ρ|m) + p(m)

4

x

likelihood measurements

slide-5
SLIDE 5

5

slide-6
SLIDE 6

What’s next?

  • 1. Modeling structured priors with priorlets

2 I f ith t t d i

  • 2. Inference with structured priors
  • 3. DOF Extraction

6

slide-7
SLIDE 7

Modeling structured priors: topology of the environment

  • Environment divided into Surfaces

S f di id d i t R i Surfaces divided into Regions

Different f Different regions of the same surfaces 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

7

y

  • f neighbors
slide-8
SLIDE 8

Modeling structured priors: our solution

Ch ll d fi iti f th i

( )

  • Challenge: definition of the prior p(m)
  • 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 0th and 1st
  • rder expansion (could be extended)

8

slide-9
SLIDE 9

Modeling structured priors: examples

name: Polygonal prior

  • rder: 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)] name: Rectangular prior specializes: Polygonal prior priorlet different_region: (alpha_2 == alpha_1 ‐ pi/2) || … (alpha_2 == alpha_1 + pi/2)

9

slide-10
SLIDE 10

Modeling structured priors: examples

name: Circular prior

  • rder: 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: # it is likely that the radius is around 2.0 model_likelihood (r13 ‐ 2.0)^2

r ≈ 2

10

slide-11
SLIDE 11

Modeling structured priors

  • A prior is defined by 3 priorlets

A “ i ” i l t

  • A “same region” priorlet
  • A “different region” priorlet
  • A “different surface” priorlet
  • A priorlet defines part of an optimization problem

x = (ρ, α) x (ρ, α) P ( ) priorlet same region

Equalities

min Ph(x)+... s.t. f(x) = 0 and g(x) ≤ 0 p g f(x) = 0 g(x) ≤ 0 h(x)

q Inequalities

11

g( ) h(x)

Energies

slide-12
SLIDE 12

Inference with structured priors

  • Final problem:

structure constraints d d l

max(log(p(˜ ρ|ρ)) + hT (x) s.t. fT (x) = 0 ( )

  • )

depend on topology geometric constraints

and gT (x) ≤ 0 and x ≤ x ≤ x

  • Two groups of variables:
  • Discrete,topology T (division in regions and surfaces,
  • utliers)
  • Continuous, x
  • Strategy: nested optimization
  • Outer loop, greedy on T with relaxation
  • Inner loop, homotopy method with penalty functions

12

slide-13
SLIDE 13

Inference with structured priors

  • Homotopy method with penalty functions

S l i t i d i i i ti bl b it ti l Solving a constrained minimization problem by iteratively solving an unconstrained minimization problem

Constrained problem Unconstrained problem

min h(x) s.t. f(x) = 0 and g(x) ≤ 0 min h(x) + λ(f(x)2 + max(0, g(x))2)

(

and g(x) ≤ 0

  • Convergence under proper conditions as
  • We also use a log-barrier method

λ → ∞

13

slide-14
SLIDE 14

Inference with structured priors

Initial estimate Final estimate x0

x

Initial estimate Covariance Model Covariance f(x) = 0

<n

<m

14

( )

m << n

slide-15
SLIDE 15

15

slide-16
SLIDE 16

16

slide-17
SLIDE 17

17

slide-18
SLIDE 18

18

slide-19
SLIDE 19

19

slide-20
SLIDE 20

20

slide-21
SLIDE 21

21

slide-22
SLIDE 22

22

slide-23
SLIDE 23

23

slide-24
SLIDE 24

24

slide-25
SLIDE 25

25

slide-26
SLIDE 26

26

slide-27
SLIDE 27

Covariance shrinkage

Initial estimate Projected covariance Final estimate x0

x

ker∇f(x)

Useful to

Covariance Model

recover structure from local constraints constraints

<n

<m

<<

f(x) = 0

27

m << n

slide-28
SLIDE 28

DOF extraction

  • Previous approaches: DOF intrinsic in the

representation representation

r

(xc, yc)

  • Our approach: recover the DOF from ∇f(x)

28

slide-29
SLIDE 29

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) f(x) = 0 → FREE = Ker(∇f)

FREE = EXTRINSIC + INTRINSIC

span δx

δq

STRUCTURE

29

DOF due to the possible motion of the robot

slide-30
SLIDE 30

Conclusions and future work

  • Conclusions

Y i i ibl d fi f k i hi h i

  • Yes, it is possible to define a framework in which priors

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