从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.17页第178页中进行了解释。如果你表示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算法）来进行迭代优化，以尽量减少投影误差。