版权声明:本文为博主原创文章,未经博主允许不得转载。
前几天看了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;
实验结果还是不对.
- 与第一种类似,但是是减去一个值
- 参考rgbdslam-v2中的方式
这里略过,当时的确去找了,发现他没有显式得调用Rt而是将其转换到Eigen几何中去了。
- 直接把T相加;这样的结果还是不对.
- 旋转矩阵
- 使用opencvrgbd odometry来做实验.
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博,感谢愿意把自己的劳动成果分享出去的人。