Get translation and rotation matrix of an object

转自：https://groups.google.com/forum/#!topic/android-opencv/0ImAjpAL5t4

cvFindExtrinsicCameraParams2 is the old C name of the function. In C++ and Java API it is named solvePnP

Here is a usage example from Java API tests:

Mat intrinsics = Mat.eye(3, 3, CvType.CV_32F);
intrinsics.put(0, 0, 400);
intrinsics.put(1, 1, 400);
intrinsics.put(0, 2, 640 / 2);
intrinsics.put(1, 2, 480 / 2);

List<Point3> points3d = new ArrayList<Point3>();
List<Point> points2d = new ArrayList<Point>();
int minPnpPointsNum = 4;

for (int i = 0; i < minPnpPointsNum; i++) {
    double x = Math.random() * 100 - 50;
    double y = Math.random() * 100 - 50;
    points2d.add(new Point(x, y));
    points3d.add(new Point3(0, y, x));
}

Mat rvec = new Mat();
Mat tvec = new Mat();
Calib3d.solvePnP(points3d, points2d, intrinsics, new Mat(), rvec, tvec);

Mat truth_rvec = new Mat(3, 1, CvType.CV_64F);
truth_rvec.put(0, 0, 0, Math.PI / 2, 0);

Mat truth_tvec = new Mat(3, 1, CvType.CV_64F);
truth_tvec.put(0, 0, -320, -240, 400);

assertMatEqual(truth_rvec, rvec, EPS);
assertMatEqual(truth_tvec, tvec, EPS);