从 3d 点及其 2d 对应关系计算旋转和平移矩阵答案

【问题标题】：Computing rotation and translation matrix from 3d points and their 2d correspondences从 3d 点及其 2d 对应关系计算旋转和平移矩阵
【发布时间】：2014-05-11 08:34:07
【问题描述】：

我有一组 3d 点 (P3)、它们的 2d 对应关系 (P2) 和一个相机矩阵 (A)。如何使用 SVD 找到旋转和平移向量？我认为等式是 P2 = A*[R|t]*P3。但是，我如何使用 SVD 来查找 rvec 和 tvec（比如在 openCV 中使用 cvSVD）？一个简短的算法或链接会很有帮助。

【问题讨论】：

标签： opencv computer-vision camera-calibration

【解决方案1】：

如果您知道或猜到相机矩阵A（以及可选的失真系数），最简单的方法是使用函数cv::solvePnP (doc link) 或其强大的版本cv::solvePnPRansac (doc link) .

如果你不知道相机矩阵，我认为你无法估计旋转矩阵R和平移向量t。但是，您可以使用直接线性变换 (DLT) 算法估计 A*R 和 A*t，该算法在 Hartley 和 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 算法），以最小化重投影误差。

【讨论】：

对于二维点，三个坐标代表 [x,y,w]。但是，对于 3D 点，四个坐标是否代表 [x,y,z,w]？
@user2672886 是的。