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

Question

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

Answer 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 算法)，以最小化重投影误差。