直接线性变换法
直接线性变换法(DLT)通常都为
x
1
=
M
x
2
\bold{x_{1}}=\bold{Mx_{2}}
x1=Mx2形式,在不同问题中我们要求解的内容不一样。在PnP问题中,我们已知3D点坐标和对应的2D投影,去求矩阵M。在线性三角化中,我们已知变换矩阵和2D投影坐标去求3D点坐标。今天先写下线性三角化吧,PnP挖个坑,以后有时间再写。
代码
/**
* @brief 给定投影矩阵P1,P2和图像上的点kp1,kp2,从而恢复3D坐标
*
* @param kp1 特征点, in reference frame
* @param kp2 特征点, in current frame
* @param P1 投影矩阵P1
* @param P2 投影矩阵P2
* @param x3D 三维点
* @see Multiple View Geometry in Computer Vision - 12.2 Linear triangulation methods p312
*/
void Initializer::Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D)
{
// 在DecomposeE函数和ReconstructH函数中对t有归一化
// 这里三角化过程中恢复的3D点深度取决于 t 的尺度,
// 但是这里恢复的3D点并没有决定单目整个SLAM过程的尺度
// 因为CreateInitialMapMonocular函数对3D点深度会缩放,然后反过来对 t 有改变
cv::Mat A(4,4,CV_32F);
A.row(0) = kp1.pt.x*P1.row(2)-P1.row(0);
A.row(1) = kp1.pt.y*P1.row(2)-P1.row(1);
A.row(2) = kp2.pt.x*P2.row(2)-P2.row(0);
A.row(3) = kp2.pt.y*P2.row(2)-P2.row(1);
cv::Mat u,w,vt;
cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
x3D = vt.row(3).t();
x3D = x3D.rowRange(0,3)/x3D.at<float>(3);
}