TEK5030

Lecture 6.3 Optimizing over poses

Trym Vegard Haavardsholm

TEK5030

Nonlinear state estimation

We have seen how we can find the MAP estimate of our unknown states given measurements by representing it as a nonlinear least squares problem

2olve argminS∗ ← −Δ

Δ AΔ b

, Linearize at tX←A b

1t tX X+ ∗← +Δ2

0Choose a suitable inital estimate X

2

1argmin ( )

i

m

i i iX i

X h X∗

=

= −∑ Σz

argmax ( | )MAP

XX p X Z=

TEK5030

The indirect tracking method

3

2argmin ( )

cw

wcw cw i i

iπ∗ = −∑

TT T x u

Minimize geometric error over the camera pose

TEK5030

Rotations and poses are Lie groups

Rotations in 3D: Poses in 3D:

TEK5030

Rotations and poses are Lie groups

Rotations in 3D: Poses in 3D:

Rotations and poses are not vector spaces! (They lie on manifolds)

TEK5030

Nonlinear state estimation

We have seen how we can find the MAP estimate of our unknown states give measurements by representing it as a nonlinear least squares problem

2olve argminS∗ ← −Δ

Δ AΔ b

, Linearize at tX←A b

1t tX X+ ∗← +Δ6

0Choose a suitable inital estimate X

2

1argmin ( )

i

m

i i iX i

X h X∗

=

= −∑ Σz

argmax ( | )MAP

XX p X Z=

Rotations and poses are not vector spaces! (They lie on manifolds) How do we optimize?

TEK5030

The corresponding Lie algebra

Rotations in 3D:

TEK5030

The corresponding Lie algebra

Rotations in 3D: Remember the axis-angle representation:

𝜙𝜙

𝐯𝐯

( )cos 1 cos sinTab φ φ φ ∧= + − +R I vv v

TEK5030

The corresponding Lie algebra

Rotations in 3D: Remember the axis-angle representation:

( )cos 1 cos sinTab φ φ φ ∧= + − +R I vv v 𝜙𝜙

𝐯𝐯 When 𝜙𝜙 is small: cos 𝜙𝜙 ≈ 1 sin 𝜙𝜙 ≈ 𝜙𝜙

TEK5030

( )cos 1 cos sinTab φ φ φ

φ

∧

∧ ∧

= + − +

≈ + = +

R I vv v

I v I ω

The corresponding Lie algebra

Rotations in 3D: Remember the axis-angle representation:

𝜙𝜙

𝐯𝐯 When 𝜙𝜙 is small: cos 𝜙𝜙 ≈ 1 sin 𝜙𝜙 ≈ 𝜙𝜙

TEK5030

The corresponding Lie algebra

Rotations in 3D: Poses in 3D:

TEK5030

The corresponding Lie algebra

Rotations in 3D: Poses in 3D:

The corresponding Lie algebras

are vector spaces!

TEK5030

We can relate the group and algebra through the matrix exponential and matrix logarithm

Relation between group and algebra

TEK5030

We can relate the group and algebra through the matrix exponential and matrix logarithm

Relation between group and algebra

TEK5030

Tangent space

The Lie algebra is the tangent space around the identity element of the group • The tangent space is the “optimal” space

in which to represent differential quantities related to the group

• The tangent space is a vector space with the same dimension as the number of degrees of freedom of the group transformations

Dellaert, F., & Kaess, M. (2017). Factor Graphs for Robot Perception

TEK5030

Perturbations

We can represent steps and uncertainty as perturbations in the tangent space

exp( )∧=T ξ T

exp( )∧=R ω R

TEK5030

Jacobians for perturbations on SO(3)

( )exp( )[ ]

∧∧

=

∂ ⊕= − ⊕

∂ω 0

ω R xR x

ω

( )exp( )∧∂ ⊕ ∂ ⊕= =

∂ ∂

ω R x R x Rx x

⊕ =R x RxGroup action on points:

TEK5030

Jacobians for perturbations on SE(3)

( )3 3

exp( )[ ]

∧∧

×

=

∂ ⊕ = − ⊕ ∂

ξ 0

ξ T xI T x

ξ

( )exp( )∧∂ ⊕ ∂ ⊕= =

∂ ∂

ξ T x T x Rx x

⊕ = +T x Rx tGroup action on points:

TEK5030

Summary

• Updates on rotations and poses as perturbations using Lie algebra

• Jacobians for these perturbations

• We are ready to solve

19

exp( )∧=R ω R

exp( )∧=T ξ T

2argmin ( )

cw

wcw cw i i

iπ∗ = −∑

TT T x u

TEK5030

Supplementary material

• Ethan Eade, “Lie Groups for 2D and 3D transformations” • José Luis Blanco Claraco, “A tutorial on SE(3) transformation

parameterizations and on-manifold optimization”

Chapter 7