RGB-D visual odometry 初探

 

RGB-D visual odometry 初探

版权声明：本文为博主原创文章，未经博主允许不得转载。

目录(?)[+]

前几天看了GX博的一起做slam的博客，被他认真的态度和幽默的文采打动。（人家都已经把大部分工作做了,我们做点后续的工作是应当的啦），所以我觉得我应该写点东西，与大家一起学习和体会vslam的美。

高博的博客总共分为了七个项目，每一个都是在前面的基础上进行迭代的，基本上每一个增加一个topic。一不小心就从一开始看到第四个，下面讲一讲我对第四、五个的理解。

1.对关键函数的理解

• solvePnP和 findEssentialMat+recoverPose之间的差别

学习一个事物最有效的方式就是和已知的东西进行对比。在之前做单目视觉中，我们使用基本矩阵+姿态恢复的方式，这里采用的是solvePnP的方式， 那么这两者有什么不同呢，比较一下就知道啦！

solvePnP： Finds anobject pose from 3D-2D point correspondences，即是从persperctive-n-points，从透视点到平面点的转换关系；  API：C++: bool solvePnP(InputArray objectPoints,InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec,OutputArray tvec, bool useExtrinsicGuess=false, int flags=ITERATIVE )；（图1中的特征点的三维坐标，图2中的特征点的二维坐标）

可选的方法有三种： iterative迭代法，p3p方法，EPNP；在迭代法中，即根据Levenberg-Marquardt优化算法，将图1中的三维点的重投影点与图2中的点作为重投影误差，最小化这个误差，得到最佳的位置。 p3p采用了Gao XS文章中的文章，使用这种方法只能使用4个对应点。EPNP使用Moreno的文章中的方法。

问题1：imagePoints为什么不是三维坐标？

因为不需要,想想再ransac中，误差的计算是通过将图1中的三维点投影到图2图像上计算残差，这样就够了。

问题2：Levenberg-Marquardt算法流程图。

非线性函数优化算法。初学者可以看看牛顿法等基本最优化算法来加深对LM的理解，http://www.brnt.eu/phd/node10.html这个地方有对这个问题进行综合讲解的。

• findEssentialMat+recoverPose

findEssentialMat：Calculates an essential matrix from the corresponding points in twoimages；从两个图像的对应点中，计算出基本矩阵（进而可以直接得到trans和rot，但是具有不确定性，所以使用基本矩阵来表示）；C++: Mat findEssentialMat(InputArray points1, InputArray points2, double focal=1.0,Point2d pp=Point2d(0, 0), int method=RANSAC, double prob=0.999,double threshold=1.0, OutputArray mask=noArray() )，注 points1/2需要大于5个点；使用的方法有：Ransac；LMedS（最小中值二乘法）。两者使用了共同的数学基础是核线几何（epipolar geometry）关系，对应点与基础矩阵相乘为0的性质。

recoverPose：Recover relative camera rotation and translation from an estimatedessential matrix and the corresponding points in two images, using cheiralitycheck. Returns the number of inliers which pass the check. 通过计算出的基本矩阵，分解出trans和rotation向量。因为有多个（4中情况）解，所以后续使用对应点进行cheirality检查，通过的点作为inliers；很显然，最终的结果选择inliers最多的点。recoverPose中第一步是分解基础矩阵，得到（R1，t）（R1，-t),(R2,t),(R2,-t), 通过分解基础矩阵，只能得到translation的方向，所以在decomposeEssentialMat函数中返回的是单位向量。

2.前面各个工程的主要功能简介

至此，一个基本的无任何优化rgbd slam就完成了，姿态+制图；但是这样一个简单的过程有很多问题，效率与准确性是系统永远需要考虑的地方，有以下地方还需要进一步改进：1, 并不是每一帧都需要来做相对姿态的计算。2, 如果渐进式的拼接点云，那么每次一个小的点云和非常大的点云来拼接，肯定是比较慢的；rgbdslamv2中的显示方式应该不是这种。

告诉小白怎么使用cmake等工具	特征提取与匹配
深度图转化为点云	rgbd的视觉里程计
利用视觉里程计获得的姿态信息进行点云的拼接

3.使用long office数据实验

G博的代码是写得很认真的，为了满足我实验的需求，我对文件读取部分改动了一点点代码1：将图像文件的读取方式改进了以下。首先得到文件名列表的文件，然后将start_id 和 end_id 表示文件的顺序，而不是之前的文件名，这样可以方便使用各种数据集”。在获得文件名列表时，采用tum提供的associate.py 工具，首先 groundtruth和depth.txt, 然后再和rgb.txt进行合并，最后产生的文件在本代码中作为数据文件使用。

我打算把visualodometry的信息单独拿出来，以研究相对姿态计算的准确性。 一开始的时候对Rt矩阵不是很了解，尝试了很多，虽然都是错误的做法，但是我觉得依然有必要将这些贴出来，希望readers看了之后能少走一些弯路。
现在出现的问题主要是关于T-vec和R-vec的计算问题;相对姿态Rt与相机姿态之间的关系，找了好久没找着（其实就是一个矩阵的相乘而已）。

• 使用T- = T- + 相对旋转矩阵的转置*相对位移.

 

得到的结果是x方向差不多,但是y方向有明显的偏移.（在一开始，我觉得x、y的方向上变化时比较大的，所以将z都置为0，便于观察，后面会证明这样一个小小的改动会对整个实验造成什么样的影响，多花了很多时间）。

• 使用avisingh的方法;在计算translation的时候,是使用的全局的方向,乘以t;
  
  

实验结果还是不对.

• 与第一种类似,但是是减去一个值

.参考http://stackoverflow.com/questions/18637494/camera-position-in-world-coordinate-from-cvsolvepnp

• 参考rgbdslam-v2中的方式

这里略过，当时的确去找了，发现他没有显式得调用Rt而是将其转换到Eigen几何中去了。

• 直接把T相加;这样的结果还是不对.

• 旋转矩阵

经过向高手询问之后,发现相对姿态的累积是需要在旋转矩阵中来做的,不然会有问题，前面的一些工作相当于是被Avi Singh（https://github.com/avisingh599/mono-vo）给误导了，建议初学者不要去看这位印度帅哥的博客。我换到旋转矩阵之后，实验结果依然不对。那下一步只能去查是不是Rt计算的有问题了。

• 使用opencvrgbd odometry来做实验.

考虑到前面的不成功，可能有两个方面的问题：（a） sovlepnp解出来的Rt是不准确的，（b） 旋转矩阵的相乘有问题。考虑到前面已经测试了6种相乘的情况，是该试一试Rt的准确性了。所以我将opencvrgbdodometry写成一个ros包,visual_odometry_opencvodom; 实验结果依然是令人失望的。主要的结果就是(明天把问题摆出来,发现translation非常小,没法弄,学习研究一下rgbdodometry);

Rgbdodometry 的结果非常小（translation趋近于0）,而且每次都不一样.有的时候会有比较大的值出现,但是很多时候是错误的.

• 显示x/z;

前面的7种做法都是显示了x/y,z=0;  在做第六个工程的过程中，发现轨迹的x、z方向上的变化时比较大的，而在y方向上变化不大，所以之前我们的显示方法有一些问题，没有把主要的变化展示出来，前面的那些差别，主要是对x的。现在我们将轨迹的x/z显示在rviz中的x、y轴上，第（6）种尝试是正确的，在freiburg3_long_office中，大致显示了相机的运动轨迹图。

从上图中可以看出，绿色部分实际上是描述出了相机的运动轨迹。而红色的线条是真值（，本应该是保证真值的，但是tum的作者其实是把y和z对换了）。单独把groundtruth显示了一下，发现他的坐标的确是在x/y方向为主的，在z方向上倒是没有什么变化。这说明TUM在做groundtruth的时候，是把轨迹在地图上显示，这样就把原本属于Z方向上的变量，换成了y方向，以利于显示，那么，同时实时显示两幅图的确是不重合的，那么现在直接在三维上进行显示，看看结果。

结果大致是正确的。通过这么多反复的失败和再尝试，弄明白了很多细节信息，细节是很重要的，人的信心和对别人的信任，就是从细节的不断被验证中得来的。关注细节，重视细节。

注：后面把整个工程走完之后，我会把代码放到github上去 https://github.com/junwangcas；感谢G博，感谢愿意把自己的劳动成果分享出去的人。