相机姿态检测计算cv::solvePnP，图像到世界，世界到图像坐标

cameraMatrix:
4991.213689, 0.000000, 568.008086, 0.000000, 4994.153756, 503.518542, 0.000000, 0.000000, 1.000000
distCoeffs:
-1.072106, 18.843906, -0.000801, 0.016985, -105.303328
R-Matrix:
0.999985, -0.002114, -0.005147, 0.002243, 0.999682, 0.025136, 0.005093, -0.025147, 0.999671
T-Matrix:
-32.022342, -54.243943, 752.223045
世界坐标到图像坐标:
wX = 0.000,wY = 0.000,X = 357.8,Y = 146.2
wX = 0.000,wY = 76.000,X = 355.1,Y = 647.5
wX = 38.000,wY = 38.000,X = 607.2,Y = 396.1
wX = 76.000,wY = 76.000,X = 859.0,Y = 648.9
wX = 95.000,wY = 95.000,X = 983.9,Y = 774.3
wX = 95.000,wY = 0.000,X = 983.5,Y = 147.7
wX = 95.000,wY = 38.000,X = 984.5,Y = 397.3
图像坐标到世界坐标:
X = 356.0,Y = 146.0,wX = 0.072,wY = 0.395
X = 357.0,Y = 646.0,wX = 0.462,wY = 75.673
X = 606.0,Y = 396.0,wX = 37.823,wY = 37.993
X = 859.0,Y = 648.0,wX = 75.950,wY = 75.816
X = 985.0,Y = 775.0,wX = 94.910,wY = 94.849
X = 984.0,Y = 148.0,wX = 94.758,wY = 0.450
X = 984.0,Y = 399.0,wX = 94.759,wY = 38.311
~~~~标定成功~~~~~
 

cv::Point3f getWorldPoints(cv::Mat rotationMatrix, cv::Mat cameraMatrix, cv::Mat tvec, cv::Point2f inPoints)
{
    // 获取图像坐标
    cv::Mat imagePoint = (cv::Mat_<double>(3, 1) << inPoints.x, inPoints.y, 1.0);

    // 计算比例参数S
    cv::Mat tempMat = rotationMatrix.inv() * cameraMatrix.inv() * imagePoint;
    cv::Mat tempMat2 = rotationMatrix.inv() * tvec;

    double zConst = 0.0;  // 请提供zConst的值，这是一个常量
    double s = zConst + tempMat2.at<double>(2, 0);
    s /= tempMat.at<double>(2, 0);

    // 计算世界坐标
    cv::Mat wcPoint = rotationMatrix.inv() * (s * cameraMatrix.inv() * imagePoint - tvec);
    cv::Point3f worldPoint(wcPoint.at<double>(0, 0), wcPoint.at<double>(1, 0), wcPoint.at<double>(2, 0));

    return worldPoint;
}

int main() {
    // 世界坐标
    try
    {
        std::vector<cv::Point3f> objP;
        objP.push_back(cv::Point3f(0, 0, 0));
        objP.push_back(cv::Point3f(10, 0, 0));
        objP.push_back(cv::Point3f(0, 10, 0));
        objP.push_back(cv::Point3f(10, 10, 0));

        cv::Mat objectPointsMat;
        cv::Mat(objP).convertTo(objectPointsMat, CV_32F);

        // 左相机图像坐标
        std::vector<cv::Point2f> imgP_left;
        imgP_left.push_back(cv::Point2f(100, 100));
        imgP_left.push_back(cv::Point2f(200, 100));
        imgP_left.push_back(cv::Point2f(100, 200));
        imgP_left.push_back(cv::Point2f(200, 200));

        cv::Mat imagePointsMat_left;
        cv::Mat(imgP_left).convertTo(imagePointsMat_left, CV_32F);

        // 左相机内参矩阵和畸变系数
        cv::Mat leftCameraMatrix = (cv::Mat_<double>(3, 3) << 500, 0, 320, 0, 500, 240, 0, 0, 1);
        cv::Mat leftDistCoeffs = cv::Mat::zeros(1, 5, CV_64F);

        // 估计左相机的旋转和平移矩阵
        cv::Mat R_left, T_left;
        cv::solvePnP(objectPointsMat, imagePointsMat_left, leftCameraMatrix, leftDistCoeffs, R_left, T_left);
     // 转换为旋转矩阵
 // 转换为旋转矩阵
        cv::Mat R_left_matrix;
        cv::Rodrigues(R_left, R_left_matrix);

        if (1)
        {
            // 相机坐标
            cv::Point3f cameraCoordinate(10, 10, 1);

            // 使用cv::projectPoints将相机坐标投影到图像坐标
            std::vector<cv::Point2f> imgPoints;
            std::vector<cv::Point3f> objPoints(1, cameraCoordinate);
            cv::projectPoints(objPoints, R_left_matrix, T_left, leftCameraMatrix, leftDistCoeffs, imgPoints);

            // 输出结果
            std::cout << "Camera Coordinate: (" << cameraCoordinate.x << ", " << cameraCoordinate.y << ", " << cameraCoordinate.z << ")" << std::endl;
            std::cout << "Image Coordinate: (" << imgPoints[0].x << ", " << imgPoints[0].y << ")" << std::endl;
        }
        if (1)
        {
            // 请提供正确的图像坐标
            cv::Point2f inPoints(200, 200);

            // 获取世界坐标
            cv::Point3f worldPoint = getWorldPoints(R_left_matrix, leftCameraMatrix, T_left, inPoints);

            // 输出结果
            std::cout << "World Coordinate: (" << worldPoint.x << ", " << worldPoint.y << ", " << worldPoint.z << ")" << std::endl;

        }
    }
    catch (const cv::Exception& e) {
        std::cerr << "OpenCV Exception: " << e.what() << std::endl;
    }
    catch (const std::exception& e) {
        std::cerr << "Standard Exception: " << e.what() << std::endl;
    }
    catch (...) {
        std::cerr << "Unknown Exception" << std::endl;
    }

    return 0;
}