视觉定位与感知期末大作业报告

一、作业要求

本课程大作业内容是使用自己的手机或相机，对同一个场景，分别在三个角度拍摄三张图像，最终获得三张图像拍摄时的姿态和位置，以及图像中特征点的三维坐标信息，并评估和验证所实现算法的正确性和精度性能。

二、准备工作

在实验开始前，我们需要准备如下东西：

• MATLAB R2018B：考虑到作业过程中部分题目需要自己手动实现，故用MATLAB来实现比较简单。
• OpenCV：OpenCV具有很多库函数，对于不需要自己实现的部分用起来十分便捷。
• ROS_Melodic：本来可以做很多东西的，但是在这里只是用来做摄像头标定，因为以前用过，所以选择直接用这个来标定摄像头，但是安装ros过程比较麻烦，没有的话也可以使用MATLAB或者OpenCV相应的包进行摄像头校准。
• 其他物件：USB摄像头，IPad（iPad就用来做棋盘标定板和aruco目标检测）。

三、实验步骤

1. 实现思路

算法实现的思路是，首先选择其中两张图像，通过特征点匹配基础矩阵的求解得到两个拍摄视角的相对位姿，然后使用三角化得到对应特征点的三维点云。对于第三张图，我们可通过相机位姿估算的方式求解其姿态和位置。最后我们可以采用非线性最小二乘法对稀疏三维点和三个相机 Pose 同时优化，得到最终结果。

2. 摄像头标定

本次作业使用ROS对USB摄像头进行标定，主要步骤如下：

• 安装usb_cam驱动，并编译完成
• 对usb_cam中的usb_cam-test.launch进行修改，具体是将 “”中的video0改成与所用usb_camera相对应的video序号，在我电脑中他的序号为video2，所以我把他改成了video2。

>
  >
    >
    >
    >
    >
    >
    >
  >
  >
    >
    >
  >
>

• 检测usb_camera使用情况，正常的话弹出一个窗口，其中是摄像头图像。

roslaunch usb_cam usb_cam-test.launch-

• 使用camera_calibration进行校准，使用教程在ros.org中有，在这里不赘述，校准过程及结果如下图：
  

图1 校准过程中

图2 校准完成（图片没截好，请忽略）

图3 校准结果

• 至此，校准完成，得到相机的内参矩阵，并存储在/tmp/calibrationdata.tar.gz.中。

3. 在合适场景下拍摄三张照片

依照作业的要求，我们需要从图片中估测出当前图像的位置，那么我们同时也需要获得当前的绝对位置。

4. 图像去畸变处理

本步骤使用MATLAB来完成，根据第2点所得到的相机内参，进行图像点坐标的映射，最终得到无畸变的图像，其结果如下图所示，所得到的去畸变效果很好，同时将图片存储为后面的题做准备。

图4 去畸变结果对比

5. 特征点匹配

本小题可以不使用自己手动完成的算法，我们可以使用OpenCV的库函数来实现，这样可以更加便捷有效。
我们使用已经去畸变好了的图像，首先找出其中的特征点，再进行特征点的匹配工作，最后筛选出比较好的特诊点进行显示，其匹配结果如下：

图5 ORB特征点

图6 特征点匹配结果（上：优化前；下：优化后）

6. 本质矩阵

在本次的求解过程中，我们利用上一节已经求得的特征点对，在其已经优化好的基础上使用OpenCV中的库函数findEssentialMat进行求解，另一方面，编写函数findEssentialMat2，使用RANSAC算法以及八点法，求出了基础矩阵F，其求解结果如下：

F=
-1.38939e-39  1.86691e-38  -8.55247e-17  
3.73987e-48  -6.88189e-18  0.00778422  
5.65961e-17  5.26351e-315  0  
E=
-0.00923454  -0.0142003  -0.0391074  
0.0203505  0.00358894  -0.705741  
0.412591  0.573815  0.0138426  

7. 从本质矩阵中分解R，t

本小节选择采用上一节中所求得的E矩阵进行分解，在这里需要注意的是findEssentialMat函数在64位系统中默认的返回值为CV_64F类型的，需要进行类型转换才能进行下一步SVD分解，在这里我使用convertTo函数对其进行类型转换：

 E1.convertTo(E,CV_32F);

通过svd分解后，可以得到四组[R，t]解，这4组解分别为[R1,t]、[R1,-t]、[R2,t]、[R2,-t]：

E=
-0.00923454  -0.0142003  -0.0391074  
0.0203505  0.00358894  -0.705741  
0.412591  0.573815  0.0138426  
一共存在四组解
第一组：
R1=
-0.0078106  -0.00781251  2.22206e-109  
8.73722e-63  1.19918e-79  -0.0078106  
1.06469e-19  1.58735e-314  0  
t=
5.26354e-315  2.26128  2.37152e-322  
第二组：
R1=
-0.0078106  -0.00781251  2.22206e-109  
8.73722e-63  1.19918e-79  -0.0078106  
1.06469e-19  1.58735e-314  0  
t=
-5.26354e-315  -2.26128  -2.37152e-322  
第三组：
R2=
-0.0078106  0.0078125  -2.22206e-109  
-8.73722e-63  1.19918e-79  0.0078106  
1.06469e-19  5.26354e-315  0  
t=
5.26354e-315  2.26128  2.37152e-322  
第四组：
R2=
-0.0078106  0.0078125  -2.22206e-109  
-8.73722e-63  1.19918e-79  0.0078106  
1.06469e-19  5.26354e-315  0  
t=
-5.26354e-315  -2.26128  -2.37152e-322

8. 采用三角化进行三维点云重构

考虑到上一节中在数据类型转换的过程中存在问题，本节使用MATLAB变成，对已经求得的本质矩阵进行分解，并使用三角化方法求得正确的[R,t]解。
检查求得的R t 是否符合接受 R,t ，一组成功的匹配。最后给出的结果是这组匹配中有多少匹配是能够在这组 R,t 下正确三角化的（即 Z都大于0），并且输出这些三角化之后的三维点。如果三角化生成的三维点 Z小于等于0，且三角化的“前方交会角”（余弦是 cosParallax）不会太小，那么这个三维点三角化错误，舍弃。通过了 Z的检验，之后将这个三维点分别投影到两张影像上，计算投影的像素误差，误差大于2倍中误差，舍弃。
在本次的代码中，用到的匹配点文件来自于前几题的存储，其存储格式为“[x1,y1] [x2,y2]”,不足之处是需要手动把他改成“x1 y1 x2 y2”的格式。
本节的重构点云如图7所示：

图7 三维点云重构

分析：可能是由于匹配点过少，导致重构结果不是太好。

9. 3D-2D对应关系与位资预估

图8 T8匹配结果

如上图第三张图片的匹配效果较差，在此基础上对第三张图片进行位资预估，得到的坐标图片如下：

图8 T8匹配结果

四、总结

实验评估

本次实验使用普通的USB摄像头，并对其进行了校准，对图片进行去畸变处理，在特征点匹配的过程中表现良好，但是受限与相机清晰度和算法局限性的因素，后面的三维重构误差比较大，不能很好的进行定位与构图，总的效果较差。
另一方面，综合比较前后效果，发现OpenCV结果的效果要明显优于MATLAB的，可以在下一步优化过程中使用OpenCV实现。

个人总结

本次的视觉大作业对我来说难度比较大，在完成的过程中，我主要参考了邹老师的课程内容、高博的《视觉SLAM十四讲》以及ORB_SLAM的部分源代码，还与身边的同学讨论交流。在两天之内完成两个大作业，实在感觉力不从心，因此在后面的题目完成中有些匆忙，没有精益求精达到更好的效果。总的来说，我在OpenCV的使用中还存在许多问题，有待下一步学习，同时具体落实到算法的实现过程，还需要进一步的加强学习。

五、致谢

感谢邹老师和助教学姐这一学期以来的教导，能与你们一起学习SLAM相关的知识是我的荣幸，希望以后还能有机会与你们一起学习。
再次感谢，祝邹老师和黄媛学姐工作顺利，万事胜意！