SE(3)的BCH扰动模型

地图点的世界坐标透过SE3转换成相机坐标，如果相机的SE3有微小的变动，那转换的坐标的变动Jacobian就是SE(3)的BCH扰动模型

当我们在优化SE3的时候，Jacobian就是SE3的微小变动，对于最终投影点的变化，此时我们会把这个过程分为2个阶段，第一个阶段是SE3的变化对于世界坐标转成相机坐标的变化，第二个阶段是相机坐标的变化导致投影点的变化（这个部分与SE3无关，和相机参数有关）

 也就是

    void EdgeSE3ProjectXYZOnlyPose::linearizeOplus()
    {

        //
        g2o::VertexSE3Expmap *vi = static_cast<g2o::VertexSE3Expmap *>(_vertices[0]);
        Eigen::Vector3d xyz_trans = vi->estimate().map(Xw);//这里

        double x = xyz_trans[0];
        double y = xyz_trans[1];
        double z = xyz_trans[2];

        Eigen::Matrix<double, 3, 6> SE3deriv;
        SE3deriv << 0.f, z, -y, 1.f, 0.f, 0.f,
                -z, 0.f, x, 0.f, 1.f, 0.f,
                y, -x, 0.f, 0.f, 0.f, 1.f;
        /*
         *  0   z   -y  1   0   0
         *  -z  0   x   0   1   0
         *  y   -x  0   0   0   1
         *
         */

    
        _jacobianOplusXi = -pCamera->projectJac(xyz_trans) * SE3deriv;
    }

里面的 _jacobianOplusXi = -pCamera->projectJac(xyz_trans) 第一阶段 * SE3deriv 第二阶段

SE3deriv的推导如下

 最终推导结果 SE3deriv  = [ I, -(Rp+t)]

可是代码里输入的是 [-(Rp+t), I]

SE3deriv << 0.f, z, -y, 1.f, 0.f, 0.f,
        -z, 0.f, x, 0.f, 1.f, 0.f,
        y, -x, 0.f, 0.f, 0.f, 1.f;
/*
 *  0   z   -y  1   0   0
 *  -z  0   x   0   1   0
 *  y   -x  0   0   0   1
 */

不一样的原因是我们写推导过程的偏微分是先写旋转的变化再写平移的变化，但是VertexSE3Expmap里面是先输入平移的t在输入旋转的r，所以左右位置要互换

      explicit SE3Quat(const MatrixBase<Derived>& v)
        {
          assert((v.size() == 6 || v.size() == 7) && "Vector dimension does not match");
          if (v.size() == 6) {
            for (int i=0; i<3; i++){
              _t[i]=v[i];
              _r.coeffs()(i)=v[i+3];
            }