ceres中解析求导的用法

LocalParameterizations are deprecated in Ceres Solver 2.2.1

LocalParameterization -> Manifold

ceres2.2.1之前的版本用的是LocalParameterizations来定义优化量的定义和求导，在之后的版本被弃用，用的是Manifold类

class CERES_EXPORT Manifold {
 public:
  virtual ~Manifold();

  // Dimension of the ambient space in which the manifold is embedded.
  virtual int AmbientSize() const = 0;

  // Dimension of the manifold/tangent space.
  virtual int TangentSize() const = 0;

  virtual bool Plus(const double* x,
                    const double* delta,
                    double* x_plus_delta) const = 0;

  virtual bool PlusJacobian(const double* x, double* jacobian) const = 0;

  virtual bool RightMultiplyByPlusJacobian(const double* x,
                                           const int num_rows,
                                           const double* ambient_matrix,
                                           double* tangent_matrix) const;

  virtual bool Minus(const double* y,
                     const double* x,
                     double* y_minus_x) const = 0;

  virtual bool MinusJacobian(const double* x, double* jacobian) const = 0;
};

在解析求导时，需要写自定义的类，继承自Manifold，然后重写这几个虚函数。
在研究ceres_ip时，跟随最新的版本，对代码进行了更新

class CERES_EXPORT SE3Manifold final: public ceres::Manifold {
public:

    int AmbientSize() const override { return 6; }
    int TangentSize() const override { return 6; }

    bool Plus(const double* x,
              const double* delta,
              double* x_plus_delta) const override ;
    bool PlusJacobian(const double* x, double* jacobian) const override ;

    bool Minus(const double* y,
               const double* x,
               double* y_minus_x) const override;
    bool MinusJacobian(const double* x, double* jacobian) const override;

};

bool SE3Manifold::Plus(const double *x, const double *delta, double *x_plus_delta) const {
    Eigen::Map<const Eigen::Matrix<double, 6, 1>> lie(x);
    Eigen::Map<const Eigen::Matrix<double, 6, 1>> delta_lie(delta);

    Sophus::SE3d T = Sophus::SE3d::exp(lie);
    Sophus::SE3d delta_T = Sophus::SE3d::exp(delta_lie);
    Eigen::Matrix<double, 6, 1> x_plus_delta_lie = (delta_T * T).log();

    for(int i = 0; i < 6; ++i) x_plus_delta[i] = x_plus_delta_lie(i, 0);

    return true;
}

bool SE3Manifold::PlusJacobian(const double *x, double *jacobian) const {
    ceres::MatrixRef(jacobian, 6, 6) = ceres::Matrix::Identity(6, 6);
    return true;
}


bool SE3Manifold::Minus(const double *y, const double *x, double *y_minus_x) const {
    return true;
}

bool SE3Manifold::MinusJacobian(const double *x, double *jacobian) const {
    return true;
}

需要说明的是，AmbientSize返回的是定义的优化量的维度，如优化量是四元数+平移向量，一共是7个数据维度，TangentSize返回的是实际的自由度6，四元数是过参数化，实际自由度是3，所以一共是6个自由度。

problem.SetManifold(se3, new SE3Manifold());