【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)系统变量

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

​ PROJECT_SOURCE_DIR:工程所在目录

​ 2)SET指令

​ 显式定义变量

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

​ 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")

​ 若使用FIND_PACKAGE:

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

​ 7)TARGET_LINK_LIBRARIES指令

​ 添加需要链接的共享库

​ 第一个参数:源文件

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

target_link_libraries(myslam ${THIRD_PARTY_LIBS})

​ 8)ADD_LIBRARY指令

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

​ 第一个参数:库的名字

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

add_library(myslam 
        frame.cpp
        mappoint.cpp
		)

​ 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)
(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 <iostream>
#include <new>
#include <memory>

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;
}
#include <iostream>
#include <new>
#include <memory>

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

int main()
{
    auto y = fun();
}
weak_ptr

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

循环引用

#include <iostream>
#include <new>
#include <memory>

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)基于 shared_ptr 构造

#include <iostream>
#include <new>
#include <memory>

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
}

(2)lock方法

#include <iostream>
#include <new>
#include <memory>

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";
    }
}
unordered_map

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

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

#include <iostream>
#include <map>

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<const char, int>
        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;
    }
}

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

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

#include <iostream>
#include <map>

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;
}
(2)多线程及线程锁
1.多线程

(1)构造方式

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

#include <iostream>
#include  <thread>
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;
}

(2)join()方法

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

#include <iostream>
#include  <thread>
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;
}
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[ \begin{matrix} f^i_u & 0 & c^i_u & -f^i_ub^i_x \\ 0 & f^i_v & c^i_v & 0 \\ 0 & 0 & 1 & 0 \end{matrix} \right] \tag{3} Precti= fui000fvi0cuicvi1fuibxi00 (3)
​ 第四列数据 − f u i b x i -f^i_ub^i_x fuibxi 中的 b x i b^i_x bxi 表示此相机相对与车辆坐标系原点的距离,只有平移没有旋转。

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

(2)相机模型

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

1 相机内参数

​ 由KITTI数据集给出

2 相机外参数

​ 对于现实世界的空间点P,我们需要将其转换到相机坐标系:
P c = T c w P w P_c = T_{cw}P_w Pc=TcwPw
Tips:世界坐标系不变

3 具体方法

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

​ 2)相机坐标系中的点 P c P_c Pc 转换到像素坐标系

4 Camera类的构造

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

​ 通过数据集对相机位姿初始化:由数据集可以得到,Cam0 为世界坐标系的原点, b x i b_x^i bxi 为 第 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) {}
	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
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++;
}
3 工厂模式
Frame::Ptr Frame::CreateFrame() {
    static long factory_id = 0;
    Frame::Ptr new_frame(new Frame);
    new_frame->id_ = factory_id++;
    return new_frame;
}
(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为右图
    Feature(std::shared_ptr<Frame> frame, const cv::KeyPoint &kp)
        : frame_(frame), position_(kp) {}
(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_;
MapPoint::MapPoint(long id, Vec3 position) : id_(id), pos_(position) {}
2 创建路标点

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

MapPoint::Ptr MapPoint::CreateNewMappoint() {
    static long factory_id = 0;
    MapPoint::Ptr new_mappoint(new MapPoint);
    new_mappoint->id_ = factory_id++;
    return new_mappoint;
}
    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;
    };
3 观察点
	void AddObservation(std::shared_ptr<Feature> feature) {
        std::unique_lock<std::mutex> lck(data_mutex_);
        observations_.push_back(feature);
        observed_times_++;
    }
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;
        }
    }
}
(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;
    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;  // 激活的关键帧数量
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_;
    }
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";
}

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

评论 4
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值