李群李代数在SLAM系统中的使用

作者: 陈瓜瓜_ARPG | 来源:发表于2019-11-15 15:53 被阅读0次

这篇文章针对有一定SLAM基础的同学或者对李群李代数的应用感兴趣的数学专业同学,已经很小众了，但对于真正干这行并想要更深理解的同学可能会很有帮助，因此花了些时间整理发出来。要从理论推导到后面代码应用都读懂难度还是比较大，尽量讲解。
李群李代数其实已经算SLAM里最深的数学知识了(至少我目前接触到的)，而事实上的SLAM应用也只是用了它们中的很小一部分。这篇文章不会详细地介绍李群李代数，毕竟google或者百度一下lie group SLAM之类的已经有不少人写过相关博客或者论文了。这篇文章的重点是针对开源的SLAM系统，推导出和代码中应用一致的公式，以帮助相关专业的同学知道李群李代数的现实应用。下面这个链接是<视觉SLAM十四讲>的作者所写的关于李群李代数的入门
视觉SLAM中的数学基础第三篇李群与李代数 - 半闲居士 - 博客园
关于定义之类的我就不写了，提取出几个比较关键的部分。
李群关键点: 旋转矩阵 $\mathbf{R}$ 和转换矩阵 $\mathbf{T} = \begin{bmatrix} \mathbf{R} & \mathbf{t} \\ \mathbf{0} & 1 \end{bmatrix}$ 分别属于SO(3)和SE(3)李群，我们重点针对SO(3)讲解。李群有几个(仅仅是这篇文章要使用的)重要性质. 性质1: SO(3)和SE(3)李群矩阵和它的转置互为逆矩阵 $\mathbf{R}\mathbf{R}^T = \mathbf{1}$ ，行列式为1(有为-1的情况但不会在SLAM中出现) $det \mathbf{R} = 1$ 。 性质2: 加法不封闭性和乘法封闭性，即两个李群矩阵相加后一般就不再是李群了，也就是李群当中不存在加法。两个李群想乘之后仍然是李群。现在如果有一个物体旋转了角度 $\mathbf{R}_1$ ，再此基础上我们如果再旋转一定角度，数学操作是对这个 $\mathbf{R}_1$ 再乘上另一个旋转矩阵 $\mathbf{R}_2$ ，最终得到的 $\mathbf{R}_2\mathbf{R}_1$ 仍然是一个旋转矩阵。但是你如果把这两个旋转矩阵加起来 $\mathbf{R}_1 + \mathbf{R}_2$ 则没有任何实际的物理意义。性质3:李群中的元素并不是独立的，一个旋转矩阵有9个元素，而实际旋转的自由度是3.
李代数关键点:每一个李群都有一个对应的李代数，我们记为小写的so(3)和se(3)。李代数的元素是线性独立的。一个旋转矩阵对应的李代数有3个元素。记一个李代数为 $\phi = [\phi_1, \phi_2, \phi_3]^T$ ，定义如下操作
$\phi^{\wedge} = \begin{bmatrix} 0 & -\phi_3 & \phi_2 \\ \phi_3 & 0 & \phi_1 \\ -\phi_2 & \phi_1 & 1 \end{bmatrix}$
一个李群和它的李代数的对应关系为矩阵指数关系
$\mathbf{R} = exp(\phi^{\wedge}) = \mathbf{I} + \phi^{\wedge} + \frac{1}{2!} (\phi^{\wedge})^2 + \frac{1}{3!} (\phi^{\wedge})^3 +... = \sum_{n=0}^{\infty}\frac{1}{n!} (\phi^{\wedge})^n \tag{1}$
李代数是有加法封闭性的，即两个李代数相加 $\phi_1+ \phi_2$ 仍可以得到一个李代数，对这个新的so(3)李代数进行指数操作可以得到一个新的旋转矩阵 $exp((\phi_1+ \phi_2)^{\wedge})$ 。
李群李代数之间的关系对旋转的求导有很大的帮助。第一个链接前言部分里的公式(2)所提到的非线性最小二乘就是SLAM问题的关键。我把那个公式再写一下
$\underset{\mathbf{T}}{min} \ u(\mathbf{T}) = \sum_{i=1}^{N}|| \mathbf{q}_i - \mathbf{T}\mathbf{p}_i||^2 \tag{2}$
已知两对匹配的3d点(实际应用中可能是图像的2d点和它现实中的3d点的匹配，或者2d图像点和它在另一图像中的对应2d点匹配) $\mathbf{p}和\mathbf{q}$ ，找到一个转换矩阵 $\mathbf{T}$ 使他们匹配后点与点之间的距离最小。最小二乘都会涉及到对要求解的量的求导。即
$\frac{ d(\mathbf{q}_i - \mathbf{T}\mathbf{p}_i)}{d\mathbf{T}}$
$\mathbf{p}和\mathbf{q}$ 是已知常量，所以关键在于
$\frac{ d( \mathbf{T}\mathbf{p}_i)}{d\mathbf{T}} \tag{3}$
这个式子乍一看还不简单？上下的\mathbf{T}约掉，结果自然就是 $\mathbf{p}$ ，但是实际上它是一个转换矩阵，对一个转换矩阵，这么做是错误的。回想导数的定义
$\frac{df(x)}{dx} = \lim_{\delta x \rightarrow 0} \frac{f(x+\delta x) - f(x)}{\delta x} \tag{4}$
应用在式子3中就应该是
$\frac{d(\mathbf{T}\mathbf{p}_i)}{d\mathbf{T}} = \lim_{\delta \mathbf{T} \rightarrow 0} \frac{(\mathbf{T}+\delta \mathbf{T})\mathbf{p} - \mathbf{T}\mathbf{p}}{\delta \mathbf{T}} \tag{5}$
现在你应该看到问题来了，由于李群的加法不封闭性， $(\mathbf{T}+\delta \mathbf{T})$ 根本就没有意义，不再是李群。导数的定义对李群不再有意义，我们上面假设能分子分母直接划掉相同的变量得到结果是根据导数的定义得来的，即假设加法成立，我们的式5就可以得到
$\frac{d(\mathbf{T}\mathbf{p}_i)}{d\mathbf{T}} = \lim_{\delta \mathbf{T} \rightarrow 0} \frac{(\mathbf{T}+\delta \mathbf{T})\mathbf{p} - \mathbf{T}\mathbf{p}}{\delta \mathbf{T}} = \lim_{\delta \mathbf{T} \rightarrow 0} \frac{\delta \mathbf{T}\mathbf{p}}{\delta \mathbf{T}} = \mathbf{p}$
但很遗憾，不行。
那么我们如何对转换矩阵求导呢？我们下面要讲解的应用中，把对转换矩阵的求导分开为对位移的求导和对旋转的求导(实际上有直接的公式可以应用不用分开，但是分开求会很直观)。我们前面写到 $\mathbf{T} = \begin{bmatrix} \mathbf{R} & \mathbf{t} \\ \mathbf{0} & 1 \end{bmatrix}$ ， $\mathbf{p}$ 写为齐次点即 $(\mathbf{p}, 1)^T$ ，代入式(3)，我们得到
$\frac{ d( \mathbf{R}\mathbf{p}_i + \mathbf{t},1)^T}{d\mathbf{T}}$
1作为常数部分我们不管，前半部分可以分别对旋转求导和对平移求偏导。
$\frac{ \partial( \mathbf{R}\mathbf{p}_i + \mathbf{t})}{\partial \mathbf{t}} \\ \frac{ \partial( \mathbf{R}\mathbf{p}_i + \mathbf{t})}{\partial \mathbf{R}}$
对位移的偏导非常好解决，位移是有加法的，求偏导时 $\mathbf{R}\mathbf{p}_i$ 视为常亮，所以上式的位移部分可以直接得到答案那就是单位矩阵 $\mathbf{I}$ 。一切的问题都来源于对旋转矩阵的偏导
$\frac{ \partial( \mathbf{R}\mathbf{p}_i + \mathbf{t})}{\partial \mathbf{R}} = \frac{ \partial( \mathbf{R}\mathbf{p}_i )}{\partial \mathbf{R}}$
旋转矩阵也是不存在加法的。那么我们该怎么办呢？接下来就是李代数的应用了。我们把上式等价于对旋转的李代数求导！为什么可以这么等价？我给出一个直观的解释。我们知道，平移之所以能很简单的求导是因为满足加法封闭性，即如果我们把对平移的求导应用到式(4)中，会有一个 $\delta \mathbf{t}$ ，这个 $\delta \mathbf{t}$ 趋于0表示针对当前的 $\mathbf{t}$ 趋于不移动。当对旋转求导时，我们应该找到一个小量，"加"到旋转上，使物体对于当前旋转，不再旋转，只要我们设计出一个小量，满足这个条件，我们就可以针对这个小量求导。我们不能直接加上一个所有元素都趋于0的旋转小量 $\delta\mathbf{R}$ ，但是旋转对应的李代数可以。假设针对当前的旋转的李代数 $\mathbf{R} = exp(\phi^{\wedge})$ ，加上一个小量， $\mathbf{R}' = exp((\phi + \delta \phi)^{\wedge})$ ，当 $\delta \phi$ 趋于0时，旋转不变，于是乎，我们可以针对这个小量求导。有点晦涩，多思考一下，这已经是我能想到的最直观的解释了，理论证明可无穷尽也。那么对旋转的求导可以变换为
$\begin{align} \frac{ \partial( \mathbf{R}\mathbf{p}_i )}{\partial \mathbf{R}} & = \frac{ \partial( exp(\phi^{\wedge})\mathbf{p}_i )}{\partial exp(\phi^{\wedge})} & = \lim_{\delta \phi \rightarrow 0} \frac{exp((\phi + \delta \phi)^{\wedge})\mathbf{p} - exp(\phi^{\wedge}\mathbf{p})}{\delta \phi} \end{align} \tag{6}$
这个式子是可以求解的。这个式子在很多有关SLAM的书籍中写到过。这里还是用大家很可能有的中文教材<视觉SLAM十四讲>来讲解的。上面的式子对应的是第一版十四讲4.3.3的推导结果。这是李代数求导的第一种方法。这种方法其实相对接下来要讲的第二种方法直观，因为至少我们是根据导数的定义，实实在在的有李代数的加法来求解的。第二种求导方法则不再拘泥于单纯意义上的加法了。
旋转矩阵真的没有加法吗？其实对于旋转来说，乘法就是加法，我们要对一个旋转"加上"另一个旋转，实际上是对旋转矩阵做乘法。所以我们如果对当前的旋转量乘一个近乎于单位矩阵的旋转，其实就是对它"加"了一个小量，当这个旋转小量趋于单位矩阵时，就没有旋转发生。旋转矩阵趋于单位阵对应的李代数就趋于0向量。那么第二个求导方法就是针对当前的旋转乘上一个小的(近乎于单位矩阵)的旋转小量，并对它的李代数趋于0时求极限。
$\begin{align} \frac{ \partial( \mathbf{R}\mathbf{p}_i )}{\partial \mathbf{R}} & = \frac{ \partial( exp(\phi^{\wedge})\mathbf{p}_i )}{\partial exp(\phi^{\wedge})} & = \lim_{\delta \varphi \rightarrow 0} \frac{exp(\varphi^{\wedge})exp(\phi^{\wedge})\mathbf{p} - exp(\phi^{\wedge}\mathbf{p})}{\delta \varphi} \end{align} \tag{7}$
这就是十四讲第一版书上4.3.4扰动模型了。不过正如书上所说，"扰动" $exp(\varphi)$ 可以是左扰动也可以是右扰动。书上给出的求导结果是左扰动结果，下面我给出右扰动的推导过程以及结果。
$\begin{align} \frac{\partial{ exp(\phi^{\wedge}) \mathbf{p} }}{\partial{\phi}} & = \underset{\varphi \to 0}{lim} \frac{exp(\phi^{\wedge}) exp(\varphi^{\wedge})\mathbf{p} - exp(\phi^{\wedge})\mathbf{p}}{\varphi} \\ & \overset{李群性质1}{\approx} \underset{\varphi\to0}{lim} \frac{exp(\phi^{\wedge}) (\mathbf{I}+\varphi^{\wedge})\mathbf{p} - exp(\phi^{\wedge})\mathbf{p}}{\varphi} \\ & = \underset{\varphi \to 0}{lim} \frac{exp(\phi^{\wedge}) \varphi^{\wedge} \mathbf{p}}{\varphi} \\ & \overset{李代数性质2}{=} - \underset{\varphi \to 0}{lim} \frac{exp(\phi^{\wedge}) \mathbf{p}^{\wedge}\varphi }{\varphi}\\ & = -exp(\phi^{\wedge}) \mathbf{p}^{\wedge} \\ & = -\mathbf{R} \mathbf{p}^{\wedge} \end{align}$
这里"李群性质1"是当李代数很小时(所以我们称之为扰动)， $exp(\varphi^{\wedge}) \approx \mathbf{I} + \varphi^{\wedge}$ ，这里 $\mathbf{I}$ 是3x3单位矩阵。"李代数性质2"是 $\varphi^{\wedge} \mathbf{p} = - \mathbf{p}^{\wedge}\varphi$ 。读者可以自己证明。

下面是激动人心的实际应用

有了李群的导数，接下来就是实际应用部分了。我这里的实际应用不是说让你自己写几行代码去实现，而是去看看比较成熟的SLAM系统是在哪里用到上面的推导的。这需要你打开相关的论文和代码对比来看，需要些精力。我经常操作的是视觉惯性slam，好的开源库有VINS-FUSION,VINS-Mono(都来自于港科大，前者更新有更强的应用不过后者被更多人熟知),okvis等。okvis是多年前的作品，非常经典，但是代码难度比前面的大不少。综合了一下情况我决定使用VINS-Mono的代码，把李代数的求导结果和他们的代码里一一对应起来。代码在他们的github中 HKUST-Aerial-Robotics/VINS-Mono: A Robust and ... - GitHub
我们以VINS-Mono想要求解的reprojection error为例。
vins的原论文 VINS-Mono: A Robust and Versatile Monocular Visual ... - arXiv 式25把处于不同图像的对应的2d点投影到同一单位球上然后求残差。
在vins-mono里projection_factor.cpp里Evaluate函数中包含了误差函数以及倒数的结果。
首先来看前面几行几个参数

    Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);
    Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);

    Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]);
    Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]);

    Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]);
    Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]);

    double inv_dep_i = parameters[3][0];

这几行的parameter来源于传入的要优化的参数，具体来源于在estimator.cpp中的这两行

ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]);

其中para_Pose[imu_i]对应于parameter[0]，是IMUi时刻的位姿，para_Pose[imu_j]对应于parameter[1]，是IMUj时刻位姿，para_Ex_Pose[0]是相机和IMU的外参，对应parameter[2]。最后一个就对应inv_dep_i.即该像素点深度的倒数。这几个是想要被优化的量。随后几行

    Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i;
    Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;
    Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;
    Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);
    Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic);

对应vins的原论文式25第三个 $\mathcal{P}_l^{c_j}$ 的求法。注意式子中的back projection函数 $\pi_c^{-1}(\cdot)$ 在原文中它的作用是turns a pixel location into a unit vector using camera intrinsic parameters。具体参见论文图6. 代码中的pts_i对应的是公式中的
$\pi_c^{-1}( \begin{bmatrix} u_l^{c_i} \\ v_l^{c_i} \end{bmatrix} )$
这是论文中所说的指向球面的unit vector。用它除以深度的倒数(乘上深度)，就对应代码中的pts_i / inv_dep_i，获得一个在相机坐标系下的3d点
$\frac{1}{\lambda}_l \pi_c^{-1}( \begin{bmatrix} u_l^{c_i} \\ v_l^{c_i} \end{bmatrix} )$
这个3d点乘以相机到body(或者说IMU)坐标系下的转换矩阵得到在IMU坐标系下的坐标。
$\mathbf{R}_c^b \frac{1}{\lambda}_l \pi_c^{-1}( \begin{bmatrix} u_l^{c_i} \\ v_l^{c_i} \end{bmatrix} + \mathbf{p}_c^b )$
对应的代码中的qic * pts_camera_i + tic。剩下的式25的第3个式子的位姿变换和代码的每一行一一对应。上式用 $\mathbf{T}_{b_i}^{\omega}$ 转到世界坐标系，再转到 $j$ 图像所在的坐标系最后转到 $j$ 相机下的unit vector即pts_camera_j.
$\mathcal{P}_l^{c_j} = \mathbf{R}_b^c( \mathbf{R}_{\omega}^{b_j}( \mathbf{R}_{b_i}^{\omega}( \mathbf{R}_c^b \frac{1}{\lambda}_l \pi_c^{-1}( \begin{bmatrix} u_l^{c_i} \\ v_l^{c_i} \end{bmatrix} + \mathbf{p}_c^b ) + \mathbf{p}_{b_i}^{\omega} - \mathbf{p}_{b_j}^{\omega}) - \mathbf{p}_c^b \tag{8}$
随后
代码中

#ifdef UNIT_SPHERE_ERROR 
    residual =  tangent_base * (pts_camera_j.normalized() - pts_j.normalized());
#else
    double dep_j = pts_camera_j.z();
    residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>();
#endif

用UNIT_SPHERE_ERROR才和式25的第一行对应，如果不用的话就和普通的pinhole相机模型的点对应。为了方便起见，我还是推导的不用UNIT_SPHERE_ERROR所得到的雅各比矩阵。因为我发现VINS, parameter.h中的#define UNIT_SPHERE_ERROR这一行被注释掉了。
代码下一行
residual = sqrt_info * residual;
okvis也有这一行，这是因为ceres能接受的最小二乘优化只能是最简单的形式 $e^Te$ ，而带有协方差的优化的形式为 $e^TP^{-1}e$ ，所以代码中会对 $P^{-1}$ 做LLT分解，有 $LL^T = P^{-1}$ ，那个sqrt_info自然就是 $L^T$ 了。
综上针对某一个像素点在 $i,j$ frame下的像素坐标我们的residual就是

$\mathbf{r} = (\mathcal{P}_l^{c_j} /z_j - \widehat{\mathcal{P}}_l^{c_j} )_{1:2}$

式子中 $\widehat{\mathcal{P}}_l^{c_j}$ 来源于测量(比如我们通过特征点匹配的方法知道了i,j是对应的，那时得到的j的坐标就是通过测量得到的), $(\cdot)_{1:n}$ 表示取向量第1位到第n位。把上式具体写出来就是
$\mathbf{r} = \begin{bmatrix} x_{c_j}/z_{c_j} - \widehat{u}_{c_j} \\ y_{c_j}/z_{c_j} - \widehat{v}_{c_j} \end{bmatrix} \tag{9}$
residual需要求导的对象是body在 $i$ frame时的pose，即 $(\mathbf{R}_{b_i}^{\omega}, \mathbf{p}_{b_i}^{\omega})$ 。
首先我们计算简单的对位置求导。
$\frac{\partial{\mathbf{r}}}{\partial{\mathbf{p}_{b_i}^{\omega}}} = \frac{\partial{\mathbf{r}}}{\partial{\mathcal{P}_l^{c_j}}} \frac{\partial{\mathcal{P}_l^{c_j}}}{\partial{\mathbf{p}_{b_i}^{\omega}}}$
其中 $\mathbf{r}$ 为2维， $\mathcal{P}_l^{c_j}$ 为3d点 $(x_{c_j}, y_{c_j}, z_{c_j})$ ，很容易得到
$\frac{\partial{\mathbf{r}}}{\partial{\mathcal{P}_l^{c_j}}} = \begin{bmatrix} 1/z_{c_j} & 0 & -x_{c_j}/z_{c_j}^2 \\ 0 & 1/z_{c_j} & -y_{c_j}/z_{c_j}^2 \end{bmatrix} \tag{10}$
然后(8)式对 $\mathbf{p}_{b_i}^{\omega}$ 求导，得到
$\frac{\partial{\mathbf{r}}}{\partial{\mathcal{P}_l^{c_j}}} = \mathbf{R}_b^c \mathbf{R}_{\omega}^{b_j}$
对应代码中的

jaco_i.leftCols<3>() = ric.transpose() * Rj.transpose();

即
$\frac{\partial{\mathbf{r}}}{\partial{\mathcal{P}_l^{c_j}}} = \mathbf{R}_b^c \mathbf{R}_{\omega}^{b_j} = (\mathbf{R}_c^b)^{-1} (\mathbf{R}_{b_j}^{\omega})^{-1} = (\mathbf{R}_c^b)^T (\mathbf{R}_{b_j}^{\omega})^T$
这里用到了SO(3)群的基本性质，SO(3)下的矩阵的逆等于它的转置。
之后我们对旋转求导数
$\frac{\partial{\mathbf{r}}}{\partial{\mathbf{R}_{b_i}^{\omega}}} = \frac{\partial{\mathbf{r}}}{\partial{\mathcal{P}_l^{c_j}}} \frac{\partial{\mathcal{P}_l^{c_j}}}{\partial{\mathbf{R}_{b_i}^{\omega}}}$
导数的前一部分和式10一样，后一部分涉及到李群的求导，为书写方便我们先令
$\frac{1}{\lambda}_l \pi_c^{-1}( \begin{bmatrix} u_l^{c_i} \\ v_l^{c_i} \end{bmatrix}) = \mathcal{P}_l^{c_i}$ ，因为左式本来就是 $i$ frame像素点投影到相机平面的3d点。后一部分
$\begin{align} \frac{\partial{\mathcal{P}_l^{c_j}}}{\partial{\mathbf{R}_{b_i}^{\omega}}} & \overset{代入式8}{=} \frac{\partial{\mathbf{R}_b^c( \mathbf{R}_{\omega}^{b_j}( \mathbf{R}_{b_i}^{\omega}( \mathbf{R}_c^b \mathcal{P}_l^{c_i} + \mathbf{p}_c^b ) + \mathbf{p}_{b_i}^{\omega} - \mathbf{p}_{b_j}^{\omega}) - \mathbf{p}_c^b}}{\partial{\mathbf{R}_{b_i}^{\omega}}} \\ & = \frac{\partial{\mathbf{R}_b^c \mathbf{R}_{\omega}^{b_j} \mathbf{R}_{b_i}^{\omega} \mathbf{R}_c^b \mathcal{P}_l^{c_i} }}{\partial{\mathbf{R}_{b_i}^{\omega}}} \end{align}$
现在只对 $\mathbf{R}_{b_i}^{\omega}$ 求导数，那其他量视为是常量，我们令 $C_1 = \mathbf{R}_b^c \mathbf{R}_{\omega}^{b_j}$ ，另外3d点 $\mathbf{R}_c^b\mathcal{P}_l^{c_i}$ 得到的是body 坐标系下的3d点，我们记为 $\mathcal{P}_l^{b_i}$ 。那么上式就变成了
$\begin{align} \frac{\partial{\mathcal{P}_l^{c_j}}}{\partial{\mathbf{R}_{b_i}^{\omega}}} & = \frac{\partial{C_1 \mathbf{R}_{b_i}^{\omega} \mathcal{P}_l^{b_i} }}{\partial{\mathbf{R}_{b_i}^{\omega}}} \\ & = C1 \frac{\partial{ \mathbf{R}_{b_i}^{\omega} \mathcal{P}_l^{b_i} }}{\partial{\mathbf{R}_{b_i}^{\omega}}} \end{align} \tag{11}$
现在的问题就是如何得到 $\frac{\partial{ \mathbf{R}_{b_i}^{\omega} \mathcal{P}_l^{b_i} }}{\partial{\mathbf{R}_{b_i}^{\omega}}}$ 了,简单记为 $\frac{\partial{ \mathbf{R} \mathcal{P} }}{\partial{\mathbf{R}}}$ 。那这儿很明显了，应用我们右扰动的推导结果，可以直接得到
$\begin{align} \frac{\partial{\mathcal{P}_l^{c_j}}}{\partial{\mathbf{R}_{b_i}^{\omega}}} & = C_1\frac{\partial{ \mathbf{R}_{b_i}^{\omega} \mathcal{P}_l^{b_i} }}{\partial{\mathbf{R}_{b_i}^{\omega}}} \\ & \overset{因为用了李群性质1}{\approx} -C_1 \mathbf{R}_{b_i}^{\omega} (\mathcal{P}_l^{b_i})^{\wedge} \end{align}$
代入 $C_1 = \mathbf{R}_b^c\mathbf{R}_{\omega}^{b_j}$ ，我们就有了
$\frac{\partial{\mathcal{P}_l^{c_j}}}{\partial{\mathbf{R}_{b_i}^{\omega}}} \approx - \mathbf{R}_b^c\mathbf{R}_{\omega}^{b_j} \mathbf{R}_{b_i}^{\omega} = - (\mathbf{R}_c^{b})^T(\mathbf{R}_{b_j}^{\omega})^T \mathbf{R}_{b_i}^{\omega}(\mathcal{P}_l^{b_i})^{\wedge} \tag{12}$
这和代码中的

jaco_i.rightCols<3>() = ric.transpose() * Rj.transpose() * Ri * -Utility::skewSymmetric(pts_imu_i);

对应。
总的来说代码第一个求导部分得到了残差对 $i$ frame处位姿的求导。

        if (jacobians[0])
        {
            Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_pose_i(jacobians[0]);

            Eigen::Matrix<double, 3, 6> jaco_i;
            jaco_i.leftCols<3>() = ric.transpose() * Rj.transpose();\\对位移求导
            jaco_i.rightCols<3>() = ric.transpose() * Rj.transpose() * Ri * -Utility::skewSymmetric(pts_imu_i);\\对旋转求导

            jacobian_pose_i.leftCols<6>() = reduce * jaco_i;\\reduce为式10
            jacobian_pose_i.rightCols<1>().setZero(); \\这里有一个0列，回顾前面可以发现我们在求偏导时忽略了一个1的
        }

其他部分的推导我就不讲了，以后有时间的话我会写上IMU部分的推导，有预积分会麻烦很多。
你自己如果有兴趣的话，你可以推导okvis的reprojection error部分，它也是用ceres优化，优化部分的结构和VINS-MONO相似。虽然他们的代码更难读懂，但是我已经推导过了，和前面的讲解一致，所以你也可以试一下。

李群李代数在SLAM系统中的使用

下面是激动人心的实际应用

相关文章

网友评论

延伸阅读

深度阅读

栏目导航

热点阅读