# Position and Rotation of two cameras. Which function's I need? In what order?

I would like to compute the relative position and Rotation of two cameras with openCv. I use two images with dots. Here are the two images.

I know the horizontal value (X) and vertical value (Y) of each points in the two images.

I would like to compute the relative position and rotation of the two cameras.

I tested with Blender if that's possible. With motion tracking Blender was able to compute the relative position and rotation. It takes 8 points or more. The result is very exact.

Here is my Blender test 3D View.

I found many openCv functions. But I do not know which function's I need. stereoCalibrate ? findFundamentalMat?

Which function's I need? In what order?

edit retag close merge delete

Sort by » oldest newest most voted

Do you have the (relative) 3d position of your points? Or only the projections into the two images? If you have them, have a look at solvePNP.

more

I answer to the post of @Nepolo:

UPDATE 2015.07.06 -----UPDATE 2015.07.06------- UPDATE 2015.07.06

Let alpha, beta and gamma the yaw, pitch and roll angles as defined here (slide 50, Bernard BAYLE), also called Tait-Bryan angles Z1Y2X3:

The final rotation matrix can be calculated as the the matrix resulting of the multiplication of the 3 rotation matrices about each axis:

Your resulting rotation matrix is decomposed as:

  cout <<  Qx.t()<<endl;
//RESULT
//[1, 0, 0;
// 0, 0.9999999980133975, 6.303336619882133e-05;
// 0, -6.303336619882133e-05, 0.9999999980133975]

cout <<  Qy.t()<<endl;
//RESULT
//[0.9999999997416501, 0, 2.273102883446428e-05;
//0, 1, 0;
//-2.273102883446428e-05, 0, 0.9999999997416501]

cout <<  Qz.t()<<endl;
//RESULT
//[0.985944918764345, -0.1670706951047037, 0;
//0.1670706951047037, 0.985944918764345, 0;
//0, 0, 1]


We have Qx == identity (more or less), Qy == identity (more or less) and alpha_angle=atan2(0.1670706951047037, 0.985944918764345) but you cannot end up with:

That should be the correct values Roll 5° Pitch 0° Yaw -45°

Some test code:

#include <iostream>
#include <opencv2/opencv.hpp>

cv::Mat rotationMatrixX(const double angle) {
cv::Mat Rx = (cv::Mat_<double>(3, 3) << 1.0, 0.0, 0.0,
0.0, cos(angle), -sin(angle),
0.0, sin(angle), cos(angle));
return Rx;
}

cv::Mat rotationMatrixY(const double angle) {
cv::Mat Ry = (cv::Mat_<double>(3, 3) << cos(angle), 0.0, sin(angle),
0.0, 1.0, 0.0,
-sin(angle), 0.0, cos(angle));
return Ry;
}

cv::Mat rotationMatrixZ(const double angle) {
cv::Mat Rz = (cv::Mat_<double>(3, 3) << cos(angle), -sin(angle), 0.0,
sin(angle), cos(angle), 0.0,
0.0, 0.0, 1.0);
return Rz;
}

cv::Mat yawPitchRollToRotationMatrix(const double roll, const double pitch, const double yaw) {
cv::Mat Rx = rotationMatrixX(roll);
cv::Mat Ry = rotationMatrixY(pitch);
cv::Mat Rz = rotationMatrixZ(yaw);

return Rz*Ry*Rx;
}

}

return deg*CV_PI/180.0;
}

int main(int argc, char **argv) {

cv::Mat R = yawPitchRollToRotationMatrix(roll, pitch, yaw);
std::cout << "R=" << R << std::endl;

cv::Mat mtxR,mtxQ;
cv::Mat Qx,Qy,Qz;
cv::Vec3d e;

e= RQDecomp3x3(R, mtxR,mtxQ,Qx, Qy, Qz);
std::cout << "Euler angles (Roll-Pitch yaw angles) in degree=" << e << std::endl;

return 0;
}


and the corresponding output:

R=[0.7071067811865476, 0.7044160264027586, -0.06162841671621935;
-0.7071067811865475, 0.7044160264027587, -0.06162841671621935;
0, 0.08715574274765817, 0.9961946980917455]
Euler angles (Roll-Pitch-Yaw angles) in degree=[5, 0, -45]

more

Thank you very much. After long time I could continue testing the code. That was a great help.

( 2015-08-20 16:22:41 -0500 )edit

Do you have the (relative) 3d position of your points.

I don’t know the (relative) 3d position of the points. I know the two images, the focal length and the sensor size of the cameras.

I tested with Blender if that's possible. Blender motion tracking can compute the relative position and rotation of the two cameras. Blender needs 8 points in both images. The result is very exact.

It is possible. But how?

I have found the openCv function findFundamentalMat.

findFundamentalMat also requires min 8 points* in both images. This is the same rule as in Blender.

*CV_FM_8POINT for an 8-point algorithm. N>=8

CV_FM_RANSAC for the RANSAC algorithm. N>=8

CV_FM_LMEDS for the LMedS algorithm. N>=8

And I found the function stereoCalibrate.

## UPDATE 2015.06.29 -----UPDATE 2015.06.29------- UPDATE 2015.06.29

Why 8 Points? As LBerger said, every pair of points gives you a constraint in the form of p2'Fp1 = 0. Why you need 8 points is not that obvious.

The animation programm Blender can compute the points and position of the camera. Blender say that you needs min 8 points.

I take a look at the function computeCorrespondEpilines. Here is the result. Image 2 looks like very good.

Image 1

Image 2

I have copied the image into Blender.

Here is the result.

But what happens next? How can I get the following values? X, Y, Z, Roll, Pitch and Yaw

Here is my code.

#include <QCoreApplication>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <vector>
#include <iostream>
using namespace cv;
using namespace std;

Mat image1;
Mat image2;
Mat  F;

int main(int argc, char *argv[])
{
QCoreApplication a(argc, argv);

// Open image 1
QString fileName = "cam1c.jpg";

// Open image 2
QString fileName2 = "cam2c.jpg";

// this are the points
// I added the point values manually (it is a test)
vector<Point2f> image1_points;
vector<Point2f> image2_points;
image1_points.push_back(Point(403.83,299.63));
image2_points.push_back(Point(401.38 ,300.03));

image1_points.push_back(Point(311.5,388.5));
image2_points.push_back(Point(310.45 ,378.28));

image1_points.push_back(Point(741.9,72.08));
image2_points.push_back(Point(567.58 ,160.20));

image1_points.push_back(Point(488.45,211.58));
image2_points.push_back(Point(397.43 ,237.73));

image1_points.push_back(Point(250.6,200.43));
image2_points.push_back(Point(314.95 ,229.7));

image1_points.push_back(Point(171,529.08));
image2_points.push_back(Point(359.9 ,477.5));

image1_points.push_back(Point(400,227.75));
image2_points.push_back(Point(272.78 ,251.90));

image1_points.push_back(Point(513.95,414));
image2_points.push_back(Point(508.15 ,361.03));

image1_points.push_back(Point(280.68,140.9));
image2_points.push_back(Point(223.55 ,178.93));

image1_points.push_back(Point(479.58,220.48));
image2_points.push_back(Point(355.98 ,244.63));

image1_points.push_back(Point(621.95,122.48));
image2_points.push_back(Point(454.78 ...
more

If in first image a point is at coordinate p1 and in second image at p2 then you have p2'Fp1 where F is fondamental Matrix. F give you epipolar line.

( 2015-06-20 14:23:21 -0500 )edit

Why 8 Points? As LBerger said, every pair of points gives you a constraint in the form of p2'Fp1 = 0. Why you need 8 points is not that obvious. But https://en.wikipedia.org/wiki/Eight-p... and this nice song: https://www.youtube.com/watch?v=DgGV3... help a bit.

( 2015-06-22 01:37:24 -0500 )edit
1

I think that you need to compute first the Essential matrix from the Fundamental matrix as: E = transpose(K')FK, with K and K' the intrinsic matrix.

After, you have to extract the camera motion from the essential matrix, and you can look at these links:

( 2015-06-29 12:12:23 -0500 )edit

Using @FooBar answer my program is and I hope it's good.

#include <opencv2/opencv.hpp>
#include <iostream>
#include <map>
#include <fstream>

using namespace cv;
using namespace std;

double PI = acos(-1.0);

int main(int argc, char** argv)
{
RNG rng(0xFFFFFFFF);
vector< Mat > point(8);
vector< Point3d > p3d(point.size()); // Real point in world coordinate
vector< Point2d > p2d(point.size()); // point in image coordinate p2d=A[R|t]p3d  http://docs.opencv.org/doc/tutorials/calib3d/camera_calibration/camera_calibration.html

// Real point randomly place in cube center at (10.5,10.5,10.5) with width 1
for (int i = 0; i < point.size(); i++)
{

Mat_<double> v(4, 1);
v(0, 0) = 10 + rng.uniform((double)0, (double)1);
v(1, 0) = 10 + rng.uniform((double)0, (double)1);
v(2, 0) = 10 + rng.uniform((double)0, (double)1);
v(3,0) = 1;
point[i] = v;
p3d[i].x = v(0, 0);
p3d[i].y = v(1, 0);
p3d[i].z = v(2, 0);
}
Mat_<double> mInt(3, 3);
// Intrinsic parameter of webcam c270
mInt(0, 0) = 4520; mInt(0, 1) = 0; mInt(0, 2) = 600;
mInt(1, 0) = 0; mInt(1, 1) = 5490; mInt(1, 2) = 440;
mInt(2, 0) = 0; mInt(2, 1) = 0; mInt(2, 2) = 1;
// Camera rotation  (1.2rd,0.9rd,0.7rd)
Mat_<double> angle(3, 1);
angle(0, 0) = 1.2; angle(1, 0) = 0.9; angle(2, 0) = 0.7;
Mat r;
Mat rt = Mat::eye(3, 4, CV_64F);
// Camera translation  (2.4,4.8,0)
rt.at<double>(0, 3) = 2.4;
rt.at<double>(1, 3) = 4.8;
rt.at<double>(2, 3) = 0;
// make matrix rotation
Rodrigues(angle, r);

for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
rt.at<double>(i, j) = r.at<double>(i, j);

Mat art;
// Mat product intrinsic and extrinsic parameter
gemm(mInt,rt,1,Mat(),0,art);
cout << art<<endl;
vector< Mat > pImg(point.size());
for (int i = 0; i < point.size(); i++)
{
gemm(art, point[i], 1, Mat(), 0, pImg[i]);
// Homogenous coordinate
pImg[i]=pImg[i] / pImg[i].at<double>(2, 0);
p2d[i] = Point2d(pImg[i].at<double>(0, 0), pImg[i].at<double>(1, 0));
cout << point[i] << "\t" << pImg[i]  << endl;
}
// p3d real point- p2d image point what are values of camera orientation and translation
Mat rvec, tvec;
solvePnP(p3d, p2d, mInt, Mat(), rvec, tvec);
cout << "Rvec camera = " << rvec << endl;
cout << "Tvec camera= " << tvec << endl;
return 0;


}

more

Official site

GitHub

Wiki

Documentation