SLAM从入门到放弃：SLAM十四讲第七章习题（9）

以下均为简单笔记，如有错误，请多多指教。

9. 使用Sophus的SE3类，自己设计g2o的节点与边，实现PnP和ICP的优化。
   答：对于PnP问题，需要重新设计的节点包括相机节点、Landmark节点和一个相机-Landmark边；而对于ICP问题，由于边已经实现了，所以只需要实现节点即可。

PnP问题

PnP问题相对简单，其实可以参考g2o/types/sba/types_six_dof_expmap.h编写即可。

#include "sophus/se3.h"

// 相机顶点
class CameraVertex: public BaseVertex<6,Sophus::SE3>
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  CameraVertex(){}

  // 使用sophus进行左乘更新
  virtual void oplusImpl(const double *update)
  {
    Eigen::Matrix<double,6,1> seUpdate(update);
    _estimate = Sophus::SE3::exp(seUpdate)*_estimate; 
  }

  bool read(std::istream &is);
  bool write(std::ostream &os) const;
}

//Landmark顶点
class LandmarkVertex: public BaseVertex<3,Eigen::Vector3d>
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  LandmarkVertex(){}

  virtual void oplusImpl(const double *update)
  {
    _estimate += Eigen::Vector3d(update);
  }

  bool read(std::istream &is);
  bool write(std::ostream &os) const;
}

//边
//描述的就是Landmark投影到图像上
class CameraLandmarkEdge: public BaseBinaryEdge<2,Eigen::Vector2d,LandmarkVertex,CameraVertex>
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  CameraLandmarkEdge(){}
  
      virtual void computeError()
    {
        const LandmarkVertex* point = dynamic_cast<const LandmarkVertex*> ( _vertices[0] );
        const CameraVertex*    pose = dynamic_cast<const CameraVertex*> ( _vertices[1] );

        // Camera
        const CameraParameters *cam = static_cast<const CameraParameters*>(parameer(0));

        // measurement is p, point is p'
        // 这里需要检查一下
        Eigen::Matrix3d rotate  = pose->estimate().rotationMatrix();
        Eigen::Vector3d trans   = pose->estimate().translation();
        Eigen::Vector3d transp = rotate*point->estimate() + trans;
        _error = _measurement - cam->cam_map( transp );
    
    }

    bool read ( istream& in ) {}
    bool write ( ostream& out ) const {}
}

---

ICP问题

首先介绍一下在原文中ICP问题是怎么处理，代码如下所示。其中linearizeOplus是表示手动求解jac矩阵的函数；由于BaseUnaryEdge是单边，因此_jacobianOplusXi表示的就是表示该边针对李代数的导数。
根据SLAM十四讲P76中4.3.5的公式
$$\frac{\partial (TP)}{\partial \delta \xi }=\left[\begin{array}{cc}I& -(Rp+t{)}^{\wedge }\\ 0^T& 0^T\end{array}\right]$$
因此很明显可以发现在ICP中$f=X-TP$中对李代数的导数展开就可以得到_jacobianOplusXi中的内容，不过要注意的是顺序，公式中平移量在前，而代码中平移量在后。

// g2o edge
class EdgeProjectXYZRGBDPoseOnly : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3Expmap>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    EdgeProjectXYZRGBDPoseOnly( const Eigen::Vector3d& point ) : _point(point) {}

    virtual void computeError()
    {
        const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap*> ( _vertices[0] );
        // measurement is p, point is p'
        _error = _measurement - pose->estimate().map( _point );
    }

    virtual void linearizeOplus()
    {
        g2o::VertexSE3Expmap* pose = static_cast<g2o::VertexSE3Expmap *>(_vertices[0]);
        g2o::SE3Quat T(pose->estimate());
        Eigen::Vector3d xyz_trans = T.map(_point);
        double x = xyz_trans[0];
        double y = xyz_trans[1];
        double z = xyz_trans[2];

        _jacobianOplusXi(0,0) = 0;
        _jacobianOplusXi(0,1) = -z;
        _jacobianOplusXi(0,2) = y;
        _jacobianOplusXi(0,3) = -1;
        _jacobianOplusXi(0,4) = 0;
        _jacobianOplusXi(0,5) = 0;

        _jacobianOplusXi(1,0) = z;
        _jacobianOplusXi(1,1) = 0;
        _jacobianOplusXi(1,2) = -x;
        _jacobianOplusXi(1,3) = 0;
        _jacobianOplusXi(1,4) = -1;
        _jacobianOplusXi(1,5) = 0;

        _jacobianOplusXi(2,0) = -y;
        _jacobianOplusXi(2,1) = x;
        _jacobianOplusXi(2,2) = 0;
        _jacobianOplusXi(2,3) = 0;
        _jacobianOplusXi(2,4) = 0;
        _jacobianOplusXi(2,5) = -1;
    }

    bool read ( istream& in ) {}
    bool write ( ostream& out ) const {}
protected:
    Eigen::Vector3d _point;
};

本人设计的节点，感觉思路似乎很简答的~

#include "sophus/se3.h"

// 相机顶点
class CameraVertex: public BaseVertex<6,Sophus::SE3>
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  CameraVertex(){}

  // 使用sophus进行更新
  virtual void oplusImpl(const double *update)
  {
    Eigen::Matrix<double,6,1> seUpdate(update);
    _estimate = Sophus::SE3::exp(seUpdate)*_estimate; 
  }

  virtual bool read(std::istream &is);
  virtual bool write(std::ostream &os) const;
}

// g2o edge
class EdgeProjectXYZRGBDPoseOnly : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, CameraVertex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    EdgeProjectXYZRGBDPoseOnly( const Eigen::Vector3d& point ) : _point(point) {}

    virtual void computeError()
    {
        const CameraVertex* pose = static_cast<const CameraVertex*> ( _vertices[0] );
        
        // measurement is p, point is p'
        // 这里需要检查一下
        Eigen::Matrix3d rotate  = pose->estimate().rotationMatrix();
        Eigen::Vector3d trans   = pose->estimate().translation();
        Eigen::Vector3d transp = rotate*_point->estimate() + trans;
        _error = _measurement - transp;
    }

    bool read ( istream& in ) {}
    bool write ( ostream& out ) const {}
protected:
    Eigen::Vector3d _point;
};