Ask Your Question

Stereosystem triangulate points

asked 2016-09-11 10:57:10 -0600

TiBe gravatar image

Hi guys,

currently I am working on a program that takes 3d coordinates from non rectified stereo images with triangulate. First I give you a short insight into my program.

I tried it on this way (OpenCV, C++):

Calibrate the cameras with several chessboard images from various positions. I create the 3D points of my chessboard in its own coordinate system as follow

vector<Point3f>  obj;
for (int j = 0; j<sChess.board_n; j++)
    obj.push_back(Point3f(0.02377*(j % sChess.board_w), 0.02377*(j / sChess.board_w), 0));  

To get the camera intrinsic I use stereoCalibrate and the extrinsic I calculate with solvePnP. So i get the rotation and translation vector of each camera.

double rms3 = stereoCalibrate(xObjektPoints, xImageCam1, xImageCam2,
    intrinsic, distCoeffs[0],
    intrinsic2, distCoeffs[1],
    Size(3000, 4000), R, T, E, F,
    TermCriteria(CV_TERMCRIT_ITER + CV_TERMCRIT_EPS, 100, 1e-5),

for (int i=0; i<xObjektPoints.size(); i++)
    solvePnP(xObjektPoints[i], xImageCam1[i], intrinsic, distCoeffs[0], rVec1, tVec1);
    solvePnP(xObjektPoints[i], xImageCam2[i], intrinsic2, distCoeffs[1], rVec2, tVec2);

Next I create two Projection Matrices. I convert the rotation vector with Rodrigues to a rotation matrix and chain the rotation matrix with the translation vector to a 3x4 matrix. Last but not least I multiply both 3x4 matrices with the respective camera matrix (intrinsic).

Rodrigues(rVec1, mRot1); // mRot1 is 3x3
Rodrigues(rVec2, mRot2); // mRot2 is 3x3

Mat rt_a, rt_b;
hconcat(mRot1, tVec1, rt_a);  
hconcat(mRot2, tVec2, rt_b); 

Mat mProjection1, mProjection2;
mProjection1 = intrinsic * rt_a;
mProjection2 = intrinsic2* rt_b;

As last step I triangulate the images points

double ImagePoints1[3] = { points1[i].x, points1[i].y, 1 };
double ImagePoints2[3] = { points2[i].x, points2[i].y, 1 };

        Mat mPoint1 = Mat(3, 1, CV_64FC1, ImagePoints1);
        Mat mPoint2 = Mat(3, 1, CV_64FC1, ImagePoints2);

        Mat test = triangulate_Linear_LS(mP1, mP2, mPoint1, mPoint2);

Thats the Stereo-System, Image is from Matlab

image description

Now my question:

To test the program I use the first chessboard images from the stereosystem, because I know all 3D coordinates (object points from 1), find the corners and triangulate them.

My results look good because the calculated 3D points are the same as the object points from 1

The Z-Value is near to 0, I tkink the chessboard is probably the origin. But I want that the z value reflects the distance from the stereo camera system to object for example the first corner top left X: 0 ; Y: 0 ; Z: 64.

Do you know what I mean?

I hope someone can help me....thanks.

edit retag flag offensive close merge delete

2 answers

Sort by ยป oldest newest most voted

answered 2016-09-11 16:50:06 -0600

Tetragramm gravatar image

So you need to invert one of the rotation/translation matrices first.

Rodrigues(rvec1, R);
R = R.t();
t = (-R * tvec1);

That is the code to do that. The result there is the rotation and translation from the camera to the chessboard. If you chain that with the rotation and translation from the chessboard to the second camera, you have it's location and orientation with respect to the first camera (which is now all zeros in rvec and tvec).

Fortunately, OpenCV has a function for that, composeRT.

Give that a try and see if it helps.

edit flag offensive delete link more


Thanks for your help. But I think I'm doing something wrong. You say I need to invert one of the rotation/translation matrices

for (int i=0; i<xObjektPoints.size(); i++)
    solvePnPRansac(xObjektPoints[i], xImageCam1[i], intrinsic, distCoeffs[0], rVec1, tVec1);
    solvePnPRansac(xObjektPoints[i], xImageCam2[i], intrinsic2, distCoeffs[1], rVec2, tVec2);

Rodrigues(rVec1, mRot1); // R is 3x3
mRot1 = mRot1.t();  // rotation of inverse
tVec1 = -mRot1 * tVec1; // translation of inverse
Rodrigues(mRot1, rVec1);

To chain them I do this

composeRT(rVec1, tVec1, rVec2, tVec2, rVec3, tVec3);
Rodrigues(rVec3, mRot2); // R is 3x3
TiBe gravatar imageTiBe ( 2016-09-12 01:09:50 -0600 )edit

And this...the variables mProjection1, mProjection2 I hand over to triangulate

hconcat(mRot1, tVec1, rt_a);  
hconcat(mRot2, tVec3, rt_b);

Mat mProjection1, mProjection2;
mProjection1 = intrinsic * rt_a;
mProjection2 = intrinsic2 * rt_b;

Unfortunately that was wrong, can you help me?

TiBe gravatar imageTiBe ( 2016-09-12 01:11:28 -0600 )edit

You need to remember that you are setting the first camera to (0,0,0) with no rotation. So once you have chained them together to get the location of camera 2, you set rVec1 to zero and tvec1 to zero, and use those to calculate your projection matrix for camera 1.

Tetragramm gravatar imageTetragramm ( 2016-09-12 21:01:35 -0600 )edit

Okay, I will give it a try...thank you

TiBe gravatar imageTiBe ( 2016-09-13 12:52:08 -0600 )edit

answered 2016-09-19 01:15:28 -0600

TiBe gravatar image

updated 2016-09-19 07:02:25 -0600

I have a new you can see, I create my projection matrix as described above.

The z value of the triangulation is not good. As you can see the z value is constantly increasing although I triangulate the reference chessboard. Normally, the Z value would still be 0


image description

Can anyone help me?

edit flag offensive delete link more

Question Tools

1 follower


Asked: 2016-09-11 10:57:10 -0600

Seen: 2,808 times

Last updated: Sep 19 '16