Conformal Geometric Algebra in Stochastic Optimization Problems ...

212 CHAPTER 8. APPLICATIONS IN OMNIDIRECTIONAL VISION
8.2.2 Initial Estimates
In case of the point-line method simply the approved geometric technique introduced
in chapter 4, which is also used in section 7.3 to provide the initial estimate,
is employed.
The initial estimate for the line-plane estimation can be provided at very low costs.
Moreover, it shortens the overall computation time. The aim is to rotate the model
such that the unit direction vectors, denoted by { ˆ d 1...N }, of the lines come to lie on
the respective projection planes. Let the normal vectors of the planes be given by
{ˆn 1...N }. Hence pre-aligning the model means finding a rotation matrix R ∈ � 3×3
such that
(∀i): ˆn T i R ˆ di = 0.
By Rodrigues’s formula (3.65), it is known that the matrix R describing a rotation
through angle θ about a fixed axis, given by a unit normal vector â = [a1;a2; a3],
can be calculated by
R = exp(θA)
For small angles,
= I3 + sinθ A + (1 − cos θ)A 2 ,
R ≈ I3 + θA
A :=
⎡ ⎤
0 −a3 a2
⎣ a3 0 −a1⎦.
−a2 a1 0
can be used as a good approximation. With this relationship and due to the skew
symmetric structure of A ′ := θA, it is possible to solve for a ′ = [θa1; θa2; θa3]: each
correspondence pair (ˆni, ˆ di) gives one row
⇐⇒
⇐⇒
ˆn T i A ′ ˆ di = −ˆn T i ˆ di
vec( ˆ diˆn T i ) T vec(A ′ ) = −ˆn T i ˆ di
( ˆ di3ˆni2 − ˆ di2ˆni3)a ′ 1 + ( ˆ di1ˆni3 − ˆ di3ˆni1) a ′ 2 + ( ˆ di2ˆni1 − ˆ di1ˆni2)a ′ 3 = −ˆn T i ˆ di
of an overdetermined system of linear equations. Using the solution a ′ a first approximation
R [1] of R can be computed. After transforming the lines by means
of R [1] , the method can be reapplied. Any such succeeding iteration yields a new
rotation until the procedure is stopped because the lines were close enough to the
planes. It can be inferred that after t iterations
R ≈ R [t] . ..R [2] R [1] .
The convergence of this method is analyzed in [102] with the result that very few
(below five) iterations are necessary to maintain a high accuracy, even if the amount
of rotation is nearly 180 ◦ (and despite assuming small angles). What makes this
technique so effective is the detour via the Lie algebra so(3) instead of directly
searching the Lie group SO(3), which comprises R, see section 4.6.