以下均為簡單筆記，如有錯誤，請多多指教。

9. 使用Sophus的SE3類，自己設計g2o的節點與邊，實現PnP和ICP的優化。 答：對於PnP問題，需要重新設計的節點包括相機節點、Landmark節點和一個相機-Landmark邊；而對於ICP問題，由於邊已經實現了，所以只需要實現節點即可。 PnP問題

#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;
}

//邊
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 {}
}
 

#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;
};