RM角度解析

RM角度解析

角度求解的流程

• 输入检测到的旋转矩形
• 提取旋转矩形顶点并按一定顺序排列得二维点向量
• 以实际装甲的中心为原点建立物体坐标系，求解四个定点的坐标并排序得三维点向量（顺序与上面的二维点一一对应）
• 调用solvePnP求解从物体坐标系到相机坐标系的旋转r、平移向量t（平移向量就是物体坐标系原点，即目标中心在相机坐标系中的坐标，下面是根据t来求解偏转角度）
• 测量并计算得到相机坐标系到云台坐标系的旋转矩阵A和偏移向量b，然后对t实施坐标变换，从相机坐标系变换到云台坐标系tt=At+b
• 设tt=(x,y,z), 俯仰角：$Pitch=atan(\frac{y}{z})$, 偏航角：$Yaw=atan(\frac{x}{z})$
• 角度的修正，考虑炮管的偏移和重力作用，对pitch角进行修正

变换矩阵A和平移向量b的求解：

设相机坐标系绕X轴你是逆时针旋转$\theta$后与云台坐标系的各个轴向平行，则旋转矩阵为

$$A= \begin{bmatrix} 1 & 0 & 0\\ 0 & cos(\theta) & sin(\theta)\\ 0 & -sin(\theta) & cos(\theta) \end{bmatrix}$$
设相机坐标系的原点在云台坐标系中的坐标为 $(x_0,y_0,z_0)$， 则
$$b= \begin{bmatrix} x_0\\y_0\\z_0 \end{bmatrix}$$
若相机坐标系下一点 $P(x,y,z)$,在云台坐标系下的坐标 $(x_1,y_1,z_1)$为
$$\begin{bmatrix} x_1\\y_1\\z_1 \end{bmatrix}=A\begin{bmatrix} x_0\\y_0\\z_0 \end{bmatrix}+b$$

角度求解类

AngleSolver类结构
class AngleSolver : public RectPnPSolver 
{
public:
    AngleSolver(xxx);
    void setScaleZ(double scale);
    void setRelationPoseCameraPTZ(const cv::Mat & rot_camera_ptz, const cv::Mat & trans_camera_ptz, double y_offset_barrel_ptz);

    void getTarget2dPoinstion(const cv::RotatedRect & rect, std::vector<cv::Point2f> & target2d, const cv::Point2f & offset);
    bool getAngle(const cv::RotatedRect & rect, double & angle_x, double & angle_y, double bullet_speed = 0, double current_ptz_angle = 0.0, const cv::Point2f & offset = cv::Point2f());

    void tranformationCamera2PTZ(const cv::Mat & pos, cv::Mat & transed_pos);
    void adjustPTZ2Barrel(const cv::Mat & pos_in_ptz, double & angle_x, double & angle_y, double bullet_speed = 0.0, double current_ptz_angle = 0.0);

public:
    cv::Mat position_in_camera; //在相机坐标系下的坐标
    cv::Mat position_in_ptz; //在云台坐标系下的坐标
private:
    cv::Mat trans_camera2ptz; //相机坐标系到云台坐标系的平移向量
    cv::Mat rot_camera2ptz; //相机坐标系到云台坐标系的旋转矩阵

    // 炮管与云台在y轴上的偏移（cm）
    double offset_y_barrel_ptz;

    // 检测的距离（cm）
    double min_distance;
    double max_distance;
    double scale_z;
};

角度求解的核心函数

//利用检测出来的旋转矩形计算出云台转角的函数
bool AngleSolver::getAngle(const cv::RotatedRect & rect, double & angle_x, double & angle_y, double bullet_speed, double current_ptz_angle, const cv::Point2f & offset)
{
    if (rect.size.height < 1)
        return false;
    vector<Point2f> target2d;
    getTarget2dPoinstion(rect, target2d, offset); //获取旋转矩形的顶点并排序

    cv::Mat r;
 //求解相机的外参，即从物体坐标系到相机坐标系的旋转矩阵rot、平移向量trans
    solvePnP4Points(target2d, r, position_in_camera);
    position_in_camera.at<double>(2, 0) = scale_z * position_in_camera.at<double>(2, 0);
    if (position_in_camera.at<double>(2, 0) < min_distance || position_in_camera.at<double>(2, 0) > max_distance)
    {
        cout << "out of range: [" << min_distance << ", " << max_distance << "]\n";
        return false;
    }
  //将目标在相机坐标系中的坐标转换成在云台坐标系中的坐标
    tranformationCamera2PTZ(position_in_camera, position_in_ptz); 
    // 计算炮管的偏转角度
    adjustPTZ2Barrel(position_in_ptz, angle_x, angle_y, bullet_speed, current_ptz_angle);

    return true;
}

  //求解相机的外参，即从物体坐标系到相机坐标系的旋转矩阵rot、平移向量trans
void RectPnPSolver::solvePnP4Points(const std::vector<cv::Point2f> & points2d, cv::Mat & rot, cv::Mat & trans)
{
    if(width_target < 10e-5 || height_target < 10e-5)
    {
        rot = cv::Mat::eye(3,3,CV_64FC1);
        trans = cv::Mat::zeros(3,1,CV_64FC1);
        return;
    }
    std::vector<cv::Point3f> point3d;
    double half_x = width_target / 2.0;
    double half_y = height_target / 2.0;

    point3d.push_back(Point3f(-half_x, -half_y, 0));
    point3d.push_back(Point3f(half_x, -half_y, 0));
    point3d.push_back(Point3f(half_x, half_y, 0));
    point3d.push_back(Point3f(-half_x, half_y, 0));

    cv::Mat r;
    cv::solvePnP(point3d, points2d, cam_matrix, distortion_coeff, r, trans);
    Rodrigues(r, rot);
}

//像素坐标转换到摄像机的坐标
void RectPnPSolver::solvePnP4Points(const std::vector<cv::Point2f> & points2d, cv::Mat & rot, cv::Mat & trans)
{
    if(width_target < 10e-5 || height_target < 10e-5)
    {
        rot = cv::Mat::eye(3,3,CV_64FC1);
        trans = cv::Mat::zeros(3,1,CV_64FC1);
        return;
    }
    std::vector<cv::Point3f> point3d;
    double half_x = width_target / 2.0;
    double half_y = height_target / 2.0;

    point3d.push_back(Point3f(-half_x, -half_y, 0));
    point3d.push_back(Point3f(half_x, -half_y, 0));
    point3d.push_back(Point3f(half_x, half_y, 0));
    point3d.push_back(Point3f(-half_x, half_y, 0));

    cv::Mat r;
  //将图像坐标转换到摄像机坐标
    cv::solvePnP(point3d, points2d, cam_matrix, distortion_coeff, r, trans);
    Rodrigues(r, rot);
}

//opencv函数solvePnP
bool cv::solvePnP   (   InputArray      objectPoints,
        InputArray      imagePoints,
        InputArray      cameraMatrix,
        InputArray      distCoeffs,
        OutputArray     rvec,
        OutputArray     tvec,
        bool    useExtrinsicGuess = false,
        int     flags = SOLVEPNP_ITERATIVE 
    )   

//获取旋转矩形的顶点并排序
void AngleSolver::getTarget2dPoinstion(const cv::RotatedRect & rect, vector<Point2f> & target2d, const cv::Point2f & offset){
    Point2f vertices[4];
    rect.points(vertices); //获取旋转矩形的四个定点
    Point2f lu, ld, ru, rd;
    sort(vertices, vertices + 4, [](const Point2f & p1, const Point2f & p2) { return p1.x < p2.x; }); //按照定点的x坐标进行排序
    if (vertices[0].y < vertices[1].y){
        lu = vertices[0];
        ld = vertices[1];
    }
    else{
        lu = vertices[1];
        ld = vertices[0];
    }
    if (vertices[2].y < vertices[3].y)  {
        ru = vertices[2];
        rd = vertices[3];
    }
    else {
        ru = vertices[3];
        rd = vertices[2];
    }

    target2d.clear();
    target2d.push_back(lu + offset);
    target2d.push_back(ru + offset);
    target2d.push_back(rd + offset);
    target2d.push_back(ld + offset);
}

//角度工厂中getAngle函数，判断角度求解器是否为空，并为角度求解器设置参数
bool AngleSolverFactory::getAngle(const cv::RotatedRect & rect, TargetType type, double & angle_x, double & angle_y, double bullet_speed, double current_ptz_angle, const cv::Point2f & offset)
{
    if(slover == NULL){
        std::cerr << "slover not set\n";
        return false;
    }

    double width = 0.0, height = 0.0;
    if(type == TARGET_RUNE){
        width = rune_width;
        height = rune_height;
    }
    else if(type == TARGET_ARMOR){
        width = armor_width;
        height = armor_height;
    }
    else if(type == TARGET_SAMLL_ATMOR){
        width = small_armor_width;
        height = small_armor_height;
    }
    cv::RotatedRect rect_rectifid = rect;
    AngleSolverFactory::adjustRect2FixedRatio(rect_rectifid, width/height);
    slover->setTargetSize(width, height);
    return slover->getAngle(rect_rectifid, angle_x, angle_y, bullet_speed, current_ptz_angle, offset);
}