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

1、深度相机特点

深度相机我使用的是CamBoard PICO flexx，它的特点就是能同时获取灰度&深度信息。


可以用Opencv来处理灰度图像，进行POSIT计算及图像识别。用OpenGL来进行3D点云显示，也可以加入PCL来进行3D点云计算。
这里，我使用OpenCV的cv::solvePnP函数来获取定位信息。

2、solvePnP结果

首先使用ORB算法提取前后帧之间的匹配点，

效果还比较正常，耗时约40ms。
然后针对匹配的相关点分别使用cv::solvePnP计算其外参数，得到R1,R2,T1,T2向量。
使用cvRodrigues2（）变换之后，通过



就可计算出其与三坐标轴角度。
然后通过前后帧结果相减，即可得到实际的相对转角、位移。

3、实验结果

理论是可行的，现实是骨感的。此时深度相机放置不动，理论上测量的旋转角度结果应为[0,0,0]，结果如图

可以看到其结果是完全不稳定的，虽然第一帧效果不错，但是后面的结果却像是开了正弦曲线一样，一会正一会负，结果不稳定，不可用。

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]


所以说solvePnPRansac()方法的核心其实是多次循环使用solvePnP()、projectPoints()、norm()。实际运行中却经常报错，继续测试。

实际测试

相机内参数矩阵、畸变向量，之前都可以通过Matlab Toolbox标定出来，现在就可以用solvePnP()得到T、R向量，然后直接用projectPoints()及深度相机中的3D点投影到2D，然后来对比投影的2D与实际的2D的差别。

反投影的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

少年，努力到来生吧！