设计前端(一)

1.搭建VO框架

A. SLAM库是一个小型库，库的组织结构如下：

1.bin 用来存放可执行的二进制文件。
2.include/myslam存放SLAM模块的头文件，主要是.h文件，当把包含目录设到include，引用自己的头文件时，需要写include“myslam/xxx.h”。
3.src 存放源代码文件，主要是.cpp文件。
4.test 存放测试文件，也是.cpp文件。
5.lib 存放编译好的库文件。
6.config 存放配置文件。
7.cmake_modules 第三方库的cmake文件，在使用g2o之类的库会用到它。

2 . 确定基本数据结构

1.帧：相机采集到的图像单位，包含一个或一对图像。还有特征点、位姿、内参等信息。
2.路标：路标即图像中的特征点，在相机运动后，我们还能估计他们的3D位置(对极几何、PnP、ICP、光流法和直接法等)。通常都是把路标点放在一个地图当中，并将新来的帧与地图中的路标点进行匹配，估计相机位姿。
3.配置文件：在程序中遇到各种各样的参数，相机的内参、特征点数量等，较好的方式是在外部定义一个配置文件，程序运行读取该配置文件的参数值。
4.坐标变换：会经常遇到坐标系之间的转换，世界到相机、相机到归一化、归一化到像素等，定义一个类把这些操作都放一起更方便。

3. 5个类及其功能

5个类：Frame为帧，Camera为相机模型、MapPoint为特征点/路标点，Map管理特征点，Config提供配置参数。

camera类：存储摄像机各个坐标系的转换关系
camera.h ：

#ifndef CAMERA_H  //防止头文件重复引用，防止在俩处引用此头文件时出现类的重复定义
#define CAMERA_H

#include "myslam/common_include.h"  //将一些常用的头文件都放在这里面，避免每次书写都用很长的include

namespace myslam    //用namesapce myslam 将类定义包裹起来
{

// Pinhole RGBD camera model
class Camera
{
public:
    typedef std::shared_ptr<Camera> Ptr; //把智能指针定义为camera类型指针，传递函数使用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  vetor3d 三维向量
    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.cpp

#include "myslam/camera.h"

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; //相机坐标系的点转到世界坐标系，T_c_w.inverse()表示逆矩阵
}

Vector2d Camera::camera2pixel ( const Vector3d& p_c )//相机空间点转化成像素点
{
    return Vector2d (
        fx_ * p_c ( 0