基于opencv pnp的单目测距与姿态解算
单目测距与双目测距一样需要完成的第一步是相机的标定
推荐用matlab进行标定，标定的方法可以参看这个博客http://blog.csdn.net/dreamharding/article/details/53700166
和https://blog.csdn.net/heroacool/article/details/51023921
在添加工具箱时注意选择第二个add withsubfolders（和子文件夹一起添加），否则容易导致用calib命令时打不开工具箱
标定相机需要注意的第二个问题是，由于使用的是外接usb摄像头其传输的像素大小由于usb带宽限制为640*480，所以标定使用的图片大小也需要缩放至640*480否则会导致标定出来的内参比实际使用时由于带宽影响导致图像缩小的内参大数倍
标定完相机得到内参矩阵和畸变向量后就可以开始进行opencv的单目测距
单目测距使用的是sovelpnp函数，其第一个参数是输入的三维点坐标矩阵（单位是cm），注意此三维坐标不能为随意点（ps:网上有些的三维点写的比较随意），三维点需要与第二个参数imgPoints的点一一对应（imgPoints是输入的图像二维点向量）
第三个参数是由上一步得到的相机内参矩阵（9*9）
第四个参数是相机的外参（1*5）
第五个参数是输出的相机旋转向量
第六个参数输出的是相机的平移向量
通过旋转向量和平移向量就可以得到相机坐标系相对于世界坐标系的旋转参数与平移情况
关于世界坐标系与相机坐标系转换的原理公式问题可以参看
http://blog.csdn.net/chenmohousuiyue/article/details/78157509?locationnum=9&fps=1
下面来发表一下我的理解
通过小孔成像的原理，根据相似三角形定理x*Z=f*X,其中的x是像素宽度，f是对应轴上的焦距，X是物体的实际长度，Z是物体到相机的距离
然后考虑到主点pinhole plane中的点的坐标u（相对于（0，0）左上角的偏移量），就可以得到x方向的像素坐标，y方向同理可得
然后通过线性代数的知识把上述的式子写成矩阵相乘
我这里进行了逆向的推导证明式子的成立
可以发现其中的第一个矩阵是相机的内参矩阵。
上述式子得到的是相机坐标系与图像坐标系之间的关系，
由于相机坐标系与世界坐标系存在xyz轴的平移与yaw,pinch,row的旋转，及世界坐标系转换到相机坐标系需要乘上R(选择矩阵)T（平移矩阵）
那么可以得到像素坐标系与世界坐标系的关系就是在原有的像素坐标系到相机坐标系的式子的基础上乘以RT矩阵
即有：x =M*[R|t]*X，其中的M是相机的内参矩阵，R,T是世界坐标转换为相机坐标的旋转平移矩阵
到这一步后可以发现在等式x =M*[R|t]*X中我们已知x,X,M就可以得到R,T，所以这里也可以反应出在相机标定中缩放图像为640*480的重要性，以及把像素点和世界坐标点一一对应的重要性
这里求R,T矩阵的过程就是使用opencv的sovelpnp输入像素坐标（及公式的x），世界坐标(及公式的X)，输入相机的内参矩阵和畸变矩阵（及公式的M）可以求得rvec和tvec及公式的（R,T矩阵）
在通过sovelpnp得到相机与世界坐标系转换关系的R,T矩阵后就是需要通过R,T矩阵求解出相机坐标系相对于世界坐标系的欧拉角关系和距离关系
旋转矩阵的公式推导可以参看这个图假使p点在原坐标系中的点为（x，y），在旋转角度o后的坐标系中的坐标为（x',y'）
可以得到数学公式为x'=xcoso-ysino,y'=xsino+ysino;
单个轴的旋转矩阵为
旋转矩阵R[3][3]
R[3][3]=R(row)*R(pitch)*R(yaw)得到的，可以得到总的
然后求解角度的公式为
其中rm就是那个3*3rvtc的矩阵，由sovelpnp求得,然后距离转换公式我就不知道怎么推倒就直接给我网上找的代码了
最后的结果是测距精度为1cm左右
