pnp(Perspective-n-Point

四种解决算法（DLT、P3P、EPnP、BA）

DLT
P3P:P3P问题是已知三个3D目标点与其2D投影之间的对应关系，来确定标定相机的位姿问题。

EPnP

旋转方式

左手系与右手系图片理解_左手系和右手系的区别_一骑红尘荔枝来的博客-CSDN博客

slam学习——旋转向量、旋转矩阵、欧拉角、四元素的概念和运用_前途似海_来日方长的博客-CSDN博客

旋转向量

旋转矩阵

欧拉角

四元数

旋转

对于右手系，逆时针旋转是正向。

像素坐标系到相机坐标系

目的是获取相机中目标物体到相机的距离（在这里就是平移矩阵

这里的像素坐标系和图像坐标系，只是涉及到二维平面的平移，像素坐标系是以右上角为（0，0），图像坐标系是以图片的中间为（0，0）

阅读了官方文档之后发现，这里的input的二维数组是像素。

imagePoints	对应图像点的数组，Nx2 1 通道或 1xN/Nx1 2 通道，其中 N 是点数。vector也可以在这里传递。

首先要明确solvepnp中使用的那个坐标系，那个坐标是世界坐标系，用于获得像素坐标系中识别的到的像素点的位置。

solvepnp中涉及的两个输入数组，一个是在像素坐标系下面图像中的像素坐标值，另一个是世界坐标系下面的图像中的物体具体的像素坐标值

这里我们需要明确，我们之后的处理是基于相机坐标系的。

solvepnp是获取两个坐标系之间之间的平移矩阵，和旋转矩阵（这里就是相机坐标系和我们刚开始设置的世界坐标系）这里我们只是想获取装甲板相对于相机坐标系的一个位置，也就是距离，所以这里的旋转矩阵对于我们来说无所谓，所以我们只需重点将这里的世界坐标系的原点定到装甲板上，通过solvepnp获取到两个坐标系原点之间的距离。所以我们将世界坐标系原点定到装甲板的中心，xyz的方向定义无所谓（只需要正确表示出在这个坐标系下面，装甲板的各个角点的实际坐标即可。

这里我直接借鉴马老师的图片。

solvepnp

一文了解PnP算法，python opencv中的cv2.solvePnP()的使用，以及使用cv2.sovlePnP()方法标定相机和2D激光雷达_点亮～黑夜的博客-CSDN博客

PnP解算及SolvePnp用法_寒韩Glory的博客-CSDN博客

函数使用

objectPoints：特征点的世界坐标，坐标值需为float型，不能为double型，可以为mat类型，也可以直接输入vector

imagePoints：特征点在图像中的像素坐标，可以输入mat类型，也可以直接输入vector，注意输入点的顺序要与前面的特征点的世界坐标一一对应

(像素坐标就是在像素坐标系下的）

cameraMatrix：相机内参矩阵

distCoeffs：相机的畸变参数【Mat_(5, 1)】

rvec：输出的旋转向量：

tvec：输出的平移向量

useExtrinsicGuess：用于SOLVEPNP迭代的参数。如果为true（1），函数使用提供的rvec和tvec值分别作为旋转和平移向量的初始近似，并进一步优化它们。默认值为False。

flags：PnP的计算方法

相机坐标系到云台坐标系

这里的目的是为了更准确的发射子弹，也就是从枪口开始计算落点。（但是由于云台坐标系是

也就是从1到2

看图，也就是x不变，y和z需要进行变化，z轴坐标变为y轴坐标

y轴坐标变为-z轴坐标

（这里需要明确一个概念，对于坐标的变换，和对于坐标轴的变换是相反的。这个比较好理解，也就是当yoz平面绕x轴顺时针旋转时，y的坐标是逆时针旋转（也就是转换参考对象）

除了坐标轴的旋转外，还需要对坐标轴进行平移（因为云台和相机之间有一定距离）

void TargetSolver::photo_yuntai()//用来实现相机坐标系到云台坐标系
{
    double x_pian,y_pian,z_pian=0;
    //设置装甲板中心到相机坐标系的坐标
    object.x=tvec.at<double>(0,0);
    object.y=tvec.at<double>(1,0);
    object.z=tvec.at<double>(2,0);


    //实现相机坐标系到云台坐标系
    object.y=object.z;
    object.z=-object.y;



    //相机坐标系到云台坐标，还设计到平移
    object.x+=x_pian;
    object.y+=y_pian;
    object.z+=z_pian;

}

云台坐标系到世界坐标系

由于地心引力的影响，重力是竖直向下的，同时为了更便于观察，我们也需要将，坐标系由云台坐标系切换到世界坐标系。这里的世界坐标系的意思也就是保持坐标系的原点不动。让x轴y轴和地面平行，z轴垂直于地面。

所以这里我们需要获取小车云台的pitch和yaw轴的偏角。（由于云台不会翻滚，所以不用考虑云台的roll）

任务重点索引

该类的一个方法实现由云台坐标系转换到绝对坐标系，绝对坐标系的原点和云台坐标系重合，也位于云台Yaw轴和Pitch轴的交点，但坐标轴定义不同，云台yaw角度为0的方向为y轴方向，过原点且垂直于地面向上的方向为z轴方向，云台yaw角度为-90°的方向为x轴方向，也就是绝对坐标系坐标轴定义符合一个右手系。所以该函数主要需要做的工作有两个，一个是将坐标轴相对原点的位置（坐标轴的定义）由云台坐标系转换到绝对坐标系，一个是将坐标系旋转到与绝对坐标系重合，坐标轴完全平行，也就是将云台姿态角归零，将坐标系转正的过程，此时即完成了云台坐标系到绝对坐标系的转换。关于坐标系的旋转，我们将从电控端获得当前时刻的云台姿态，其中包含云台的yaw，pitch，roll信息，利用该信息进行旋转，可以分步利用二维平面旋转的方式实现，也可以用旋转矩阵等其他的方式实现。
关于坐标系的旋转，还有几个需要注意的问题。一个是如果使用分步的二维旋转来实现，那么需要一定思考清楚，三个轴的旋转顺序以及在xoy平面内的旋转方向。另一个问题是电控端绝对坐标系对于xyz坐标系的定义是通过左手系来定义的，而旋转方向依然是通过右手系定义的，即电控的x轴和z轴与视觉的x轴和z轴重合，但是电控的y轴和视觉的-y轴重合，所以电控端yaw轴正方向则为电控y轴向x轴的旋转方向。我们所收到的电控端向视觉端发送的云台姿态也是定义在电控的绝对坐标系及旋转方向下的，仔细思考，坐标系定义的不同，将给我们的坐标系旋转带来哪些影响？是否有物理量（主要是旋转方向和旋转顺序）将会因此改变？

仔细任务说明，我们在这里规定了云台坐标系的y轴是roll,x是pitch，z轴是yaw轴。（由于不会有翻滚，也就是说这里不会设计到y轴的转动）

同时考虑一下电控端是左手系。也就是只有y轴是相反的。

我们要让云台坐标系进行偏转，我认为先转哪个轴都是无所谓的，我这里先进行x轴的转动，再进行z轴的转动。

void TargetSolver::yuntai_world()//用来实现云台坐标系到世界坐标系
{
    //这里先假设获取了云台的pitch和yaw角度
    double pitch=5;
    double yaw=5;


      //绕z轴旋转yaw
    double temp_x=object.x;
    object.x=object.x*cos(yaw*Pi/180)-object.y*sin(yaw*Pi/180);
    object.y=temp_x*sin(yaw*Pi/180)+object.y*cos(yaw*Pi/180);

    //绕x轴旋转pitch
    double temp_y=object.y;
    object.y=object.y*cos(pitch*Pi/180)-object.z*sin(pitch*Pi/180);
    object.z=temp_y*sin(pitch*Pi/180)+object.z*cos(pitch*Pi/180);

  



}