回环检测与建图
一、基于检测框的重建（选题 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
