问在高分辨率图像上使用Charuco时的错误姿态估计

EN

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);