SLAM are often used in Lie Lie algebra with quaternions to represent the rotation transformation, which often need to derive a mathematical formula to derive the go, every minute to get a big head. But in SLAM is often used where so few fixed nature, so there is no need too much in-depth study of these mathematical basis for, just remember that some of the commonly used formulas and nature can be. Therefore, I am here on these mathematical basis for a simple summary for future use in the project.
Rotating representation
SLAM, often used in three ways to express the rotational transformation space, are: rotation vector, rotation matrix, quaternion.
Rotation vector
Rotation vectors can be very intuitive represent a rotational transformation space: hypothesis space there is a unit vector \ (\ mathbf {n} \ ) , to \ (\ mathbf {n} \ ) as the axis and the rotational direction \ (\ mathbf {n} \) same, the rotation angle \ (\ Theta \) . The rotation transformation may be referred to as a "diagonal axis" in combination: \ (<\ Theta, \ mathbf {n-}> \) , may be combined in the form of work vectors: \ (\ mathbf {\ Phi} = \ Theta \ {n-mathbf} \) .
Rotation matrix
Rotation matrix \ (\ mathbf {R} _ {AB} \) may be represented by an object pose a \ (A \) the rotational transformation to pose \ (B \) process, if the \ (A \) and \ (B \ ) as a coordinate system, \ (\ mathbf {R} _ {BA} \) may represent a coordinate system \ (a \) in the coordinate system (B \) \ posture of. Set \ (W is \) for the world coordinate system of the object by the attitude (\ mathbf {R} _ { WA} \) \ converts the gesture \ (\ mathbf {R} _ {WB} \) can be \ (\ mathbf { R} _ {AB} \) to indicate that the: \ (\ mathbf {R & lt} _ {the WB} = \ mathbf {R & lt} _ {WA} \ mathbf {R} _ {AB} \) .
If the coordinate relationship is very clearly, the index may be directly removed directly referred to as \ (\ mathbf {R} \ ) wherein. \ (\ mathbf {R} \ ) in units of orthogonal matrix, which has the following properties:
- \ (\ mathbf {R} \ ) of the rows (columns) are unit vectors and are orthogonal to each;
- \ (\ mathrm {it} (\ mathbf {R}) = 1 \) ;
- \(\mathbf{R}^{-1} = \mathbf{R}^\mathrm{T}\)
Quaternion
Quaternion can be defined as:
\ [\ mathbf} = {Q + W + XI + YJ ZK \]
Wherein \ (i, j, k \ ) are three imaginary unit quaternion, which satisfy the following three imaginary properties:
\ [\ left \ {\ the begin Matrix} {I ^ J ^ 2 + 2 + K ^ 2 = 1 \\ ij = k , ji = -k \\ jk = i, kj = -i \\ ki = j, ik = -j \ end {matrix} \ right. \]
Quaternion can also be expressed as
\ [\ mathbf {q} = [w, x, y, z] ^ \ mathrm {T} \]
or
\[ \mathbf{q} = [s,\mathbf{v}], \mathbf{v} = [x,y,z]^\mathrm{T} \in \mathbb{R}^3 \]
We can use the unit quaternions to represent an arbitrary spatial rotation transformation, which meet the conditions \ (w ^ 2 + x ^ 2 + y ^ 2 + z ^ 2 = 1 \) Quaternion quaternion units.
Conversion between three ways
The three mathematical tools can indicate the rotation space, they are in fact equivalent, can be interchangeable.
Rotation vector \ (\ leftrightarrow \) rotation matrix
Rotating vector is set \ (\ mathbf {\ Phi} = \ Theta \ n-mathbf {} \) , which is equivalent to the rotation matrix is \ (\ R & lt mathbf {} \) , conversion between the two may be used Luo Rodriguez formula is achieved, i.e.
\ [\ mathbf {R} = \ cos \ theta \ mathbf {I} + (1 - \ cos \ theta) \ mathbf {n} \ mathbf {n} ^ \ mathrm {T} + \ sin \ theta \ mathbf { n} ^ {\ wedge} \]
Wherein \ (\ mathbf {n} ^ {\ wedge} \) by \ (\ mathbf {n} \ ) turned into a symmetric matrix, i.e.
\ [\ mathbf {n} ^ {\ wedge} = \ begin 0 & -n_3 bmatrix} {& n_3 and n_2 & 0 \\ \\ & -n_1 of n_1 & 0 & -n_2 \ bmatrix End {} \]
\ (\ {n-mathbf} \) is antisymmetric matrices can also be used \ ( [\ mathbf {n}] _ {\ times} \) is represented.
Conversely, the \ (\ mathbf {R} \ ) into \ (\ mathbf {n} \ ) method is:
\ [\ left \ {\ the begin Matrix {} \ Theta = \ arccos (\ FRAC {\ mathrm {tr} (\ mathbf {R
}) -. 1} {2}) \\ \ mathbf {R} \ mathbf {n} = \ mathbf {n} \ end {matrix} \ right \] wherein \ (\ mathbf {R} \ mathbf {n} = \ mathbf {n} \) represents the matrix \ (\ mathbf {R} \ ) is characterized by a value corresponding to a unit eigenvector.
Rotation vector \ (\ leftrightarrow \) quaternion
Rotating vector is set \ (\ mathbf {\ Phi} = \ Theta \ n-mathbf {} \) , the quaternion equivalent thereto as \ (\ mathbf {Q} \) , there are:
\ [\ mathbf { q} = \ cos \ frac { \ theta} {2} + \ mathbf {n} \ sin \ frac {\ theta} {2} = [\ cos \ frac {\ theta} {2}, \ mathbf {n} \ sin \ frac {\ theta}
{2}] ^ \ mathrm {T} \] this conversion of the imaginary and the Euler's formula is very similar, i.e., \ (\ mathrm {e} ^ {j \ theta} = \ cos \ Theta + J \ SiN \ Theta \) , where an analogy can remember.
Rotation matrix \ (\ leftrightarrow \) quaternion
Provided Quaternion \ (\ mathbf {Q} = Q_0 q_2j + + + q_1i q_3k \) , which is equivalent to the rotation matrix is \ (\ R & lt mathbf {} \) , the transformation between them:
\ [\ mathbf {R} = \ begin {bmatrix } 1-2q_2 ^ 2-2q_3 ^ 2 & 2 q_1 q_2 - 2 q_0 q_3 & 2 q_1 q_3 + 2 q_0 q_2 \\ 2 q_1 q_2 + 2 q_0 q_3 & 1 - 2 q_1 ^ 2 - 2 q_3 ^ 2 & 2 q_2 q_3 - 2 q_0 q_1 \\ 2 q_1 q_3 - 2 q_0 q_2 & 2 q_2 q_3 + 2 q_0 q_1 & 1 - 2 q_1 ^ 2 - 2 q_2 ^ 2 \ end {bmatrix} \]
\[ \begin{matrix} q_0=\frac{\sqrt{\mathrm{tr}(\mathbf{R})+1}}{2} & q_1 = \frac{m_{23}-m_{32}}{4 q_0} & q_2 = \frac{m_{31}-m_{13}}{4 q_0} & q_3 = \frac{m_{12} - m_{21}}{4 q_0} \end{matrix} \]
Lie Lie algebra of nature
The above mentioned rotation matrix \ (\ mathbf {R} \ ) may constitute a Lie SO (3), i.e. a special orthogonal group, and the rotation vector \ (\ mathbf {\ phi} = \ theta \ mathbf {a} \) may constitute a Lie algebra \ (\ mathfrak {SO} (. 3) \) . Due to introduction of the Lie algebra, so the derivation of the rotation matrix becomes very convenient. It will use a large number of transitions between Li Lie algebra and matrix when rotating in the derivation of the SLAM or pose of the object for optimization.
Index and logarithmic mapping
Conversion between Lie algebras Li Index and number can be implemented.
Exponential map:
\ [\ R & lt mathbf} = {\ exp (\ mathbf {\ Phi ^ {} \} Wedge) = \ exp (\ Theta \ mathbf {a} ^ {\ wedge}) = \ cos \ theta \ mathbf {I} + (1- \ cos \ theta) \ mathbf {a} \ mathbf {a} ^ {\ mathrm {T}} + \ sin \ theta \ mathbf {a} ^ {\ wedge} \]
This formula is not above looks very acquaintance? ? ? Yes, it is above had said before the Rodriguez formula.
Logarithmic Mapping:
\ [\ mathbf {\ Phi} = \ LN (\ mathbf {R & lt}) ^ {\ VEE} = \ left (\ sum_ {n-= 0} ^ {\ infty} \ FRAC {(-. 1) ^ n} {n + 1}
(\ mathbf {R} - \ mathbf {I}) ^ {n + 1} \ right) ^ {\ vee} \] this equation looks a little complicated, but it does not matter anyway also not be used much, because we do not usually spend a formula to calculate the number of maps, but calculated using the trace rotation matrix, see the rotation vector \ (\ leftrightarrow \) rotation matrix one has introduction.
BCH official
Exponential function \ (\ exp (x) \) (where x is a scalar) with \ (\ exp (x_1) \ exp (x_2) = \ exp (x_1 + x_2) \) properties, but Lie algebras index mapping whether as \ (\ exp (\ mathbf { \ phi} _1 ^ {\ wedge}) \ exp (\ mathbf {\ phi} _1 ^ {\ wedge}) = \ exp ((\ mathbf {\ phi} _1 + \ mathbf {\ phi} _2) ^ {\ wedge}) \) the nature of it? Unfortunately, this property does not exist, but can be approximated using the BCH.
BCH formula is as follows:
\ [\ LN (\ exp (\ mathbf {\ Phi} _1 ^ {\ Wedge}) \ mathbf {\ Phi} _2 ^ {\ Wedge})) ^ {\ VEE} \ approx \ left \ { \ begin {matrix} \ mathbf { J} _l (\ mathbf {\ phi} _2) ^ {- 1} \ mathbf {\ phi} _1 + \ mathbf {\ phi} _2 & \ mathbf {\ phi} _1 \ in \ sigma \\ \ mathbf {\ phi } _1 + \ mathbf {J} _r (\ mathbf {\ phi} _1) ^ {- 1} \ mathbf {\ phi} _2 & \ mathbf {\ phi} _2 \ in \ sigma \ end {matrix} \ right . \]
Wherein \ (\ Sigma \) represents a set amount of microwaves. In the above formula, a formula for the first approximation by the left, to the right by the second approximation formula. This now be explained as follows.
For the first equation, assuming rotation matrix is \ (\ mathbf {R} _2 \) (which corresponds to Lie algebra \ (\ mathbf {\ Phi} _2 \) ), for \ (\ mathbf {R} _2 \) left by a slight rotation matrix \ (\ mathbf {R & lt} _1 \) (which corresponds to the Lie algebra is \ (\ mathbf {\ Phi} _1 \) ), the \ (\ mathbf {R} _1 \ mathbf {R} _2 \) corresponding to the Lie algebra is approximately \ (\ mathbf {J} _l (\ mathbf {\ phi} _2) ^ {- 1} \ mathbf {\ phi} _1 + \ mathbf {\ phi} _2 \) , where \ (\ mathbf {J} _l (\ mathbf {\ phi} _2) ^ {- 1} \ mathbf {\ phi} _1 \) to the left by a slight rotation matrix \ (\ mathbf {R} _1 \) Lie algebra caused part of the increase.
For the second equation, assuming rotation matrix is \ (\ mathbf {R} _1 \) (which corresponds to Lie algebra \ (\ mathbf {\ Phi} _1 \) ), for \ (\ mathbf {R} _1 \) Right by a slight rotation matrix \ (\ mathbf {R & lt} _2 \) (which corresponds to the Lie algebra is \ (\ mathbf {\ Phi} _2 \) ), the \ (\ mathbf {R} _1 \ mathbf {R} _2 \) corresponding to the Lie algebra is approximately \ (\ mathbf {J} _r (\ mathbf {\ phi} _1) ^ {- 1} \ mathbf {\ phi} _2 + \ mathbf {\ phi} _1 \) , where \ (\ mathbf {J} _r (\ mathbf {\ phi} _1) ^ {- 1} \ mathbf {\ phi} _2 \) to the left by a slight rotation matrix \ (\ mathbf {R} _2 \) Lie algebra caused part of the increase.
BCH formula can also be written in the following form, where \ (\ Delta \ mathbf {\ phi} \) is added a slight rotation:
\ [\ the begin {Matrix} \ exp (\ of Delta \ mathbf {\ Phi ^ {\ Wedge} }) \ exp (\ mathbf { \ phi ^ {\ wedge}}) = \ exp ((\ mathbf {J} _l ^ {- 1} (\ mathbf {\ phi}) \ Delta \ mathbf {\ phi} + \ mathbf {\ phi}) ^ {\ wedge}) \\ \ exp (\ mathbf {\ phi ^ {\ wedge}}) \ exp (\ Delta \ mathbf {\ phi ^ {\ wedge}}) = \ exp ((\ mathbf {\ phi} + \ mathbf {J} _r ^ {- 1} (\ mathbf {\ phi}) \ Delta \ mathbf {\ phi}) ^ {\ wedge}) \\ \ exp ((\ mathbf {\ phi} + \ Delta \ mathbf {\ phi}) ^ {\ wedge}) = \ exp ((\ mathbf {J} _l \ Delta \ mathbf {\ phi}) ^ {\ wedge}) \ exp ( \ mathbf {\ phi} ^ { \ wedge}) = \ exp (\ mathbf {\ phi} ^ {\ wedge}) \ exp ((\ mathbf {J} _r \ Delta \ mathbf {\ phi}) ^ {\ wedge}) \ end {matrix} \]
Lie algebra derivation
Suppose we have a spatial point (\ mathbf {p} \) \ has been rotated to give a \ (\ mathbf {R & lt} \ mathbf {P} \) , and now we want to calculate the point after the rotation of derivatives with respect to the rotation matrix, That is:
\ [\ FRAC {\ partial (\ R & lt mathbf {} \ mathbf {P})} {\ partial \ R & lt mathbf {^ {} \ mathrm {T}}} \]
Because of \ (\ mathbf {R} \ in \ mathrm {SO} (3) \) is away in addition, so in order to facilitate the derivation, we can use the Lie algebra to derivation, i.e. the formula is converted to a formal:
\ [\ frac {\ partial (\ mathbf {R} \ mathbf {p})} {\ partial \ mathbf {R} ^ {\ mathrm {T}}} \ Leftrightarrow \ frac {\ partial (\ exp (\ mathbf { \ phi} ^ {\ wedge} ) \ mathbf {p})} {\ partial \ mathbf {\ phi}} = - (\ mathbf {R} \ mathbf {p}) ^ {\ wedge} \ mathbf {J} _l \]
On formula results are obtained with BCH-multiplied approximated obtained, since \ (\ mathbf {J} _l \) is present, computational complexity increases, so the disturbance model can be used (by the left) to be simplified. Results left disturbance model is:
\ [\ FRAC {\ partial (\ mathbf {R & lt} \ mathbf {P})} {\ partial \ mathbf {R & lt} ^ {\ mathrm {T}}} \ Leftrightarrow \ FRAC {\ partial (\ mathbf {R} \ mathbf {p})} {\ partial \ phi} = - (\ mathbf {R} \ mathbf {p}) ^ {\ wedge} \]
Just above formula directly to the memory and used, if you want to know how derived, we found in the reference [1] p74 ~ p75.
The nature of the quaternion
Here mainly on four numerous operations and some properties to make a brief summary. Specific tutorial quaternion read reference [2].
Four numerous additions
\[ \mathbf{p} + \mathbf{q} = [p_w, \mathbf{p}_v]^{\mathrm{T}} + [q_w, \mathbf{q}_v]^{\mathrm{T}} = [p_w + q_w, \mathbf{p}_v + \mathbf{q}_v]^{\mathrm{T}} \]
Four multiplication countless
Adder quaternion symbol \ (\ otimes \) FIG. Which algorithm is as follows:
\[ \mathbf{p} \otimes \mathbf{q} = \begin{bmatrix} p_w q_w - \mathbf{p}_v^\mathrm{T} \mathbf{q}_v \\ p_w \mathbf{q}_v + q_w \mathbf{p}_v + \mathbf{p}_v \times \mathbf{q}_v \end{bmatrix} \]
Quaternion multiplication is not commutative, i.e.:
\ [\ mathbf {P} \ otimes \ mathbf {Q} \ NEQ \ mathbf {Q} \ otimes \ mathbf {P} \]
However quaternion multiplication still is associative, i.e.:
\ [(\ mathbf {P} \ otimes \ mathbf {Q}) \ otimes \ mathbf {R & lt} = \ mathbf {P} \ otimes (\ mathbf {Q} \ otimes \ mathbf {r}) \ ]
Therefore, quaternions also distributes over the adder, i.e.:
\ [\ mathbf {P} \ otimes (\ mathbf {Q} + \ R & lt mathbf {}) = \ mathbf {P} \ otimes \ mathbf {Q} + \ mathbf {p} \ otimes \ mathbf {
r} \] or:
\ [(\ mathbf {P} + \ mathbf {Q}) \ otimes \ mathbf {R & lt} = \ mathbf {P} \ otimes \ mathbf {R & lt} + \ mathbf {q} \ otimes \ mathbf {r} \]
Multiplying two quaternions can also be used in the form of a matrix representation, namely:
\ [\ mathbf {Q} _1 \ otimes \ mathbf {Q} _2 = [\ mathbf {Q}] _ L \ mathbf {Q} _2 = [\ mathbf {q}] _ R \ mathbf {q} _1 \]
而其中的\([\mathbf{q}]_L\) 与 \([\mathbf{q}]_R\)分别如下：
\[ [\mathbf{q}]_L = \begin{bmatrix} q_w & -q_x & -q_y & -q_x \\ q_x & q_w & -q_z & q_y \\ q_y & q_z & q_w & -q_x \\ q_z & -q_y & q_x & q_w \end{bmatrix} = q_w \mathbf{I} + \begin{bmatrix} 0 & -\mathbf{q}_v^\mathbf{T} \\ \mathbf{q}_v & [\mathbf{q}_v]_{\times} \end{bmatrix} \]
\[ [\mathbf{q}]_R = \begin{bmatrix} q_w & -q_x & -q_y & -q_x \\ q_x & q_w & q_z & -q_y \\ q_y & -q_z & q_w & q_x \\ q_z & q_y & -q_x & q_w \end{bmatrix} = q_w \mathbf{I} + \begin{bmatrix} 0 & -\mathbf{q}_v^\mathbf{T} \\ \mathbf{q}_v & -[\mathbf{q}_v]_{\times} \end{bmatrix} \]
These two formulas above is useful in proving derivation formula or process, we can focus on.
references
[1] "vision SLAM fourteen speaking - From Theory to Practice" Ko, Tao
[2] "Quaternion kinematics for the error-state Kalman filter" Joan Sola.