Computing rotation and translation matrix from 3d points and their 2d correspondences

Question

I have a set of 3d points(P3), their 2d correspondences(P2) and a camera matrix(A). How do I use SVD to find the rotation and translation vectors? I think the equation is P2 = A*[R|t]*P3. However, how do I use SVD to find the rvec and tvec (say using cvSVD in openCV)? A brief algorithm or a link would be very helpful.

Answer 1

If you know or guessed the camera matrix A (and optionally the distortion coefficients), the simplest approach is to use the function cv::solvePnP (doc link) or its robust version cv::solvePnPRansac (doc link).

If you do not know the camera matrix, I don't think you can estimate the rotation matrix R and translation vector t. However, you can estimate A*R and A*t using the Direct Linear Transform (DLT) algorithm, which is explained in Hartley's & Zisserman's book in §7.1 p178. If you denote P = A*[R | t], then you can estimate P as follows:

cv::Mat_<double> pts_world(npoints,4), pts_image(npoints,3);
// [...] fill pts_world & pts_image
cv::Mat_<double> C = cv::Mat_<double>::zeros(3*npoints,12);
for(int r=0; r<npoints; ++r)
{
    cv::Mat_<double> pt_world_t = pts_world.row(r);
    double x = pts_image.at<double>(r,0);
    double y = pts_image.at<double>(r,1);
    double w = pts_image.at<double>(r,2);
    C.row(3*r+0).colRange(4,8) = -w*pt_world_t;
    C.row(3*r+0).colRange(8,12) = y*pt_world_t;
    C.row(3*r+1).colRange(0,4) = w*pt_world_t;
    C.row(3*r+1).colRange(8,12) = -x*pt_world_t;
    C.row(3*r+2).colRange(0,4) = -y*pt_world_t;
    C.row(3*r+2).colRange(4,8) = x*pt_world_t;
}
cv::Mat_<double> P;
cv::SVD::solveZ(C,P); // P is a 12x1 column vector
P = P.reshape(1,3); // Reshape P to be a standard 3x4 projection matrix

After that, a good idea would be to perform an iterative optimization (e.g. using Levenberg-Marquardt algorithm), in order to minimize the reprojection error.