test transform between two cameras with projectPoints? [closed]

edit

I have a stereo calibrated camera pair, so i have known intrinsic and extrinsics.

I have a chessboard visible in both cameras.

What i want to do is:

Find the chessboard in both frames. Run SolvePnP from camera A.

Using the stereo calibration extrinsics, project the 3d points from camera B, using the extrinsics as the camera position.

Use the distance between the projected points on camera B frame, and the chessboard points in camera B frame to assess the extrinsic validity.

I have the following function, which displays the points, but they seem much further off than i would expect from the extrinsic values. Is this the correct approach here?

void main()
{

    //build object points
    int grid_size_x = 7;
    int grid_size_y = 5;
    double squareSize = 4.5;

    std::vector<cv::Point3f> object_points_;

    //set up nboard
    for (int y = 0; y < grid_size_y; y++)
    {
        for (int x = 0; x < grid_size_x; x++)
        {
            object_points_.push_back(cv::Point3f(x * squareSize, y * squareSize, 1));
        }
    }


    cv::Mat camMatrixB, distCoeffsB, camMatrixA, distCoeffsA, offsetT, offsetR;

    bool readOk = readCameraParameters("CalibrationData.xml", camMatrixB, distCoeffsB, camMatrixA, distCoeffsA, offsetT, offsetR);
    if (!readOk) {
        cerr << "Invalid camera file" << endl;
    }



    //solvepnp from A frame to get board position.

    std::vector<cv::Point2f> corners_z;

    cv::Mat imagez = cv::imread("FrameA.jpg");
    cv::Mat imageGrayz;
    cv::cvtColor(imagez, imageGrayz, cv::COLOR_BGR2GRAY);
    cv::Mat cv_translationA, cv_rotationA;

    //find board in image A
    bool found_chessboardA = cv::findChessboardCorners(imageGrayz, cv::Size(grid_size_x, grid_size_y), corners_z, 0);

    if (found_chessboardA)
    {

        cv::cornerSubPix(imageGrayz, corners_z, cv::Size(3, 3), cv::Size(-1, -1), cv::TermCriteria(cv::TermCriteria::MAX_ITER + cv::TermCriteria::EPS, 50, 0.01));

        //solve for board pose in A
        cv::solvePnP(object_points_, corners_z, camMatrixA, distCoeffsA, cv_rotationA, cv_translationA);


        //set the B pose to be the calculated offset amount, relative to the A pose.
        cv::Mat offsetPos = cv::Mat(1, 3, CV_64F);
        offsetPos.at<double>(0,0) = cv_translationA.at<double>(0, 0) - (offsetT.at<double>(0,0) /10);
        offsetPos.at<double>(0, 1) = cv_translationA.at<double>(0, 1) - (offsetT.at<double>(0,1) / 10);
        offsetPos.at<double>(0, 2) = cv_translationA.at<double>(0, 2) - (offsetT.at<double>(0, 2) / 10);


        cv::Mat offsetR2;
        cv::Rodrigues(offsetR, offsetR2);
        cv:Mat offsetRot = cv::Mat(1, 3, CV_64F);
        offsetRot.at<double>(0, 0) = cv_rotationA.at<double>(0, 0) - offsetR2.at<double>(0, 0);
        offsetRot.at<double>(0, 1) = cv_rotationA.at<double>(0, 1) - offsetR2.at<double>(0, 1);
        offsetRot.at<double>(0, 2) = cv_rotationA.at<double>(0, 2) - offsetR2.at<double>(0,2);




        //reproject the object points and look at error.
        cv::Mat imageSD = cv::imread("FrameB.jpg");
        cv::Mat imageGray;
        cv::cvtColor(imageSD, imageGray, cv::COLOR_BGR2GRAY);
        cv::Mat cv_translationD, cv_rotationD;
        std::vector<cv::Point2f> corners_;

        //find board in image
        bool found_chessboardB = cv::findChessboardCorners(imageGray, cv::Size(grid_size_x, grid_size_y), corners_, 0);

        if (found_chessboardB)
        {
            cv::cornerSubPix(imageGray, corners_, cv::Size(3, 3), cv::Size(-1, -1), cv::TermCriteria(cv::TermCriteria::MAX_ITER + cv::TermCriteria::EPS, 50, 0.01));

            //test repro
            std::vector<cv::Point2f> projectedPointsSD;
            cv::projectPoints(object_points_, offsetRot, offsetPos, camMatrixB, distCoeffsB, projectedPointsSD);

            for (int p = 0; p < projectedPointsSD.size(); p++)
            {
                cv::drawMarker(imageSD, projectedPointsSD[p], cv::Scalar(255, 255, 255), cv::MARKER_TILTED_CROSS, 8, 1, 8);
            }
            for (int p = 0 ...