视觉slam第四讲笔记：优化理论基础：李群李代数求导

视觉SLAM第四讲笔记

一.符号说明

$\dot{R}$ ：矩阵R的一阶导数
$\hat{}$ ： 将一个向量变为反对称矩阵，
$\stackrel{ˇ}{}$ ： 将一个反对称矩阵变为向量
$$\hat{a}=A=\left(\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right),\check{A}=a$$

二.问题：

问题：什么样的相机位姿最符合当前的观测数据？
解决方法：将其构建为优化问题，求解最有的Rt，使得误差最小。旋转矩阵有正交单位的约束，采用李代数可以将其化为无约束的优化问题。
数学模型： 某时刻机器人位姿 $T$,观测到位于世界坐标的 $p$点的数据为$z$,设观测噪声为$w$, 则$$z=Tp+w$$理想情况下为$z=Tp$, 故误差为
$$e=z-Tp$$对于N个点，则有N个式子，需要找出 $T$ 满足整体误差最小，即
$$\underset{T}{\min }J(T)=\sum _{i=1}^N||z_i-T{p}_i|{|}_2^2$$因此需要计算 $J(T)$ 关于 $T$ 的导数。

思路：

• 1.用李代数表示姿态，根据李代数加法对李代数求导
• 2.对李群左乘或右乘微小扰动，对扰动求导。

三.李群和李代数：

3.1李群

特殊正交群：旋转矩阵的集合：$SO(n)$= { $R\in {\mathbb{R}}^{n\times n}|R{R}^{{}^T}=I,det(R)=1$ }
特殊欧氏群：变换矩阵的集合：$SE(3)$= { $T=$$\left(\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right)\in {\mathbb{R}}^{4\times 4}$|$R\in SO(3),t\in {\mathbb{R}}^3$}
群：一种集合只有一种运算的代数结构
李群：具有连续（光滑）性质的群

3.2李代数的引出

根据旋转矩阵的性质，$R{R}^T=I$,对两边分别求导，可知
$$\dot{R(t)}R(t{)}^T=-(\dot{R(t)}R(t{)}^T{)}^T$$即为反对称矩阵，故根据符号说明处，可以找到一个与之对应的三维向量
$$\dot{R(t)}R(t{)}^T=\widehat{\varphi (t)}$$所以$$\dot{R(t)}=\widehat{\varphi (t)}R(t)=\left(\begin{array}{ccc}0& -{\varphi }_3& {\varphi }_2\\ {\varphi }_3& 0& -{\varphi }_1\\ -{\varphi }_2& {\varphi }_1& 0\end{array}\right)R(t)$$
可以看出，每次对 $R$ 求导只需要左乘 $\hat{\varphi }(t)$
对$R(t)在t=t_0$时刻进行泰勒展开：
$$\dot{R}=\hat{\varphi }(t_0)R(t)={\hat{\varphi }}_{0}R(t)$$解得：
$$R(t)=exp({\hat{\varphi }}_{0}t)$$该泰勒展开只在t=0附近有效。
效果：旋转矩阵和反对称矩阵通过指数关系联系在一起。即当某个时刻的R已知，可以求得向量$\varphi$来描述R在局部的导数关系，$\varphi$即李代数。

3.3李代数：

每个李群都有相应的李代数，李代数描述了李群的局部性质。
李代数=1个集合+1个数域+1个二元运算[,]组成.

李代数$\mathfrak{s}\mathfrak{o}(3)$：

定义在 ${\mathbb{R}}^3$上的向量，$\varphi$为$SO(3)$对应的李代数。
因为每个向量$\varphi$对应一个反对称矩阵$$\Phi =\hat{\varphi }=\left(\begin{array}{ccc}0& -{\varphi }_3& {\varphi }_2\\ {\varphi }_3& 0& -{\varphi }_1\\ -{\varphi }_2& {\varphi }_1& 0\end{array}\right)$$定义李代数运算，即两个向量${\varphi }_1,{\varphi }_2$的李括号为：
$$[{\varphi }_1,{\varphi }_2]=\check{({\Phi }_1{\Phi }_2-{\Phi }_2{\Phi }_1)}$$
李代数$\mathfrak{s}\mathfrak{o}(3)$小结：
集合$\mathfrak{s}\mathfrak{o}(3)$的元素是三维向量或者三维反对称矩阵，可以表示旋转矩阵的导数：
s o ( 3 ) = { ϕ ∈ R 3 , Φ = ϕ ^ ∈ R 3 × 3 } \mathfrak{so(3)}=\{\phi\in \mathbb{R}^{3},\Phi =\hat\phi\in \mathbb{R}^{3\times 3}\} so(3)={ϕ∈R3,Φ=ϕ^​∈R3×3}
并且和特殊正交群$SO(n)$的关系为$$R=exp(\hat{\varphi })$$

李代数$\mathfrak{s}\mathfrak{e}(3)$：

位于${\mathbb{R}}^6$空间中，为特殊欧氏群$SE(3)$对应的李代数
s e ( 3 ) = { ξ = ( ρ ϕ ) ∈ R 6 , ρ ∈ R 3 , ϕ ∈ s o ( 3 ) , ξ ^ = ( ϕ ^ ρ 0 T 0 ) ∈ R 4 × 4 } \mathfrak{se(3)}= \{ \xi=\begin{pmatrix} \rho \\ \phi \end{pmatrix} \in \mathbb{R}^{6},\rho \in \mathbb{R}^{3},\phi\in \mathfrak{so(3)} ,\hat{\xi}= \begin{pmatrix} \hat{\phi} & \rho \\ 0^T & 0 \end{pmatrix}\in \mathbb{R}^{4 \times 4}\} se(3)={ξ=(ρϕ​)∈R6,ρ∈R3,ϕ∈so(3),ξ^​=(ϕ^​0T​ρ0​)∈R4×4}
元素 $\xi$ 为六维向量, $\rho$表示平移（不是直接的平移），，$\varphi$表示旋转，$\hat{\xi }$表示将六维向量转为四维矩阵，此处不再表示反对称
李代数$\mathfrak{s}\mathfrak{e}(3)$对应的运算为：
[ ξ 1 , ξ 2 ] = { ξ 1 ^ ξ 2 ^ − ξ 2 ^ ξ 1 ^ ˇ } [\xi_{1},\xi_{2}]=\{\check{\hat{\xi_{1}}\hat{\xi_{2}}-\hat{\xi_{2}}\hat{\xi_{1}}}\} [ξ1​,ξ2​]={ξ1​^​ξ2​^​−ξ2​^​ξ1​^​ˇ​}

四.指数和对数映射

问题：由前面已知 $R=exp(\hat{\varphi })$，问题：如何计算矩阵的指数$exp(\hat{\varphi })$？
定义：矩阵的指数：指数映射

李代数$\mathfrak{s}\mathfrak{o}(3)$的指数映射

因为，任意矩阵 $A$ 的指数映射为 $$exp(A)=\sum _{n=0}^{\infty }\frac{1}{n!}{A}^n$$所以对$\mathfrak{s}\mathfrak{o}(3)$中的任意元素$\varphi$对应的三维矩阵 $\hat{\varphi }$可以定义指数映射为
$$R=exp(\hat{\varphi })=\sum _{n=0}^{\infty }\frac{1}{n!}(\hat{\varphi }{)}^n$$对于三维向量$\varphi$定义模长和方向表示为$\varphi =\theta a$，其中$a$为单位向量,带入上式$\hat{\varphi }=\theta \hat{a}$,则
$$exp(\hat{\varphi })=exp(\theta \hat{a})=\sum _{n=0}^{\infty }\frac{1}{n!}(\theta \hat{a}{)}^n=\cos \theta I+(1-\cos \theta )a{a}^T+\sin \theta \hat{a}$$
上式和Rodrigues变换一致，说明：$\mathfrak{s}\mathfrak{o}(3)$为旋转向量组成的空间，其中的元素为旋转向量，其对应的指数映射为Rodrigues变换。通过上式，可以将李代数$\mathfrak{s}\mathfrak{o}(3)$中的任一旋转向量对应到$SO(3)$的旋转矩阵。

定义对数映射：
$$\varphi =ln\check{(R)}=(\sum _0^{\infty }\frac{(-1{)}^n}{n+1}(R-I{)}^{n+1})\stackrel{ˇ}{}$$

---

小结：李代数是旋转向量，指数映射为旋转向量到旋转矩阵的Rodrigues变换，对数映射是旋转矩阵到旋转向量的运算，旋转矩阵的导数可以有旋转向量指定。

---

李代数$\mathfrak{s}\mathfrak{e}(3)$的指数映射

根据类似的泰勒展开，
$$T=exp(\hat{\xi })=\left(\begin{array}{cc}{\sum }_{n=0}^{\infty }\frac{1}{n!}(\hat{\varphi }{)}^n& {\sum }_{n=0}^{\infty }\frac{1}{(n+1)!}(\hat{\varphi }{)}^n\rho \\ 0^T& 1\end{array}\right)=\left(\begin{array}{cc}R& J\rho \\ 0^T& 1\end{array}\right)=T$$根据 $\varphi =\theta a$，则
$$J=\frac{\sin \theta }{\theta }I+(1-\frac{\sin \theta }{\theta })a{a}^T+\frac{1-\cos \theta }{\theta }\hat{a}$$
小结：平移部分经过指数映射后，发生了以$J$为系数矩阵的线性变换。
$$t=J\rho$$

小结：图解

下次附上来：P71

五.李代数求导

BCH公式与近似形式

两个李代数指数映射乘积不符合指数和对数运算的规律，由BCH公式给出：
$$ln(R_1{R}_2)=ln(exp({\hat{\varphi }}_1)exp({\hat{\varphi }}_2))\stackrel{ˇ}{}\approx \{\begin{array}{l}{J}_l({\varphi }_2{)}^{-1}{\varphi }_1+{\varphi }_2，当{\varphi }_1为小量\\ J_r({\varphi }_1{)}^{-1}{\varphi }_2+{\varphi }_1，当{\varphi }_2为小量\end{array}$$
说明： =当旋转矩阵$R$(对应李代数 $\varphi$ )左乘微小旋转矩阵$\Delta R$(对应李代数$\Delta \varphi$)，可看作在原有李代数$\varphi$的基础上，加上 $J_l(\varphi {)}^{-1}\Delta \varphi$;当旋转矩阵 $R$ 右乘微小旋转矩阵 $\Delta R$，可看作在原有李代数$\varphi$的基础上，加上 $J_r(\varphi {)}^{-1}\Delta \varphi$;需要注意左乘还是右乘！
根据左乘模型，因为
$$J_l(\varphi )=\frac{\sin \theta }{\theta }I+(1-\frac{\sin \theta }{\theta })a{a}^T+\frac{1-\cos \theta }{\theta }\hat{a}$$所以
$$J_l(\varphi {)}^{-1}=\frac{\theta }{2}\cot \frac{\theta }{2}I+(1-\frac{\theta }{2}\cot \frac{\theta }{2})a{a}^T-\frac{\theta }{2}\hat{a}$$
可知 $$J_r(\varphi )=J_l(-\varphi )$$

---

小结：

对于李群$SO(3)$，李代数$\mathfrak{s}\mathfrak{o}(3)$下的微小旋转：
乘法：$$\Delta R\cdot R=exp(\Delta \hat{\varphi })exp(\hat{\varphi }))=exp((\varphi +J_l(\varphi {)}^{-1}\Delta \varphi )\hat{})$$加法：
$$exp((\varphi +\Delta \varphi \widehat{)})=exp((J_l\Delta \varphi )\widehat{)}exp(\hat{\varphi })=exp(\hat{\varphi })exp((J_r\Delta \varphi )\widehat{)}$$此处$J_l$为3*3 矩阵。

对于李群$SE(3)$，李代数$\mathfrak{s}\mathfrak{e}(3)$下的微小变换亦类似，只是此处$J_l$为6*6 矩阵。

李代数$\mathfrak{s}\mathfrak{o}(3)$下的求导

对空间点 $p$ 进行旋转得到 $Rp$ , $R$ 对应的李代数为 $\varphi$ ,根据导数的定义，对旋转矩阵求导，得到旋转后的坐标相对于旋转的导数为$$\frac{\partial (Rp)}{\partial R}=\frac{\partial (exp(\hat{\varphi })p)}{\partial \varphi }=-(Rp)\hat{}{J}_l$$

扰动模型（左乘）

对旋转矩阵 $R$ 左乘扰动$\Delta R$ , $R$ 对应的李代数为 $\varphi$ , $\Delta R$ 对应的李代数为 $\phi$, 根据导数的定义，对扰动的李代数求导，$$\frac{\partial (Rp)}{\partial \phi }=-(Rp)\hat{}$$

对变换矩阵 $T$的左乘扰动类似：
$$\frac{\partial (Tp)}{\partial \delta \xi }=\left(\begin{array}{cc}I& -(Rp+t)\hat{}\\ 0^T& 0^T\end{array}\right)=-(Tp{)}^{⨀}$$
$⨀$:将齐次坐标的空间点变为4*6矩阵

六.Sophus李代数库实践

编译安装

mkdir build
cd build
cmake ..
sudo make install

报错：
1.Sophus/sophus/so2.cpp:33:26:error: lvalue required as left operand of assignment
unit_complex_.real() = 1.;
2. Sophus/sophus/so2.cpp:33:26:error: lvalue required as left operand of assignment
unit_complex_.imag() = 1.;*
解决：该错误位于so2.cpp源码文件下：将开头的

  unit_complex_.real() = 1.;
  unit_complex_.imag() = 0.;

修改为

  unit_complex_.real(1.);
  unit_complex_.imag(0.);

设置CMakeLists.txt:

cmake_minimum_required(VERSION 3.14)
project(FourthuseSophus)

set(CMAKE_CXX_STANDARD 14)

find_package( Sophus REQUIRED )
include_directories( ${Sophus_INCLUDE_DIRS} )

include_directories(/usr/local/include/eigen3)

add_executable(FourthuseSophus main.cpp)
target_link_libraries( FourthuseSophus ${Sophus_LIBRARIES} )

Sophus库下的李代数运算

容易出错：切记SO3是矩阵，是李群，以李代数输出。旋转向量是李代数

#include <iostream>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>
#include <sophus/so3.h>
#include <sophus/se3.h>

using namespace std;


int main() {
    //设沿着z轴旋转90度
    Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2,Eigen::Vector3d(0,0,1)).toRotationMatrix();
    Sophus::SO3 SO3_R(R);                //从旋转矩阵构造李群
    Sophus::SO3 SO3_v(0,0, M_PI/2 );     //从欧拉角构造李群
    Eigen::Quaterniond q(R);
    Sophus::SO3 SO3_q(q);                //四元数构造李群
    cout<< SO3_q<<SO3_R<<SO3_v<<endl;

    Eigen::Vector3d so33 (1, 1, 1);
    Sophus::SO3 SO33 =Sophus::SO3::exp(so33); //从旋转向量构造李群
    cout<<SO33<<endl;
    // SO3是李群，本是矩阵，此处用李代数输出，所以是三维向量，旋转向量是李代数，所以此处输出结果遗址

    //从李群获得李代数 .log
    Eigen::Vector3d so3R = SO3_R.log();
    Eigen::Vector3d so3V = SO3_v.log();
    Eigen::Vector3d so3v= SO33.log();
    cout<<so3R.transpose()<<"\n"<<so3V.transpose()<<"\n" <<so33.transpose()<<endl;

    //从向量到反对称矩阵 hat
    Eigen::Matrix3d hat = Sophus::SO3::hat(so3v);
    cout<<"vector so3 to matrix = \n"<< hat<<endl;

    //从反对称矩阵到向量 vee
    Eigen::Vector3d vee =Sophus::SO3::vee(hat);
    cout<<"matrix to vector=\n"<< vee.transpose()<<endl;

    //so3扰动模型
    Eigen::Vector3d updateso3(1e-4,0,0);
    Sophus::SO3 SO3updated = Sophus::SO3::exp(updateso3)*SO3_R;//左乘扰动
    cout<<"左乘之前："<<SO3_R <<"扰动更新后："<< SO3updated<<endl;

    /**********下面为对SE（3）的操作***********/
    Eigen::Vector3d t(1,0,0);
    Sophus::SE3 SE3RT(R,t);               //此处R可以为旋转矩阵、四元数
    cout << "SE3RT=\n"<<SE3RT<<endl;

    typedef Eigen::Matrix<double,6,1> Vector6d;           //重命名，方便写六维向量
    Vector6d se3 = SE3RT.log();           //获得李代数,平移在前，旋转在后
    cout<<"se3="<<se3.transpose()<<endl;

    cout <<"se3 hat="<<endl<<Sophus::SE3::hat(se3)<<endl; //注意并非反对称的，跟原理提到的一样
    cout <<"se3 vee="<<endl<<Sophus::SE3::vee(Sophus::SE3::hat(se3)).transpose()<<endl;

    //se3扰动模型
    Vector6d updatese3;
    updatese3.setZero();
    updatese3(0,0)=1e-4;
    Sophus::SE3 SE3updated = Sophus::SE3::exp(updatese3)*SE3RT;
    cout <<"updatedse3="<<endl<< SE3updated.matrix()<<endl;
    
    return 0;
}

七.单目相机尺度不确定性和相似变换群

用到了再说：P15、P79

参考

[1]: Xiang Gao, Tao Zhang, Yi Liu, Qinrui Yan, 14 Lectures on Visual SLAM: From Theory to Practice, Publishing House of Electronics Industry, 2017.

强烈安利《视觉slam十四讲》，这是极好的一本书