Hi, I am trying to triangulate points of tracked marker by using calibration board near it,. This is how it is done:
cv::solvePnPRansac(constants::calibration::rig.rig3DPoints, corners1, camMat, distortion,
rvecPNP1, tvecPNP1, false, 200, 5.0, 0.999);
cv::solvePnPRansac(constants::calibration::rig.rig3DPoints, 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:
point: [-0.0113968, -0.0236187, -0.000169321]
point: [-0.0113949, -0.0236159, -0.000168893]
point: [-0.011395, -0.0236219, -0.000169062]
point: [-0.0113946, -0.0236289, -0.000169148]
point: [-0.0113922, -0.0236095, -0.000168429]
point: [-0.0113952, -0.0236172, -0.000168985]
point: [-0.0113946, -0.0236198, -0.000168924]
point: [-0.0113956, -0.0236198, -0.000169066]
point: [-0.0113944, -0.0236077, -0.000168686]
point: [-0.0113954, -0.0236207, -0.000169061]
point: [-0.0113957, -0.0236164, -0.00016904]
point: [-0.0113953, -0.0236187, -0.000169051]
point: [-0.0113926, -0.0236058, -0.000168453]
point: [-0.0113951, -0.0236117, -0.000168868]
point: [-0.0113946, -0.0236115, -0.000168785]
point: [-0.0113956, -0.0236141, -0.00016902]
point: [-0.0113964, -0.0236195, -0.000169284]
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, I mean they too close to zero, may be I am doing something wrong?