Ceres optimization based on sophus

0. Preface

In many optimization problems , Especially the problem of sensor fusion , The quantities that exist in a space called a manifold must be modeled , For example, the rotation of the sensor is represented by the number of quaternions / Direction . The addition in the flow pattern is ⊞ Express . Take the rotation matrix update as an example ：

LocalParameterization The interface allows the user to define parameter blocks and associate them with the manifold to which they belong . It is defined by Plus (⊞) Operation and its application in Δ=0 In relation to Δ To achieve .

class LocalParameterization {
    
 public:
  virtual ~LocalParameterization() {
    }
  //  Addition in flow pattern space 
  virtual bool Plus(const double* x,   
                    const double* delta,
                    double* x_plus_delta) const = 0;
  //  Calculate the Jacobian matrix 
  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; //  Parameter block  x  The dimension of the environment space .
  virtual int LocalSize() const = 0; // Δ  The dimension of the tangent space 
};

We can often find in VINS,ORB This kind of operation is included in the series , The addition of flow patterns can be effectively realized by local parameterization （ It's actually a perturbation calculation ）. Here the author provides an example based on Sophus Of Ceres Optimization plan .

1. Sophus

The question is the same as Data storage sequence and update The use of SE3::exp(Eigen::Matrix<double,6,1> &a) Operation related

Eigen Inside Quaterniond The element storage address and the order of parameters in the initialization constructor are inconsistent .

1. Eigen initialization Quaterniond(w,x,y,z), among w It's the real part ,x,y,z The imaginary part corresponding to the three axes

   But this sum Eigen Inside Quaterniond The data is stored in different order , Storage order x,y,z,w.

2. Sophus Inside SO3 Partly from Eigen::Quaternion Express ,translation use Eigen::Vector3d Express . adopt SE3::data() You can get SE3 Of stored data double* The pointer , The storage address sequence is as follows ：

   data()[0] Corresponding Quaterniond x

   data()[1] Corresponding Quaterniond y

   data()[2] Corresponding Quaterniond z

   data()[3] Corresponding Quaterniond w

   data()[4] Corresponding SE3 Of tx

   data()[5] Corresponding SE3 Of ty

   data()[6] Corresponding SE3 Of tz

   however Sophus::SE3d::exp(const Eigen::Vector6d &a) Function a Of The first three dimensions correspond to SE3 in translation Part of , and The latter three dimensions correspond to SO3 Part of . and Sophus The order of internal storage is inconsistent . So in SLAM in When optimizing the pose , utilize Sophus and ceres Note this order .

Practice a little Sophus + ceres Yes SE3 To analyze Jacobian When optimizing , Inherit ceres::CostFunction Customize your own CostFunction, Code in Jacobian Analytical form . Then customize a corresponding LocalParameterization.

… Please refer to Ancient Moon House