李群与李代数对SLAM移动场景的运动坐标表示

Representing a moving scene.

  一些必须知识点：skew matrix 是$so(3)$李代数中的，对应到$SO(3)$李群中的旋转矩阵$R$; twist matrix 是$se(3)$李代数中的，对应到$SE(3)$李群中的变换矩阵$T$。一些基本的知识点，skew matrix如何表达等，读者也应该了解。

  本篇博文，将介绍，李群与李代数的关系，如何利用李群与李代数，构造便于表示的旋转/变换矩阵，来表示移动的场景和目标。

  核心内容是：传统的旋转矩阵$R$和变换矩阵$T$，如果要在欧式空间进行构造，那么构造这个矩阵非常复杂，需要9个参数描述旋转，12个参数描述变换（9个旋转+3个平移）。而且，在欧式空间中，相机的移动，所依赖的变换矩阵，不是时间的变量，每次移动与上一次移动，其变换矩阵建立不起时间的联系，即微分方程。

  因此，采用李代数，在tangent space中，构造skew matrix: $w^{\wedge }$或者 twist matrix: ${\xi }^{\wedge }$，只需要极少的参数，skew matrix 只需要3维，twist matrix只需要6个参数。通过指数映射回李群，获得旋转矩阵/变换矩阵。从求解计算复杂度和参数的个数，都能明显的下降。

  此外，相机的移动，对于李群中的矩阵，采用乘法来表示位姿点变化$X_1=T{X}_0$。而在李代数中，可以通过skew matrix 和 twist matrix来表示变换的相对速度，即当前时刻的3D坐标对时间的倒数，表示位姿（可以是相机的，可以是世界坐标系中目标点）的变换速度。任何的变换，在无穷小时间上看，都是微小的旋转/平移，因此，可以很方便的计算速度和位姿。

 下面，是通过对李代数的介绍，指数映射，对数映射，刚体的旋转，刚体的变换（旋转+平移），随后给定参考坐标$X_0$，移动相机，对场景进行拍摄，介绍如何通过李代数来表示场景的移动过程，参考的坐标点，在过程中，是如何随着相机移动变换的。
（这里不涉及到具体过程：如特征点提取，特征点匹配，ORB算法等，建立匹配点关系，求得变换矩阵等等。）

  相对于前一篇《机器人与视觉——李群与李代数，李括号性质的分析与证明》的博文，本文用文字来描述李群与李代数，及其对应的相机移动的运动表示应用。个人感觉这种方法，更能直观的理解，李群与李代数的原理，和为何选取这种表达方法来简化机器人、相机位姿求解问题。

1. Lie group and Lie algebra.

  Using Lie algebra, we do not need to construct a complicated transformation matrix, which consists of a 3x3 rotation matrix and a 3x1 translation vector. R is a rotation matrix satisfied by R*RT=I. We only need 6 parameters, v and w. And the matrix becomes a 4x4 twist matrix.

  Taking the tangent space, and modeling the elements in Lie group (Rotation, Transformation) by corresponding the element in the tangent space.

1.1 $so(3)$->$SO(3)$, only for rotation.

1.1.1 The exponential map.

  $so(3)$->$SO(3)$ . The skew matrix $w^{\wedge },w\in {\mathbb{R}}^3$ in Lie algebra $so(3)$ is corresponding to the rotation matrix $R\in {\mathbb{R}}^{3\times 3}$ in the Lie group $SO(3)$. We can use exponential map applied to the skew matrix $w^{\wedge }$ , then, we can get the $R$. (We can use Rodrigues’ formula to compute the $R$.)
$$exp:so(3)->SO(3);w^{\wedge }->e^{w^{\wedge }}.$$
So, through the exponential map, we have:
$$R=e^{w^{\wedge }}=I+sin(\theta )n^{\wedge }+(1-cos\theta )n^{\wedge }{n}^{\wedge }.$$
  By introducing Lie algebra, we don’t need to explicitly construct a rotation matrix $R$ with so many constraints. The 9 parameters in rotation matrix $R $ can be represented by 3 parameters in $w$. Then, applied the exponential map to its skew matrix. We can get $R$. These constraints are:
$$R^{T}R=I,det(R)=1;r_1\ast r_2=r_2\ast r_3=0,$$
each component is orthogonal to another in the rotation $R$.

1.1.2 The Logarithm map.

  One can also use the Logarithm of $SO(3)$, to map the rotation matrix into $so(3)$, and get the $w$. Typically, we use axis-angle to represent the $w=\theta \mathbf{n}$, where $\theta$ is the rotation angle, and $\mathbf{n}$ is the rotation axis. This means a rotation around the axis $\mathbf{n}$ by an angle of $\theta$ (if $\Vert n\Vert =1$):
$$\theta =arccos(\frac{trace(R)-1}{2})$$

$$\mathbf{n}=\frac{1}{2sin(\theta )}\left(\begin{array}{c}{r}_{32}-r_{23}\\ r_{13}-r_{31}\\ r_{21}-r_{12}\end{array}\right)$$

1.2 $se(3)$->$SE(3)$, for rigid-body motion.

  The motion of a rigid-body is determined by specifying the translation $T$ to any given point, and a rotation matrix $R$ to rotate the coordinate frame at the given point.

  The $4\times 4$ matrix ${\xi }^{\wedge }$ is a twist (You can rotate and translate the rigid-body at the same time, which resulted in a twist effect in the processing, the effect figure is illustrated below). The twist in Lie algebra $se(3)$ at the tangent space of origin corresponding to the Lie group $SE(3)$.

The effect of twist. Twist的效果就是，又有旋转，也有平移，他的轨迹是扭曲的。

  We can also use a twist matrix ${\xi }^{\wedge }\in se(3)$ or its twist coordinate $\xi \in {\mathbb{R}}^6$, $w$ is the skew matrix, and $v$ is the 3D vector.
$${\xi }^{\wedge }={\left(\begin{array}{c}v\\ w\end{array}\right)}^{\wedge }=\left(\begin{array}{cc}{w}^{\wedge }& v\\ 0& 0\end{array}\right)\in {\mathbb{R}}^{4\times 4},$$

$$\xi ={\left(\begin{array}{cc}{w}^{\wedge }& v\\ 0& 0\end{array}\right)}^{\vee }=\left(\begin{array}{c}v\\ w\end{array}\right)\in {\mathbb{R}}^6,$$

$se(3)->SE(3)$, using the twist matrix ${\xi }^{\wedge }$.
$$exp:se(3)->SE(3);{\xi }^{\wedge }->e^{{\xi }^{\wedge }}.$$

$$g=(R,T)=exp({\xi }^{\wedge }).$$

​  The 12 parameters in the transformation matrix $g$ now only need 6 parameters to represent, which means 3 rotation freedom and 3 translation freedom.

  Given: $g=(R,T)\in SE(3)$, there exist many twist coordinates $\xi =(v,w)\in {\mathbb{R}}^6$ such that $g=exp({\xi }^{\wedge })$.

  Proof: the skew matrix can be computed through the rotation matrix $e^{w^{\wedge }}=R$, which is the same one in $SO(3)->so(3)$. Once we know $w$, the velocity vector $v\in {\mathbb{R}}^3$ could also be computed by solving the equation:
$$\frac{(I-e^{w^{\wedge }})w^{\wedge }v+w{w}^{T}v}{|w{|}^2}=T.$$
  As described above, when given the twist matrix ${\xi }^{\wedge }$, we can directly compute the transformation matrix (either exponential map or Rodrigues’ formula is ok.):
$$g(t)=e^{{\xi }^{\wedge }}=\left(\begin{array}{cc}{e}^{w^{\wedge }}& \frac{(I-e^{w^{\wedge }})w^{\wedge }v+w{w}^{T}v}{|w{|}^2}\\ 0& 1\end{array}\right)=\left(\begin{array}{cc}R& T\\ 0& 1\end{array}\right).$$
We can use Rodrigues’ formula to compute $R$. After that, we can compute $T$, since $w,v$ are known.

2. The motion of the camera.

  When observing a scene from a moving camera, the coordinates and velocity of a point in the camera coordinate will change over time. At this point, we use rigid body transformation to represent the motion from a fixed world frame to the moving camera frame at time $t$.
$$X(t)=g(t)X_0,$$
where $X_0$ is the point in the world coordinate. This transformation models the point’s change over time $t$.

​  Why do we use Lie group and Lie algebra, when using the traditional method, every time the camera moves, we need to compute a new rotation and translation matrix to represent the new transformation $T$. And the matrix $T$ change over time is not differentiable, not summable, every change of the camera pose is independent in the $SE(3)$. But the motions of the camera at infinitesimal is a continuous procedure, which means the camera is rotated infinitesimally and translated infinitesimally. We can use velocity and movement to represent it: $X(dt)=I+\dot{g}\ast dt$.

2.1 Concatenation of motions over frame.

​  The transformation from points in frame at $t_1$ to the points in frame at $t_2$ by transformation is $g(t_2,t_1)$:
$$X(t_2)=g(t_2,t_1)X(t_1).$$
So, at $t_3$, we have:
$$g(t_3,t_1)=g(t_3,t_2)g(t_2,t_1).$$
By transferring the coordinate of $t_1$ to $t_2$ and back, we have:
$$X(t_1)=g(t_1,t_2)X(t_2)=g(t_1,t_2)g(t_2,t_1)X(t_1).$$
Thus, we have:
$$g(t_1,t_2)g(t_2,t_1)=I<=>g(t_1,t_2)=g(t_2,t_1{)}^{-1}.$$

2.2 Rules of velocity transformation.

  At time $t$, $X(t)=g(t)X_0$, how it changes over time $t$ is the velocity:
$$\dot{X}(t)=\dot{g}(t)X_0=\dot{g}(t)g^{-1}(t)X(t).$$
By introducing the twist coordinates:
$$V^{\wedge }(t)=\dot{g}(t)g^{-1}(t)=\left(\begin{array}{cc}{w}^{\wedge }(t)& v(t)\\ 0& 0\end{array}\right)\in se(3).$$
So, we have:
$$\dot{X}(t)=V^{\wedge }(t)X(t),$$
this indicates the velocity of points in the camera frame. $V^{\wedge }(t)$ is the relative velocity of the world coordinate frame as viewed from the camera frame. (Camera is fixed, wathcing the object in the world transformation.)

2.3 The adjoint map.

  How to view one point in another camera frame $B$ in the Lie algebra? Suppose camera frame $B$ to current frame $A$ is displaced by a transformation $g_{xy}:Y(t)=g_{xy}X(t)$ .

Then, the velocity in the new frame is:
$$\dot{Y}(t)=g_{xy}\dot{X}(t)=g_{xy}{V}^{\wedge }(t)X(t)=g_{xy}{V}^{\wedge }(t)g_{xy}^{-1}Y(t).$$
The relative velocity of points observed from another camera frame $B$ is represented by the twist:
$$V_y^{\wedge }=g_{xy}{V}^{\wedge }{g}_{xy}^{-1}\equiv a{d}_{g_{xy}}(V^{\wedge }).$$
So, observing the scene from camera $B$’s view, and the scene is transformed by camera $A$, is called the adjoint map on $se(3)$:
$$a{d}_g:se(3)->se(3);{\xi }^{\wedge }->g{\xi }^{\wedge }{g}^{-1}.$$
  An adjoint map is used to model the rotation/transformation between frames or cameras in Lie algebra. Based on the adjoint map, one needn’t construct a difficult matrix to map the object in the world coordinate to a new view frame.

3. Summary.

  We summarize the skew matrix and twist matrix in Lie algebra and their corresponding matrixs in the Lie group, respectively.

4. References.

  1. Chapter 2 - Representing a Moving Scene.pdf
  2. Multiple View Geometry - Lecture 3, Technical University of Munich.
  3. 机器人与视觉——李群与李代数，李括号性质的分析与证明。