RM角度解析

RM角度解析

角度求解的流程

• 輸入檢測到的旋轉矩形
• 提取旋轉矩形頂點並按一定順序排列得二維點向量
• 以實際裝甲的中心爲原點建立物體座標系，求解四個定點的座標並排序得三維點向量（順序與上面的二維點一一對應）
• 調用solvePnP求解從物體座標系到相機座標系的旋轉r、平移向量t（平移向量就是物體座標系原點，即目標中心在相機座標系中的座標，下面是根據t來求解偏轉角度）
• 測量並計算得到相機座標系到雲臺座標系的旋轉矩陣A和偏移向量b，然後對t實施座標變換，從相機座標系變換到雲臺座標系tt=At+b
• 設tt=(x,y,z), 俯仰角：Pitch=atan(yz) , 偏航角：Yaw=atan(xz)
• 角度的修正，考慮炮管的偏移和重力作用，對pitch角進行修正

變換矩陣A和平移向量b的求解：

設相機座標系繞X軸你是逆時針旋轉θ 後與雲臺座標系的各個軸向平行，則旋轉矩陣爲

A=⎡⎣⎢1000cos(θ)−sin(θ)0sin(θ)cos(θ)⎤⎦⎥

設相機座標系的原點在雲臺座標系中的座標爲(x0,y0,z0) ， 則

b=⎡⎣⎢x0y0z0⎤⎦⎥

若相機座標系下一點P(x,y,z) ,在雲臺座標系下的座標(x1,y1,z1) 爲

⎡⎣⎢x1y1z1⎤⎦⎥=A⎡⎣⎢x0y0z0⎤⎦⎥+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);
}