solvepnp函数-世界坐标系
一、
二、
世界坐标系是任意选定的，可以任意事先定义，然后给出每个特征点在世界坐标系下的三维坐标，然后以一定顺序存储这些点。
特征点的像素坐标，一般是通过角点检测算法直接得到的，角点检测算法有他自己的存储点的顺序，此时需要把输入的像素点的存储顺序人为改变，使其和输入的世界坐标的顺序对应相同。
角点检测算法有他自己的存储点的顺序，或者不改变像素点的存储顺序，然后根据点与点的对应关系 确定世界坐标系。
三、旋转向量转换旋转矩阵
应用罗德里格斯变换
double rm[9];
cv::Mat rotM(3, 3, CV_64FC1, rm);
Rodrigues(rvec, rotM);
四、由旋转矩阵求得姿态角
代码：
float theta_z = atan2(rotMat[1][0], rotMat[0][0])*57.2958;
float theta_y = atan2(-rotMat[2][0], sqrt(rotMat[2][0] * rotMat[2][0] + rotMat[2][2] * rotMat[2][2]))*57.2958;
float theta_x = atan2(rotMat[2][1], rotMat[2][2])*57.2958;
=============================================================================================
=============================================================================================
在OpenCV中，可以使用calibrateCamera函数，通过多个视角的2D/3D对应，求解出该相机的内参数和每一个视角的外参数。
使用C++接口时的输入参数如下：
objectPoints - 每一个视角中，关键点的世界坐标系。可以使用vector < vector
imagePoints - 每一个视角中，关键点的图像坐标系。可以使用vector < vector
这个值可以通过findChessboardCorners函数从图像中获得。注意：传入findChessboardCorners函数的patternSize参数，要和objectPoints中的行列数统一。
imageSize - 图像尺寸。
flags - 参数。决定是否使用初始值，扭曲参数个数等。
输出参数如下：
cameraMatrix - 3*3的摄像机内矩阵。
distCoeffs - 4*1（具体尺寸取决于flags）。对图像坐标系进行进一步扭曲。
这两个参数是内参数，可以把摄像机坐标系转换成图像坐标系。
rvecs - 每一个视图的旋转向量。vector
tvecs - 每一个视图的平移向量。vector
这两个参数是外参数，每一个视图不同，可以把世界坐标系转换成摄像机坐标系。
问题：这个世界坐标系是什么样的？
在objectPoints中，我们实际只设定了每个交叉点的间隔，具体xyz指向哪里却没有给出。
这时候，需要使用drawChessboardCorners函数，把检测到的2D点在原图上显示出来：
这个函数按照先变化x，而后变化y的方式依次画点。先画的行为红色，逐渐变为蓝色。
回顾我们写objectPoints时的方式，可以推断出世界坐标的方向：图中x轴正方向向左，y轴正方向远离镜头，根据右手螺旋，z轴正方向向下。
总结来说，calibrateCamera函数给出的世界坐标方向，是由obejctPoints的设定顺序，以及findChessboardCorners的检测顺序共同决定的。
但是，findChessboardCorners先检测那个角点并不确定。前例从右下角开始，以下就从左上角开始：
另一种常见用法是：先用calibrateCamera标定好摄像机内参，而后使用solvePnP函数只求解外参。solvePnP函数的参数意义和calibrateCamera类似。
---------------------
作者：shenxiaolu1984
来源：CSDN
原文：https://blog.csdn.net/shenxiaolu1984/article/details/50165635