Ask Your Question

recoverPose and triangulatePoints - 3D results are incorrect

asked 2018-09-06 19:51:29 -0500

maym86 gravatar image

updated 2018-09-07 17:08:09 -0500

Hello I am having some trouble with triangulatePoints and the resulting 3D points are coming out incorrectly. I am using Opencv 3.4.2 on ubuntu 16.04. This is the function that I use to triangulate from two vectors of cv::Poin2f containing matches (I have verified that these matches are correct and make sense between the images).

std::vector<cv::Point3d> triangulate(const cv::Point2f &pp, double focal, const std::vector<cv::Point2f> &points0, const std::vector<cv::Point2f> &points1, const cv::Mat &P0, const cv::Mat &P1){

    cv::Mat p_mat0(2, static_cast<int>(points0.size()), CV_64F);
    cv::Mat p_mat1(2, static_cast<int>(points1.size()), CV_64F);

    for (int i = 0; i < p_mat0.cols; i++) {<double>(0, i) = (points0[i].x  - pp.x) / focal;<double>(1, i) = (points0[i].y  - pp.y) / focal;<double>(0, i) = (points1[i].x  - pp.x) / focal;<double>(1, i) = (points1[i].y  - pp.y) / focal;

    cv::Mat points_4d;
    cv::triangulatePoints(P0, P1, p_mat0, p_mat1, points_4d);
    std::vector<cv::Point3d> results;
    for (int i = 0; i < points4d.cols; i++) {
        results.emplace_back(cv::Point3d(<double>(0, i) /<double>(3, i),
          <double>(1, i) /<double>(3, i),
          <double>(2, i) /<double>(3, i)));
    return results;

The input are the features tracked between two frames and the P0 and P1 are generated from findEssentialMatrix and recoverPose between the two sets of points. I know that points0 and points1 are reversed for the first two functions and switching them doesn't make a difference.

E = cv::findEssentialMat(points1, points0, focal_, pp_, cv::RANSAC, 0.999, 1.0, mask);
int res = recoverPose(E, points1, points0, R, t, focal_, pp_, mask);
cv::Mat P;
hconcat(R, t, P);
triangulate(pp, focal, points0, points1, cv::Mat::eye(3, 4, CV_64FC1), P);

The tracked features are as follows:

image description

The resultant P mat for this example is as follows:

[0.999994351093655, 0.00216084437549141, -0.002574593630994021, -0.08315501607210679;
-0.002166127328107902, 0.9999955507641086, -0.002050937440673792, 0.02514760517749096;
0.002570150419386047, 0.002056502752743189, 0.9999945825469503, 0.9962192736821971];

The result looks like this where the plots at the bottom are the x and y and x and z coordinates of the result, centered on the red cross:

image description

Plotted with this loop:

for (int j = 0; j < points_3d.size(); j++) {
     cv::Point2d draw_pos = cv::Point2d(points_3d[j].x * scale + drawXY.cols / 2,
                      points_3d[j].y * scale + drawXY.rows / 2);
     cv::circle(drawXY, draw_pos, 1, cv::Scalar(0, 255, 0), 1);

Any idea what the issue is, I have been struggling with this for a few days.

edit retag flag offensive close merge delete


Camera coordinates are typically described with +Z being out of the camera. What that means, is your points should be stretched out along the axis you compressed for that picture. Try using the VIZ module and rotate it around, see if that helps.

Tetragramm gravatar imageTetragramm ( 2018-09-06 19:58:34 -0500 )edit

I have plotted in viz too. The result is the same. It's not a plotting issue.

maym86 gravatar imagemaym86 ( 2018-09-06 19:59:37 -0500 )edit

I have updated the post to include a 2d plot of the x and z coordinates.

maym86 gravatar imagemaym86 ( 2018-09-06 20:14:25 -0500 )edit

Did you plot them in 3D in viz? You can use that to draw the camera positions as well, which will help with the visualizing.

What I think is going on is that some of the points triangulated to behind the camera, which you should filter out. That would leave two "arms" of the cloud, that are the points down the street, some of which are slightly mis-matched and appear farther away than they should.

Tetragramm gravatar imageTetragramm ( 2018-09-07 17:39:28 -0500 )edit

1 answer

Sort by ยป oldest newest most voted

answered 2018-09-07 18:16:19 -0500

maym86 gravatar image

updated 2018-09-08 16:45:45 -0500

Filtering made all the difference. Once I removed all the points marked by the mask from recoverPose, the negative z points and the far away points (cv::norm(point - origin) > 200 ) then things improved. After rescaling by 10x the result looks as expected. Thanks for your help.

edit flag offensive delete link more

Question Tools



Asked: 2018-09-06 19:43:40 -0500

Seen: 1,451 times

Last updated: Sep 08 '18