Revision history [back]

Triangulate from stereo fisheye cameras?

I am trying to triangulate points from a calibrated pair of fisheye cameras.

I have a calibration that looks pretty good (checked with viewing rectified images).

I have my P1 and P2 matrices from the stereo calibration process. I have my matched orb points in left and right frames (checked with cv::drawMatches).

i run the following:

std::vector<cv::Point2f> pntsMatchedLeft;
std::vector<cv::Point2f> pntsMatchedRight;

DetectKeypointsOrb(left, d1, kp1);
DetectKeypointsOrb(right, d2, kp2);


std::vector<cv::DMatch> matches;
matches = RobustMatching(d1, d2, kp1, kp2);

//check matches by drawing them.


for (int x = 0; x < matches.size(); x++)
    {
        cv::Point2f pnt2dL = kp1[matches[x].queryIdx].pt;
        pntsMatchedLeft.push_back(pnt2dL);

        cv::Point2f pnt2dR = kp1[matches[x].trainIdx].pt;
        pntsMatchedRight.push_back(pnt2dR);

    }

std::vector<cv::Point2f> pntsMatchedLeftUD;
std::vector<cv::Point2f> pntsMatchedRightUD;


cv::fisheye::undistortPoints(pntsMatchedLeft, pntsMatchedLeftUD, K1, D1);
cv::fisheye::undistortPoints(pntsMatchedRight, pntsMatchedRightUD, K2, D2);

cv::Mat point3d_homo;
cv::triangulatePoints(P1,P2,
        pntsMatchedLeftUD, pntsMatchedRightUD,
        point3d_homo);

assert(point3d_homo.cols == triangulation_points1.size());

// create point cloud
std::vector< Eigen::Vector3f> cloud;
for (int i = 0; i < point3d_homo.cols; i++) {
    Eigen::Vector3f point;
    cv::Mat p3d;
    cv::Mat _p3h = point3d_homo.col(i);
        convertPointsFromHomogeneous(_p3h.t(), p3d);
        point.x() = p3d.at<double>(0);
        point.y() = p3d.at<double>(1);
        point.z() = p3d.at<double>(2);

        cloud.push_back(point);

    }

But the points that i get are, for example:

1.65398e+22 0 0
3.68707e+12 0 0
5.02082e+15 1.42875e-27 0
2.26795e+17 0 0
-3.87201e+15 0 0
-4.601e+24 0 0
-1.35347e+20 0 0
3.52328e+15 -1.29677e-25 0
4.1635e+12 -1.29677e-25 0
-1.08779e+15 -1.29677e-25 0
1-5.17322e+24 0 0

(I would expect no zeros)

What am I doing wrong here? Do i need to undistort the points as I am doing? Skipping that step seems to make minimal difference.

Thank you!