视觉里程计（Visual Odometry, VO），通过使用相机提供的连续帧图像信息（以及局部地图，先不考虑）来估计相邻帧的相机运动，将这些相对运行转换为以第一帧为参考的位姿信息，就得到了相机载体（假设统一的刚体）的里程信息。先上一张本文主要内容的框图：
初始化实例
在实例化跟踪器的时候会实例化一个初始化实例，有一些比较重要的参数需要注意下，看代码注释以及初始值，参数值也可以在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)矩阵
假设\(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，描述的是两个平面之间的映射关系。
上图表示场景中的平面\(π\)在两相机的成像，设平面\(π\)在第一个相机坐标系下的单位法向量为\(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(三角化)
问题
- 评估H和F矩阵的好坏时是如何转为卡方检验的？
- triangulator::triangulate还没有彻底搞明白？