深度相机结合cv--solvePnP函数获取定位

1、深度相机特点

深度相机我使用的是CamBoard PICO flexx,它的特点就是能同时获取灰度&深度信息。
深度相机结合cv--solvePnP函数获取定位_第1张图片
深度相机结合cv--solvePnP函数获取定位_第2张图片
可以用Opencv来处理灰度图像,进行POSIT计算及图像识别。用OpenGL来进行3D点云显示,也可以加入PCL来进行3D点云计算。
这里,我使用OpenCV的cv::solvePnP函数来获取定位信息。

2、solvePnP结果

首先使用ORB算法提取前后帧之间的匹配点,
深度相机结合cv--solvePnP函数获取定位_第3张图片
效果还比较正常,耗时约40ms。
然后针对匹配的相关点分别使用cv::solvePnP计算其外参数,得到R1,R2,T1,T2向量。
使用cvRodrigues2()变换之后,通过
深度相机结合cv--solvePnP函数获取定位_第4张图片
这里写图片描述
深度相机结合cv--solvePnP函数获取定位_第5张图片
就可计算出其与三坐标轴角度。
然后通过前后帧结果相减,即可得到实际的相对转角、位移。

3、实验结果

理论是可行的,现实是骨感的。此时深度相机放置不动,理论上测量的旋转角度结果应为[0,0,0],结果如图
深度相机结合cv--solvePnP函数获取定位_第6张图片
可以看到其结果是完全不稳定的,虽然第一帧效果不错,但是后面的结果却像是开了正弦曲线一样,一会正一会负,结果不稳定,不可用

4、结果分析

也许是考虑到solvePnP不太可靠,OpenCV中还加入了solvePnPRansac()方法,其实就是使用RANSAC方法来提取合适的点来进行solvePnP。

关于RANSAC方法,其核心理念就是,从众多样本中随机取出几个点,计算出相应的映射矩阵(如从3D映射到2D,从2D映射到另一个2D),然后使用该映射矩阵将所有(2D/3D)样本都映射成2D结果,让后将“2D结果”与实际2D进行距离分析,计算距离在某reprojectionError范围内的点的数量,然后取该范围内点的数量最多的映射矩阵为结果最优矩阵。大家可参考[https://en.wikipedia.org/wiki/Random_sample_consensus]
[http://blog.csdn.net/qq_25352981/article/details/46914837]
深度相机结合cv--solvePnP函数获取定位_第7张图片
深度相机结合cv--solvePnP函数获取定位_第8张图片
所以说solvePnPRansac()方法的核心其实是多次循环使用solvePnP()、projectPoints()、norm()。实际运行中却经常报错,继续测试。

实际测试

相机内参数矩阵、畸变向量,之前都可以通过Matlab Toolbox标定出来,现在就可以用solvePnP()得到T、R向量,然后直接用projectPoints()及深度相机中的3D点投影到2D,然后来对比投影的2D与实际的2D的差别。
深度相机结合cv--solvePnP函数获取定位_第9张图片
反投影的2D结果为

-12.5 98.26 98.88 113.62
-47.75 75.46 70.86 82.019

计算的结果完全对不上,现在开始严重质疑cv::solvePnP()得到T、R向量的结果准确性了。
我已经将所有矩阵、向量的类型都变为了CV_64F,所以不应该是数据溢出。

问题分析

当同一点的坐标由2D转为3D时,存在一定的误差,需要 最小二乘法 优化一下,OpenCV在这一块做的优化不够,所以需要去添加Eigen、g2o这样的专业的矩阵计算库。
g2o: A General Framework for Graph Optimization: 一种求位置和速度的ORB方法。
BOW:Bag of Words: 一种模式识别(非监督聚类分类)方法,用于闭环检测。
eigen: C ++库,有效支持线性代数,矩阵和矢量运算,数值分析及其相关的算法。
相关的库链接:
https://github.com/RainerKuemmerle/g2o
http://eigen.tuxfamily.org/index.php?title=Main_Page

少年,努力到来生吧!

你可能感兴趣的:(使用心得,OpenCV,深度相机)