openVSLAM——实现与代码解析

文件结构

参考：开源SLAM框架学习——OpenVSLAM源码解析： 第一节 安装和初探

OpenVSLAM实现

源码：OpenVSLAM

配置：社区

参考：教程

• Eigen ：3.3.0及之后的版本
• g2o ：安装最新版本.
• SuiteSparse ：系数矩阵库，安装g2o需要
• FBoW：特征点字典，使用指定版本
• yaml-cpp：0.6.0 及之后的版本
• OpenCV ： 3.3.1及之后的版本
• Pangolin： 可视化工具，安装最新版本

输入：视频/图片、配置文件

配置文件.yaml

#==============#
# Camera Model #
#==============#

Camera.name: "EuRoC monocular"
#相机设置：单目monocular、双目stereo、RGBD
Camera.setup: "monocular"
#成像模型：透视perspective、鱼眼fisheye、全景equirectangular
Camera.model: "perspective"

# 相机内参
Camera.fx: 458.654
Camera.fy: 457.296
Camera.cx: 367.215
Camera.cy: 248.375

# 畸变参数：透视成像为5个畸变系数；鱼眼相机为k1 k2 k3 k4四个畸变系数；全景没有设置畸变系数
Camera.k1: -0.28340811
Camera.k2: 0.07395907
Camera.p1: 0.00019359
Camera.p2: 1.76187114e-05
Camera.k3: 0.0

# 视频帧率
Camera.fps: 20.0
# 图像宽高
Camera.cols: 752
Camera.rows: 480

# 颜色模式Gray, RGB, BGR
Camera.color_order: "Gray"

#================#
# ORB Parameters #
#================#
#一帧图像所提取的最多关键点数
Feature.max_num_keypoints: 1000
#金字塔层数和尺度因子
Feature.scale_factor: 1.2
Feature.num_levels: 8
#FAST角点检测的灰度阈值
Feature.ini_fast_threshold: 20
#如果上面的阈值没有检测到角点，采用更低的阈值
Feature.min_fast_threshold: 7

...

运行结果（图书馆）

OpenVSLAM模块

跟踪模块

通过关键点匹配和位姿优化，对连续输入到OpenVSLAM的每一帧估计其摄像机的位姿。这个模块还决定是否插入一个新的关键帧(KF)。当一个帧被认为是一个新的关键帧KF时，它被发送给映射和全局优化模块。在跟踪模块中，基本的数据处理包括初始化、帧跟踪、重定位、位姿优化以及插帧处理。

1.初始化

特征点提取与匹配：orb

正则化特征点：为了后续使用RANSAC（随机从大样本中抽取小样本用于求解问题）计算SVD分解等到更一致的结果，消除特征点在图像中位置分布对结果的影响。

//src/openvslam/solve/common.cc
void normalize(const std::vector<cv::KeyPoint>& keypts, std::vector<cv::Point2f>& normalized_pts, Mat33_t& transform) {
    float mean_x = 0;//平均值
    float mean_y = 0;
    const auto num_keypts = keypts.size();//统计数量
    normalized_pts.resize(num_keypts);//将点向量设置为同样大小
    //计算平均值
    for (const auto& keypt : keypts) {
        mean_x += keypt.pt.x;
        mean_y += keypt.pt.y;
    }
    mean_x = mean_x / num_keypts;
    mean_y = mean_y / num_keypts;
    
    float mean_l1_dev_x = 0;
    float mean_l1_dev_y = 0;
    for (unsigned int index = 0; index < num_keypts; ++index) {
        normalized_pts.at(index).x = keypts.at(index).pt.x - mean_x;//xi-x
        normalized_pts.at(index).y = keypts.at(index).pt.y - mean_y;//yi-y

        mean_l1_dev_x += std::abs(normalized_pts.at(index).x);//取绝对值
        mean_l1_dev_y += std::abs(normalized_pts.at(index).y);
    }

    mean_l1_dev_x = mean_l1_dev_x / num_keypts;//平均偏差σx
    mean_l1_dev_y = mean_l1_dev_y / num_keypts;//平均偏差σy

    //取逆
    const float mean_l1_dev_x_inv = static_cast<float>(1.0) / mean_l1_dev_x;
    const float mean_l1_dev_y_inv = static_cast<float>(1.0) / mean_l1_dev_y;

    for (auto& normalized_pt : normalized_pts) {
        normalized_pt.x *= mean_l1_dev_x_inv;
        normalized_pt.y *= mean_l1_dev_y_inv;
    }

    //正则化矩阵
    transform = Mat33_t::Identity();
    transform(0, 0) = mean_l1_dev_x_inv;
    transform(1, 1) = mean_l1_dev_y_inv;
    transform(0, 2) = -mean_x * mean_l1_dev_x_inv;
    transform(1, 2) = -mean_y * mean_l1_dev_y_inv;
}

2D-2D对极约束：对极约束描述了图像中特征点位置与帧间相对运动之间的关系。

详情见2D-2D：对极约束

计算本质矩阵E、基础矩阵F，估计两帧图像之间的变换矩阵T

基础矩阵的求解：八点法

当t为0时，相机只做了旋转，对于任意的R，E都为0，因此单目初始化必须要有平移，不能只旋转。

单应矩阵H

原理见SLAM入门之视觉里程计(5)：单应矩阵

针对t为0的情况，可使用单应矩阵求解相机运动。

单应矩阵的求解：直接线性变换DLT

程序中同时求解单应矩阵和基础矩阵，评价两者好坏，从F或H恢复相机运动R，t

//src/openvslam/initialize/perspective.cc
bool perspective::initialize(const data::frame& cur_frm, const std::vector<int>& ref_matches_with_cur) 
{
    ......

    // 计算基础矩阵F和单应矩阵H
    auto homography_solver = solve::homography_solver(ref_undist_keypts_, cur_undist_keypts_, ref_cur_matches_, 1.0);
    auto fundamental_solver = solve::fundamental_solver(ref_undist_keypts_, cur_undist_keypts_, ref_cur_matches_, 1.0);
    std::thread thread_for_H(&solve::homography_solver::find_via_ransac, &homography_solver, num_ransac_iters_, true);
    std::thread thread_for_F(&solve::fundamental_solver::find_via_ransac, &fundamental_solver, num_ransac_iters_, true);
    thread_for_H.join();
    thread_for_F.join();

    // 计算两个矩阵的得分
    const auto score_H = homography_solver.get_best_score();
    const auto score_F = fundamental_solver.get_best_score();
    const float rel_score_H = score_H / (score_H + score_F);

    // 如果rel_score_H>0.4，选着用H矩阵求解RT
    if (0.40 < rel_score_H && homography_solver.solution_is_valid()) {
        const Mat33_t H_ref_to_cur = homography_solver.get_best_H_21();
        const auto is_inlier_match = homography_solver.get_inlier_matches();
        return reconstruct_with_H(H_ref_to_cur, is_inlier_match);
    }
    // 如果rel_score_H<0.4，选着用F矩阵求解RT
    else if (fundamental_solver.solution_is_valid()) {
        const Mat33_t F_ref_to_cur = fundamental_solver.get_best_F_21();
        const auto is_inlier_match = fundamental_solver.get_inlier_matches();
        return reconstruct_with_F(F_ref_to_cur, is_inlier_match);
    }
    else {
        return false;
    }
}

从H或F中恢复R,T后，会得到若干组候选的R、T，需要计算获取最佳的。（评估R，t的同时对特征点进行三角化）

下图为从本质矩阵中分解R、T的四种情况，红色点为投影点，O为相机光心，蓝色为相机成像面，只有第一种情况是正确的R、T。

// src/openvslam/initialize/base.cc
bool base::find_most_plausible_pose(const eigen_alloc_vector<Mat33_t>& init_rots, const eigen_alloc_vector<Vec3_t>& init_transes,const std::vector<bool>& is_inlier_match, const bool depth_is_positive)
{
    //assert作用：如果()内表达式为0（假），它先向stderr打印一条出错信息，然后通过调用 abort 来终止当前进程。
    assert(init_rots.size() == init_transes.size());
    const auto num_hypothesis = init_rots.size();//存储每组候选位姿
    // 三角化点
    std::vector<eigen_alloc_vector<Vec3_t>> init_triangulated_pts(num_hypothesis);
    // 设置每个3D点的有效/无效标志
    std::vector<std::vector<bool>> init_is_triangulated(num_hypothesis);
    // 每个3D点的两个观测值之间的视差
    std::vector<float> init_parallax(num_hypothesis);
    // 有效3D点的数量
    std::vector<unsigned int> nums_valid_pts(num_hypothesis);
    //逐个候选位姿进行check_pose
    for (unsigned int i = 0; i < num_hypothesis; ++i) 
    {
        nums_valid_pts.at(i) = check_pose(init_rots.at(i), init_transes.at(i), is_inlier_match, depth_is_positive,
                                          init_triangulated_pts.at(i), init_is_triangulated.at(i), init_parallax.at(i));
    }

    rot_ref_to_cur_ = Mat33_t::Zero();
    trans_ref_to_cur_ = Vec3_t::Zero();

    // 选取有效点最多的那组位姿，进行如下几个判断：
    const auto max_num_valid_pts_iter = std::max_element(nums_valid_pts.begin(), nums_valid_pts.end());//max_element用来来查询最大值，使用max_element返回的值减去数组头地址即为该最大值在数组的序号
    const unsigned int max_num_valid_index = std::distance(nums_valid_pts.begin(), max_num_valid_pts_iter);//distance返回两个元素之间的距离，也即确定该组位姿的在数组中的位置

    // 1.有效3D点数量必须大于min_num_triangulated_（50），*max_num_valid_pts_iter为取该位置的值
    if (*max_num_valid_pts_iter < min_num_triangulated_) {
        return false;
    }

    // 2.一个好的结果应该是只有一组有比较多的有效点，如果发现有很多组都有个数相近的有效点，则这些位姿都不对
    const auto num_similars = std::count_if(nums_valid_pts.begin(), nums_valid_pts.end(),
                                            [max_num_valid_pts_iter](unsigned int num_valid_pts) {
                                                return 0.8 * (*max_num_valid_pts_iter) < num_valid_pts;
                                            });//count_if统计满足条件的数量：将前两个参数指定的区域的值逐个传入第三个参数函数中，判断是否满足条件，最后将统计的数目return。在这里，如果这组位姿的有效点*0.8小于某个值，则计数。
    if (1 < num_similars) {
        return false;
    }

    // 3.视差不能太小，因为太小的视差评估出的深度不可靠，这里的阈值parallax_deg_thr_=1度
    if (init_parallax.at(max_num_valid_index) < parallax_deg_thr_) {
        return false;
    }

    // 存储重建的地图，R,T,3D点
    rot_ref_to_cur_ = init_rots.at(max_num_valid_index);
    trans_ref_to_cur_ = init_transes.at(max_num_valid_index);
    triangulated_pts_ = init_triangulated_pts.at(max_num_valid_index);
    is_triangulated_ = init_is_triangulated.at(max_num_valid_index);

    return true;
}

三角化

详情见三角化求深度值（求三位坐标）

Vec3_t triangulator::triangulate(const cv::Point2d& pt_1, const cv::Point2d& pt_2, const Mat34_t& P_1, const Mat34_t& P_2) 
{
    //H矩阵
    Mat44_t A;
    //H矩阵每一行赋值
    A.row(0) = pt_1.x * P_1.row(2) - P_1.row(0);//u1p1(3)-p1(1)
    A.row(1) = pt_1.y * P_1.row(2) - P_1.row(1);//v1p1(3)-p1(2)
    A.row(2) = pt_2.x * P_2.row(2) - P_2.row(0);//u2p2(3)-p2(1)
    A.row(3) = pt_2.y * P_2.row(2) - P_2.row(1);//v2p2(3)-p2(2)
	//SVD求解，齐次坐标X为H的最小奇异值的奇异向量
    const Eigen::JacobiSVD<Mat44_t> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
	//因为SVD解可能不满足齐次坐标形式（第四个元素为0），所以需要进行归一化处理
    const Vec4_t v = svd.matrixV().col(3);
    return v.block<3, 1>(0, 0) / v(3);//归一化  block：从下标(0,0)开始，取3*1区域
}

如果三角化点数小于阈值min_num_triangulated_（50），则将当前帧与下一帧进行匹配，重新进行初始化

bool initializer::try_initialize_for_monocular(data::frame& curr_frm) {
    assert(state_ == initializer_state_t::Initializing);

    match::area matcher(0.9, true);
    const auto num_matches = matcher.match_in_consistent_area(init_frm_, curr_frm, prev_matched_coords_, init_matches_, 100);//统计初始帧（参考帧）和当前帧匹配对数,init_matches_用来存放匹配的关键点的描述子?

    //如果匹配对数小于min_num_triangulated_
    if (num_matches < min_num_triangulated_) {
        // 重置初始化，把下一帧当作初始化的参考帧
        reset();
        return false;
    }

    // 尝试用当前帧进行初始化
    assert(initializer_);
    return initializer_->initialize(curr_frm, init_matches_);
}

2.创建地图

// src/openvslam/module/initializer
bool initializer::create_map_for_monocular(data::frame& curr_frm) 
{
    assert(state_ == initializer_state_t::Initializing);

    eigen_alloc_vector<Vec3_t> init_triangulated_pts;
    {
        assert(initializer_);//初始化
        init_triangulated_pts = initializer_->get_triangulated_pts();//获取三角化点
        const auto is_triangulated = initializer_->get_triangulated_flags();//get_triangulated_flags()为bool向量，判断是否三角化了

        //将没有进行三角化的匹配点标记为无效
        for (unsigned int i = 0; i < init_matches_.size(); ++i) {
            if (init_matches_.at(i) < 0) {
                continue;//跳过循环体的剩余语句并执行下一次循环
            }
            if (is_triangulated.at(i)) {
                continue;
            }
            init_matches_.at(i) = -1;
        }

        // 以初始帧为原点，设置当前帧的位姿
        init_frm_.set_cam_pose(Mat44_t::Identity());
        Mat44_t cam_pose_cw = Mat44_t::Identity();
        cam_pose_cw.block<3, 3>(0, 0) = initializer_->get_rotation_ref_to_cur();//旋转矩阵
        cam_pose_cw.block<3, 1>(0, 3) = initializer_->get_translation_ref_to_cur();//平移
        curr_frm.set_cam_pose(cam_pose_cw);

        // 破坏初始化，将其设置为空指针
        initializer_.reset(nullptr);
    }

    // 为当前帧和初始帧创建关键帧（在这里是将这两帧设置为关键帧？）
    auto init_keyfrm = new data::keyframe(init_frm_, map_db_, bow_db_);
    auto curr_keyfrm = new data::keyframe(curr_frm, map_db_, bow_db_);

    // 计算关键帧对应的bow词袋向量
    init_keyfrm->compute_bow();
    curr_keyfrm->compute_bow();

    //将关键帧添加入map DB中   
    map_db_->add_keyframe(init_keyfrm);
    map_db_->add_keyframe(curr_keyfrm);

    //  更新帧统计信息
    init_frm_.ref_keyfrm_ = init_keyfrm;
    curr_frm.ref_keyfrm_ = curr_keyfrm;
    map_db_->update_frame_statistics(init_frm_, false);
    map_db_->update_frame_statistics(curr_frm, false);

    // 对于当前帧和初始帧的匹配点，进行如下操作：
    for (unsigned int init_idx = 0; init_idx < init_matches_.size(); init_idx++) {
        const auto curr_idx = init_matches_.at(init_idx);
        if (curr_idx < 0) {
            continue;
        }

        // 创建landmark（也即特征点所对应的世界坐标系下的点）
        auto lm = new data::landmark(init_triangulated_pts.at(init_idx), curr_keyfrm, map_db_);

        //  将lm与关键帧关联起来，向lm中添加信息：关键帧和对应的特征点id
        init_keyfrm->add_landmark(lm, init_idx);
        curr_keyfrm->add_landmark(lm, curr_idx);
        lm->add_observation(init_keyfrm, init_idx);
        lm->add_observation(curr_keyfrm, curr_idx);

        // 更新lm的描述子和几何信息
        lm->compute_descriptor();
        lm->update_normal_and_depth();

        // 设置当前帧的2D-3D关联信息
        curr_frm.landmarks_.at(curr_idx) = lm;
        curr_frm.outlier_flags_.at(curr_idx) = false;

        // 将lm加入map DB中
        map_db_->add_landmark(lm);
    }

    // BA优化
    const auto global_bundle_adjuster = optimize::global_bundle_adjuster(map_db_, num_ba_iters_, true);
    global_bundle_adjuster.optimize();

    // 计算尺度因子scale：将关键帧中lm（世界坐标系下）转为关键帧坐标系下，对深度排序取中值，将深度中间值设置为1，sclae=1.0 / median_depth
    const auto median_depth = init_keyfrm->compute_median_depth(init_keyfrm->camera_->model_type_ == camera::model_type_t::Equirectangular);
    const auto inv_median_depth = 1.0 / median_depth;
    if (curr_keyfrm->get_num_tracked_landmarks(1) < min_num_triangulated_ && median_depth < 0) {
        spdlog::info("seems to be wrong initialization, resetting");
        state_ = initializer_state_t::Wrong;
        return false;
    }
    //更改地图尺度：将当前关键帧的t和lm乘以尺度因子
    scale_map(init_keyfrm, curr_keyfrm, inv_median_depth * scaling_factor_);

    // 更新当前帧的位姿（lm和t变化了）
    curr_frm.set_cam_pose(curr_keyfrm->get_cam_pose());

    // 设置地图的起始关键帧
    map_db_->origin_keyfrm_ = init_keyfrm;

    spdlog::info("new map created with {} points: frame {} - frame {}", map_db_->get_num_landmarks(), init_frm_.id_, curr_frm.id_);
    state_ = initializer_state_t::Succeeded;
    return true;
}

补充：

lm描述子：lm被多帧观测到，获取lm对应特征点，获取所有特征点的描述子的汉明距离，将汉明距离最接近中值的特征点描述子作为lm描述子。

lm几何信息：平均观测方向和深度范围。lm在不同帧中的深度和观测方向（lm点到相机中心点）

• 待填充中…

参考：hardjet’s blog