c_cpp 如何在图像上绘制方向矩阵

Posted 2021-05-14

void drawOrientationInImage(cv::Mat *_img, const cv::Point2f &_centroid,
                            const cv::Mat &_rotMat) {
    double axisScale = 100.;
    cv::Mat xAxis = _rotMat.col(0) * axisScale;
    cv::Mat yAxis = _rotMat.col(1) * axisScale;
    cv::Mat zAxis = _rotMat.col(2) * axisScale;

    cv::Point2f xAxisProj = _centroid;
    xAxisProj.x += xAxis.at<float>(0);
    xAxisProj.y += xAxis.at<float>(1);

    cv::Point2f yAxisProj = _centroid;
    yAxisProj.x -= yAxis.at<float>(0);
    yAxisProj.y -= yAxis.at<float>(1);

    cv::Point2f zAxisProj = _centroid;
    zAxisProj.x -= zAxis.at<float>(0);
    zAxisProj.y -= zAxis.at<float>(1);

    cv::line(*_img, _centroid, xAxisProj, cv::Scalar(0, 0, 255));
    cv::line(*_img, _centroid, yAxisProj, cv::Scalar(0, 255, 0));
    cv::line(*_img, _centroid, zAxisProj, cv::Scalar(255, 0, 0));
}