回环检测与建图
一、基于检测框的重建(选题 5)
物体检测是计算机视觉中一个常见的任务。通常,检测网络会输出所有图片中存在 的物体,每个物体以一个 2D 外包框表示,每个框还带有物体标签、置信度等信息。这是一个传统的2D视觉问题,但是,通过SLAM的方式,如果我们在多个视角看到同一个物体的外包框,则可以推断该物体的 3D 位置。 请参照文献 [2] 和 [3](或自行搜索对应文献),
完成下列任务:
- 推导基于对偶二次曲面的重建算法,说明其工作原理;
- 使用 g2o 或 ceres 实现该算法,完成一个演示用例。检测框可使用任意检测网络完成。
1.1 对偶二次-基本概念
1)二元二次型
二次曲面是三维空间中由4×4对称矩阵Q表示的曲面。在对偶形式中,二次曲面由一组切向平面定义,这些平面围绕二次曲面形成一个包络线。这双二次曲面\(Q^∗\)定义以便所有平面π满足\(π^TQ^∗π= 0\)。二次曲面一般是球体、椭球体、双曲面、圆锥或圆柱体。
二次曲面有9个自由度。 它们对应于对称矩阵的十个独立元素 ,其中一个用于标度。我们可以用10个矢量\(q =(q_1,…,q_{10})\)代表一个通用的双二次曲面。
当一个对偶二次曲面被投影到一个成像平面上时,它会创建一个对偶二次曲线,遵循简单的规则\(C^*\) = P\(Q^*\) \(P^T\)。这里,\(P = K[R|t]\)是包含摄像机内外部参数的摄像机投影矩阵。二次曲线是二次曲面的二维对应物,并形成形状,如圆、椭圆、抛物线或双曲线。
- 约束二次曲面参数化
对偶二次曲面的一般形式既可以表示球面、椭球等封闭曲面,也可以表示抛物面、双曲面等非封闭曲面。由于只有前者是有意义的表示对象地标,我们使用一个约束的对偶二次表示,以确保所表示的表面是一个椭球或球体。
我们将对偶二次曲面参数化,
\(Q^∗ = Z \overset{˘}{Q^∗} Z^T\)
其中,\(\overset{˘}{Q^∗}\)是一个椭球中心在原点,Z是一个齐次变换,为一个任意的旋转和平移。 具体的 \(Z=\begin{bmatrix} R(\Theta ) & t\\ 0_3^T & 1 \end{bmatrix}\) 和 \(\overset{˘}{Q^∗} = \begin{bmatrix} s_1^2 & 0 & 0 &0 \\ 0 & s_2^2 & 0 & 0\\ 0 & 0 & s_3^2 &0 \\ 0 & 0 & 0 & -1 \end{bmatrix}\) 在\(t = (t_1,t_2,t_3)\)是二次曲面质心平移,\(R(θ)\)是一个旋转矩阵定义的角度\(θ=(θ_1,θ_2,θ_3)\),\(s = (s_1,s_2,s_3)\)是二次曲面的形状沿椭球的三个参数。
1.2 基于Apriltag 的相机位姿估计
图 1.1 . 基于检测框的相机位姿估计系统
该系统apriltag_AR的检测的API由 AprilTag 3 提供,当相机拍摄到不同的tag后会识别出对应的id,如图1.2所示:
图 1.2. tag识别
此处我用一个tag作为目标物体,由相机进行检测,由此估计相机的位姿和tag的检测点。然后由估计出相机的位姿pose,然后再将tag四个角的坐标和相机位姿进行位姿的BA优化,如图 1.3 所示。此处用一个立方体可视化相机的位姿估计效果。
图 1.3. 相机位姿估计效果
1.3 基于普通图片的相机位姿估计(待续)
将Apriltag的标签替换成图案 A,通过提取A的ORB特征点信息,然后由相机进行检测匹配,检测到相同的ORB特征信息后,用 cv::solvePnP
计算相机位姿。最后用光流法进行跟踪目标物体。
图案 A
1.4 环境重建(待续)
后续加入目标检测物和周围环境的特征点信息,进行环境的三维重建。
1.5 代码
apriltag_AR: https://github.com/suljaxm/apriltag_AR
二、Kitti 的双目视觉里程(选题1)
KITTI 的 Odometry 提供了左右双目的视觉图像(以及激光,但本作业不使用激光数据),并提供了标定信息。它一共含有若干个 Sequence,其中一部分 Sequence 的真实轨迹是开放的,另一部分则是隐藏的,作为测试使用。在 KITTI 官网可以上传你对测试部分的轨迹估计,系统会计算与真实轨迹的差异,并给出评分。 现在我们已经介绍了所有关于视觉 SLAM 方面的内容,你可以基于已有算法,实现 一个双目的视觉里程计了。Kitti 官网 odometry 分类下提供了双目相机的所有信息。请你根据已学知识,实现双目视觉里程计,然后运行任何一个 sequence,并与标准轨迹比较。
以下是实现过程中的一些提示。这是通常 SLAM 中的做法,你可以作为参考,但并不一定必须使用这样的结构。 1. 你可以先实现一个 Frame-by-Frame 的里程计,即仅估计当前图像与上一个图像 之间的运动,然后把它们组成完整的相机轨迹。这样的工作方式应该是有效的, 但误差会很快累积,导致轨迹发散。
2.接下来,从轨迹中提取关键帧,对关键帧进行 bundle adjustment。由于 BA 过程重投影误差必定是下降的,可以有效的抑制误差累积。同时,你需要一个机制来管理所有地图点和关键帧。
3.最后,请测试你的方案在 kitti 上的表现,例如轨迹精度,运行时间等等。
2.1 双目相机标定
通过张正友标定法,求解左右相机内参数矩阵M、畸变系数矩阵D、以及右摄像头相对于左摄像头的旋转矩阵R和平移向量t。
标定步骤,
步骤一:双目相机读取左右目图片。
步骤二:使用函数findChessboardCorners
, initCameraMatrix2D
, stereoCalibrate
获得双目相机的内参数,并保存在 intrinsics.yml
文件中。
步骤三:使用函数 stereoRectify
获得双目相机外参数,并保存在 extrinsics.yml
文件中。
图 2.1 矫正的双目显示图
2.2 深度图计算
借助之前标定的相机参数,我们获得已经处理好的双目图片如下:
图 2.2 左右双目图片
通过双目计算深度的公式,获得深度图,视差图和深度图如下:
图 2.3 视差图和深度图
2.3 基于双目的VO(待续)
通过修改SLAM14讲中 基于RGBD的视觉里程计,将VO中的深度图替换成由双目计算出深度图。
2.4 KITTI数据集测试(待续)
2.5 代码
kingcent_stereo_demo: https://github.com/suljaxm/kingcent_stereo_demo