ceres位姿图优化

ceres位姿图优化实践

高博《视觉slam十四讲第二版》ch10中有关于位姿图优化部分的讲解，在代码部分。高博只展示了g2o版本的位姿图优化的代码，但是在之前的章节中，高博说尽量多使用ceres来进行优化。。。。因此我就想实践一下ceres版本的位姿图优化。
在这篇博客里边我自己推了一下采用四元数表示旋转的位姿图误差表达式以及其关于状态变量的雅阁比矩阵公式。
另外一种表示旋转的方法为李群李代数，可以参考李代数版本推导。

四元数推导位姿图优化的公式

误差项的公式：

下标$[\cdot {]}_{xyz}$表示取四元数的虚数部分。
状态变量：

注：
1.四元数表示方法：$q=[q_w,q_x,q_y,q_z{]}^T$
2.将误差项写成关于状态变量的函数: $r_B^{6\times 1}=f(x)$

其中

求误差项关于状态变量的雅阁比矩阵：


采用扰动方式求导，其中

注：

$q_v$是四元数的虚部，$q_w$是四元数的实部。

至此便推导完成了误差项以及误差项关于状态变量的雅阁比矩阵的公式。

代码

https://github.com/qqqGpe/slam14-ch10-ceres

效果

遇到的问题

1. Eigen的矩阵默认是列优先(Column-major)的，但是ceres的CostFunction是行优先(Row-major)的。因此在SizedCostFunction()当中进行映射的时候要注意对应的顺序。下面这部分代码在map的时候使用了Eigen::RowMajor

        if(jacobians){
            if(jacobians[0]) {
                Eigen::Map<Eigen::Matrix<double,6,6, Eigen::RowMajor>> jacobian_i(jacobians[0]);//注意这里的Eigen::RowMajor
                Matrix6d J = JRInv(Sophus::SE3::exp(residual));
                jacobian_i = -J * pose_j.inverse().Adj();
            }

            if(jacobians[1]){
                Eigen::Map<Eigen::Matrix<double,6,6, Eigen::RowMajor>> jacobian_j(jacobians[1]);//注意这里的Eigen::RowMajor
                Matrix6d J = JRInv(Sophus::SE3::exp(residual));
                jacobian_j = J * pose_j.inverse().Adj();
            }
        }

2. 李代数的表达形式是 $\xi =[\rho ,\varphi {]}^T$要注意这里的$\rho$和从文档中读出来的$t$不是一个值，而是存在一定的转化关系
   
   可以看到这里$t=J\rho$。

3. 使用Sophus::SE3::exp()就可以直接从李代数变换到李群了，不需要进行反对称变换。

4. 误差函数和雅阁比矩阵需要同时加入信息矩阵的信息。