8.空间位置解算

        把一个三维的东西变成了一维或者二维。

因此，行列式值为0，也代表这个变换是不可逆的，因为如果三维变成了二维，那么可以视为有一个维度被压缩了，并且是压缩成0了。所以经过了这个变换以后，再也找不到这个维度原来的信息，无法实施逆变换。

         我们可以用相机标定算出的内参和畸变系数外加透视的数学模型去拟合这个过程，从而实现二维到三维的转变。故在还原前，需先进行相机标定。

        先说这几个坐标系：

                像素坐标系：（u，v）单位像素

                        左上角为原点，

                        水平向右为u轴，

                        竖直向下为v轴。

                图像坐标系：（x，y）单位mm

                        将像素坐标系原点移动到中心水平向右x轴，竖直向下为y轴，并将单位变为mm，这里的变换关系：

    (dx为每个像素在横轴x上的尺寸，dy为每个像素在纵轴y上的尺寸)

                相机坐标系：（Xc，Yc，Zc）（在这一步还原了深度信息，其中Zc方向与光轴相同）

                世界坐标系：（Xw，Yw，Zw）刚体变换，根据旋转矩阵R和平移向量T将相机坐标系旋转平移移动到用户定义下的世界坐标系。

        根据OpenCV函数 solvePnP() 指出，我们只需要提供objectPoints、imagePoints、cameraMatrix、distCoeffs 就可以求出相机到物体的旋转向量 rvecs 和平移向量 tvecs 。而 cameraMatrix 和distCoeffs 在 相机标定 中已经求出。objectPoints又是我们定义的，imagePoints是检测出来的像素坐标，故可以使用 solvePnP（输入点必须大于等于4且对象点必须共面）。

        使用c++在此处 应注意矩阵的double和float类型的差异。

        用OpenCV的函数可以将旋转矩阵和旋转向量相互转化。

cv::Rodrigues(rot_vec_, rot_mat_); //rot_vec_旋转向量，rot_mat_旋转矩阵

 世界坐标系与​​​像素坐标系相互转换：

 有了旋转矩阵和平移向量，根据以下公式我们就可以实现相机坐标系到世界坐标系的转化：

其中：R为3x3旋转矩阵，T为3x1平移向量。Xc为相机坐标系，Xw为世界坐标系坐标。Zc方向为摄像头光轴方向，根据R T变换得到世界坐标系为用户在solvePnP中定义的世界坐标系。

这里的XYZ便是世界坐标Xw，Yw，Zw。 得到的（Xw，Yw，Zw）这个坐标本质是摄像机坐标系与表示三维场景结构的世界坐标系之间的绝对位姿关系（摄像机在用户定义下的世界坐标中的位姿）。进而得到目标俯仰角和偏航角。

学习过程中，有什么问题欢迎大家提出，有什么错误也感谢指出。