三维旋转矩阵_三维重建中的旋转(Rotation)

旋转在三维重建中是比较重要的，这里主要对旋转的性质及应用做一些总结。

1. 旋转矩阵

设某个单位正交基

经过一次旋转变成了

。那么，对于同一个向量

(注意该向量并没有随着坐标系的旋转而发生运动)，它在两个坐标系下的坐标为

和

。

由坐标的定义，有：

为描述两个坐标之间的关系，(1)左右两边同时乘以

，则有

其中

即为旋转矩阵。旋转矩阵的集合定义为:

由于旋转矩阵是正交阵，它的逆(即转置)描述了一个相反的旋转，则有

。显然，

刻画了一个相反的旋转。

2. 李群和李代数 (Lie Group and Lie Algebra)

这里我们只描述旋转空间上的 李群和 李代数。

李群是指具有连续光滑性质的群。

在实数空间上是连续的。我们能够直观想象一个刚体能够连续地在空间中运动，所以他们都是李群。每个李群都有与之对应的李代数，李代数描述了李群的局部性质。旋转空间上的李群已经在公式(3)中做了描述。

2.1 旋转空间上的李代数推导

设

为某个相机的旋转，随时间连续变化，即

为关于时间

的函数

。由于

为正交阵，则有

等式两边对

求导，则有

。整理可得，

由(5)可以看出

是一个 反对称矩阵 (反对称矩阵的定义:

)。

而对于 反对称矩阵，我们总能找到一个三维向量

与之对应。一般地，

表示向量到反对称阵的变换。因此，我们有

对公式(6), 左右两边同时右乘

可得

公式(7)是一个形如

的常微分方程，对方程两边同时去倒数，则有

。显然，

的解为

，进一步可得

。将公式(7)代入，可得

2.2 李代数

李群

对应的李代数是定义在

上的向量，记为

。每个

都可以生成一个反对称矩阵

:

一般说，

的元素是三维向量或者三维反对称矩阵，不加区别:

至此，我们知道

是一个由三维向量组成的集合，每个向量对应到一个反对称矩阵，可以表达旋转矩阵的倒数。它与

的关系通过 指数映射 (exponential mapping)给定:

2.3 推导 $mathfrak{so}(3)$ 上的指数映射

由 公式 (10) 可知，它是一个矩阵的指数。在李群和李代数中，称为 指数映射 (exponential mapping)。

我们知道指数函数的`幂级数`为

同样地，对于

中的任意元素

，它的指数映射为

令

，其中

为方向，

为长度为1的方向向量。对于

，有两个性质：

• 

• 

利用这两个性质，指数映射可以写为:

公式 (12) 就是 罗德里格斯公式， 指数映射即罗德里格斯公式！

同样，我们可以定义`对数映射`，把 $SO(3)$ 中的元素对应到 $mathfrak{so}(3)$ 中 :

不过，一般不会按照泰勒展开计算对数映射，而是通过旋转矩阵恢复李代数：

令转轴为

，转角为

，

(1) 计算转角

。对于转角

，由罗德里格斯公式，有

，因此，

。

(2) 计算转轴

，由于旋转轴上的向量在旋转后不发生改变，有

。因此，转轴

是矩阵

特征值为1对应的特征向量。求解次方程，再归一化，就得到了转轴。最后，李代数可以写为

。

3. 四元数 (Quanternion)

四元数是Hamilton找到的一种扩展的复数，拥有一个实部和三个虚部，可表示为 :

这三个虚部满足以下关系:

3.1 四元数的运算

设

，则

3.2 四元数表示旋转

假设旋转绕单位向量

进行了角度为

的旋转，则该旋转的四元数定义为:

令

，则

。即

和

表示同一个旋转。

令

。

4. 旋转矩阵、角轴表示法、四元数之间的转换 4.1 旋转矩阵与四元数 四元数到旋转矩阵的转换

通过 3.2 节，即可通过四元数计算得到旋转矩阵。

旋转矩阵到四元数的转换

令旋转矩阵为

则旋转矩阵到四元数的转换如下:

4.2 四元数与角轴表示法 四元数到角轴表示法的转换

采用 3.2 节的符号表示，很容易得到

角轴表示法到四元数的转换

通过 3.2 节即可得到。

4.3 旋转矩阵与角轴表示法的转换 角轴表示法到旋转矩阵

很显然，通过指数映射，使用罗德里格斯公式就可以将角轴表示法变换到旋转矩阵

旋转矩阵到角轴表示法

一般会先将旋转矩阵转换到四元数，再通过四元数转换到角轴表示法。

5. 罗德里格斯旋转公式 (Rodrigues' rotation formula)

和罗德里格斯公式不同(李代数到李群的指数映射)，`罗德里格斯旋转公式`是用于对向量进行旋转的，即 给定空间中的一个三维点(具体形式是一个三维向量)，如何通过轴角表示法对三维点进行旋转。这个公式也被扩展用于计算指数映射，也就是罗德里格斯公式。罗德里格斯旋转公式可以通过以下公式计算得到:

5.1 对三维点进行旋转

实际中，使用轴角表示法对三维点进行旋转会分两种情况：

-

相比0较大时。这个时候可以通过 罗德里格斯旋转公式计算

-

相比0较小时。根据罗德里格斯公式，

当

较小时，

。因此，

。于是，

注： 对于李代数

，其中

为单位转轴，因此

，于是

。因此，有

。

5.2 Ceres中的实现

template<typename T> inline
void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]) {
  const T theta2 = DotProduct(angle_axis, angle_axis);
  if (theta2 > T(std::numeric_limits<double>::epsilon())) {
    // Away from zero, use the rodriguez formula
    //
    //   result = pt costheta +
    //            (w x pt) * sintheta +
    //            w (w . pt) (1 - costheta)
    //
    // We want to be careful to only evaluate the square root if the
    // norm of the angle_axis vector is greater than zero. Otherwise
    // we get a division by zero.
    //
    const T theta = sqrt(theta2);
    const T costheta = cos(theta);
    const T sintheta = sin(theta);
    const T theta_inverse = T(1.0) / theta;

    const T w[3] = { angle_axis[0] * theta_inverse,
                     angle_axis[1] * theta_inverse,
                     angle_axis[2] * theta_inverse };

 // Explicitly inlined evaluation of the cross product for
 // performance reasons.
    const T w_cross_pt[3] = { w[1] * pt[2] - w[2] * pt[1],
                              w[2] * pt[0] - w[0] * pt[2],
                              w[0] * pt[1] - w[1] * pt[0] };
    const T tmp =
        (w[0] * pt[0] + w[1] * pt[1] + w[2] * pt[2]) * (T(1.0) - costheta);

    result[0] = pt[0] * costheta + w_cross_pt[0] * sintheta + w[0] * tmp;
    result[1] = pt[1] * costheta + w_cross_pt[1] * sintheta + w[1] * tmp;
    result[2] = pt[2] * costheta + w_cross_pt[2] * sintheta + w[2] * tmp;
  } else {
    // Near zero, the first order Taylor approximation of the rotation
    // matrix R corresponding to a vector w and angle theta is
    //
    //   R = I + hat(w) * sin(theta)
    //
    // But sintheta ~ theta and theta * w = angle_axis, which gives us
    //
    //  R = I + hat(w)
    //
    // and actually performing multiplication with the point pt, gives us
    // R * pt = pt + w x pt.
    //
    // Switching to the Taylor expansion near zero provides meaningful
    // derivatives when evaluated using Jets.
    //
    // Explicitly inlined evaluation of the cross product for
    // performance reasons.
    const T w_cross_pt[3] = { angle_axis[1] * pt[2] - angle_axis[2] * pt[1],
                              angle_axis[2] * pt[0] - angle_axis[0] * pt[2],
                              angle_axis[0] * pt[1] - angle_axis[1] * pt[0] };

    result[0] = pt[0] + w_cross_pt[0];
    result[1] = pt[1] + w_cross_pt[1];
    result[2] = pt[2] + w_cross_pt[2];
  }
}
           

6.

上的距离测量 (Distance Measure) (1) Bi-invariant distance

如果对于所有的

和

，我们有 :

那么这样的一个距离测量称为 Bi-invariant。

(2) Angular Distance (Geodesic Distance)

定义 R 和 S 之间的 `angular distance` 为旋转 $SR^T$ 的角度，并且处于区间 $[0, pi]$ 里。因此，

距离测量函数

和

的旋转角度相等。需要注意，我们可能会等价地写为

，

，

，因为这些都表示同一个旋转。

(3) Chordal Distance

旋转

和

之间的 chordal distance 定义为:

其中，

表示矩阵的 Frobenius 范数。 chordal distance 和 angular distance`之间可以通过罗德里格斯公式关联起来:

具体来讲，令

，由于

和

正交，且

，因此，我们有

因此，

7. 扰动模型对旋转求导

设左扰动

对应的李代数为

。对

求导，有

References

1. 视觉SLAM十四讲 从理论到实践
2. https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula
3. https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation
4. Hartley, Richard, Trumpf, et al. Rotation Averaging[J]. International Journal of Computer Vision, 2013, 103(3):267-305.