【Final Project】Kitti的双目视觉里程计（1）

1.从CMake文件了解整体结构

（1）前置工作

​ 0）文件结构

.
├── app
│   ├── CMakeLists.txt
│   └── run_kitti_stereo.cpp
├── CMakeLists.txt
├── cmake_modules
│   ├── FindCSparse.cmake
│   ├── FindG2O.cmake
│   └── FindGlog.cmake
├── config
│   └── default.yaml
├── include
│   └── myslam
│       ├── algorithm.h
│       ├── backend.h
│       ├── camera.h
│       ├── common_include.h
│       ├── config.h
│       ├── dataset.h
│       ├── feature.h
│       ├── frame.h
│       ├── frontend.h
│       ├── g2o_types.h
│       ├── map.h
│       ├── mappoint.h
│       ├── viewer.h
│       └── visual_odometry.h
├── src
│   ├── backend.cpp
│   ├── camera.cpp
│   ├── CMakeLists.txt
│   ├── config.cpp
│   ├── dataset.cpp
│   ├── feature.cpp
│   ├── frame.cpp
│   ├── frontend.cpp
│   ├── map.cpp
│   ├── mappoint.cpp
│   ├── viewer.cpp
│   └── visual_odometry.cpp
└── test
    ├── CMakeLists.txt
    └── test_triangulation.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

​ 1）系统变量

​ PROJECT_BINARY_DIR：编译发生的当前目录

​ PROJECT_SOURCE_DIR：工程所在目录

​ 2）SET指令

​ 显式定义变量

set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
1
2

​ 3）ADD_EXECUTABLE指令

​ 4）ADD_SUBDIRECTORY指令

​ 1 ADD_SUBDIRECTOR(src bin)：源文件夹src编译的文件将保存在/build/bin下

​ 2 ADD_SUBDIRECTOR(src)：源文件夹src编译的文件将保存在/build/src下

​ 5）LIST指令

​ 告诉cmake在哪儿去找的.cmake文件

​ 6）INCLUDE_DIRECTORIES指令

​ 寻找库的头文件

​ 指定路径：

include_directories("/usr/local/include/eigen3")
1

​ 若使用FIND_PACKAGE：

find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
1
2

​ 7）TARGET_LINK_LIBRARIES指令

​ 添加需要链接的共享库

​ 第一个参数：源文件

​ 第二个参数：所依赖的库

target_link_libraries(myslam ${THIRD_PARTY_LIBS})
1

​ 8）ADD_LIBRARY指令

​ 添加名为…的库，库的源文件可指定

​ 第一个参数：库的名字

​ 第二个参数：库的源文件

add_library(myslam 
        frame.cpp
        mappoint.cpp
		)
1
2
3
4

​ 9）ADD_TEST指令

​ 需要与ENABLE_TESTRING()配合使用

​ 10）FOREACH——ENDFOREACH

FOREACH (test_src ${TEST_SOURCES})
    ADD_EXECUTABLE(${test_src} ${test_src}.cpp)
    TARGET_LINK_LIBRARIES(${test_src} ${THIRD_PARTY_LIBS} myslam)
    ADD_TEST(${test_src} ${test_src})
ENDFOREACH (test_src)
1
2
3
4
5

（2）分析

​ 1）最外层CMakeLists.txt

​ 1 把所需要的库添加进来

​ 2 自己编写程序的头文件添加进来

​ 3 编译的中间位文件做好分组

​ 4 将添加进来的库打个包，为内层调用作准备

​ 2）/src/CMakeLists.txt

​ 1 src文件夹下存放源代码文件

​ 2 将源码文件与库链接起来

​ 3 这些文件本身也可以看成库

​ 3）/test/CMakeLists.txt

​ 测试文件

​ 4）app/CMakeLists.txt

​ 1 主程序

​ 2 调用编写的文件以及库文件

2.前置知识

（1）智能指针

shared_ptr

​ 基于引用计数的共享内存解决方案

1）基本用法

#include 
#include 
#include 

int main()
{
    //int * x = new int(3);    
    std::shared_ptr<int> x(new int(3));
    std::cout << x.use_count() << std::endl;    //引用计数
    std::shared_ptr<int> y = x;
    std::cout << y.use_count() << std::endl;
    std::cout << x.use_count() << std::endl;
}
1
2
3
4
5
6
7
8
9
10
11
12
13

#include 
#include 
#include 

std::shared_ptr<int> fun()
{
    std::shared_ptr<int> res(new int(100));
    return res;
}

int main()
{
    auto y = fun();
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14

weak_ptr

​ 防止循环引用而引入的智能指针

循环引用

#include 
#include 
#include 

struct Str
{
    std::shared_ptr<Str> m_nei;

    ~Str()
    {
        std::cout << "~Str() is called!\n";
    }
};

int main()
{
    std::shared_ptr<Str> x(new Str{});    //[x] = 1
    std::shared_ptr<Str> y(new Str{});    //[y] = 1
    x->m_nei = y;    //[y] = 2
    y->m_nei = x;    //[x] = 2
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

（1）基于 shared_ptr 构造

#include 
#include 
#include 

struct Str
{
    std::weak_ptr<Str> m_nei;

    ~Str()
    {
        std::cout << "~Str() is called!\n";
    }
};

int main()
{
    //[]表示引用计数
    std::shared_ptr<Str> x(new Str{});    //[x] = 1
    std::shared_ptr<Str> y(new Str{});    //[y] = 1
    x->m_nei = y;    //[y] = 1
    y->m_nei = x;    //[x] = 1
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

（2）lock方法

#include 
#include 
#include 

struct Str
{
    std::weak_ptr<Str> m_nei;

    ~Str()
    {
        std::cout << "~Str() is called!\n";
    }
};

int main()
{
    //[]表示引用计数
    std::shared_ptr<Str> x(new Str{});    //[x] = 1
    std::shared_ptr<Str> y(new Str{});    //[y] = 1
    x->m_nei = y;    //[y] = 1
    y->m_nei = x;    //[x] = 1

    if (auto ptr = x->m_nei.lock(); ptr)
    {
        std::cout << "Can access pointer\n";
    }
    else
    {
        std::cout << "Cannot access pointer\n";
    }
}
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

unordered_map

​ 与map类似，与 set / map 相比查找性能更好，但插入操作一些情况下会慢

1树中的每个结点是一个 std::pair

#include 
#include 

int main()
{
    std::map<char, int> m{{'a', 3}, {'b', 4}};
    for (auto ptr = m.begin();ptr != m.end(); ptr++)
    {
        auto p = *ptr;    //std::pair
        std::cout << p.first << ' ' << p.second << std::endl;
    }
    //基于range-based-for
    for (auto p : m)
    {
        std::cout << p.first << ' ' << p.second << std::endl;
    }
    //或者
    for (auto [k, v] : m)
    {
        std::cout << k << ' ' << v << std::endl;
    }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

2键 (pair.first) 需要支持使用 < 比较大小，或者采用自定义的比较函数来引入大小关系

3访问元素： find / contains / [] / at

#include 
#include 

int main()
{
    std::map<int, bool> m;
    m.insert(std::pair<const int, bool>(3, true));
    auto ptr = m.find(3);
    std::cout << ptr->first << '' << ptr->second;
    std::cout << m[3] << std::endl;
}
1
2
3
4
5
6
7
8
9
10
11

（2）多线程及线程锁

1.多线程

（1）构造方式

​ 假如直接执行run()函数，那么由于run函数内部是一个无限循环（A），则主函数中的无限循环（B）永远无法被执行；通过新开一个线程，A与 B同时执行了。

#include 
#include  
class A
{
public:
    void run()
    {
        while (true)
        {
            std::cout << "AAAA" << std::endl;
            std::this_thread::sleep_for(std::chrono::milliseconds(50));
        }

    }

};
int main()
{
    A aa;
    //aa.run();
    std::thread thr(&A::run, &aa);
    while (true)
    {
        std::cout << "BBBB" << std::endl;
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
    }
    return 0;
}
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

（2）join()方法

​ 阻塞当前线程直至 执行完毕再执行下面代码。

#include 
#include  
class A
{
public:
    void run()
    {
        while (true)
        {
            std::cout << "AAAA" << std::endl;
            std::this_thread::sleep_for(std::chrono::milliseconds(50));
        }

    }

};
int main()
{
    A * aa = new A;
    //aa.run();
    std::thread thr(&A::run, aa);
    thr.join();
    /*
    while (true)
    {
        std::cout << "BBBB" << std::endl;
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
    }
     */
    return 0;
}
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

2.互斥

（1）unique_lock

3.从相机数据开始

（1）KITTI Odometry数据集

传感器安装位置

​ Kitti传感器主要包含相机（Cam0-4）、GPS/IMU、激光雷达（Velodyne Laserscanner）。本课题中只用到了相机，所以只讨论相机。

​ 在calib.txt文件中P0到P4分别对应四个相机数据，其中，P0的原点为车辆坐标系原点，P0, P1, P2, P3 分别代表对应的相机内参矩阵, 大小为 3x4：
P r e c t i = [ f u i 0 c u i − f u i b x i 0 f v i c v i 0 0 0 1 0 ] (3) P^{i}_{rect}= \left[

\right] \tag{3} Precti​=⎣ ⎡​fui​00​0fvi​0​cui​cvi​1​−fui​bxi​00​⎦ ⎤​(3)
​ 第四列数据 $-f_u^i{b}_x^i$ 中的 $b_x^i$ 表示此相机相对与车辆坐标系原点的距离，只有平移没有旋转。

​ 通过 b ，将四个相机的相对关系描述出来。

（2）相机模型

​ 此部分为复习，复习课本知识并思考如何融入项目之中。

1 相机内参数

​ 由KITTI数据集给出

2 相机外参数

​ 对于现实世界的空间点P，我们需要将其转换到相机坐标系：
$$P_c=T_{cw}{P}_w$$
Tips：世界坐标系不变

3 具体方法

​ 1）世界坐标系中的点P转换到相机坐标系

​ 2）相机坐标系中的点 $P_c$ 转换到像素坐标系

4 Camera类的构造

​ 相机内参数以及双目相机基线长度由数据集给出，唯一一个不确定的参数便是相机的位姿。

​ 通过数据集对相机位姿初始化：由数据集可以得到，Cam0 为世界坐标系的原点，$b_x^i$ 为 第 i 个相机与Cam0的平移关系，旋转关系由于安装原因没有旋转。

4.基本数据结构

需要处理的数据：

​ 1 图像：双目视觉中的一对图像，可以称为一帧。

​ 2 提取特征

​ 3 通过特征计算3D位置，即路标。

（1）图像（帧）

1 构造函数

​ 通过构造函数，我认为能够迅速了解一个类。

​ 1 对于一系列的图像，可以为其编号

​ 2 该帧出现的时间

​ 3 关键帧

​ 4 相机的位姿

​ 5 读入的左右图像

Frame::Frame(long id, double time_stamp, const SE3 &pose, const Mat &left, const Mat &right)
        : id_(id), time_stamp_(time_stamp), pose_(pose), left_img_(left), right_img_(right) {}
1
2

	typedef std::shared_ptr<Frame> Ptr;

    unsigned long id_ = 0;           // id of this frame
    unsigned long keyframe_id_ = 0;  // id of key frame
    bool is_keyframe_ = false;       // 是否为关键帧
    double time_stamp_;              // 时间戳，暂不使用
    SE3 pose_;                       // Tcw 形式Pose
    std::mutex pose_mutex_;          // Pose数据锁
    cv::Mat left_img_, right_img_;   // stereo images
1
2
3
4
5
6
7
8
9

2 创建帧

​ 分配帧id以及关键帧id

Frame::Ptr Frame::CreateFrame() {
    static long factory_id = 0;
    Frame::Ptr new_frame(new Frame);
    new_frame->id_ = factory_id++;
    return new_frame;
}

void Frame::SetKeyFrame() {
    static long keyframe_factory_id = 0;
    is_keyframe_ = true;
    keyframe_id_ = keyframe_factory_id++;
}
1
2
3
4
5
6
7
8
9
10
11
12

3 工厂模式

Frame::Ptr Frame::CreateFrame() {
    static long factory_id = 0;
    Frame::Ptr new_frame(new Frame);
    new_frame->id_ = factory_id++;
    return new_frame;
}
1
2
3
4
5
6

（2）从帧中提取特征

​ 特征充当了一个中介，连接起3维世界中的点与图像中的点。

​ 那么最主要的信息便是从帧图像中提取到的2D位置，哪一个帧有此特征以及对应的3D路标也是需要考虑的。

    typedef std::shared_ptr<Feature> Ptr;

    std::weak_ptr<Frame> frame_;         // 持有该feature的frame
    cv::KeyPoint position_;              // 2D提取位置
    std::weak_ptr<MapPoint> map_point_;  // 关联地图点

    bool is_outlier_ = false;       // 是否为异常点
    bool is_on_left_image_ = true;  // 标识是否提在左图，false为右图
1
2
3
4
5
6
7
8

    Feature(std::shared_ptr<Frame> frame, const cv::KeyPoint &kp)
        : frame_(frame), position_(kp) {}
1
2

（3）路标点

​ 路标点最重要的信息是它的3D位置，其次是被哪些特征所观察。

1 构造函数

​ ID，3D位置

    typedef std::shared_ptr<MapPoint> Ptr;
    unsigned long id_ = 0;  // ID
    bool is_outlier_ = false;
    Vec3 pos_ = Vec3::Zero();  // Position in world
    std::mutex data_mutex_;
    int observed_times_ = 0;  // being observed by feature matching algo.
    std::list<std::weak_ptr<Feature>> observations_;
1
2
3
4
5
6
7

MapPoint::MapPoint(long id, Vec3 position) : id_(id), pos_(position) {}
1

2 创建路标点

​ 工厂模式创建路标点，可以看出与创建帧有相似性

MapPoint::Ptr MapPoint::CreateNewMappoint() {
    static long factory_id = 0;
    MapPoint::Ptr new_mappoint(new MapPoint);
    new_mappoint->id_ = factory_id++;
    return new_mappoint;
}
1
2
3
4
5
6

    Vec3 Pos() {
        std::unique_lock<std::mutex> lck(data_mutex_);
        return pos_;
    }

    void SetPos(const Vec3 &pos) {
        std::unique_lock<std::mutex> lck(data_mutex_);
        pos_ = pos;
    };
1
2
3
4
5
6
7
8
9

3 观察点

	void AddObservation(std::shared_ptr<Feature> feature) {
        std::unique_lock<std::mutex> lck(data_mutex_);
        observations_.push_back(feature);
        observed_times_++;
    }
1
2
3
4
5

void MapPoint::RemoveObservation(std::shared_ptr<Feature> feat) {
    std::unique_lock<std::mutex> lck(data_mutex_);
    for (auto iter = observations_.begin(); iter != observations_.end();
         iter++) {
        if (iter->lock() == feat) {
            observations_.erase(iter);      //erase() 删除
            feat->map_point_.reset();
            observed_times_--;
            break;
        }
    }
}
1
2
3
4
5
6
7
8
9
10
11
12

（4）地图类

​ 为了实际持有Frame和MapPoint对象，定义地图类。

1 成员变量

	typedef std::shared_ptr<Map> Ptr;
    typedef std::unordered_map<unsigned long, MapPoint::Ptr> LandmarksType;
    typedef std::unordered_map<unsigned long, Frame::Ptr> KeyframesType;
1
2
3

    std::mutex data_mutex_;

    LandmarksType landmarks_;         // all landmarks
    LandmarksType active_landmarks_;  // active landmarks
    KeyframesType keyframes_;         // all key-frames
    KeyframesType active_keyframes_;  // all key-frames

    Frame::Ptr current_frame_ = nullptr;

    // settings
    int num_active_keyframes_ = 7;  // 激活的关键帧数量
1
2
3
4
5
6
7
8
9
10
11

2 实现功能

    /// 增加一个关键帧
    void InsertKeyFrame(Frame::Ptr frame);
    /// 增加一个地图顶点
    void InsertMapPoint(MapPoint::Ptr map_point);

    /// 获取所有地图点
    LandmarksType GetAllMapPoints() {
        std::unique_lock<std::mutex> lck(data_mutex_);
        return landmarks_;
    }
    /// 获取所有关键帧
    KeyframesType GetAllKeyFrames() {
        std::unique_lock<std::mutex> lck(data_mutex_);
        return keyframes_;
    }

    /// 获取激活地图点
    LandmarksType GetActiveMapPoints() {
        std::unique_lock<std::mutex> lck(data_mutex_);
        return active_landmarks_;
    }

    /// 获取激活关键帧
    KeyframesType GetActiveKeyFrames() {
        std::unique_lock<std::mutex> lck(data_mutex_);
        return active_keyframes_;
    }
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

void Map::InsertKeyFrame(Frame::Ptr frame) {
    current_frame_ = frame;
    if (keyframes_.find(frame->keyframe_id_) == keyframes_.end()) {
        keyframes_.insert(make_pair(frame->keyframe_id_, frame));
        active_keyframes_.insert(make_pair(frame->keyframe_id_, frame));
    } else {
        keyframes_[frame->keyframe_id_] = frame;
        active_keyframes_[frame->keyframe_id_] = frame;
    }

    if (active_keyframes_.size() > num_active_keyframes_) {
        RemoveOldKeyframe();
    }
}
/*
	keyframes_.end():指向map容器最后一个的下一个
	keyframes_.find():通过键查找对象是否在容器里，若不存在，就指向end
	上述if语句判断传入帧的id是否在容器中，若在，就用新的把旧的替换掉；若不存在，就添加进去。
*/

void Map::InsertMapPoint(MapPoint::Ptr map_point) 	//与添加关键帧一样
{
    if (landmarks_.find(map_point->id_) == landmarks_.end()) {
        landmarks_.insert(make_pair(map_point->id_, map_point));
        active_landmarks_.insert(make_pair(map_point->id_, map_point));
    } else {
        landmarks_[map_point->id_] = map_point;
        active_landmarks_[map_point->id_] = map_point;
    }
}


void Map::RemoveOldKeyframe() {
    if (current_frame_ == nullptr) return;
    // 寻找与当前帧最近与最远的两个关键帧
    double max_dis = 0, min_dis = 9999;
    double max_kf_id = 0, min_kf_id = 0;
    auto Twc = current_frame_->Pose().inverse();
    for (auto& kf : active_keyframes_) {
        if (kf.second == current_frame_) continue;
        auto dis = (kf.second->Pose() * Twc).log().norm();
        if (dis > max_dis) {
            max_dis = dis;
            max_kf_id = kf.first;
        }
        if (dis < min_dis) {
            min_dis = dis;
            min_kf_id = kf.first;
        }
    }

    const double min_dis_th = 0.2;  // 最近阈值
    Frame::Ptr frame_to_remove = nullptr;
    if (min_dis < min_dis_th) {
        // 如果存在很近的帧，优先删掉最近的
        frame_to_remove = keyframes_.at(min_kf_id);
    } else {
        // 删掉最远的
        frame_to_remove = keyframes_.at(max_kf_id);
    }

    LOG(INFO) << "remove keyframe " << frame_to_remove->keyframe_id_;
    // remove keyframe and landmark observation
    active_keyframes_.erase(frame_to_remove->keyframe_id_);
    for (auto feat : frame_to_remove->features_left_) {
        auto mp = feat->map_point_.lock();
        if (mp) {
            mp->RemoveObservation(feat);
        }
    }
    for (auto feat : frame_to_remove->features_right_) {
        if (feat == nullptr) continue;
        auto mp = feat->map_point_.lock();
        if (mp) {
            mp->RemoveObservation(feat);
        }
    }

    CleanMap();
}

void Map::CleanMap() {
    int cnt_landmark_removed = 0;
    for (auto iter = active_landmarks_.begin();
         iter != active_landmarks_.end();) {
        if (iter->second->observed_times_ == 0) {
            iter = active_landmarks_.erase(iter);
            cnt_landmark_removed++;
        } else {
            ++iter;
        }
    }
    LOG(INFO) << "Removed " << cnt_landmark_removed << " active landmarks";
}
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
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94

感受：代码读下来只能说需要时间去理解，脑子里隐约有了个关于框架，基本的参数以及参数的功能有了一定的了解，但是对于一些辅助参数比如各种 ID，时间戳等在自己编写程序的时候能够思考加上，以及选用什么样的容器也是需要思考的地方。