Sophus 李群 --[SO3]

by luoshi006

参考：
http://ethaneade.com/lie.pdf
http://www.qiujiawei.com/understanding-quaternions/#4

Sophus 李群

简介

本文主要介绍表示三维空间变换（transformation）的李群。李群是一个拓扑群，也是一个光滑流形。主要用于机器人及计算机视觉领域。
Sophus 在 Eigen 的基础上，实现了 李群 李代数 的计算。三维旋转的计算以四元数为基础（Eigen :: Quaterniond）。

//根据高博文章，此处采用相同的代码版本；
git clone https://github.com/strasdat/Sophus.git 
cd Sophus 
git checkout a621ff 

---

SO3

变量

SO3 类中只声明一个变量：

protected:
  Quaterniond unit_quaternion_;

计算过程中所需的 旋转矩阵 反对称矩阵 等，都在调用时通过 四元数 计算。

Note
unit_quaternion_ 为 单位四元数，$(w,x,y,z)$。
用于表示旋转的四元数都需要归一化，将四元数的自由变量限制在 三维。

四元数

四元数的一般形式：

$$q = w+x\mathbf i + y \mathbf j + z \mathbf k$$

也可以如下表示：

$$q = [w,\mathbf v] = [w,x\mathbf i + y \mathbf j + z \mathbf k]$$

纯四元数：实部为 0 的四元数，

$$q = [0, \mathbf v] = [0,x\mathbf i + y \mathbf j + z \mathbf k]$$

共轭四元数：虚部取反的四元数，

$$q=[w,\mathbf v]\\ q^*=[w,-\mathbf v]$$

共轭四元数的乘积：

$$q\cdot q^* = [w,\mathbf v]\cdot [w,-\mathbf v]\\ = [w^{2} - \mathbf {v}\cdot (-\mathbf {v}),-w\mathbf {v}+w\mathbf {v}+\mathbf {v}\times (-\mathbf {v})]\\ = [s^{2} + \mathbf {v}^{2}, \mathbf {0}]\\ $$

四元数范数

$$||q||=\sqrt{w^2 + \mathbf v ^2}$$

四元数归一化
四元数除以其范数，得到归一化形式。

$$\mathbf q' = \frac {\mathbf q}{\sqrt {w^{2}+v^{2}}}$$

单位四元数：范数为 1 的四元数，【欧拉参数】

$$q=[cos\frac{\theta}{2}, \mathbf v\cdot sin\frac{\theta}{2}]$$

其中，$||\mathbf v|| = 1$。

对于单位四元数，有：

$$q\cdot q^* = [1,\mathbf 0]=[1,0,0,0]\\ q^{-1} = q^*\\ $$

---

函数

构造函数

  SO3();
  SO3(const SO3 & other);
  SO3(const Matrix3d & _R);
  SO3(const Quaterniond & unit_quaternion);
  SO3(double rot_x,
      double rot_y,
      double rot_z);

构造函数的实现比较简单，直接调用 Eigen::Quaterniond 的构造函数完成 单位四元数 的初始化。

运算符重载

void    SO3::operator=(const SO3 & other)
SO3     SO3::operator*(const SO3& other) const
void    SO3::operator*=(const SO3& other)

四元数的乘法表示两次旋转依次执行。

Vector3d SO3::operator*(const Vector3d & xyz) const{
  return unit_quaternion_._transformVector(xyz);
  //Rotation of a vector by a quaternion.
  //向量 xyz 以 四元数 进行旋转；
}

求逆

对于 $R \in SO(3)$ ， $R^{-1} = R^T$ 。旋转矩阵的逆对应于四元数的共轭。

SO3 SO3
::inverse() const
{
  return SO3(quaternion_.conjugate());
}

求旋转矩阵

调用 eigen 库，求取四元数对应的旋转矩阵。

Matrix3d SO3
::matrix() const
{
  return quaternion_.toRotationMatrix();
  //Convert the quaternion to a 3x3 rotation matrix. 
}

---

SO3 的伴随 Adjoint

用于在 SO3 的正切平面上切换。
定义：

$$\omega \in so(3), \mathbf R \in SO(3)\\ \mathbf R\cdot exp(\omega) = exp(Adj_ \mathbf R \cdot \omega)\cdot \mathbf R$$

推导：

$$\begin{equation}\begin{split} exp(Adj_ \mathbf R \cdot \omega) &= \mathbf R \cdot exp(\omega) \cdot \mathbf R^{-1}\\ Adj_\mathbf R \cdot \omega &= \mathbf R \cdot (\sum_{i=1}^{3}\omega_i G_i)\cdot \mathbf R^{-1}\\ &=\mathbf R\cdot \omega_\times\cdot \mathbf R^{-1}\\ &=(\mathbf R \omega)_\times\\ 故：Adj_\mathbf R &= \mathbf R\\ \end{split}\end{equation}$$

注：
若 $P$ 是一个 $n \times n$ 阶可逆矩阵，则

$$e^{P^{-1}AP} = P^{-1}e^AP$$

Matrix3d SO3
::Adj() const
{
  return quaternion_.toRotationMatrix();
}

---

生成元 Generator

李代数 $so(3)$ 的生成元，即 $so(3)$ 互不相干的独立变量。各个生成元之间相互正交。

$$\begin{equation}\begin{split}\\ \mathcal \omega = [\omega_1, \omega_2, \omega_3]^T &\in \mathbb R^3\\ \mathcal \omega_\times = \omega_1G_1+\omega_2G_2+\omega_3G_3 &\in so(3)\\ \end{split}\end{equation}$$

其中，生成元：

$$G_1= \begin{pmatrix} 0 & 0 & 0\\ 0 & 0 & -1\\ 0 & 1 & 0\\ \end{pmatrix},\\ G_2= \begin{pmatrix} 0 & 0 & 1\\ 0 & 0 & 0\\ -1 & 0 & 0\\ \end{pmatrix}\\ G_3= \begin{pmatrix} 0 & -1 & 0\\ 1 & 0 & 0\\ 0 & 0 & 0\\ \end{pmatrix}$$

Matrix3d SO3
::generator(int i)
{//参数i 对应生成元索引号；
  assert(i>=0 && i<3);
  Vector3d e;
  e.setZero();
  e[i] = 1.f;
  return hat(e);
}

其中，

Matrix3d SO3
::hat(const Vector3d & v)
{
  Matrix3d Omega;
  Omega <<  0, -v(2),  v(1)
      ,  v(2),     0, -v(0)
      , -v(1),  v(0),     0;
  return Omega;
}

---

对数运算

对于单位四元数，有$q=[cos\frac{\theta}{2}, \mathbf v\cdot sin\frac{\theta}{2}]$。其中，$||\mathbf v|| = 1$。
对数运算：

$$[\mathbf{\omega} t]_\times = ln(\mathbf R) = \frac{\theta}{2sin\theta} \cdot (\mathbf{R-R^T})$$
角速度矢量：
$$\theta \mathbf v = \frac{\theta}{sin\frac{\theta}{2}}\cdot \mathbf v \cdot sin\frac{\theta}{2}$$

// Sophus first commit 版本代码；
Vector3d SO3
::log() const
{
  return SO3::log(*this);
}

Vector3d SO3
::log(const SO3 & other)
{
  double q_real = other.quaternion_.w();
  if (q_real>1.-SMALL_EPS)
  {
    return Vector3d(0.,0.,0.);
  }

  double theta = 2.*acos(q_real);
  double theta_by_sin_half_theta = theta/sqrt(1. - q_real*q_real);

  return Vector3d(theta_by_sin_half_theta*other.quaternion_.x(),
                  theta_by_sin_half_theta*other.quaternion_.y(),
                  theta_by_sin_half_theta*other.quaternion_.z());
}

在后期版本中，对这部分代码优化：

https://arxiv.org/pdf/1107.1119.pdf

$$\overline{log}\left[\begin{matrix}w\\v\end{matrix}\right] =\left\{ \begin{aligned}0 \ \ \ \ & v=0\\ \frac{atan(||v||/w)}{||v||} v \ \ \ \ & v\neq 0, w\neq0\\ \pm\frac{\pi/2}{||v||}v \ \ \ \ &w=0\\ \end{aligned} \right.\\ $$

Vector3d SO3
::logAndTheta(const SO3 & other, double * theta)
{

    double n = other.unit_quaternion_.vec().norm();
    double w = other.unit_quaternion_.w();
    double squared_w = w*w;

    double two_atan_nbyw_by_n;
    // Atan-based log thanks to
    //
    // C. Hertzberg et al.:
    // "Integrating Generic Sensor Fusion Algorithms with Sound State
    // Representation through Encapsulation of Manifolds"
    // Information Fusion, 2011

    if (n < SMALL_EPS)
    {
      // If quaternion is normalized and n=1, then w should be 1;
      // w=0 should never happen here!
      assert(fabs(w)>SMALL_EPS);

      two_atan_nbyw_by_n = 2./w - 2.*(n*n)/(w*squared_w);
    }
    else
    {
      if (fabs(w)<SMALL_EPS)
      {
        if (w>0)
        {
          two_atan_nbyw_by_n = M_PI/n;
        }
        else
        {
          two_atan_nbyw_by_n = -M_PI/n;
        }
      }
      two_atan_nbyw_by_n = 2*atan(n/w)/n;
    }

    *theta = two_atan_nbyw_by_n*n;
    return two_atan_nbyw_by_n * other.unit_quaternion_.vec();
}

---

指数映射

使用三维角速度矢量 $\omega$ 构造四元数，得到SO3，$q=[cos\frac{\theta}{2}, \mathbf v\cdot sin\frac{\theta}{2}]$。
其中，$\theta = \sqrt{\omega^T \omega}$

SO3 SO3
::exp(const Vector3d & omega)
{
  double theta = omega.norm();

  if(theta<SMALL_EPS)
  {
    return SO3(Quaterniond::Identity());
  }

  double half_theta = 0.5*theta;
  double sin_half_theta = sin(half_theta);
  double sin_half_theta_by_theta = sin_half_theta/theta;
  return SO3(Quaterniond(cos(half_theta),
                         sin_half_theta_by_theta*omega.x(),
                         sin_half_theta_by_theta*omega.y(),
                         sin_half_theta_by_theta*omega.z()));
}

在后期版本中，对这部分代码优化：
在小角度情况下加入优化细节【没看懂】

SO3 SO3
::expAndTheta(const Vector3d & omega, double * theta)
{
  *theta = omega.norm();
  double half_theta = 0.5*(*theta);

  double imag_factor;
  double real_factor = cos(half_theta);
  if((*theta)<SMALL_EPS)
  {
    double theta_sq = (*theta)*(*theta);
    double theta_po4 = theta_sq*theta_sq;
    imag_factor = 0.5-0.0208333*theta_sq+0.000260417*theta_po4;
  }
  else
  {
    double sin_half_theta = sin(half_theta);
    imag_factor = sin_half_theta/(*theta);
  }

  return SO3(Quaterniond(real_factor,
                         imag_factor*omega.x(),
                         imag_factor*omega.y(),
                         imag_factor*omega.z()));
}

---

vee 运算（$\cdot^\vee$）

vee 运算由反对称矩阵得到三维角速度矢量：

$$\cdot^{\vee} : \omega_\times \in \mathbb{R}^{n\times n} \rightarrow \mathbb{R}^n$$

$$\omega_\times = \begin{pmatrix} 0&-\omega_3&\color{magenta}{\omega_2}\\ \color{magenta}{\omega_3}&0&-\omega_1\\ -\omega_2&\color{magenta}{\omega_1}&0 \end{pmatrix}$$

Vector3d SO3
::vee(const Matrix3d & Omega)
{
  assert(fabs(Omega(2,1)-Omega(1,2))<SMALL_EPS);
  assert(fabs(Omega(0,2)-Omega(2,0))<SMALL_EPS);
  assert(fabs(Omega(1,0)-Omega(0,1))<SMALL_EPS);
  return Vector3d(Omega(2,1), Omega(0,2), Omega(1,0));
}

李括号

李括号定义了两个向量场之间的差异，此处李括号封闭叉乘运算。

Vector3d SO3
::lieBracket(const Vector3d & omega1, const Vector3d & omega2)
{
  return omega1.cross(omega2);
}

deltaR

此处得到 $2\omega$，在后期代码中已删除。

Vector3d SO3::
deltaR(const Matrix3d & R)
{
  Vector3d v;
  v(0) = R(2,1)-R(1,2);
  v(1) = R(0,2)-R(2,0);
  v(2) = R(1,0)-R(0,1);
  return v;
}