计算旋转和平移矩阵从三维点和他们的二维对应

Question

我有一组3d点(P3)，它们的2d对应点(P2)和一个相机矩阵(a)。我如何使用SVD来找到旋转和平移向量?我想方程是P2 = A*[R|t]*P3。然而，我如何使用SVD来找到rvec和tvec(说在openCV中使用cvSVD)?一个简短的算法或链接将非常有帮助。

Answer 1

如果您知道或猜测相机矩阵A(以及可选的失真系数)，最简单的方法是使用函数cv::solvePnP(文档链接)或其鲁棒版本cv::solvePnPRansac(文档链接)。

如果你不知道相机矩阵，我认为你不能估计旋转矩阵R和平移向量t。但是，您可以使用直接线性变换(DLT)算法估计A*R和A*t，这在Hartley's &Zisserman的书，§7.1 p178。如果你表示P = A*[R | t]，那么你可以估计P如下:

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

之后，一个好主意是执行迭代优化(例如使用Levenberg-Marquardt算法)，以最小化重投影误差。