视觉SLAM十四讲 第九 实践章 设计前端

Posted on | In |

第九 实践章 设计前端

目标:

  • 设计一个视觉里程计的前端

  • 理解SLAM软件框架是如何搭建

  • 理解在前端设计中容易出现问题的地方，以及修补方式。

1、搭建VO框架

单目视觉相对复杂，RGBD最简单，没有初始化，也没有尺度问题

1
2
3
4
5
6
7
8
-bin    可执行文件
-include/myslam    头文件.h
-src    源代码cpp
-test   测试用文件cpp
-lib
-config
-cmake_modules 三方库的cmake文件
-CmakeList.txt

1.1、数据结构：

帧

路标

配置文件

坐标转换

1
2
3
4
5
6
stateDiagram

Frame --> Camera
Frame --> MapPoint : has many
Map --> MapPoint: has many
Config

1.2、Camera类

  • 存储相机内参和外参

  • 相机坐标系，像素坐标系，世界坐标系的转换

  • 世界坐标系（需要一个相机的内参）

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
#include "myslam/common_include.h"

namespace myslam
{

// Pinhole RGBD 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

  • 智能指针，Camera指针类型，传递参数时候，只需用Camera::Ptr类型即可

  • SE3表达相机的位姿

camera.cpp

  • 相关的原理与第五章讲解的内容一致，坐标转换。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
#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;
}

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_
    );
}

Vector3d 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 );
}


}

1.3、Frame类

1.4 MapPoint类

1.5 Map类

1.6 Config类

2、基本VO操作，特征提取，匹配

3、改进：优化PNP的结果

4、改进：局部地图

5、小结