Bundle Adjustment 相关源码分析

本文为原创博客，转载请注明出处：https://blog.csdn.net/q_z_r_s

机器感知 一个专注于SLAM、三维重建、机器视觉等相关技术文章分享的公众号

• Vertex成员变量类型分析

  Vertex: public BaseVertex<D, T>
  //base_vertex.h::
  template <int D, typename T>
  class BaseVertex : public OptimizableGraph::Vertex {
      public:
      typedef T EstimateType;
      ...
      EstimateType _estimate;    
  };
  //由此可知,_estimate的类型为T
  

• Edge成员变量分析

  Edge: public BaseBinaryEdge<D, E, Xi, Xj>
  //base_binary_edge.h::
  template <int D, typename E, typename VertexXi, typename VertexXj>
  class BaseBinaryEdge : public BaseEdge<D, E>
  //base_edge.h::
   template <int D, typename E>
  class BaseEdge : public OptimizableGraph::Edge {
      typedef E Measurement;
      typedef Eigen::Matrix<double, D, 1, Eigen::ColMajor> ErrorVector;
      ...
      Measurement _measurement;
      ErrorVector _error;
  };
  //由此可知,_error的类型为Matrix<double, D, 1>,_measurement的类型为E
  

• SE3Quat 成员变量

  //SE3Quat实际上是7维的,而派生VertexSE3Expmap时确实6维的
  Eigen::Quaterniond _r;
  Vector3D _t;
  

• SE3Quat 核心构造函数

  SE3Quat();//_r初始化为单位阵,_t初始化为0
  //用旋转矩阵R和平移向量t来初始化_r、_t
  SE3Quat(const Matrix3D& R, const Vector3D& t):_r(Eigen::Quaterniond(R)),_t(t)
  //用四元数q和平移向量t来初始化_r、_t
  SE3Quat(const Eigen::Quaterniond& q, const Vector3D& t):_r(q),_t(t)
  

• SE3Quat 核心成员函数

  Vector6d log()//返回SE(3)对应的李代数
  Vector3D map(const Vector3D & xyz)//根据类中_r和_t给出xyz映射后的坐标
  static SE3Quat exp(const Vector6d & update) {//与log相反,返回李代数对应的SE3Quat
  	Vector3D omega;//旋转:角轴
      for (int i=0; i<3; i++)
          omega[i]=update[i];
      Vector3D upsilon;//平移
      for (int i=0; i<3; i++)
          upsilon[i]=update[i+3];
      double theta = omega.norm();
      Matrix3D Omega = skew(omega);
      Matrix3D R;
      Matrix3D V;
      if (theta<0.00001)
      {
          //TODO: CHECK WHETHER THIS IS CORRECT!!!
          R = (Matrix3D::Identity() + Omega + Omega*Omega);
          V = R;
      }
      else
      {
          Matrix3D Omega2 = Omega*Omega;
          R = (Matrix3D::Identity()
               + sin(theta)/theta *Omega
               + (1-cos(theta))/(theta*theta)*Omega2);
          V = (Matrix3D::Identity()
               + (1-cos(theta))/(theta*theta)*Omega
               + (theta-sin(theta))/(pow(theta,3))*Omega2);
      }
      return SE3Quat(Eigen::Quaterniond(R),V*upsilon);
  }
  //重载*运算符,在types_six_dof_example.h::VertexSE3Expmap::oplusImpl中使用
  inline SE3Quat operator* (const SE3Quat& tr2) const{
      SE3Quat result(*this);
      result._t += _r*tr2._t;
      result._r*=tr2._r;
      result.normalizeRotation();
      return result;
  }
  //重载输出流<<操作符
  inline std::ostream& operator <<(std::ostream& out_str, const SE3Quat& se3)
  {
      out_str << se3.to_homogeneous_matrix()  << std::endl;
      return out_str;
  }
  

• VertexSBAPointXYZ

  class G2O_TYPES_SBA_API VertexSBAPointXYZ : public BaseVertex<3, Vector3D> {
  public:
      ...
  	virtual void setToOriginImpl() {
        _estimate.fill(0.);
      }
      virtual void oplusImpl(const double* update)
      {
        Eigen::Map<const Vector3D> v(update);
        //XYZ为R(3)空间,可以直接进行相加
        _estimate += v;
      }
  };
  

• VertexSE3Expmap

  class G2O_TYPES_SBA_API VertexSE3Expmap : public BaseVertex<6, SE3Quat> {
  public:
      ...
      virtual void setToOriginImpl() {
          //通过上面的分析可以知道_estimate为SE3Quat类型,使用默认初始化函数
          _estimate = SE3Quat();
      }
      virtual void oplusImpl(const double* update_)  {
          Eigen::Map<const Vector6d> update(update_);
          //首先使用SE3Quat静态成员函数exp(指数映射)把Vector6D转化为SE3Quat类中的_r和_t
          setEstimate(SE3Quat::exp(update)*estimate());
      }
  };
  

• EdgeProjectXYZ2UV

  class G2O_TYPES_SBA_API EdgeProjectXYZ2UV : public  BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap> {
  public:
      ....
      void computeError()  {
        //在世界坐标系中的相机位姿
        const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
        const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>
            (_vertices[0]);//世界坐标系3D点坐标
        const CameraParameters * cam//相机内参
            = static_cast<const CameraParameters*>(parameter(0));
        Vector2D obs(_measurement);//观测值,这里为特征点像素坐标(u,v)
        //观测值-重投影值(相机内参映射(位姿映射(3D点坐标)))
        _error = obs-cam->cam_map(v1->estimate().map(v2->estimate()));
      }
      virtual void linearizeOplus();
      CameraParameters * _cam;
  };