RGB相机与CubeEyeS100D ToF相机立体标定偏移问题求助

我正在进行CubeEyeS100D ToF相机与标准USB RGB相机的立体标定，目标是将RGB图像叠加到ToF数据生成的点云上。目前遇到的问题是标定输出图像错误或像素偏移过大。我使用Windows系统，基于C++、CMake和OpenCV实现标定，程序输出的校正后图像存在明显偏移。

以下是我的实现代码：

void stereoCalibration()
{
    //Load in calibration images (paths need to be correct!) (RGB is on the right):
    std::vector<cv::String> fileNames_left, fileNames_right;
    cv::glob("../../../TestIntrin/right/", fileNames_right, false);
    cv::glob("../../../TestIntrin/left/", fileNames_left, false);
    //Initialise size of chessboard (needs to be correct!):
    const cv::Size BoardSize{ 6, 4 };
    std::vector<std::vector<cv::Point2f>> imagePoints_left(fileNames_left.size());
    std::vector<std::vector<cv::Point2f>> imagePoints_right(fileNames_right.size());
    //create vectors to contain corners
    std::vector<std::vector<cv::Point2f>> corners_left(fileNames_left.size());
    std::vector<std::vector<cv::Point2f>> corners_right(fileNames_right.size());

    cv::Mat img_left, img_right, gray_left, gray_right;
    try {
        if (fileNames_left.size() != fileNames_right.size()) {
            throw std::runtime_error("Vectors are not of the same size.");
        }
        if (fileNames_left.empty() || fileNames_right.empty() || fileNames_left.back() == "" || fileNames_right.back() == "") {
            throw std::runtime_error("Last element in one or both vectors is not filled.");
        }
    }
    catch (const std::runtime_error& e) {
        std::cerr << e.what() << std::endl;
        return;
    }

    // Detect feature points
    for (int i = 0; i < fileNames_left.size(); ++i) {
        // 1. Read in the image an call cv::findChessboardCorners()
        img_left = cv::imread(fileNames_left[i]);
        img_right = cv::imread(fileNames_right[i]);
        //std::cout << "RGB res: " << img_right.size() << " " << img_right.type() <<  " Tof res: " << img_left.size() << " " << img_left.type() << std::endl;
        cv::cvtColor(img_left, gray_left, cv::COLOR_BGR2GRAY);
        cv::cvtColor(img_right, gray_right, cv::COLOR_BGR2GRAY);

        bool found_left = cv::findChessboardCorners(gray_left, BoardSize, corners_left[i], cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_FILTER_QUADS);
        bool found_right = cv::findChessboardCorners(gray_right, BoardSize, corners_right[i], cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_FILTER_QUADS);

        // 2. Use cv::cornerSubPix() to refine the found corner detections
        if (found_left && found_right) {
            cv::cornerSubPix(gray_left, corners_left[i], cv::Size(5, 5), cv::Size(-1, -1), cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 30, 0.1));
            cv::cornerSubPix(gray_right, corners_right[i], cv::Size(5, 5), cv::Size(-1, -1), cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 30, 0.1));
        }
        else if (!found_left) {
            std::cout << "Didnt find left " << corners_left[i] << std::endl;
        }
        else if (!found_right) {
            std::cout << "Didn't find right " << corners_right[i] << std::endl;
        }
        else {
            std::cout << "DRATS" << std::endl;
        }

        // Display (Can be removed after later but is good for quality assurance)
        //cv::drawChessboardCorners(img_left, BoardSize, corners_left[i], found_left);
        //cv::drawChessboardCorners(img_right, BoardSize, corners_right[i], found_right);
        //cv::imshow("chessboard detection left", img_left);
        //cv::imshow("chessboard detection right", img_right);
        //cv::waitKey(0);
    }

    std::vector<std::vector<cv::Point3f>> object_points;
    // 3. Generate checkerboard (world) coordinates Q. The board has 7 x 5
   // fields with a size of 30x30mm
    int square_size = 30;
    std::vector<cv::Point3f> objp;
    for (int i = 0; i < BoardSize.height; i++) {
        for (int j = 0; j < BoardSize.width; j++) {
            objp.push_back(cv::Point3f(j * square_size, i * square_size, 0));
        }
    }

    // Puts objp objects into the vector object_points.
    for (auto v : corners_left) {
        object_points.push_back(objp);
    }

    std::vector<std::vector<cv::Point2f>> left_img_points, right_img_points;
    for (int i = 0; i < corners_left.size(); i++) {
        std::vector<cv::Point2f> v1, v2;
        for (int j = 0; j < corners_left[i].size(); j++) {
            v1.push_back(cv::Point2f((double)corners_left[i][j].x, (double)corners_left[i][j].y));
            v2.push_back(cv::Point2f((double)corners_right[i][j].x, (double)corners_right[i][j].y));
        }
        left_img_points.push_back(v1);
        right_img_points.push_back(v2);
    }

    // Load in the camera matrices and the distortion coefficients
    cv::Mat tmpr, tmpl;
    cv::Vec<float, 5> k_left, k_right;
    cv::FileStorage fileKr("../../../TestIntrin/Config/RGBIntrinsic.xml", cv::FileStorage::READ);
    fileKr["K"] >> tmpr;
    fileKr.release();
    cv::FileStorage filekr("../../../TestIntrin/Config/RGBDistortion.xml", cv::FileStorage::READ);
    filekr["k"] >> k_right;
    filekr.release();
    cv::FileStorage fileKl("../../../TestIntrin/Config/ToFIntrinsic.xml", cv::FileStorage::READ);
    fileKl["K"] >> tmpl;
    fileKl.release();
    cv::FileStorage filekl("../../../TestIntrin/Config/ToFDistortion.xml", cv::FileStorage::READ);
    filekl["k"] >> k_left;
    filekl.release();
    cv::Matx33f K_right(tmpr);
    cv::Matx33f K_left(tmpl);

    cv::Matx33f newK_right = cv::getOptimalNewCameraMatrix(K_right, k_right, img_right.size(), 1, img_right.size());
    cv::Matx33f newK_left = cv::getOptimalNewCameraMatrix(K_left, k_left, img_left.size(), 1, img_left.size());


    //Stereo calibrate
    cv::Mat R, F, E;
    cv::Vec3d T;
    int flag = 0;
    flag |= cv::CALIB_FIX_INTRINSIC;
    cv::TermCriteria criteria_stereo = cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 30, 0.001);
    cv::stereoCalibrate(object_points, left_img_points, right_img_points, newK_left, k_left, newK_right, k_right, img_left.size(), R, T, E, F, flag, cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 30, 0.001));

    //stereo rectify
    cv::Mat R1, R2, P1, P2, Q;
    cv::stereoRectify(newK_left, k_left, newK_right, k_right, img_right.size(), R, T, R1, R2, P1, P2, Q);

    std::cout << "R: " << R << "\n T: " << T << "\n R1: " << R1 << "\n R2: " << R2 << "\n P1: " << P1 << "\n P2: " << P2 << "\n Q: " << Q << std::endl;

    //Create maps to rectify images with
    cv::Mat lmapx, lmapy, rmapx, rmapy;
    cv::Mat imgU1, imgU2;

    cv::initUndistortRectifyMap(newK_left, k_left, R1, P1, img_left.size(), CV_32F, lmapx, lmapy);
    cv::initUndistortRectifyMap(newK_right, k_right, R2, P2, img_right.size(), CV_32F, rmapx, rmapy);
    cv::remap(img_left, imgU1, lmapx, lmapy, cv::INTER_LANCZOS4, cv::BORDER_CONSTANT, 0);
    cv::remap(img_right, imgU2, rmapx, rmapy, cv::INTER_LANCZOS4, cv::BORDER_CONSTANT, 0);
    cv::Mat concat;
    cv::hconcat(imgU1, imgU2, concat);
    //Show undistorted images
    cv::imshow("rectified left", imgU1);
    cv::imshow("rectified right", imgU2);
    cv::imshow("Concat", concat);
    cv::waitKey(0);

    cv::FileStorage fileRGB("../../../TestIntrin/Config/RGBStereo.xml", cv::FileStorage::WRITE);
    fileRGB << "mapx" << rmapx;
    fileRGB << "mapy" << rmapy;
    cv::FileStorage fileToF("../../../TestIntrin/Config/ToFStereo.xml", cv::FileStorage::WRITE);
    fileToF << "mapx" << lmapx;
    fileToF << "mapy" << lmapy;
}

我已完成两台相机的内参标定并保存了相机矩阵与畸变系数，采集了30组同时包含棋盘格的图像。该代码在双RGB相机标定中运行正常，推测ToF与RGB相机的标定存在遗漏点，无法上传标定相关文件。

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