triangulatePoints: why calculated data is wrong

asked 2018-09-21 21:01:13 -0500

Michael G. gravatar image

updated 2018-09-22 05:00:50 -0500

Hi, I am trying to triangulate points of tracked marker by using calibration board near it. This is how it is done:

    cv::solvePnPRansac(objpoints, corners1, camMat, distortion, 
                       rvecPNP1, tvecPNP1, false, 200, 5.0, 0.999);
    cv::solvePnPRansac(objpoints, corners2, camMat, distortion, 
                       rvecPNP2, tvecPNP2,  false, 200, 5.0, 0.999);

    Mat rodriguesRPNP1, rodriguesRPNP2;
    cv::Rodrigues(rvecPNP1, rodriguesRPNP1);
    cv::Rodrigues(rvecPNP2, rodriguesRPNP2);

    // relative pose by rotation and translation composition
    rvecPNP = rodriguesRPNP1.inv() * rodriguesRPNP2;
    tvecPNP = rodriguesRPNP2 * tvecPNP2 - rodriguesRPNP1 * tvecPNP1;

    cv::Mat P1 = Mat::eye(cv::Size(4, 3), CV_64F);
    cv::Mat P2;
    cv::hconcat(rvecPNP, tvecPNP, P2);

    Mat F = findFundamentalMat(coords3, coords4, cv::FM_RANSAC, 3, 0.999);

    cv::undistortPoints(coords3, coords3, camMat, distortion);
    cv::undistortPoints(coords4, coords4, camMat, distortion);

    std::vector<Point2f> coords5, coords6;
    cv::correctMatches(F, coords3, coords4, coords5, coords6);

    std::vector<cv::Point3d> points3d = triangulate(coords5, coords6, P1, P2);

And triangulate() looks like this:

std::vector<cv::Point3d> triangulate(const std::vector<cv::Point2f> &coords0,
                                 const std::vector<cv::Point2f> &coords1, 
                                 const cv::Mat &P0, const cv::Mat &P1) {

    cv::Mat points4d;
    cv::triangulatePoints(P0, P1, coords0, coords1, points4d);

    std::vector<cv::Point3d> results;

    for (int i = 0, cols = points4d.cols; i < cols; i++) {

        cv::Point3d point = cv::Point3d(points4d.at<float>(0, i) / points4d.at<float>(3, i),
                                    points4d.at<float>(1, i) / points4d.at<float>(3, i),
                                   points4d.at<float>(2, i) / points4d.at<float>(3, i));
        results.emplace_back(point);
    }
    return results;
}

And this gives me triangulated points:

 [INFO] point: [0.17739, -0.0417149, -0.569071]
 [INFO] point: [0.177434, -0.0365413, -0.569185]
 [INFO] point: [0.179331, -0.046826, -0.568601]
 [INFO] point: [0.309903, -0.0841421, -0.753341]
 [INFO] point: [0.310883, -0.0879948, -0.752346]
 [INFO] point: [0.192887, -0.0294726, -0.614926]
 [INFO] point: [0.212949, -0.033606, -0.678697]
 [INFO] point: [0.212837, -0.0347488, -0.678065]
 [INFO] point: [0.216449, -0.0652773, -0.690147]
 [INFO] point: [0.262796, -0.0612305, -0.712385]
 [INFO] point: [0.266117, -0.0210713, -0.707122]
 [INFO] point: [0.284981, -0.0762475, -0.735736]

The thing is that calibrating rig's Z value is set to zero, and Z for the points must be from -3 to -7 cm, this points even not close, may be I am doing something wrong? Also x values are wrong ang this depend on the run, values looks like random. If you take a look on screenshot, and wonder what is green square (near red one), it's just the old scaled position of the marker.

Couldn't upload image on site with chrome, it can be found here https://postimg.cc/mtdLw5nk

edit retag flag offensive close merge delete

Comments

Data are, not "data is".

sjhalayka gravatar imagesjhalayka ( 2018-09-21 22:07:55 -0500 )edit

Thanks :) Though I didn't find where I've used this form..

Michael G. gravatar imageMichael G. ( 2018-09-22 04:02:24 -0500 )edit