OpenCV Q&A Forum - RSS feedhttp://answers.opencv.org/questions/OpenCV answersenCopyright <a href="http://www.opencv.org">OpenCV foundation</a>, 2012-2018.Mon, 19 Sep 2016 01:15:28 -0500Stereosystem triangulate pointshttp://answers.opencv.org/question/102128/stereosystem-triangulate-points/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++):
**1.**
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));
}
**2.**
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),
CV_CALIB_USE_INTRINSIC_GUESS + CV_CALIB_ZERO_TANGENT_DIST
);
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);
}
**3.**
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;
**4.**
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](/upfiles/14736090564625072.png)
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.Sun, 11 Sep 2016 10:57:10 -0500http://answers.opencv.org/question/102128/stereosystem-triangulate-points/Answer by TiBe for <p>Hi guys,</p>
<p>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.</p>
<p>I tried it on this way (OpenCV, C++):</p>
<p><strong>1.</strong> <br/>
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</p>
<pre><code>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));
}
</code></pre>
<p><strong>2.</strong> <br/>
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.</p>
<pre><code>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),
CV_CALIB_USE_INTRINSIC_GUESS + CV_CALIB_ZERO_TANGENT_DIST
);
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);
}
</code></pre>
<p><strong>3.</strong> <br/>
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).</p>
<pre><code>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;
</code></pre>
<p><strong>4.</strong> <br/>
As last step I triangulate the images points</p>
<pre><code>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);
</code></pre>
<p>Thats the Stereo-System, Image is from Matlab</p>
<p><img alt="image description" src="/upfiles/14736090564625072.png"/></p>
<h2>Now my question:</h2>
<p>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.</p>
<p>My results look good because the calculated 3D points are the same as the object points from 1 </p>
<p>The Z-Value is near to 0, I tkink the chessboard is probably the origin.
<strong>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.</strong></p>
<p>Do you know what I mean?</p>
<p>I hope someone can help me....thanks.</p>
http://answers.opencv.org/question/102128/stereosystem-triangulate-points/?answer=102534#post-id-102534I have a new problem...as 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**
![image description](/upfiles/14742860045725933.png)
Can anyone help me? Mon, 19 Sep 2016 01:15:28 -0500http://answers.opencv.org/question/102128/stereosystem-triangulate-points/?answer=102534#post-id-102534Answer by Tetragramm for <p>Hi guys,</p>
<p>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.</p>
<p>I tried it on this way (OpenCV, C++):</p>
<p><strong>1.</strong> <br/>
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</p>
<pre><code>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));
}
</code></pre>
<p><strong>2.</strong> <br/>
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.</p>
<pre><code>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),
CV_CALIB_USE_INTRINSIC_GUESS + CV_CALIB_ZERO_TANGENT_DIST
);
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);
}
</code></pre>
<p><strong>3.</strong> <br/>
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).</p>
<pre><code>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;
</code></pre>
<p><strong>4.</strong> <br/>
As last step I triangulate the images points</p>
<pre><code>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);
</code></pre>
<p>Thats the Stereo-System, Image is from Matlab</p>
<p><img alt="image description" src="/upfiles/14736090564625072.png"/></p>
<h2>Now my question:</h2>
<p>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.</p>
<p>My results look good because the calculated 3D points are the same as the object points from 1 </p>
<p>The Z-Value is near to 0, I tkink the chessboard is probably the origin.
<strong>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.</strong></p>
<p>Do you know what I mean?</p>
<p>I hope someone can help me....thanks.</p>
http://answers.opencv.org/question/102128/stereosystem-triangulate-points/?answer=102134#post-id-102134 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](http://docs.opencv.org/3.1.0/d9/d0c/group__calib3d.html#ga044b03d81f99370db05c52c3d0b46599).
Give that a try and see if it helps.Sun, 11 Sep 2016 16:50:06 -0500http://answers.opencv.org/question/102128/stereosystem-triangulate-points/?answer=102134#post-id-102134Comment by TiBe for <p>So you need to invert one of the rotation/translation matrices first.</p>
<pre><code>Rodrigues(rvec1, R);
R = R.t();
t = (-R * tvec1);
</code></pre>
<p>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).</p>
<p>Fortunately, OpenCV has a function for that, <a href="http://docs.opencv.org/3.1.0/d9/d0c/group__calib3d.html#ga044b03d81f99370db05c52c3d0b46599">composeRT</a>.</p>
<p>Give that a try and see if it helps.</p>
http://answers.opencv.org/question/102128/stereosystem-triangulate-points/?comment=102145#post-id-102145Thanks 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 3x3Mon, 12 Sep 2016 01:09:50 -0500http://answers.opencv.org/question/102128/stereosystem-triangulate-points/?comment=102145#post-id-102145Comment by TiBe for <p>So you need to invert one of the rotation/translation matrices first.</p>
<pre><code>Rodrigues(rvec1, R);
R = R.t();
t = (-R * tvec1);
</code></pre>
<p>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).</p>
<p>Fortunately, OpenCV has a function for that, <a href="http://docs.opencv.org/3.1.0/d9/d0c/group__calib3d.html#ga044b03d81f99370db05c52c3d0b46599">composeRT</a>.</p>
<p>Give that a try and see if it helps.</p>
http://answers.opencv.org/question/102128/stereosystem-triangulate-points/?comment=102146#post-id-102146And 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?Mon, 12 Sep 2016 01:11:28 -0500http://answers.opencv.org/question/102128/stereosystem-triangulate-points/?comment=102146#post-id-102146Comment by Tetragramm for <p>So you need to invert one of the rotation/translation matrices first.</p>
<pre><code>Rodrigues(rvec1, R);
R = R.t();
t = (-R * tvec1);
</code></pre>
<p>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).</p>
<p>Fortunately, OpenCV has a function for that, <a href="http://docs.opencv.org/3.1.0/d9/d0c/group__calib3d.html#ga044b03d81f99370db05c52c3d0b46599">composeRT</a>.</p>
<p>Give that a try and see if it helps.</p>
http://answers.opencv.org/question/102128/stereosystem-triangulate-points/?comment=102222#post-id-102222You 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.Mon, 12 Sep 2016 21:01:35 -0500http://answers.opencv.org/question/102128/stereosystem-triangulate-points/?comment=102222#post-id-102222Comment by TiBe for <p>So you need to invert one of the rotation/translation matrices first.</p>
<pre><code>Rodrigues(rvec1, R);
R = R.t();
t = (-R * tvec1);
</code></pre>
<p>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).</p>
<p>Fortunately, OpenCV has a function for that, <a href="http://docs.opencv.org/3.1.0/d9/d0c/group__calib3d.html#ga044b03d81f99370db05c52c3d0b46599">composeRT</a>.</p>
<p>Give that a try and see if it helps.</p>
http://answers.opencv.org/question/102128/stereosystem-triangulate-points/?comment=102255#post-id-102255Okay, I will give it a try...thank youTue, 13 Sep 2016 12:52:08 -0500http://answers.opencv.org/question/102128/stereosystem-triangulate-points/?comment=102255#post-id-102255