OpenCV: Get 3D coordinates from triangulatePoints

3
votes

I' trying to get cloud of points from two stereo images using OpenCV, but I can't get coordinates.

  1. I found points coordinates using Optical Flow
  2. I found projection matrixes for cameras.
Mat RT1;<br>
hconcat(R, T1, RT1);<br>
Mat P1 = C*RT1;<br>


R is 3x3 rotation matrix, T is 3x1 transform matrix (column), P1 - projection matrix.

  1. I pass them to triangulatePoints function
triangulatePoints(P1, P2, leftPoints, rightPoints, out);

P1 and P2 is 3x4 projection matrix (Mat_<double>). leftPoints and rightPoints is std::vector of Point2f. What is out? It should be 1xN matrix of 4D coords. Is this Vec4f?

I am trying get coordinates

for (int i = 0; i < out.cols; i++)
{
  Vec4f vec = out.at<Vec4f>(0, i);
  float w = vec[3];
  stream << "v " << vec[0] / w << " " << vec[1]/w << " " << vec[2]/w << "\n";
}

But I have two problems:

  • This cycle throw exception (works for small i, about 20% of out.cols)

OpenCV Error: Assertion failed (dims <= 2 && data && (unsigned)i0 < (unsigned)si ze.p[0] && (unsigned)(i1 * DataType<_Tp>::channels) < (unsigned)(size.p1 * cha nnels()) && ((((sizeof(size_t)<<28)|0x8442211) >> ((DataType<_Tp>::depth) & ((1 << 3) - 1))*4) & 15) == elemSize1()) in cv::Mat::at, file c:\opencv\build\includ e\opencv2\core\mat.inl.hpp, line 89

I think this is some kind of Index Out Range Exception

  • Result is very strange: Image

So, I am doing something wrong. How correctly work with this function and get 3D coordinates of points? I'll hope you can help me.

1
c++opencv

1 Answers

0
votes

I don't understand exactly your approach in getting the coordinates.

As far as I see, you should not be accessing elements like 

out.at<Vec4f>(0, i);

Instead, do it like this:

float x = out.at<float>(0, i);
float y = out.at<float>(1, i);
float z = out.at<float>(2, i);
float w = out.at<float>(3, i);
stream << "v " << x << " " << y << " " << z << "\n";

Or use double... depending whether you out is of type CV_32F or CV_64F.

Here is how I do it:

Mat points3DHomogeneous;
triangulatePoints(projectionMatrixL, projectionMatrixR, pointsL, pointsR, points3DHomogeneous);

projectionMatrixL:

enter image description here

projectionMatrixR:

enter image description here

pointsL:

    700 250
    200 300
    600 350
    400 400
    500 450
    600 500
    700 550
    800 600
    150 650
    1000 700

pointsR:

    690 250
    180 300
    590 350
    385 400
    495 450
    575 500
    691 550
    782 600
    120 650
    960 700

points3DHomogeneous is the result:

enter image description here