RoboMaster_SWPU铁人战队视觉组工作笔记（三）位置解算
今天在视觉工程里面加入了位置解算，看了江达小记的学习笔记https://blog.csdn.net/u010750137/article/details/97646798，感触良多啊，觉得自己也应该记录一些东西，just do it。
本工程github传送门：https://github.com/Young19961022/SWPU_2020RM_version
一、位置解算在本视觉工程中的作用
找到装甲板后，下一步就是考虑打击装甲板了，如果不知道深度信息，即枪口距离目标装甲板有多远，无疑于盲打，通过裁判系统能知道子弹射速，如果能得到枪口离目标装甲板的距离，那么根据斜抛运动公式，可近似算出仰角，体现出来的宏观表现就是能真正打中装甲板了，而位置解算还不只是得到距离，最重要的是能得到yaw、pitch轴角度误差量，让枪口瞄准装甲板，所以位置解算是非常重要的。
二、单点解算
单点解算不能得到深度信息，只能得到相对于摄像头中心的转角，仅需要一个点的坐标就能计算数据。
把目标物体在图像中的位置与图像中心的差值定义为像素差，而转给云台电机的命令是角度值，角度与像素差不是线性关系，在下面的gif图中（图片来源江达小记）可以看出当角度大于30度时，像素差与角度的差异就很大了，不能鲁莽地把像素差直接与角度画等号，送去云台控制PID里面（不然就需要根据像素差的大小用不同的PID），利用小孔成像原理，求像素差与焦距的比值的反正切值就可以得到角度。而真实的像素差需要进行摄像机标定后才能确定，一般我都是整幅图像做标定，这样会比较费时，但由于多线程的关系，也可以接受（把获取图像作为一个单独的线程，将标定放在这个线程里面，100fps的摄像头原本获取一帧图像需要10ms，加上标定的过程就变为大概14ms，可以接受）。但现在有undistortPoints函数可以对单点做标定，所以我就选择后者，处理速度会更快。
具体可参考这篇博文：https://blog.csdn.net/u010750137/article/details/97646798
关于单点解算的API实现如下：
void AngleSolver::onePointSolution(const vector<Point2f> centerPoint)
{
double fx = _params.CameraIntrinsicMatrix.at<double>(0,0);
double fy = _params.CameraIntrinsicMatrix.at<double>(1,1);
double cx = _params.CameraIntrinsicMatrix.at<double>(0,2);
double cy = _params.CameraIntrinsicMatrix.at<double>(1,2);
vector<Point2f> dstPoint;
//单点矫正
undistortPoints(centerPoint,dstPoint,_params.CameraIntrinsicMatrix,
_params.DistortionCoefficient,noArray(),_params.CameraIntrinsicMatrix);
Point2f pnt = dstPoint.front();//返回dstPoint中的第一个元素
//去畸变后的比值，根据像素坐标系与世界坐标系的关系得出,pnt的坐标就是在整幅图像中的坐标
double rxNew=(pnt.x-cx)/fx;
double ryNew=(pnt.y-cy)/fy;
yawErr = atan(rxNew)/CV_PI*180;//转换为角度
pitchErr = atan(ryNew)/CV_PI*180;//转换为角度
}
三、P4P解算
P4P解算是基于solvePnP做的，能得到深度信息，但需要四个点才能计算数据。用solvePnP最大的好处能自定义摄像头中心，因为结构设计的原因，摄像头肯定不会和枪口在同一点上，送入solvePnP中的图像坐标是基于整幅图像来定的，原点在图像左上角，而计算出来的_tVec平移矩阵是以当前摄像头中心为原点的物体所在的位置，要得到正确的转角需要对这个坐标进行平移，将原点平移到枪管上。
具体可参考这篇博文：https://blog.csdn.net/u010750137/article/details/98457477
solvePnP函数用法总结如下：https://blog.csdn.net/cocoaqin/article/details/77848588
用solvePnP计算出目标物体的平移矩阵（在世界坐标系下），经过分析求x与z的反正切值就得到yaw轴角度，求y与z的反正切值就得到pitch轴角度。
函数用法解释：
bool solvePnP(InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess=false, int flags=ITERATIVE )
objectPoints：特征点的世界坐标，坐标值需为float型，不能为double型，可以为mat类型，也可以直接输入vector
imagePoints：特征点在图像中的像素坐标，可以输入mat类型，也可以直接输入vector，注意输入点的顺序要与前面的特征点的世界坐标一一对应
cameraMatrix：相机内参矩阵
distCoeffs：相机的畸变参数【Mat_(5, 1)】
rvec：输出的旋转向量
tvec：输出的平移矩阵
最后的输入参数有三个可选项：
CV_ITERATIVE，默认值，它通过迭代求出重投影误差最小的解作为问题的最优解。
CV_P3P则是使用非常经典的Gao的P3P问题求解算法。
CV_EPNP使用文章《EPnP: Efficient Perspective-n-Point Camera Pose Estimation》中的方法求解。
关于P4P解算的API实现如下：
std::vector<double> AngleSolver::p4pSolution(const std::vector<cv::Point2f> objectPoints,int objectType)
{
if(objectType == RM::BIG_ARMOR)
solvePnP(_params.POINT_3D_OF_ARMOR_BIG,objectPoints,_params.CameraIntrinsicMatrix,
_params.DistortionCoefficient,_rVec,_tVec,false, CV_ITERATIVE);
else if(objectType == RM::SMALL_ARMOR)
solvePnP(_params.POINT_3D_OF_ARMOR_SMALL,objectPoints,_params.CameraIntrinsicMatrix,
_params.DistortionCoefficient,_rVec,_tVec,false, CV_ITERATIVE);
_tVec.at<float>(1, 0) -= _params.Y_DISTANCE_BETWEEN_GUN_AND_CAM;
_tVec.at<float>(2, 0) -= _params.Z_DISTANCE_BETWEEN_MUZZLE_AND_CAM;
yawErr = atan(_tVec.at<float>(0, 0)/_tVec.at<float>(2, 0))/CV_PI*180;//转换为角度
pitchErr = atan(_tVec.at<float>(1, 0)/_tVec.at<float>(2, 0))/CV_PI*180;//转换为角度
//计算三维空间下的欧氏距离
_euclideanDistance = sqrt(_tVec.at<float>(0, 0)*_tVec.at<float>(0, 0) + _tVec.at<float>(1, 0)*
_tVec.at<float>(1, 0) + _tVec.at<float>(2, 0)* _tVec.at<float>(2, 0));
vector<double> result;
result.resize(3); //指定容器的大小为3
result[0] = yawErr;
result[1] = pitchErr;
result[2] =_euclideanDistance;
return result;
}
四、总结
单点标定undistortPoints，输入、输出的点变量都是数组形式，这点使用的时候要注意。单点解算时输入的像素坐标是针对全幅图像而言，坐标原点在图像左上角。
从计算公式
double rxNew=(pnt.x-cx)/fx;
double ryNew=(pnt.y-cy)/fy; 就可看出来。
P4P解算时，输入的像素坐标是针对全幅图像而言，坐标原点在图像左上角。计算出的平移矩阵tVec是以当前摄像头中心为原点的物体所在的位置，要得到正确的转角需要对这个坐标进行平移，将原点平移到枪管上。
