內容總覽：VO框架的實現，對書上的代碼進行理解，寫了一部分註釋，很久以前看的了，如果有需要再回頭再捋一遍，解決解決當時疑惑的點

程序框架/數據結構

庫/目錄結構

bin: 存放可執行的二進制
include/myslam: 存放slam頭文件.h
src: 存放源代碼文件，主要是.cpp
test: 存放測試文件，也是.cpp
lib: 存放編譯好的庫文件
config: 存放配置文件
cmake_modules: 第三方庫的cmake文件，在使用如ceres等庫時使用

這裏都是自己寫的頭文件，不容易和別的庫混淆

基本數據結構

幀：好理解
路標：特徵點，可以放到地圖中進行匹配來估計位姿
配置文件：一些常用的不變的量，如內參，比例等
座標變換：各種座標之間轉換直接定義一個大類

Camera類

頭文件:

//單孔RGBDmodel
  class Camera
  {
  public:
    typedef std::shared_ptr<Camera> Ptr;    //定義了一個智能指針類型，可用於傳遞參數
    float fx_, fy_, cx_, cy_, depth_scale_;        //內參
    
    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 )
    {}
    //座標變換 ,2 表示two = to，SE3來表達相機位姿
    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 );
  };

功能理解：
1.儲存相機的內參和外參，座標轉換，外參用參數的形式輸入。
2.先定義了一個智能指針，然後是內參，構造函數初始化深度爲0
3.座標變換都用的李代數
4.類都被命名空間namespace myslam包裹，可以防止不小心定義出別的庫的同名函數

實現方法.cpp：

namespace myslam    //這東西不套上，代碼補全出不來
{
  Camera::Camera()
  {
  }
  Vector3d Camera::world2camera( const Vector3d & p_w, const SE3& T_c_w )
  {
    return T_c_w * p_w;
  }

  Vector3d Camera::camera2world( const Vector3d& p_c, const SE3& T_c_w )
  {
    return T_c_w.inverse() * p_c;
  }
  
  Vector2d Camera::camera2pixel( const Vector3d& p_c )
  {
    return Vector2d(
      fx_ * p_c( 0, 0 ) / p_c( 2, 0 ) + cx_ ,
            fy * p_c( 1, 0 ) / p_c( 2, 0 ) + cy_
       );
  }
  
  Vector2d Camera::pixel2camera( const Vector2d&p_p, double depth)
  {
    return Vector3d(
      ( p_p( 0, 0 ) - cx_ ) * depth/fx_,
            ( p_p( 1, 0 ) - cy_ ) * depth/fy_,
            depth
       );
  }
  
   Vector2d Camera::world2pixel( const Vector3d&p_w, const SE3&T_c_w)
   {
     return camera2pixel( world2camera( p_w, T_c_w ) );
     //利用了兩個變換一起
   }
   
    Vector3d Camera::pixel2world( const Vector2d&p_p, const SE3&T_c_w, double depth)
    {
      return camera2world( pixel2camera( p_p, depth ), T_c_w );
    }
}

就是些座標變化的實現

Frame類

頭文件：

class Frame
{
public:
  typedef std::shared_ptr<Frame> Ptr;
  unsigned long id_;    //這一幀的編號
  double time_stamp_;    //被記錄的時間，用於後期的匹配,時間戳
  SE3 T_c_w_;    //變換矩陣
  Camera::Ptr camera;        //針孔攝像機模型
  Mat color_, depth_;    //圖像的顏色和深度矩陣
  
public:    //數據成員
  Frame() ;
  Frame( long id, double time_stamp=0, SE3 T_c_w = SE3(), Camera::Ptr camera=nullptr,
     Mat color=Mat(), Mat depth=Mat() );
  ~Frame() ;
  
  //製造Frame函數
  static Frame::Ptr createFrame();
  
  //尋找給定關鍵點的深度
  double findDepth( const cv::KeyPoint& kp );
  
  //獲取攝像機中心
  Vector3d getCamCenter() const;
  
  //檢查點在不在這幀上
  bool isInFrame( const Vector3d& pt_world );
};
  
//路標點類，用當前幀提取到的特徵點和地圖中的特徵點匹配，來估計相機運動，需要存儲描述子
//記錄並存儲點被觀測和匹配的次數，作爲評價好壞的指標
class MapPoint
{
public:
  typedef shared_ptr<MapPoint> Ptr;
  unsigned long_id;    //ID
  Vector3d pos_;   //世界位置
  Vector3d norm_;    //視野的範數的方向
  Mat descriptor_;        //匹配的描述子
  int observed_times_;    //匹配次數？？？
int correct_times_;     //估計次數？？？
MapPoint();
MapPoint( long id, Vector3d position, Vector3d norm );
//建造函數
static MapPoint::Ptr createMapPoint();
};

幀的自我屬性：
編號，時間戳，攝像機模型，變換矩陣，圖像的顏色和深度
功能：
1.創建以一個Frame，返回帶編號的Frame指針
2.獲得攝像機中心（T的轉置的位移向量）
3.尋找給定關鍵點的深度（深度圖已有）
4.檢測某點是否在該幀上（根據攝像機座標和像素座標判斷）

MapPoint類

class MapPoint
{
public:
  typedef shared_ptr<MapPoint> Ptr;
  unsigned long_id;    //ID
  Vector3d pos_;   //世界位置
  Vector3d norm_;    //視野的範數的方向
  Mat descriptor_;        //匹配的描述子
  int observed_times_;    //觀測次數
int correct_times_;     //匹配次數
MapPoint();
MapPoint( long id, Vector3d position, Vector3d norm );
//建造函數，都是空的，就是帶的編號
static MapPoint::Ptr createMapPoint();
};

路標點類，會估計點的世界座標，用當前幀提取到的特徵點和地圖中的特徵點匹配，來估計相機運動，需要存儲描述子。
記錄並存儲點被觀測和匹配的次數，作爲評價好壞的指標

Map類

class Map  
{
public:
  typedef shared_ptr<Map> Ptr;
  //需要隨即訪問，還需要隨時插入和刪除
  unordered_map<unsigned long, MapPoint::Ptr > map_points_;    //全部路標
  unordered_map<unsigned long, Frame::Ptr > keyframes_;    //全部關鍵幀
  
  Map() {}
  
  void insertKeyFrame( Frame::Ptr frame );
  void insertMapPoint( MapPoint::Ptr map_point );
};

功能：管理所有路標點，添加、刪除等操作

Config類

頭文件:

class Config        
{
private:
  static std::shared_ptr<Config> config_;
  cv::FileStorage file_;    //可以讀取YAML文件，存放各種參數，file_[key] 調用
  
  Config () {}    //寫成單件模式，構造函數爲私有
public:
  ~Config();    //析構函數，銷燬時關閉函數
  
  //設置一個新配置文件,創建對象並讀取參數文件，析構函數也在其中
  static void setParameterFile( const std::string& filename );
  
  //載入參數值,用的是模板函數，爲的是可以獲取任意類型的參數值
  template< typename T >
  static T get( const std::string& key )
  {
    return T( Config::config_->file_[key] )l
  }
};

單件模式singleton，在程序任意地方都可以隨時提供參數值
功能：設置、讀取參數文件，因爲有多種類型，所以用模板函數

實現函數：

void Config::setParameterFile( const std::string& filename )
{
  if ( config_ == nullptr )
    config_ = shared_ptr<Config>( new Config );    //沒有指針就定義一個
  config_->file_ = cv::FileStorage( filename.c_str(), cv::FileStorage::READ );    //給智能指針的對象指向賦值
  if ( config_->file_.isOpened() == false )
  {
    //如果文件false，說明文件不存在//是不是計數器起效?
    std::cerr<<" parameter file "<<filename<<" does not exise."<<std::end;
    config_->file_.release();
    return ;
  }
}

Config::~Config()    //析構函數，關閉文件
{
  if ( file.isOpened() )
    file_.release();
}

shared_ptr<Config> Config::config_ = nullptr;

我們使用智能指針，優點是可以自動析構
讀取的文件儲存在FileStorage類中
使用範例：
1.在參數文件加入：“Camera.fx:500”。
2. myslam::Config::setParameterFile(“parameter.yaml”);
3. double fx=myslam::Config::get(“Camera.fx”);

基本的VO，特徵提取和匹配

兩兩幀

設定參考幀和當前幀，以參考幀爲座標系，把當前幀和參考幀進行特徵匹配，並估計運動關係

思路

/* 思路：每次兩個幀比較，一個參考幀（上一幀），一個當前幀，二者進行特徵匹配（以參考幀爲座標系），
* 並估計運動關係，得 T_c_w = T_c_r * T_r_w
* 當前相對世界座標系的變化 = 當前相對參考的座標變換 * 參考相對世界座標系的變換
* 求VO流程：
* 1.對新來的當前幀，提取關鍵點和描述子
* 2.如果系統沒初始化，就把當前幀當作參考幀，返回1
* 3.估計參考幀和當前幀的運動
* 4.如果成功，就把當前幀設爲參考幀，返回1
* 5.如果不成功，計算連續不成功次數，超過某個次數就置VO爲丟失狀態，
* 算法退出。沒超過該次數，就返回1
*/```
## 算法

```cpp
class VisualOdometry
  {
  public:
  typedef shared_ptr<VisualOdometry> Ptr;
  enum VOState {        //狀態，是否初始化，是否丟失
    INITIALIZING=-1,
    OK = 0,
    LOST
  };
  
  VOState state_;    //目前的VO狀態，initialize or ok or lost
  Map::Ptr map_;    //裝有全部的幀和特徵點？？
  Frame::Ptr ref_:    //參考幀
  Frame::Ptr curr_;        //當前幀
  
  cv::Ptr<cv::ORB> orb_;        //orb檢測和計算方式
  vector<cv::Point3f> pts_3d_ref_;    //參考幀中的3d點
  vector<cv::KeyPoint> keypoints_curr_;    //現在幀的特徵點
  Mat descriptors_curr;        
  Mat descriptors_ref;
  vector<cv::DMatch> feature_matched_;
  
  SE3 T_c_r_estimated_;    //參考到現在的變換矩陣估計
  int num_inliers_;     //內點
  int num_lost_;
  
  //parameters
  int  num_of_features_;
  double scale_factor_;
  int level_pyramid_;    //number of pyramid levels
  float match_ratio_;    //ratio for selecting good matches
  int max_num_lost_;
  int min_inliers_;
  
  double key_frame_min_rot;    // minimal rotation of tow key-frames
  double key_frame_min_trans;
  
  public:
    VisualOdometry();
    ~VisualOdometry();
    
    bool addFrame( Frame::Ptr frame );
    
  protected:
    //inner operation
    void extractKeyPoints();
    void computeDescriptors();
    void featureMatching();
    void poseEstimationPnP();
    void setRef3DPoints();
    
    void addKeyFrame();
    bool checkEstimatedPose();
    bool checkKeyFrame();
  };

參數：
全部的幀和特徵點，估計變換矩陣，變換最小程度限制
功能：
1.增加幀
2.提取特徵點
3.計算描述子
4.特徵匹配
5.pnp估計位姿
6.設置參考3d點
7.檢驗估計點
3.檢驗關鍵幀

bool myslam::VisualOdometry::addFrame(Frame::Ptr frame)
{
  switch( state_ )
  {
    case INITALIZING:
    {
      state_ = OK;
      curr_ = ref_ = frame;    //第一個
      map_->insertKeyFrame( frame );
      //from first frame,ref and curr
      extractKeyPoints();
      computeDescriptors();
      setRef3DPoints();
      break;
    }
    case OK:
    {    //直接用類的參數，真方便
      curr_ = frame;
      extractKeyPoints();
      computeDescriptors();
      featureMatching();
      poseEstimationPnP();
      //checkEstimatedPose中有內點數量和移動最大距離限制
      if ( checkEstimatedPose() == true )     //a good    //poseEstimationPnP
      {
    curr_->T_c_w_ = T_c_r_estimated_ * ref_->T_c_w_;
    ref_ = curr_;
    setRef3DPoints();
    num_lost = 0;        //set 0
    if ( checkKeyFrame() == true )
    {
      addKeyFrame();
    }
      }
      else
      {
    num_lost_++ ;        //以每組（ref and curr）爲單位
    if ( num_lost_ > max_num_lost_ )
    {
      state_ = LOST;
    }
    return false;
      }
      break;
    }
    case LOST:
    {
      cout<<"vo has lost."<<endl;
      break;
    }
  }
  return true;

}

新增Frame，內可判斷是否lost,內置各種函數，如獲取特徵點，計算描述子等，算完直接加入到新Frame屬性中
可估計位姿，自帶檢驗算法，檢驗內點數量和最大移動距離限制判斷是一個好的估計位姿
如果是個關鍵幀，就加入到關鍵幀中
根據丟失幀數量判斷是否丟幀

運行

if ( argc != 2 )
  {
    cout<<"usage: run_vo parameter_file"<<endl;
    return 1;
  }
  myslam::Config::setParameteFile( argv[1] );
  myslam::VisualOdometry::Ptr vo ( new myslam::VisualOdometry );
  
  string dataset_dir = myslam::Config::get<string> ( "dataset_dir" );
  cout<<"dataset: "<<dataset_dir<<endl;
  ifstream fin (dataset_dir + "/associate.txt" );
  if ( !fin )
  {
     cout<<"please generate the associate file called associate.txt!"<<endl;
     return 1;
  }
  
  vector<string> rgb_files, depth_files;
  vector<double> rgb_times, depth_times;
  while( !fin.eof() )    //讀取各種數據
  {
    string rgb_time, rgb_file, depth_time, depth_file;
    fin>>rgb_time>>rgb_file>>depth_time>>depth_file;
    rgb_times.push_back( atof( rgb_time.c_str() ) );
    depth_times.push_back( atof( depth_time.c_str() ) );
    rgb_files.push_back( dataset_dir + "/" + rgb_file );
    depth_files.push_back( dataset_dir + "/" + depth_file );
    
    if ( fin.good() == false )
      break;
  }

1.準備工作，讀取各種數據



  myslam::Camera::Ptr camera( new myslam::Camera );
  
  //visualization,初始化並設置各種關於世界座標系和攝像機座標系的信息
  cv::viz::Viz3d vis("Visual Odometry");

  cv::viz::WCoordinateSystem world_coor(1.0), camera_coor(0.5);    //座標系設置,此處可能是比例

  cv::Point3d cam_pos( 0, -1.0, -1.0 ), camera_focal_point(0,0,0), cam_y_dir(0,1,0);    //一些重要的點的位置
  cv::Affine3d cam_pose = cv::viz::makeCameraPose(cam_pos, cam_focal_point, cam_y_dir );    //what？？？
  vis.setViewerPose( cam_pose );        //視野設置？？？，應該就是這相機前面視野
  
  //渲染和顯示？？？
  world_coor.setRenderingProperty(cv::viz::LINE_WIDTH, 2.0);        //渲染屬性
  camera_coor.setRenderingProperty(cv::viz::LINE_WIDTH,1.0);
  vis.showWidget("World", world_coor);
  vis.showWidget("Camera", camera_coor);
  
  //讀取幀信息,並添加到vo裏計算
  cout<<"real total"<<rgb_files.size() <<" entried" <<endl;
  for( int i=0; i<rgb_files.size(); i++)
  {
    Mat color = cv::imread( rgb_files[i] );
    Mat depth = cv:imread( depth_files[i], -1);
    if ( color.data == nullptr || depth.data==nullptr )
      break;
    myslam::Frame::Ptr pFrame = myslam::Frame::createFrame();
    pFrame->camera_ = camera;
    pFrame->color_ = color;
    pFrame->depth_ = depth;
    pFrame->time_stamp_ = rgb_times[i];
    
    boost::timer timer;
    vo->addFrame( pFrame );
    cout<<"VO cost time: "<<timer.elapsed()<<endl;
    if ( vo->state_ == myslam::VisualOdometry::LOST )
      break;
    SE3 Tcw = pFrame->T_c_w.inverse();
    
    //show th map and the camera cam_pose
    //M構造分兩部分
    cv::Affine3d M(
      cv::Affine3d::Mat3(
                    Tcw.rotation_matrix()(0,0),Tcw.rotation_matrix()(0,1),Tcw.rotation_matrix()(0,2),
            Tcw.rotation_matrix()(1,0),Tcw.rotation_matrix()(1,1),Tcw.rotation_matrix()(1,2),
            Tcw.rotation_matrix()(2,0),Tcw.rotation_matrix()(2,1),Tcw.rotation_matrix()(2,2)
    ),
    cv::Affine3d::Vec3(
       Tcw.translation()(0,0),Tcw.translation()(1,0),Tcw.translation()(2,0)
              )
    );
    cv::imshow("image", color);
    cv::waitKey(1);
    vis.setWidgetPose("Camera", M);
    vis.spinOnce(1, false);
  }

2.初始化視覺里程計，相機的內參，映射，視野設置和相機映射相同
3.渲染屬性
4.讀取幀的信息，放入VO中計算
（1）創建幀，給幀輸入圖片的各種信息
（2）將創建的幀的屬性放入到addFrame函數中計算
5.得出矩陣M

存在的問題

/*效果並不好
* 1.只用RANSAC容易受噪聲影響        solve：用RANSAC作初始值，再用非線性優化求最優值
* 2.特徵點多是邊緣，所以特徵點的深度不準確        solve：將特徵點也優化
* 3.幀與幀的比較太依賴幀，不好的幀甚至會導致漂移        solve：讓當前幀與地圖匹配
* 4.特徵點的提取和匹配耗時太多    solve：直接法，光流法```


# 優化PnP的結果
## 方法
RANSCA提供初始值，pnp求相機位姿，g2o優化（最小化衝投影誤差）相機位姿
## g2o
**頭文件**：

```cpp
  class EdgeProjectXYZ2UVPoseOnly: public g2o::BaseUnaryEdge<2, Eigen::Vector2d, g2o::VertexSE3Expmap>
  {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
    virtual void computeError();
    virtual void linearizeOplus();
    
    virtual bool read( std::istream& in ) {}
    virtual bool write(std::ostream& os) const {};
    
    Vector3d point_;    //方便計算重投影誤差和呀可比
    Camera* camera_;
  };```

  **實現函數**：
  

```cpp
void EdgeProjectXYZ2UVPoseOnly::computeError()
{
  const g2o::VertexSE3Expmap * pose = static_cast<const g2o::VertexSE3Expmap*> ( _vertices[0] );
  _error = _measure - camera->camera2pixel(
    pose->estimate().map(point_)        //重投影誤差
                  );
}
  
  
void EdgeProjectXYZ2UVPoseOnly::linearizeOplus()        
//jacobin，優化一個位姿。是一個一元邊
{
  g2o::VertexSE3Expmap * pose = static_cast<g2o::VertexSE3Expmap*> ( _vertices[0] );

  g2o::SE3Quat T ( pose->estimate() );

  Vector3d xyz_trans = T.map( point_ );    //映射點
  double x = xyz_trans[0];
  double y = xyz_trans[1];
  double z = xyz_trans[2];
  double z_2 = z*z;
   _jacobianOplusXi(0,0)=x*y/z_2*camera_->fx_;
  _jacobianOplusXi(0,1)=-(1+(x*x/z_2))*camera_->fx_;
  _jacobianOplusXi(0,2)=y/z*camera_->fx_;
  _jacobianOplusXi(0,3)=-1./z*camera_->fx_;
  _jacobianOplusXi(0,4)=0;
_jacobianOplusXi(0,5)=x/z_2*camera_->fx_;

_jacobianOplusXi(1,0)=(1+y*y/z_2)*camera_->fy_;
_jacobianOplusXi(1,1)=-x*y/z_2*camera_->fy_;
_jacobianOplusXi(1,2)=-x/z*camera_->fy_;
_jacobianOplusXi(1,3)=0;
_jacobianOplusXi(1,4)=-1./z*camera_->fy_;
_jacobianOplusXi(1,5)=y/z_2*camera_->fy_;

}

修改PnP

void visual_odometry::poseEstimationPnP()
  {
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 2> Block;

    Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();

    Block* solver_ptr = new Block( linearSolver );

    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
    );
    //稀疏優化,設置優化器，優化方法是LM
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm( solver );
    //設置頂點，即要優化的向量，即相機位姿
    g2o::VertexSE3Expmap * pose = new g2o::VertexSE3Expmap();
    pose->setId( 0 );
    pose->setEstimate( g2o::SE3Quat(
      T_c_r_estimated_.rotation_matrix(), T_c_r_estimated_.translation()
    ));
    optimizer.addVertex( pose );
    
    //edges,設RANSAC已經提取完Inline點，現對這些點進行g2o優化
    for ( int i=0; i<inliers.rows; i++)
    {
      int index = inliers.at<int>(i,0);
      //3D->2D映射，設置邊的一系列
      EdgeProjectXYZ2UVPoseOnly * edge = new EdgeProjectXYZ2UVPoseOnly();
      edge->setId(i);        //設置邊的編號
      edge->setVertex(0, pose);    //設置邊連接的頂點，就是相機位姿
      edge->camera_ = curr_->camera_.get();
      edge->point_ = Vector3d( pts3d[Index].x, pts3d[index].y, pts3d[index].z);//什麼點？
      edge->setMeasurement( Vector2d(pts2d[Index].x, pts2d[index].y) );    //測量值
      //信息矩陣就是協方差的逆，表示每條邊誤差分擔多少，單位矩陣就是誤差分攤
      edge->setInformation(Eigen::Matrix2d::Identity() );
      optimizer.addEdge( edge );
    }
    optimizer.initializeOptimization();
    //10是迭代次數
    optimizer.optimize(10);
    T_c_r_estimated_ = SE3(
      pose->estimate().rotation(),
               pose->estimate().translation()
               );
      //上面都是對pose進行優化，先給點（來自RANSAC），然後設爲頂點，用邊優化，
    //優化完了再賦值給初始值向量

以PnP結果爲初始值，調用g2o優化
1.先設置優化器
2.設置頂點，各種信息及編號
optimizer.addVertex( pose );
3.設置邊，將邊與頂點連接
optimizer.addEdge( edge );

局部地圖

思路

將VO匹配到的特徵點放到地圖中，並將當前幀和地圖點進行匹配，計算位姿

這樣每一幀都能有所貢獻

MapPoint

class MapPoint
  {
  public:
    typedef shared_ptr<MapPoint> Ptr;
    unsigned long_id_;
    static unsigned long factory_id_;    //建造id
    bool good_;
    Vector3d pos_;        //position in world
    Vector3d norm_;        //normal of viewing direction
    Mat descriptor_;        //descriptor for matching
    
    list<Frame*> observed_frames_;    //key-frames that can observe this MapPoint
    
    int matched_times_;        //inliner matched_times_
    int visible_times_;        //being visible in current frame
    
    MapPoint();        //對地圖點的構建，每次都有元素進入
    MapPoint(
      unsigned long id,
      const Vector3d& position,
      const Vector3d& norm,
      Frame* frame = nullptr,
      const Mat& descriptor=Mat()
    );
    
    inline cv::Point3f getPositionCV() const{
      return cv::Point3f( pos_(0,0), pos_(1,0), pos_(2,0) );
    }
    
    static MapPoint::Ptr createMapPoint();        //新建，比上面少個id
    static MapPoint::Ptr createMapPoint(
      const Vector3d& pos_world,
      const vector3d& norm_,
      const Mat& descriptor,
      Frame* frame
             );

此處的地圖：將各幀的特徵點緩存到一處，構成的特徵點的集合，稱爲地圖
類的屬性：
關鍵幀，匹配描述子，匹配、觀測次數，位置

VisuOdometry類的修改

添加關鍵幀：

void VisualOdometry::addKeyFrame()
  {
    if ( map_->keyframe_.empty() )    //if not have keyframe
    {
      //first frame , add all 3d point into map_
      for ( size_t i=0; i< keypoints_curr_.size(); i++)
      {
    double d = curr_->findDepth( keypoints_curr_[i] );
    if ( d<0 )
      continue;
    Vector3d p_world = ref_->camera_->pixel2world(
      Vector2d( keypoints_curr_[i].pt.x, keypoints_curr_[i].pt.y ), curr_->T_c_w_, d
                      );
    Vector3d n = p_world - ref_->getCamCenter();    //world * cameracenter ???
    n.normalize();//正則化，是對焦距的處理？
    
    MapPoint::Ptr map_point = MapPoint::createMapPoint(
      p_world, n, descriptors_curr_.row(i).clone(), curr_.get()
                        );
    map_->insertMapPoint( map_point );
      }
    }
    map_->insertKeyFrame( curr_ );    //current frame match with mappoint,compute pose
    ref_ = curr_;
  }

在提取第一幀特徵帶你之後，將第一幀所有的特徵點放入地圖中
1.根據像素座標算出世界座標，深度在camera類裏有
2.算出normalize，調用creatMapPoint函數創造特徵點
3.將創造出來的MapPoint加入到map中
4.添加關鍵幀

map優化（匹配點）增刪:

  void VisualOdometry::optimizeMap()
  {
    //remove the hardly seen and no visible points
    for( auto iter = map_->map_point_.begin(); iter !=map_->map_point_.end(); )
    {
      if ( !curr_->isInFrame(iter->second->pos_) )    //不在幀裏就去掉
      {
    iter = map_->map_points_.erase(iter);
    continue;
      }
      float match_ratio = float(iter->second->matched_times_) / iter->second->visible_times_;
      if ( match_ratio < map_point_erase_ratio_)    //匹配的比例太少，刪除一些提高比例
      {
    iter = map_->map_points_.erase(iter);
    continue;
      }
      double angle = getViewAngle( curr_, iter->second );
      if ( angle > M_PI/6. )        //轉角過大也不要
      {
    iter = map_->map_point_.erase(iter);
    continue;
      }
      if ( iter->second->good_ == false )    //三角測量？
      {
    
      }
      iter++;
    }
    
    if ( match_2dkp-index_.size() < 100 )    //匹配點數量不夠的話，加一些地圖點
      addMapPoints();
    if( map_->map_points_.size() > 1000 )    //太多了就刪一個，同時要增加刪除的比例
    {
      iter = map_->map_point_.erase(iter);
      map_point_erase_ratio_ += 0.05;
    }
    else
      map_point_erase_ratio_ = 0.1;
    cout << "map points : " << map_->map_points_.size() << endl;
  }

在後續的幀中，使用OptimizeMap對地圖進行優化
刪除：
1.不在幀裏的
2.匹配比例小，刪一部分
3.轉動角度過大
4.匹配點太多，大於閾值。同時增加刪除的比例
增加：
1.匹配點數量太少，少於閾值

特徵匹配:

  void VisualOdometry::featureMatching()
  {
    boost::timer timer;
    vector<cv::DMatch> matches;        //存放所有匹配到的點
    //select the candidates in map
    Mat desp_map;    //構建地圖的描述子矩陣
    vector<MapPoint::Ptr> candidate;
    //allpoint
    for ( auto& allpoints: map_->map_points_ )
    {
      MapPoint::Ptr& p = allpoints.second;
      //check if p in curr frame image
           //在視野內就入選
      if( curr_->isInFrame(p->pos_) )
      {
    //add to candidate
    p->visible_times_++;
    candidate.push_back(p);
    desp_map.push_back( p->descriptor_ );
      }
    }
    
    matches_flann_.match( desp_map, descriptors_curr, matches);
    //select the best matches,distance is min
    float min_dis = std::min_element(
      matches.begin(), matches.end(),
      []( const cv::DMatch& m1, const cv::DMatch& m2)
                     {
                       return m1.distance < m2.distance;
                     } )->distance;
    )
    
    match_3dpts_.clear();
    match_2dkp_index_.clear();
    //篩選，距離最小的上面已經選出來了，這裏再把比閾值還小的去掉
    for ( cv::DMatch& m: matches )
    {
      if ( m.distance < max<float> ( min_dis*match_ratio_, 30.0 ) )
      {
    match_3dpts_.push_back( candidate[m.queryIdx] );    //query 測試下標
    match_2dkp_index_.push_back( m.trainIdx );    //樣本圖像特徵帶你描述符下標
      }
    }
    
    cout<<" good matched: " << match_3dpts_.size() << endl;
    cout << " mathc cost time: "<<timer.elapsed() << endl;

匹配之前，選出一些視野內的候選點，將它們與當前幀的特徵描述子進行匹配
1.選出內點，添加構建地圖的描述子矩陣
2.根據兩個描述子計算，將匹配信息放入matches,先搞了波最小距離（平均值？）
3.根據距離，選比最小還小的匹配點，但是如果最小距離大於某閾值，那就取小於該閾值的匹配點

缺點:
1.容易丟失，一旦丟失，就要等相機轉回來，否則就要重置VO
2.軌跡漂移，主要是誤差累計引起的

c++知識

智能指針 shared_ptr

創建：要聲明類型 shared_ptr p1;
默認初始化：智能指針中保存着空指針。
使用方法和指針類似

make_shared必須初始化，返回指向此類型shared_ptr //shared_ptr p3 = make_shared(42)
計數器：如表12.2
自動銷燬：當沒有shared_ptr再指向這個對象時，這個對象就會被銷燬
自動釋放內存：erase刪除不需要的元素後，會自動釋放相關聯的內存

直接初始化智能指針：
shared_ptr p1 = new int(1024);//錯誤！
shared_ptr p2( new int(1024) );//正確!

   //特徵匹配，拿出一些inline和當前幀的特徵描述子匹配
  void VisualOdometry::featureMatching()
  {
    boost::timer timer;
    vector<cv::DMatch> matches;        //存放所有匹配到的點？
    //select the candidates in map
    Mat desp_map;    //存放匹配到的描述子
    vector<MapPoint::Ptr> candidate;
    //allpoint
    for ( auto& allpoints: map_->map_points_ )
    {
      MapPoint::Ptr& p = allpoints.second;
      //check if p in curr frame image
      if( curr_->isInFrame(p->pos_) )
      {
    //add to candidate
    p->visible_times_++;
    candidate.push_back(p);
    desp_map.push_back( p->descriptor_ );
      }
    }
    
    matches_flann_.match( desp_map, descriptors_curr, matches);
    //select the best matches,distance is min
    float min_dis = std::min_element(
      matches.begin(), matches.end(),
                     []( const cv::DMatch& m1, const cv::DMatch& m2)
                     {
                       return m1.distance < m2.distance;
                     } )->distance;
    )
    
    match_3dpts_.clear();
    match_2dkp_index_.clear();
    //篩選，距離最小的上面已經選出來了，這裏再把比閾值還小的去掉
    for ( cv::DMatch& m: matches )
    {
      if ( m.distance < max<float> ( min_dis*match_ratio_, 30.0 ) )
      {
    match_3dpts_.push_back( candidate[m.queryIdx] );    //query 測試下標
    match_2dkp_index_.push_back( m.trainIdx );    //樣本圖像特徵帶你描述符下標
      }
    }
    
    cout<<" good matched: " << match_3dpts_.size() << endl;
    cout << " mathc cost time: "<<timer.elapsed() << endl;
  }

KirutoCode

發佈了53 篇原創文章 · 獲贊 2 · 訪問量 7158

私信關注

SLAM十四講：搭建VO框架(9)

文章目錄

程序框架/數據結構

庫/目錄結構

基本數據結構

Camera類

Frame類

MapPoint類

Map類

Config類

基本的VO，特徵提取和匹配

兩兩幀

思路

運行

存在的問題

修改PnP

局部地圖

思路

MapPoint

VisuOdometry類的修改

c++知識

智能指針 shared_ptr

推薦IntelliJ產品可用的LeetCode刷題插件

《大話數據結構》第二章算法

《大話數據結構》第一章數據結構緒論

《大話數據結構》用JAVA實現筆記整理

《大話數據結構》第七章圖

Mac下配置sublime實現LaTeX

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結