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

RoboMaster_SWPU铁人战队视觉组工作笔记(三)位置解算_第1张图片
  关于单点解算的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是以当前摄像头中心为原点的物体所在的位置,要得到正确的转角需要对这个坐标进行平移,将原点平移到枪管上。

本期作者:Young
csdn昵称:Mr.羊

你可能感兴趣的:(RM视觉)