使用Charuco板在高分辨率图像下姿态估计异常求助

我正在开展一个需要高精度姿态估计的项目，此前使用2x2规格的Aruco板无法满足精度需求，因此改用OpenCV的Charuco板方案。目前在RealSense D415相机的640x480分辨率下能正常实现姿态估计，但切换到1280x720分辨率后，绘制在板上的坐标系开始完全随机地跳动。

姿态估计核心代码 #

void ReconstructionSystem::detect_charuco_markers(cv::Mat& image, cv::Matx33f& matrix, cv::Vec<float, 5>& coef, int& centerPix_x, int& centerPix_y, cv::Vec3d& rotation, bool& arucoFound)
{
    cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50);
    cv::Ptr<cv::aruco::CharucoBoard> board = cv::aruco::CharucoBoard::create(3, 3, 0.04f, 0.02f, dictionary);
    cv::Ptr<cv::aruco::DetectorParameters> params = cv::aruco::DetectorParameters::create();
    //params->cornerRefinementMethod = cv::aruco::CORNER_REFINE_NONE;

    std::vector<int> markerIds;
    std::vector<std::vector<cv::Point2f>> markerCorners;
    cv::Mat copyImage;
    image.copyTo(copyImage);
    cv::Mat gray;
    cv::cvtColor(copyImage, gray, cv::COLOR_RGB2GRAY);
    cv::aruco::detectMarkers(gray, board->getDictionary(), markerCorners, markerIds, params);
    // if at least one marker detected
    if (markerIds.size() > 3) {
        cv::aruco::drawDetectedMarkers(image, markerCorners, markerIds);
        std::vector<cv::Point2f> charucoCorners;
        std::vector<int> charucoIds;
        cv::aruco::interpolateCornersCharuco(markerCorners, markerIds, gray, board, charucoCorners, charucoIds, matrix, coef);
        // if at least one charuco corner detected
        if (charucoIds.size() > 3) {
            cv::Scalar color = cv::Scalar(255, 0, 0);
            cv::aruco::drawDetectedCornersCharuco(image, charucoCorners, charucoIds, color);
            cv::Vec3d rvec, tvec;
            bool valid = cv::aruco::estimatePoseCharucoBoard(charucoCorners, charucoIds, board, matrix, coef, rvec, tvec);
            // if charuco pose is valid
            if (valid){
                cv::drawFrameAxes(image, matrix, coef, rvec, tvec, 0.1f);
                arucoFound = true;
            }
            else
            {
                arucoFound = false;
            }
        }
        else
        {
            arucoFound = false;
        }
    }
    else
    {
        arucoFound = false;
    }
    board = NULL;
    dictionary = NULL;
    copyImage.release();
    gray.release();
}

主循环调用代码 #

//Variables for transformation matrices
int centerPix_x = 0, centerPix_y = 0;
cv::Vec3d rotationVec;
cv::Matx33f rotation;
bool arucoWasFound = false;
std::vector<float> final_x, final_y, final_z;
std::vector<float> rotation_x, rotation_y, rotation_z;
cv::Matx33f matrix = get_cameraMatrix(path);
cv::Vec<float, 5> coef = get_distCoeffs(path);


const auto window_name = "Validation image";
cv::namedWindow(window_name, cv::WINDOW_AUTOSIZE);

// TODO Also add here that if we have iterated through X frames and not found Aruco, exit with failure
while (cv::waitKey(1) < 0 && cv::getWindowProperty(window_name, cv::WND_PROP_AUTOSIZE) >= 0 && counter < 60) {
    rs2::frame f = sensorPtr->color_data.wait_for_frame();
    // Query frame size (width and height)
    const int w = f.as<rs2::video_frame>().get_width();
    const int h = f.as<rs2::video_frame>().get_height();
    cv::Mat image(cv::Size(w, h), CV_8UC3, (void*)f.get_data(), cv::Mat::AUTO_STEP);
    cv::cvtColor(image, image, cv::COLOR_RGB2BGR);
    //detect_aruco_markers(image, matrix, coef, centerPix_x, centerPix_y, rotationVec, arucoWasFound);
    detect_charuco_markers(image, matrix, coef, centerPix_x, centerPix_y, rotationVec, arucoWasFound);

    if (arucoWasFound)
    {
        rs2::depth_frame depth = sensorPtr->depth_data.wait_for_frame();
        rs2_intrinsics intrinsic = rs2::video_stream_profile(depth.get_profile()).get_intrinsics();
        float pixel_distance_in_meters = depth.get_distance(centerPix_x, centerPix_y);
        float InputPixelAsFloat[2];
        InputPixelAsFloat[0] = centerPix_x;
        InputPixelAsFloat[1] = centerPix_y;
        float finalDepthPoint[3];
        rs2_deproject_pixel_to_point(finalDepthPoint, &intrinsic, InputPixelAsFloat, pixel_distance_in_meters);

        // Postion //
        final_x.push_back(finalDepthPoint[0]);
        final_y.push_back(finalDepthPoint[1]);
        final_z.push_back(finalDepthPoint[2]);

        // Rotation //
        rotation_x.push_back(rotationVec[0]);
        rotation_y.push_back(rotationVec[1]);
        rotation_z.push_back(rotationVec[2]);

        counter++;
    }
    cv::imshow(window_name, image);
}
cv::destroyWindow(window_name);

分辨率对比效果 #

1280x720分辨率检测图像

640x480分辨率检测图像

问题排查与解决方案 #

1. 相机内参不匹配（核心原因） #

不同分辨率对应不同的相机内参，若当前加载的是640x480的标定参数，在1280x720下会直接导致姿态估计偏差。

• 解决方法：使用OpenCV重新标定RealSense D415在1280x720分辨率下的相机矩阵和畸变系数，替换现有get_cameraMatrix(path)和get_distCoeffs(path)返回的参数。

2. 角点检测精度不足 #

高分辨率下默认的角点检测参数可能无法精准定位标记角点，导致后续姿态计算跳变：

• 解决方法：启用亚像素角点细化，修改DetectorParameters：

cv::Ptr<cv::aruco::DetectorParameters> params = cv::aruco::DetectorParameters::create();
params->cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX; // 替换原注释的NONE
params->cornerRefinementWinSize = 5; // 可根据实际调整窗口大小

3. Charuco板参数与实物不符 #

代码中定义的Charuco板为3x3方块（0.04m）、标记尺寸0.02m，若实物尺寸存在误差，高分辨率下会被放大，引发姿态跳变。

• 解决方法：重新测量实物板的精确尺寸，确保代码中参数与实物完全一致；若打印存在缩放误差，重新打印符合参数的Charuco板。

4. RGB与深度帧不同步 #

代码中在检测到Charuco后才获取深度帧，可能导致RGB帧和深度帧时序错位，干扰姿态稳定性：

• 解决方法：使用RealSense的rs2::syncer同步RGB和深度流，确保处理的是同一时刻的帧数据。

5. 姿态估计鲁棒性不足 #

当前仅要求4个Charuco角点就进行姿态估计，高分辨率下可能存在误检测角点，导致姿态异常：

• 解决方法：提高有效角点数量阈值，比如将charucoIds.size() > 3改为charucoIds.size() > 5；同时对连续帧的rvec/tvec进行加权平均，过滤异常跳变值。

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