Manifolds MTK, MTKM, and SLoM Conclusion

�-Manifolds for Estimating 3D-Orientations

Udo Frese1,based on work together with

Christoph Hertzberg1, René Wagner2 and Lutz Schröder2

1 (formerly2) Universität Bremen

RSS WS Moving Sensors, 17. July 2015

Manifolds MTK, MTKM, and SLoM Conclusion

[HWFS13] Hertzberg, C. ; Wagner, R. ; Frese, U. ; Schröder, L.:Integrating Generic Sensor Fusion Algorithms with Sound StateRepresentations through Encapsulation of Manifolds.

In: Information Fusion 14 (2013), Nr. 1, S. 57–77

Manifolds MTK, MTKM, and SLoM Conclusion

Estimation of 3D-Orientations ∈ SO(3)

3D-Orientations ∈ SO(3) need to beestimated in...

3D Localization3D SLAMMultisensor calibrationIMU based attitude estimation

00.5

11.5

−0.5

0

0.50

0.5

1

1.5

Manifolds MTK, MTKM, and SLoM Conclusion

Estimation of 3D-Orientations ∈ SO(3)

Problem:SO(3) is not a vector space

SO(3) as R3×3 (matrix):orthonormality constraintSO(3) as R4 (quaternion):unit length constraintSO(3) as R3 (Euler-angle):singularitySO(3) is a manifoldlocally like R3, globally not

00.5

11.5

−0.5

0

0.50

0.5

1

1.5

Manifolds MTK, MTKM, and SLoM Conclusion

�-Manifolds

“We write this operation [the state update] asx → x + δx , even though it may involve considerablemore than vector addition.” W. Triggs [TMHF00, p. 7]

Contribution of �-manifolds [Her08, HWFS13, WBF11]formalization of this ideaincluding expected value, covariance and Gaussian distributionformalization is novel not the actual computationnotation and encapsulation with � and �adapt least-squares and UKF simply byreplacing + by � and − by � (mainly)How far do we get formally correctly,where is a leap of ”engineering-faith” needed?

Manifolds MTK, MTKM, and SLoM Conclusion

�-Manifolds

�-Manifolds

�-manifolds S with operators

�S : S × Rn → S, (1)

�S : S × S → Rn. (2)

y = x � δ perturbes x by avector valued δδ = y � x computes the vectorδ perturbing x to y

x

x� δ

0

δ

Manifolds MTK, MTKM, and SLoM Conclusion

�-Manifolds

�-Manifolds

�-manifold S with operators

�S : S × Rn → S,�S : S × S → Rn.

x � δ smooth in δ and y � x smooth in y .range of unique values 0 ∈ V ⊂ Rn

∀y ∈ S : x � (y � x) = y (b)∀δ ∈ V :(x � δ) � x = δ (c)

∀δ1, δ2 ∈ Rn : ‖(x � δ1) � (x � δ2)‖ ≤ ‖δ1 − δ2‖ . (d)

Manifolds MTK, MTKM, and SLoM Conclusion

�-Manifolds

�-Manifolds

x� δ2

x� δ1

x

δ1

δ2

∀δ1, δ2 ∈ Rn : ‖(x � δ1) � (x � δ2)‖ ≤ ‖δ1 − δ2‖ . (d)

Manifolds MTK, MTKM, and SLoM Conclusion

�-Manifolds

�-Manifolds

∀δ1, δ2 ∈ Rn : ‖(x � δ1) � (x � δ2)‖ ≤ ‖δ1 − δ2‖ . (d)

‖x � x‖ =∥∥(x � (x � x)

)�(x � (x � x)

)∥∥ (d)

≤ ‖(x � x)− (x � x)‖ = 0

x � x = 0

x � 0 = x

Manifolds MTK, MTKM, and SLoM Conclusion

�-Manifolds

�-Manifolds

Examples

2D-Orientation (i.e. angles with periodicity)

x � δ = x + δ y � x = normalize+π−π (y − x)

3D-Orientation

x � δ = x exp δ× y � x = log(x−1y)

Manifolds MTK, MTKM, and SLoM Conclusion

�-Manifolds

� induced metric

d(x , y) := ‖y � x‖ definesa metric on Sallows to interpret variancese.g. for orientations x , y ,d(x , y) is the angle between xund y

x

x� δ

0

δ

Manifolds MTK, MTKM, and SLoM Conclusion

�-Manifolds

� induced metriccheck three properties of a metric

d(x , y) = 0⇔ x = y

d(x , y) = ‖y � x‖ = ‖(y � 0) � (y � (x � y))‖≤ ‖x � y‖ = d(y , x)

⇒ d(x , y) = d(y , x)

d(x , z) = ‖z � x‖= ‖(y � (z � y)) � (y � (x � y))‖≤ ‖(z � y)− (x � y)‖≤ ‖(x � y)‖+ ‖(z � y)‖= d(x , y) + d(y , z)

d(x , y) := ‖y � x‖

x � (y � x) = y (b)

(x � δ) � x = δ (c)

‖(x � δ1) � (x � δ2)‖ (d)

≤ ‖δ1 − δ2‖

Manifolds MTK, MTKM, and SLoM Conclusion

�-Manifolds

Gaussians on �-manifolds

µ ∈ S, X random variable ∈ Rn

Y := µ� X random variable ∈ SGaussian: N (µ,Σ) := µ�N (0,Σ), µ ∈ S, Σ ∈ Rn×n

X ∼ N (µ, Σ) = µ�N (0, Σ)∗⇔ X � µ ∼ N (0, Σ)

*only approximate

Manifolds MTK, MTKM, and SLoM Conclusion

�-Manifolds

Expected Value on �-manifolds

EX?=∫S x · p(X = x)dx . is undefined

wanted:a definition equivalent on Rn but also defined on �-manifoldsin Rn holds:

E((X − x)2) = V (X ) + (x − EX )2

EX = argminx∈Rn E(‖X − x‖2)

EX = argminx∈S E(‖X � x‖2)

Manifolds MTK, MTKM, and SLoM Conclusion

�-Manifolds

Expected Value on �-manifolds

EX = argminx∈S E(‖X � x‖2)

How to compute the expectedvalue?iterate µk+1 = µk � E (X � µk)

as Kraft [Kra03] did

x

x� δ

0

δ

Manifolds MTK, MTKM, and SLoM Conclusion

�-Manifolds

Expected Value on �-manifolds

µk+1 = µk � E (X � µk)

E (‖X � µk+1‖2)= E (‖X � (µk � E (X � µk)‖2)

≤ E (‖(X � µk)− E (X � µk)‖2)

= E(‖(X � µk)‖2 − 2(X � µk) · E (X � µk) + ‖X � µk‖2

)= E (‖(X � µk)‖2)− ‖E (X � µk)‖2

= E (‖(X � µk)‖2)− ‖µk+1 � µk‖2

Manifolds MTK, MTKM, and SLoM Conclusion

�-Manifolds

Covariance on �-manifolds

CovX = E((X � EX )(X � EX )>

)X random variable ∈ S: EX ∈ S, CovX ∈ Rn×n

E(d(X ,EX )) = E(∑

i (X � EX )2i

)= tr CovX

Manifolds MTK, MTKM, and SLoM Conclusion

�-Manifolds

Least-squares on �-manifolds

Classical Least-Squares Least-Squares on a �-Manifold

f : Rn → Rm f : S →Mf (X )− z ∼ N (0, Σ) f (X ) � z ∼ N (0, Σ)

Least Squares

x̂ = argminx∈Rn

‖f (x)− z‖2Σ x̂ = argminx∈S

‖f (x) � z‖2Σ

= argminx∈Rn

(f (x)− z)T Σ−1(f (x)− z) = argminx∈S

(f (x) � z)T Σ−1(f (x) � z)

Manifolds MTK, MTKM, and SLoM Conclusion

�-Manifolds

Gauss-Newton on �-Manifolds

Classical Gauss-Newton Gauss-Newton on a �-Manifold

f : Rn → Rm f : S →Mf (X )− z ∼ N (0, Σ) f (X ) � z ∼ N (0, Σ)

Iterate with initial guess x0 until xi converges:

J•k :=f (xi + εek )− f (xi − εek )

2εJ•k :=

(f (xi � εek ) � z)− (f (xi �−εek ) � z)

2ε

xi+1 := xi − (J>Σ−1J)−1J>Σ−1(f (xi )− z) xi+1 := xi �−(J>Σ−1J)−1J>Σ−1(f (xi ) � z)

Manifolds MTK, MTKM, and SLoM Conclusion

MTK, MTKM, and SLoM

Libraries for �-method in C++ (MTK, SLoM) [HWFS13] andMATLAB (MTKM) [WBF11]www.openslam.org

Manifold Toolkit (for Matlab)Sparse Least Squares on Manifoldsg2o is similar

Manifolds MTK, MTKM, and SLoM Conclusion

Conclusion

quantities living on manifolds require special code inestimation algorithms

�-method encapsulates special code in � and � operatorsmainly replacing + with � and − with � in algorithmsaxiomatized, including expected value, covariance, andGaussian distributionKey axiom (d) ‖(x � δ1) � (x � δ2)‖ ≤ ‖δ1 − δ2‖‖y � x‖ is a metric, in particular ‖y � x‖ = ‖x � y‖the manifold σ-point propagation by Kraft [Kra03] istheoretically sound

Manifolds MTK, MTKM, and SLoM Conclusion

Outlook

TheoreticalIn every compact, connected Lie-Group, x � δ = x exp δsatisfies (b) and (c). But is (d) satisfied in general?Related to ‖log(exp δ1 exp δ2)‖ ≤ ‖δ1 + δ2‖?

Manifolds MTK, MTKM, and SLoM Conclusion

Outlook

PracticalA lot of SLAM is solved!We need actual applications!Can SLAM survive degenerate situations?

no features or flat planelonger than the IMU can bridge?scenario A: snakerobot in USAR with camera sometimesblocked by rubblescenario B: service robot looking at a featureless wall or table

Remark: Odometry may help but many robots shake relative toodometry. Can this be modeled and handled?

Manifolds MTK, MTKM, and SLoM Conclusion

References

Hertzberg, C.:A Framework for Sparse, Non-Linear Least Squares Problems on Manifolds, Universität Bremen,Diplomarbeit, 2008. –openslam.org/slom

Hertzberg, C. ; Wagner, R. ; Frese, U. ; Schröder, L.:Integrating Generic Sensor Fusion Algorithms with Sound State Representations throughEncapsulation of Manifolds.In: Information Fusion 14 (2013), Nr. 1, S. 57–77

Kraft, E.:A Quaternion-Based Unscented Kalman Filter for Orientation Tracking.In: Proceedings of the Sixth International Conference of Information Fusion Bd. 1, 2003, 47–54

Triggs, W. ; McLauchlan, P. ; Hartley, R. ; Fitzgibbon, A.:Bundle Adjustment – A Modern Synthesis.In: Triggs, W. (Hrsg.) ; Zisserman, A. (Hrsg.) ; Szeliski, R. (Hrsg.): Vision Algorithms: Theoryand Practice.Springer Verlag, 2000 (LNCS), S. 298–375

Wagner, R. ; Birbach, O. ; Frese, U.:Rapid Development of Manifold-Based Graph Optimization for Multi-Sensor Calibration andSLAM.In: Proceedings of the International Conference on Intelligent Robots and Systems, 2011. –(under review)