ceres使用总结

CostFunction

CostFunction是有用来计算残差e和雅阁比J的，给出状态值向量[x1,x2,...xn]后按道理就可以根据方程f(x1,x2,...xn)求解出方程的结果vector和雅阁比矩阵J

class CostFunction {
 public:
  virtual bool Evaluate(double const* const* parameters,
                        double* residuals,
                        double** jacobians) = 0;
  const vector<int32>& parameter_block_sizes();
  int num_residuals() const;

 protected:
  vector<int32>* mutable_parameter_block_sizes();
  void set_num_residuals(int num_residuals);
};

1. CostFunction::parameter_block_sizes_
   每个状态值的维度，比如x1是pose的话维度就是7，x2是速度和bias的话维度就是9
2. CostFunction::num_residuals_
   残差的维度，比如视觉就是2，相对pose就是6
3. 继承函数Evaluate，必须继承用于计算出残差和雅各比的,在vins_mono的边缘化中就是使用的Evaluate函数来计算出原始的residuals和raw jacobians

//输入：parameters 输出：residuals jacobians
bool CostFunction::Evaluate(double const *const *parameters, double *residuals, double **jacobians)

1. parameters：状态值向量,维度不一定为1
   x = [ x 1 x 2 ⋮ x n ] x=\left[ \begin{array}{l} x_1 \\ x_2 \\ \vdots \\x_n\end{array} \right] x= ​x1​x2​⋮xn​​ ​
2. parameter_block_sizes_：每个状态值对应的参数维度
   s = [ s 1 s 2 ⋮ s n ] s=\left[ \begin{array}{l} s_1 \\ s_2\\ \vdots \\s_n\end{array} \right] s= ​s1​s2​⋮sn​​ ​
3. residuals：残差，维度为m的残差对应的向量为
   e = [ e 1 e 2 ⋮ e m ] e=\left[ \begin{array}{l} e_1 \\ e_2 \\ \vdots \\e_m\end{array} \right] e= ​e1​e2​⋮em​​ ​
4. jacobians:雅阁比矩阵的维度为n
   J = [ ∂ e 1 ∂ x 1 ( 1 × s 1 ) ∂ e 2 ∂ x 1 ( 1 × s 1 ) ⋯ ∂ e m ∂ x 1 ( 1 × s 1 ) ∂ e 1 ∂ x 2 ( 1 × s 2 ) ∂ e 2 ∂ x 2 ( 1 × s 2 ) ⋯ ∂ e m ∂ x 2 ( 1 × s 2 ) ⋮ ⋮ ⋮ ∂ e 1 ∂ x n ( 1 × s n ) ∂ e 2 ∂ x n ( 1 × s n ) ⋯ ∂ e m ∂ x n ( 1 × s n ) ] J=\left[ \begin{array}{l} \frac{\partial e_1}{\partial x_1}_{(1\times s_1)} & \frac{\partial e_2}{\partial x_1}_{(1\times s_1)} \cdots & \frac{\partial e_m}{\partial x_1}_{(1\times s_1)} \\ \frac{\partial e_1}{\partial x_2}_{(1\times s_2)} & \frac{\partial e_2}{\partial x_2}_{(1\times s_2)} \cdots & \frac{\partial e_m}{\partial x_2}_{(1\times s_2)} \\ \vdots &\vdots &\vdots \\ \frac{\partial e_1}{\partial x_n}_{(1\times s_n)} & \frac{\partial e_2}{\partial x_n}_{(1\times s_n)} \cdots & \frac{\partial e_m}{\partial x_n}_{(1\times s_n)} \\\end{array} \right] J= ​∂x1​∂e1​​(1×s1​)​∂x2​∂e1​​(1×s2​)​⋮∂xn​∂e1​​(1×sn​)​​∂x1​∂e2​​(1×s1​)​⋯∂x2​∂e2​​(1×s2​)​⋯⋮∂xn​∂e2​​(1×sn​)​⋯​∂x1​∂em​​(1×s1​)​∂x2​∂em​​(1×s2​)​⋮∂xn​∂em​​(1×sn​)​​ ​
5. ,第i行的维度为表示所有的残差对状态值xi的雅各比
   维度为：
   $$m\times s_i$$
   雅各比为：
   $$J_i=\left[\begin{array}{lcc}{\frac{\partial e_1}{\partial x_i}}_{(1\times s_i)}& {\frac{\partial e_2}{\partial x_i}}_{(1\times s_i)}\cdots & {\frac{\partial e_m}{\partial x_i}}_{(1\times s_i)}\end{array}\right]$$
   雅各比是按地址来表示array的，Ji就是表示的第i个雅各比的地址：
   $$J_i=>jacobians[i]$$

LocalParameterization

状态值本身维度和在流体表面维度不一致的情况下使用

class LocalParameterization {
 public:
  virtual ~LocalParameterization() = default;
  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;
  virtual bool MultiplyByJacobian(const double* x,
                                  const int num_rows,
                                  const double* global_matrix,
                                  double* local_matrix) const;
  virtual int GlobalSize() const = 0; //原始空间对应的状态值维度
  virtual int LocalSize() const = 0; //切平面上对应维度
};

只要是非线性的参数块都是定义的LocalParameterization,比如vins mono中的航向角是控制在[-180,180]之间就是一个非线性的
1.航向角相关的local parameter

• 定义非线性函数

template <typename T>
T NormalizeAngle(const T& angle_degrees) {
  if (angle_degrees > T(180.0))
  	return angle_degrees - T(360.0);
  else if (angle_degrees < T(-180.0))
  	return angle_degrees + T(360.0);
  else
  	return angle_degrees;
};

• 定义LocalParameterization的方法

class AngleLocalParameterization {
 public:

  template <typename T>
  bool operator()(const T* theta_radians, const T* delta_theta_radians,
                  T* theta_radians_plus_delta) const {
    *theta_radians_plus_delta =
        NormalizeAngle(*theta_radians + *delta_theta_radians);

    return true;
  }

  static ceres::LocalParameterization* Create() {
    return (new ceres::AutoDiffLocalParameterization<AngleLocalParameterization,
                                                     1, 1>);
  }
};

2.姿态相关的local parameter

1.声明local parameter的类型
ceres::LocalParameterization *local_parameter = new ceres::EigenQuaternionParameterization
2.添加local parameter数据
problem.AddParameterBlock(frame.q.coeffs().data(),4,local_parameter);
3.如果要参数为const的话
problem.SetParameterBlockConstant(frame.q.coeffs().data());

q.coeffs() //数据存储方式为: [x,y,z,w]

3.pose定义的local parameter

• local parameter类型定义

#include 
#include 
class PoseLocalParameterization : public ceres::LocalParameterization
{
    virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const;
    virtual bool ComputeJacobian(const double *x, double *jacobian) const;
    virtual int GlobalSize() const { return 7; };
    virtual int LocalSize() const { return 6; };
};

• 重定义广义加法

#include "pose_local_parameterization.h"
bool PoseLocalParameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const
{
    Eigen::Map<const Eigen::Vector3d> _p(x);
    Eigen::Map<const Eigen::Quaterniond> _q(x + 3);

    Eigen::Map<const Eigen::Vector3d> dp(delta);

    Eigen::Quaterniond dq = Utility::deltaQ(Eigen::Map<const Eigen::Vector3d>(delta + 3));

    Eigen::Map<Eigen::Vector3d> p(x_plus_delta);
    Eigen::Map<Eigen::Quaterniond> q(x_plus_delta + 3);

    p = _p + dp;
    q = (_q * dq).normalized();

    return true;
}

• 重定义雅各比矩阵

bool PoseLocalParameterization::ComputeJacobian(const double *x, double *jacobian) const
{
    Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> j(jacobian);
    j.topRows<6>().setIdentity();
    j.bottomRows<1>().setZero();

    return true;
}

数据内部存储顺序

1. ceres::QuaternionParameterization：内部存储顺序为(w,x,y,z)
2. ceres::EigenQuaternionParameterization：内部存储顺序为(x,y,z,w)
3. Eigen::Quaternion(w,x,y,z)：内部存储顺序为(x,y,z,w)（与构造函数没有保持一致）