视觉slam十四讲--VO框架基础学习笔记：构建vo框架，并运行TUM数据集程序

首先是创建一个project 1文件夹，进入该文件夹后，创建bin文件夹和lib文件夹，并在当前目录下创建一个CMakeLists.txt文件，内容如下：

# 声明要求的 cmake 最低版本
 cmake_minimum_required( VERSION 2.8 )
 # 声明一个 cmake 工程
 project( myslam )

# 添加一个可执行程序
# 语法：add_executable( 程序名 源代码文件 ）


set( CMAKE_CXX_COMPILER "g++" )
set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-std=c++11 -march=native -O3" )

list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )
set( EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin )
set( LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib )

############### dependencies ######################
# Eigen
include_directories( "/usr/include/eigen3" )
# OpenCV
find_package( OpenCV  REQUIRED )
include_directories( ${OpenCV_INCLUDE_DIRS} )
# Sophus 
find_package( Sophus REQUIRED )
include_directories( ${Sophus_INCLUDE_DIRS} )
# G2O
find_package( G2O REQUIRED )
include_directories( ${G2O_INCLUDE_DIRS} )

set( THIRD_PARTY_LIBS 
    ${OpenCV_LIBS}
    ${Sophus_LIBRARIES}
    g2o_core g2o_stuff g2o_types_sba
)
############### source and test ######################
include_directories( ${PROJECT_SOURCE_DIR}/include )
add_subdirectory( src )
add_subdirectory( test )

建立config文件夹和其下的文件

在project 1文件夹下创建config文件夹后，在其文件夹下创建default.yaml文件，内容如下：

%YAML:1.0
# data
# the tum dataset directory, change it to yours! 
dataset_dir: /home/li/slam/myslam/project 1/data/rgbd_dataset_freiburg1_xyz

# camera intrinsics
# fr1
camera.fx: 517.3
camera.fy: 516.5
camera.cx: 325.1
camera.cy: 249.7

camera.depth_scale: 5000

# VO paras
number_of_features: 500
scale_factor: 1.2
level_pyramid: 8
match_ratio: 2.0
max_num_lost: 10
min_inliers: 10
keyframe_rotation: 0.1
keyframe_translation: 0.1

**注意：**文件前面不能有空行或注释，而dataset_dir要修改为自己的TUM数据集所在的位置。

建立include/myslam文件夹及其下的文件

在project 1文件夹下创建include/myslam文件夹，然后在其下创建各类的头文件：
创建camera.h，内容如下：

#ifndef CAMERA_H
 #define CAMERA_H
 #include "myslam/common_include.h"
namespace myslam
{ 
 // Pinhole RGB-D camera model
class Camera
 { 
 public:
 typedef std::shared_ptr<Camera> 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

创建common_include.h文件，内容如下：

#ifndef COMMON_INCLUDE_H
#define COMMON_INCLUDE_H

// define the commonly included file to avoid a long include list
// for Eigen
#include <Eigen/Core>
#include <Eigen/Geometry>
using Eigen::Vector2d;
using Eigen::Vector3d;

// for Sophus
#include <sophus/se3.h>
#include <sophus/so3.h>
using Sophus::SE3;
using Sophus::SO3;

// for cv
#include <opencv2/core/core.hpp>
using cv::Mat;

// std 
#include <vector>
#include <list>
#include <memory>
#include <string>
#include <iostream>
#include <set>
#include <unordered_map>
#include <map>

using namespace std; 
#endif

创建config.h文件，内容为：

#ifndef CONFIG_H
#define CONFIG_H

#include "myslam/common_include.h" 

namespace myslam
{
class Config
 { 
 private: 
static std::shared_ptr<Config> config_;
 cv::FileStorage file_;
 Config () {} // private constructor makes a singleton
 public: 
 ~Config(); // close the file when deconstructing

 // set a new config file
 static void setParameterFile( const std::string& filename );

 // access the parameter values
 template< typename T >
 static T get( const std::string& key )
 {
 return T( Config::config_->file_[key] );
 }
 };

}

#endif // CONFIG_H

创建frame.h文件，内容为：

#ifndef FRAME_H
#define FRAME_H

#include "myslam/common_include.h"
#include "myslam/camera.h"

namespace myslam
{
    class MapPoint;
class Frame
 { 
public: 
 typedef std::shared_ptr<Frame> Ptr;
 unsigned long id_; // id of this frame
double time_stamp_; // when it is recorded
SE3 T_c_w_; // transform from world to camera
 Camera::Ptr camera_; // Pinhole RGB-D Camera model
Mat color_, depth_; // color and depth image

 public: // data members
 Frame();
 Frame( long id, double time_stamp=0, SE3 T_c_w=SE3(), Camera::Ptr camera=nullptr, Mat color=Mat(),
Mat depth=Mat() );
 ~Frame();

 // factory function
 static Frame::Ptr createFrame();

 // find the depth in depth map
 double findDepth( const cv::KeyPoint& kp );

 // Get Camera Center
 Vector3d getCamCenter() const;

 // check if a point is in this frame
 bool isInFrame( const Vector3d& pt_world );
 };
}

#endif // FRAME_H

创建map.h文件，内容如下：

#ifndef MAP_H
#define MAP_H

#include "myslam/common_include.h"
#include "myslam/frame.h"
#include "myslam/mappoint.h"

namespace myslam
{
class Map
 { 
 public: 
 typedef shared_ptr<Map> Ptr;
 unordered_map<unsigned long, MapPoint::Ptr > map_points_; // all landmarks
 unordered_map<unsigned lon