OpenCV: get 3D coordinates from triangulated points
I'm trying to get a point cloud from two stereo images using OpenCV, but I can't get the coordinates.
- I found the coordinates of the points using Optical Flow
- I found projection matrices for cameras.
Mat RT1;<br>
hconcat(R, T1, RT1);<br>
Mat P1 = C*RT1;<br>
R is a 3x3 rotation matrix, T is a 3x1 transformation matrix (column), P1 is a projection matrix.
- I am passing them to the triangulatePoints function
triangulatePoints(P1, P2, leftPoints, rightPoints, out);
P1 and P2 - 3x4 projection matrix (Mat_ <double>). leftPoints and rightPoints std :: vector for Point2f. What's wrong? This should be a 1xN matrix of 4D coordinates. Is it Vec4f?
I am trying to 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 is a loop exception (works for small i, around 20% out.cols)
OpenCV error: assertion failed (dims <= 2 && & & (unsigned) i0 <(unsigned) si ze.p [0] && (unsigned) (i1 * DataType <_Tp> :: channels) <(without sign) (size.p 1 * cha nnels ()) && & && & (((sizeof (size_t) <28) | 0x8442211) → ((DataType <_Tp> :: depth) and ((1 <3) - 1 )) * 4) and 15) == elemSize1 ()) in cv :: Mat :: at, file c: \ opencv \ build \ includes e \ opencv2 \ core \ mat.inl.hpp, line 89
I think this is some kind of index range exception
- The result is very strange: Image
So, I am doing something wrong. How to work correctly with this function and get 3D coordinates of points? Hope you can help me.
source to share
I don't understand exactly your approach to getting coordinates.
As far as I can see, you shouldn't be accessing elements like
out.at<Vec4f>(0, i);
Instead, do the following:
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 on whether you are of out
type CV_32F
or CV_64F
.
This is how I do it:
Mat points3DHomogeneous;
triangulatePoints(projectionMatrixL, projectionMatrixR, pointsL, pointsR, points3DHomogeneous);
projectionMatrixL
:
projectionMatrixR
:
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
- result:
source to share