第三篇 视觉里程计(VO)的初始化过程以及openvslam中的相关实现详解
视觉里程计(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<unsigned int>(100)), min_num_triangulated_(yaml_node["Initializer.num_min_triangulated_pts"].as<unsigned int>(50)), parallax_deg_thr_(yaml_node["Initializer.parallax_deg_threshold"].as<float>(1.0)), reproj_err_thr_(yaml_node["Initializer.reprojection_error_threshold"].as<float>(4.0)), num_ba_iters_(yaml_node["Initializer.num_ba_iterations"].as<unsigned int>(20)), scaling_factor_(yaml_node["Initializer.scaling_factor"].as<float>(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<initialize::perspective>(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<initialize::bearing_vector>(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<int>& 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<rel_score_H = score_H / (score_H + score_F来决定用H还是F来评估R,t, 在评估的过程中还会通过三角化确定特征点的空间位置信息 通过一些列复杂的筛选得出到满足条件的R,t即算完成初始化
看完下面的文章再回过头来看这段话。总的来说初始化过程涉及到多视几何的基础内容很多,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<cv::KeyPoint>& keypts, std::vector<cv::Point2f>& 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<bool>& is_inlier_match) //src/openvslam/initialize/perspective.cc:114 bool perspective::reconstruct_with_F(const Mat33_t& F_ref_to_cur, const std::vector<bool>& 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则返回最小)的视差
问题
- 评估H和F矩阵的好坏时是如何转为卡方检验的?
- triangulator::triangulate还没有彻底搞明白?
- OpenGL学习笔记(二):OpenGL语法、渲染管线以及具体实现过程详解
- CentOS6安装CutyCapt,实现网页截图过程以及相关问题
- Linux下安装MyEclipse和Tomcat服务器详解,以及我安装过程中所出现的问题以及解决办法,并实现一个web小程序
- 详解softmax函数以及相关求导过程
- Spring学习06--IOC实现原理以及IOC容器初始化过程
- hadoop作业初始化过程详解(源码分析第三篇) 推荐
- 多级树集合分裂(SPIHT)算法的过程详解和Matlab实现(2)数学表述
- 2440中UART的使用以及相关函数详解
- Java 初始化过程详解
- 关于vss如何连上服务器以及如何实现协同作业的相关设置
- windows程序的运行原理以及VC 的实现过程
- PostgreSQL启动过程中的那些事七:初始化共享内存和信号十九:shmem中初始化BTree相关结构
- 多级树集合分裂(SPIHT)算法的过程详解和Matlab实现(4)编码过程——排序扫描
- springframework(八)AOP的相关术语以及实现方式介绍
- 详解由VS 2010生成的Bug报告(1) - 报告的生成过程
- java程序初始化过程详解
- 用Axis2c 实现一个Web service 的小demo程序过程详解
- U-boot在开发板上移植过程详解(3)---U-boot实现源码分析(第二阶段)
- PostgreSQL启动过程中的那些事七:初始化共享内存和信号二十:shmem中初始化堆同步扫描、pg子进程相关结构
- LTE Initial Attach 的过程以及相关消息的具体解析 (一)