视觉slam--第九章project代码解析(一)

1.前言

最近在看slam中的第九章，说句实话，代码如果自己不编真的无法将其融汇贯通，不写个博客也很难对其有所理解。希望这篇博客对跟我一样是菜鸟的你有所帮助。

2.代码解析部分

1.关于头文件部分，我就不过多的讲解，不懂的可以去看一下c++，主要就是定义自己的头文件，定义自己的类，类里面的函数

并没有写出具体的方法，一般都是在对应的cpp文件中，将其函数的具体实现方式给写出来。

#ifndef CAMERA_H
#define CAMERA_H

#include "myslam/common_include.h"

namespace myslam
{

// Pinhole RGBD camera model
class Camera
{
public:
    typedef std::shared_ptr Ptr;
    float   fx_, fy_, cx_, cy_, depth_scale_;  // Camera intrinsics 

    Camera();
    Camera ( float fx, float fy, float cx, float cy, float depth_scale=0 ) :
        fx_ ( fx ), fy_ ( fy ), cx_ ( cx ), cy_ ( cy ), depth_scale_ ( depth_scale )
    {}

    // coordinate transform: world, camera, pixel
    Vector3d world2camera( const Vector3d& p_w, const SE3& T_c_w );
    Vector3d camera2world( const Vector3d& p_c, const SE3& T_c_w );
    Vector2d camera2pixel( const Vector3d& p_c );
    Vector3d pixel2camera( const Vector2d& p_p, double depth=1 ); 
    Vector3d pixel2world ( const Vector2d& p_p, const SE3& T_c_w, double depth=1 );
    Vector2d world2pixel ( const Vector3d& p_w, const SE3& T_c_w );

};

}
#endif // CAMERA_H

比如说像camera.h中，第一句话是防止了类被重复定义了，第二句话就是定了一个头文件。

#ifndef CAMERA_H
#define CAMERA_H

而typedef 主要是自定义了一个智能指针Ptr（关键还是类类指针），在某些情况下这个指针定义的是静态指针，比如在congfig中，因为所有的参数都由它来提供，有且只有一个，不需要重复new 对象了。还有就是～后面的析构函数，就是为了释放内存，函数开辟了内存空间，赋予生命，那么析够函数就是结束它的生命了。这些就是头文件中大家需要注意的点。

2.接下来主要就是讲解cpp文件中的代码

• camera.cpp    我就不讲解了，很容易理解，就是基本的世界坐标系，相机坐标系，像素坐标之间的变换，如果懂请看相机与图像的那一章。
• config.cpp     这个具体我不知道，但是会用就行了，主要通过setParameterFile( const std::string& filename )，将参数文件传进去，通过get函数获取对应的参数，get函数是个泛类型，具体方法直接写在了congfig.h之中。
• frame.cpp   这个我直接给出代码详解：

  #include "myslam/frame.h"
  
  namespace myslam
  {
  Frame::Frame()
  : id_(-1), time_stamp_(-1), camera_(nullptr)
  {
  
  }
  
  Frame::Frame ( long id, double time_stamp, SE3 T_c_w, Camera::Ptr camera, Mat color, Mat depth )
  : id_(id), time_stamp_(time_stamp), T_c_w_(T_c_w), camera_(camera), color_(color), depth_(depth)
  {
  
  }
  
  Frame::~Frame()
  {
  
  }
  
  Frame::Ptr Frame::createFrame()
  {
      static long factory_id = 0;
      return Frame::Ptr( new Frame(factory_id++) );//返回的是一个指针类型，并且还是个类类型的指针，该指针就指向一个frame对象，可以使用对应的成员变量和函数。
  }
  
  double Frame::findDepth ( const cv::KeyPoint& kp )
  {
      int x = cvRound(kp.pt.x);//四舍五入
      int y = cvRound(kp.pt.y);
      ushort d = depth_.ptr(y)[x];//y是行，x是列，还有就是为什么第一个是（）第二个是[]，这个我没有懂，大家知道的话和我说一下。
      if ( d!=0 )
      {
          return double(d)/camera_->depth_scale_;//因为变换矩阵的尺度不确定性，所以尺度做了归一化，然后这里我估计深度也是做了归一化的，所一要要除以深度因子。
      }
      else 
      {
          // check the nearby points 如果深度信息等于0，则取附近的值，正好是周围的四个值。
          int dx[4] = {-1,0,1,0};
          int dy[4] = {0,-1,0,1};
          for ( int i=0; i<4; i++ )
          {
              d = depth_.ptr( y+dy[i] )[x+dx[i]];
              if ( d!=0 )
              {
                  return double(d)/camera_->depth_scale_;
              }
          }
      }
      return -1.0;
  }
  
  
  Vector3d Frame::getCamCenter() const
  {
      return T_c_w_.inverse().translation();//这里一定要想明白相机的光心在相机坐标系下就是（0，0，0），所以求变换矩阵的逆之后，直接求对应的平移矩即R^(-1)*(-t)，
  }
  bool Frame::isInFrame ( const Vector3d& pt_world )
  { Vector3d p_cam = camera_->world2camera( pt_world, T_c_w_ ); 
  if ( p_cam(2,0)<0 )
   return false; 
  Vector2d pixel = camera_->world2pixel( pt_world, T_c_w_ ); 
  return pixel(0,0)>0 && pixel(1,0)>0 && pixel(0,0)

  • 而且顺便提一下的就是，在增量式的三维重建中，第一个相机的坐标系就是世界坐标系，其他的相机都是以第一个相机的坐标系为参考标准的。但是由于尺度不确定性，相邻两张照片之间做匹配的到的旋转向量可以转化到以第一个相机坐标系为准，但是两者之间的平移向量是个单位向量，所以得用PNP ICP来求位姿

  • 还有一个疑惑点，大家有可能想知道为什么Tcw=T_c_w_.inverse()，据我了解，T_c_w_是Camera To World，并且由上一条所知，相机真实的位姿是由当前的估计位姿和上一个相机所求出来的位姿进行相乘所得到的位姿caimshi真正的位姿。

  • map.cpp和mappoint.cpp就没有什么难度

  如果还有什么我没有讲解到的地方，可以去看另外一片博客点击打开链接。