基于Sophus的Ceres优化

0. 前言

在许多优化问题中，尤其是传感器融合问题，必须对存在于称为流形的空间中的数量进行建模，例如由四元数表示的传感器的旋转/方向。其中流型中的加法用⊞表示。以旋转矩阵更新为例：

LocalParameterization 接口允许用户定义参数块并与它们所属的流形相关联。它通过定义 Plus (⊞) 运算及其在 Δ=0 处相对于 Δ 的导数来实现。

class LocalParameterization {
    
 public:
  virtual ~LocalParameterization() {
    }
  // 流型空间中的加法
  virtual bool Plus(const double* x,   
                    const double* delta,
                    double* x_plus_delta) const = 0;
  // 计算雅克比矩阵
  virtual bool ComputeJacobian(const double* x, double* jacobian) const = 0;
  // local_matrix = global_matrix * jacobian
  virtual bool MultiplyByJacobian(const double* x,
                                  const int num_rows,
                                  const double* global_matrix,
                                  double* local_matrix) const;
  virtual int GlobalSize() const = 0; // 参数块 x 所在的环境空间的维度。
  virtual int LocalSize() const = 0; // Δ 所在的切线空间的维度
};

我们常常可以发现在VINS，ORB系列中含有这类的操作，通过局部参数化能够有效地实现流型的相加（其实是扰动计算）。这里作者提供一个基于Sophus的Ceres优化方案。

1. Sophus

这个问题和数据存储顺序及update时使用的SE3::exp(Eigen::Matrix<double,6,1> &a)操作有关

Eigen内部Quaterniond元素存储地址和初始化构造函数中参数的顺序是不一致的。

1. Eigen初始化Quaterniond(w,x,y,z),其中w是实部，x,y,z对应三个轴的虚部

   但是这和Eigen内部的Quaterniond的数据存储顺序不一样，存储顺序x,y,z,w。

2. Sophus内部SO3部分由Eigen::Quaternion表示，translation用Eigen::Vector3d表示。通过SE3::data()可以获得SE3存储的数据的double*指针，存储地址顺序如下：

   data()[0]对应Quaterniond x

   data()[1]对应Quaterniond y

   data()[2]对应Quaterniond z

   data()[3]对应Quaterniond w

   data()[4]对应SE3的tx

   data()[5]对应SE3的ty

   data()[6]对应SE3的tz

   但是Sophus::SE3d::exp(const Eigen::Vector6d &a)函数中a的前三维对应着SE3中translation的部分，而后三维才对应SO3的部分。和Sophus内部存储的顺序是不一致的。因此在SLAM中对位姿进行优化的时候，利用Sophus和ceres需要注意这个顺序。

实践了一下Sophus + ceres对SE3进行解析Jacobian的优化时，继承ceres::CostFunction自定义自己的CostFunction，在其中中编码Jacobian解析形式。然后再自定义一个相应的LocalParameterization。

…详情请参照古月居