第三篇 视觉里程计(VO)的初始化过程以及openvslam中的相关实现详解

视觉里程计(Visual Odometry, VO),通过使用相机提供的连续帧图像信息(以及局部地图,先不考虑)来估计相邻帧的相机运动,将这些相对运行转换为以第一帧为参考的位姿信息,就得到了相机载体(假设统一的刚体)的里程信息。先上一张本文主要内容的框图:
第三篇 视觉里程计(VO)的初始化过程以及openvslam中的相关实现详解_第1张图片

初始化实例

在实例化跟踪器的时候会实例化一个初始化实例,有一些比较重要的参数需要注意下,看代码注释以及初始值,参数值也可以在yaml文件中自定义。

// src/openvslam/module/initializer.h:83
//! max number of iterations of RANSAC (only for monocular initializer)
const unsigned int num_ransac_iters_;
//! min number of triangulated pts
const unsigned int min_num_triangulated_;
//! min parallax (only for monocular initializer)
const float parallax_deg_thr_;
//! reprojection error threshold (only for monocular initializer)
const float reproj_err_thr_;
//! max number of iterations of BA (only for monocular initializer)
const unsigned int num_ba_iters_;
//! initial scaling factor (only for monocular initializer)
const float scaling_factor_;


// src/openvslam/module/initializer.cc:16
initializer::initializer(const camera::setup_type_t setup_type,
                         data::map_database* map_db, data::bow_database* bow_db,
                         const YAML::Node& yaml_node)
        : setup_type_(setup_type), map_db_(map_db), bow_db_(bow_db),
          num_ransac_iters_(yaml_node["Initializer.num_ransac_iterations"].as(100)),
          min_num_triangulated_(yaml_node["Initializer.num_min_triangulated_pts"].as(50)),
          parallax_deg_thr_(yaml_node["Initializer.parallax_deg_threshold"].as(1.0)),
          reproj_err_thr_(yaml_node["Initializer.reprojection_error_threshold"].as(4.0)),
          num_ba_iters_(yaml_node["Initializer.num_ba_iterations"].as(20)),
          scaling_factor_(yaml_node["Initializer.scaling_factor"].as(1.0)) {
    spdlog::debug("CONSTRUCT: module::initializer");
}

使用不同的相机类型,初始化的方法也不相同,openvslam使用了perspective和bearing_vector两种初始化方法。

// src/openvslam/module/initializer.cc:91
void initializer::create_initializer(data::frame& curr_frm) {
    // 将当前帧设置为初始化过程中的参考帧
    init_frm_ = data::frame(curr_frm);

    // 将当前帧的特征点当作previously matched coordinates
    prev_matched_coords_.resize(init_frm_.undist_keypts_.size());
    for (unsigned int i = 0; i < init_frm_.undist_keypts_.size(); ++i) {
        prev_matched_coords_.at(i) = init_frm_.undist_keypts_.at(i).pt;
    }

    // initialize matchings (init_idx -> curr_idx)
    std::fill(init_matches_.begin(), init_matches_.end(), -1);

    // build a initializer
    initializer_.reset(nullptr);
    switch (init_frm_.camera_->model_type_) {
        case camera::model_type_t::Perspective:
        case camera::model_type_t::Fisheye: {
            initializer_ = std::unique_ptr(new initialize::perspective(init_frm_,
                                                                                                num_ransac_iters_, min_num_triangulated_,
                                                                                                parallax_deg_thr_, reproj_err_thr_));
            break;
        }
        case camera::model_type_t::Equirectangular: {
            initializer_ = std::unique_ptr(new initialize::bearing_vector(init_frm_,
                                                                                                      num_ransac_iters_,            min_num_triangulated_,
                                                                                                      parallax_deg_thr_, reproj_err_thr_));
            break;
        }
    }
    // 设置状态为初始化状态
    state_ = initializer_state_t::Initializing;
}

执行完上面的函数后回直接直接退出,然后读取下一帧图像,进入初始化阶段。下面的匹配过程,在上一篇已经详细讲过了,有一点需要注意在匹配完成后,会将匹配到的点的prev_matched_coords_改为当前帧的特征点座标。如果匹配到的点数小于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);

    if (num_matches < min_num_triangulated_) {
        // rebuild the initializer with the next frame
        reset();
        return false;
    }

    // try to initialize with the current frame
    assert(initializer_);
    return initializer_->initialize(curr_frm, init_matches_);
}

初始化过程

通过计算单应矩阵和基础矩阵,评估出两帧之间的变换矩阵。首先搞清楚几个基本概念。

基础(Fundamental)矩阵

第三篇 视觉里程计(VO)的初始化过程以及openvslam中的相关实现详解_第2张图片

假设\(I1\)\(I2\)为相邻帧,\(p1\)\(p2\)为相邻帧匹配到的特征点,P为特征点的空间位置。対极约束描述了图像中特征点位置与帧间运动信息之间的关系。用过几何计算我们可以得到如下公式(对极约束)。具体推导可以参考两视图对极约束-基础矩阵:

\[p_2^TK^{-T}t^{\wedge }RK^{-1}p_1=0\]

定义本质矩阵\(E=t^{\wedge }R\),基础矩阵\(F=K^{-T}EK\)。通过匹配点对儿的像素位置可以计算出\(E或F\),进而计算出\(R,t\)
此外,假设相邻相机只做了旋转运动,即t为0,可以看到対极约束对于任意R都成立,因此想要通过对极约束评估相机运动内在要求不能只是纯旋转运动(可以使用单应矩阵来求解)。

单应Homography

单应(Homography)是射影几何中的概念,又称为射影变换。它把一个射影平面上的点(三维齐次矢量)映射到另一个射影平面上,并且把直线映射为直线,具有保线性质。总的来说,单应是关于三维齐次矢量的一种线性变换,可以用一个3×3的非奇异矩阵\(H\)表示:

\[ \begin{pmatrix}u_1\\ v_1\\ 1\end{pmatrix}= \begin{pmatrix}h_{11} & h_{12} & h_{13}\\h_{21} & h_{22} & h_{23}\\ h_{31} & h_{32} & h_{33}\end{pmatrix} \begin{pmatrix}u_2\\ v_2\\ 1\end{pmatrix} \]
其中,\((u1,v1,1)^T\)表示图像1中的像点,\((u2,v2,1)^T\)是图像2中的像点,也就是可以通过单应矩阵H将图像2变换到图像1,描述的是两个平面之间的映射关系。

第三篇 视觉里程计(VO)的初始化过程以及openvslam中的相关实现详解_第3张图片

上图表示场景中的平面\(π\)在两相机的成像,设平面\(π\)在第一个相机坐标系下的单位法向量为\(n^T\),其到第一个相机中心(坐标原点)的距离为\(d\),则平面\(π\)可表示为:

\[n^TX_1=d\]
其中,\(X_1\)是三维点P在第一相机坐标系下的坐标,其在第二个相机坐标系下的坐标为\(X_2\),则

\[X_2 = RX_1 + t \\ X_2 = RX_1 + t\frac{1}{d}n^TX_1=(R+t\frac{1}{d}n^T)X_1=HX_1 \\ H = R+t\frac{1}{d}n^T\]
假设\(p_1,p_2\)\(X_1,X_2\)对应的图像上的点,则

\[X_1=K^{-1}p_1, X_2=K^{-2}p_1\\ H = K(R+t\frac{1}{d}n^T)K^{-1}\]
这样求解出\(H\)便可以求解出\(R,t\),并却\(t=0\)也不影响求解R。

实践

在实际中,通常会同时计算H和F。

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

    // compute H and F matrices
    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();
    ...
}
初始化
    使用两个线程分别计算H和F矩阵
        homography_solver::find_via_ransac
        fundamental_solver::find_via_ransac
    通过判断0.4

看完下面的文章再回过头来看这段话。总的来说初始化过程涉及到多视几何的基础内容很多,openvlsam的实现很大程度上都是借鉴ORB2的实现方法(三角化除外)。可以看到由于要求前后帧(可以中间隔若干帧)特征点有一定的视差,想要成功的初始化就一定要发生位移。由于同时评估H和F矩阵,纯旋转也有初始化成功的可能性,但是需要有比较多的特征点在相同的平面上。不过最好还是就有旋转又有平移吧。这里在评估过程中都是采用归一化平面上的特征点(没有深度信息),所以得到的t是不知道其物理尺度的,然后三角化又是基于R,t求解的,因此特征点的空间位置值,也是无法知道其物理尺度的,可以认为尺度和t一致。

homography_solver::find_via_ransac

首先正则化特征点,目的是为了后面使用RANSAC(就是随即从大样本集抽取小样本集用于求解问题的方法)计算SVD时得到更一致的解决,消除特征点在图像中位置分布对结果的影响。更详细的解释可参考《Multiple View Geometry in Computer Vision 》P104 “4.4 Transformation invariance and normalization”。
openvslam采样的方法略有不同,分析如下。

\\src/openvslam/solve/common.cc:6
void normalize(const std::vector& keypts, std::vector& normalized_pts, Mat33_t& transform) {
计算单应矩阵
    正则化特征点
    循环num_ransac_iters_=100次
        生成8个点的RANSAC序列
        计算正则化特征点的单应矩阵compute_H_21
        计算特征点的单应矩阵
        评估当前求解出单应矩阵的好坏,更新最好的结果
    将筛选过的好的点重新评估H矩阵,得到最优结果

正则化特征点

正则化的公式如下,还是比较直观的。

\[x_i^{\prime} = {x_i - \bar{x} \over \sigma_x},y_i^{\prime} = {y_i - \bar{y} \over \sigma_y} \\ \bar{x} = {\Sigma{x_i} \over N},\bar{y} = {\Sigma{y_i} \over N} \\ \sigma_x = {\Sigma{\left|x_i - \bar{x}\right|} \over N}, \sigma_y = {\Sigma{\left|y_i - \bar{y}\right|} \over N} \\ \begin{pmatrix}x_i^{\prime} \\ y_i^{\prime} \\ 1\end{pmatrix}= \begin{pmatrix}1/\sigma_x & 0 & -\frac{\bar{x}}{\sigma_x} \\ 0 & 1/\sigma_y & -\frac{\bar{y}}{\sigma_y} \\ 0 & 0 & 1\end{pmatrix} \begin{pmatrix}x \\ y \\ 1\end{pmatrix}=T\begin{pmatrix}x \\ y \\ 1\end{pmatrix}\]
\(x_i^{\prime},y_i^{\prime}\)为正则化后的座标值,T就是正则化矩阵。

compute_H_21

参考https://www.uio.no/studier/emner/matnat/its/UNIK4690/v16/forelesninger/lecture_4_3-estimating-homographies-from-feature-correspondences.pdf

计算特征点的单应矩阵

程序中normalized_H_21表示则正则化的参考帧到当前帧的单应矩阵。
H_21_in_sac: 帧1->帧2的homography, 注意21表示的是1->2。

\[p^{\prime}_{cur} = T_{cur}p_{cur} \\ p^{\prime}_{ref} = T_{ref}p_{ref} \\ \]
\(T_{cur},T_{ref}\)是当前帧特征点和参考帧特征点的正则化矩阵。

\[p^{\prime}_{cur}=H^{\prime}_{cr}p^{\prime}_{ref}\]

\(H^{\prime}_{rc}\)是正则化当前帧特征点与正则化参考帧特征点的单应矩阵。展开得到:

\[ T_{cur}p_{cur}=H^{\prime}_{cr}T_{ref}p_{ref} \\ p_{cur}=T^{-1}_{cur}H^{\prime}_{cr}T_{ref}p_{ref} \]
因此当前帧特征点与参考帧特征点的单应矩阵\(H_{cr}\)为:

\[H_{cr}=T^{-1}_{cur}H^{\prime}_{cr}T_{ref}\]

评估当前求解出单应矩阵的好坏

说实话,研究了半天也没搞清楚是如何和卡方检验联系起来的。在我看来就是定义了一个最小重投影误差门限(5.991),重投影误差小于门限就标记为inlier, 将最好的结果保存下来。这里面作者只累加小于门限的重投影误差,即score += 门限值 - 重投影误差,这个差值是>0的,这样做的好处大家可以思考下。

fundamental_solver::find_via_ransac

同样是先正则化特征点,然后采用RANSAC的方法计算F矩阵compute_F_21,过程和计算H矩阵完全一致。

从F或H恢复R,t

//src/openvslam/initialize/perspective.cc:91
bool perspective::reconstruct_with_H(const Mat33_t& H_ref_to_cur, const std::vector& is_inlier_match)
//src/openvslam/initialize/perspective.cc:114
bool perspective::reconstruct_with_F(const Mat33_t& F_ref_to_cur, const std::vector& is_inlier_match)

reconstruct_with_H: Motion and structure from motion in a piecewise planar environment (Faugeras et al. in IJPRAI 1988)
reconstruct_with_F: https://en.wikipedia.org/wiki/Essential_matrix#Determining_R_and_t_from_E

在得到若干组候选的R,t后,需要计算找到最佳的R,t。

// src/openvslam/initialize/base.cc:31
bool base::find_most_plausible_pose
寻找最佳位姿
    逐个候选位姿进行check_pose
    选取有效点最多的那组位姿,做以下判断:
    1.有效点数必须大于min_num_triangulated_(50);
    2.一个好的结果应该是只有一组有比较多的有效点,,如果发现有很多组都有个数相近的有效点,则这些位姿都不对;
    3.视差不能太小,因为太小的视差评估出的深度不可靠,这里的门限parallax_deg_thr_=1度;

在评估R,t的同时会对特征点进行三角化,注意保存到triangulated_pts_的点是特征点在参考帧下的三维坐标。

check_pose

// src/openvslam/initialize/base.cc:86
unsigned int base::check_pose
循环所有的匹配点
    三角化计算得到特征点在参考帧下的三维坐标
    计算视差角的余弦值(向量内积的方法)
    排除视差小于0.5度和深度为负数的三维坐标
    验证特征点是否在参考帧和当前帧中可见,重投影误差L2 < 16
    以上条件都满足的点才认为有效
对视差排序后返回第50小或者(个数小于50则返回最小)的视差

Triangulate(三角化)

问题

  1. 评估H和F矩阵的好坏时是如何转为卡方检验的?
  2. triangulator::triangulate还没有彻底搞明白?

你可能感兴趣的:(第三篇 视觉里程计(VO)的初始化过程以及openvslam中的相关实现详解)