单目工业SLR相机位姿估计欧拉角5度偏差问题排查求助

使用工业级单目SLR相机进行目标位姿估计，步骤如下：

1. 相机标定：将棋盘格置于黑纸与玻璃之间（避免粘贴形变和胶水厚度影响），装置与相机呈45度角放置（俯视图视角）。
2. 调用cv::findChessboardCorners检测棋盘格角点，结合世界坐标系物体点（Z=0）、相机内参矩阵及畸变系数，通过cv::solvePnP(object_points, corners, Cam_Mat, distCoeffs, rvecs, tvecs)求解位姿，得到旋转向量rvecs与平移向量tvecs。
3. 将rvecs转换为欧拉角时，出现5度偏差，物理测量验证结果不理想。

相关代码如下：

cv::Vec3f rotationMatrixToEulerAngles(const cv::Mat& R) {
    
    float sy = std::sqrt(R.at<double>(0, 0) * R.at<double>(0, 0) + R.at<double>(1, 0) * R.at<double>(1, 0));

    bool singular = sy < 1e-6;

    float x, y, z;
    if (!singular) {
        y = std::atan2(-R.at<double>(2, 0), sy);
        x = std::atan2(R.at<double>(2, 1)/ std::cos(y), R.at<double>(2, 2) / std::cos(y));
        z = std::atan2(R.at<double>(1, 0) / std::cos(y), R.at<double>(0, 0) / std::cos(y));
    }
    else {
        cout << "singular!\n";
        y = std::atan2(-R.at<double>(2, 0), sy);
        x = std::atan2(-R.at<double>(1, 2), R.at<double>(1, 1));
        z = 0;
    }
    double pi = 3.14159;
    double x_deg = x * (180 / pi);
    double y_deg = y * (180 / pi);
    double z_deg = z * (180 / pi);


    cout << "Rx = " << x << "\nRy = "<< y <<"\nRz = " << z << endl;
    cout << "Rx degrees = " << x_deg << "\nRy Degrees = " << y_deg << "\nRz Degrees = " << z_deg << endl;

    return cv::Vec3f(x, y, z);
}

class line_pose_estimator {
public:
    line_pose_estimator(int height, int width, int square_dim) {
        board_height = height;
        board_width = width;
        squaresize = square_dim;
        patternsize = Size(board_width, board_height);
    }

    int board_height, board_width;
    Size patternsize;
    int squaresize;

    vector<Point2f> corners;
    vector<Point3f> object_points;

    const cv::TermCriteria criteria = cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 30, 0.01);
    const int chessboardflags = CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE | CALIB_CB_FAST_CHECK;

    
    //input - grayscale image, 
    //output - translation and rotation vector of origin
    void find_origin(Mat image, string out_name, bool save_img = false) {
        string out_dir = "C:\\Users\\Username\\Documents\\Pose Estimation\\Output Data\\";
        //camera intrinsic parameters
        Mat Cam_Mat = Mat::eye(3, 3, CV_32F);

        Cam_Mat.at<float>(0, 0) = 5.1863196755281979e+03;
        Cam_Mat.at<float>(1, 1) = 5.1863196755281979e+03;
        Cam_Mat.at<float>(0, 2) = 1.9195000000000000e+03;
        Cam_Mat.at<float>(2, 0) = 1.3735000000000000e+03;

        //distortion coefficients
        cv::Mat distCoeffs(5, 1, cv::DataType<double>::type);
        distCoeffs.at<double>(0) = -2.2522257464640633e-01;
        distCoeffs.at<double>(1) = -3.3113981023075709e+00;
        distCoeffs.at<double>(2) = 0;
        distCoeffs.at<double>(3) = 0;
        distCoeffs.at<double>(4) = 1.5899309942518832e+02;

        //output vectors
        Mat rvecs(3, 1, cv::DataType<double>::type);
        Mat tvecs(3, 1, cv::DataType<double>::type);

        //create object point containers with real world square size of chessboard
        for (int i = 0; i < board_height; ++i)
            for (int j = 0; j < board_width; ++j)
                object_points.push_back(Point3f(j * squaresize, i * squaresize, 0));

        //find chessboard corners
        bool found = cv::findChessboardCorners(image, patternsize, corners);
        if (found) {
            std::cout << "Chessboard detected\n";
            cv::cornerSubPix(image, corners, Size(11, 11), Size(-1, -1), criteria);
        }
        else {
            cout << "Chessboard not detected" << endl;
        }

        for (int i = 0; i < corners.size(); i++) {
            cout << corners.at(i).x << ","<< corners.at(i).y << endl;
        }

        drawChessboardCorners(image, patternsize, Mat(corners), found);
        //projectPoints();
        if (save_img) {
            string output_filename = out_dir + out_name;
            cout << "-:" << output_filename << endl;
            cv::imwrite(output_filename, image);
        }
        if (found) {
            cv::solvePnP(object_points, corners, Cam_Mat, distCoeffs, rvecs, tvecs);

            cout << "Rvecs :\n";
            for (int i = 0; i < rvecs.rows; i++) {
                cout << rvecs.at<double>(i, 0) << endl;
            }

            Mat Rot(3, 3, cv::DataType<double>::type);
            cv::Rodrigues(rvecs, Rot, noArray());

            cout << "Rotation Mat 3X3 :\n";
            for (int i = 0; i < 3; i++) {
                for (int j = 0; j < 3; j++) {
                    cout << Rot.at<double>(i, j) << ",";
                }
                cout << endl;
            }

            cv::Vec3f EulerAngles = rotationMatrixToEulerAngles(Rot);

            cout << "Tvecs :\n";
            for (int i = 0; i < tvecs.rows; i++) {
                cout << tvecs.at<double>(i, 0) << endl;
            }

        }
}

可能的问题点 #

1. 欧拉角转换与内参矩阵错误 #

• 手动定义的pi精度不足（仅3.14159），角度转换时会累积误差；
• 欧拉角旋转顺序与物理测量的定义不匹配，且未明确坐标系变换方向（OpenCV的solvePnP输出的是世界到相机的旋转）；
• 内参矩阵设置错误：第三行(2,0)被赋值为1.3735e+03，正确内参矩阵第三行应为[0,0,1]，该错误会直接破坏投影计算逻辑。

2. 相机标定参数准确性 #

硬编码的内参和畸变系数如果不是当前相机的精准标定结果，或者标定过程中图像数量不足、姿态覆盖不全，会直接影响位姿估计精度。

3. 角点检测精度不足 #

• 玻璃反光会干扰角点检测，导致亚像素定位偏移；
• cornerSubPix的窗口大小（11x11）可能不适配工业相机高分辨率，影响亚像素精度；
• 仅用单一姿态图像计算位姿，无多姿态平均，误差无法被抵消。

4. 物理测量基准误差 #

假设的45度夹角可能存在物理测量误差，棋盘格平面或玻璃的形变也会导致实际角度与预期不符。

优化方案 #

1. 修正核心计算逻辑 #

• 替换手动pi为标准库M_PI（需包含<cmath>），提升角度转换精度；
• 修正内参矩阵，正确格式如下：

Mat Cam_Mat = Mat::eye(3, 3, CV_32F);
Cam_Mat.at<float>(0, 0) = 5.1863196755281979e+03; // fx
Cam_Mat.at<float>(1, 1) = 5.1863196755281979e+03; // fy
Cam_Mat.at<float>(0, 2) = 1.9195000000000000e+03; // cx
Cam_Mat.at<float>(1, 2) = 1.3735000000000000e+03; // cy
// 第三行保持默认的[0,0,1]

• 明确欧拉角旋转顺序，可使用cv::decomposeProjectionMatrix辅助验证旋转矩阵正确性，避免手动转换出错。

2. 重新标定相机 #

• 拍摄至少20张不同姿态的棋盘格图像（覆盖视野不同位置、角度）；
• 使用cv::calibrateCamera计算内参和畸变系数，确保重投影误差低于0.5像素；
• 用cv::undistort验证畸变校正效果，确认参数准确。

3. 提升角点检测精度 #

• 使用偏振镜消除玻璃反光，或调整光照角度；
• 将cornerSubPix窗口大小改为Size(5,5)或Size(7,7)，适配高分辨率图像；
• 增加CALIB_CB_FILTER_QUADS标志，过滤错误的棋盘格四边形；
• 采集多姿态棋盘格图像，取位姿估计的平均值降低单帧误差。

4. 验证物理测量基准 #

用高精度角度工具（如角度尺、激光测距仪）重新确认棋盘格与相机的夹角；检查棋盘格平面和玻璃的平整度，必要时更换载体。

内容的提问来源于stack exchange，提问作者Dushyant Patil