ORBSLAM中的三角化—理论与实现

《slam十四讲》一书中有解释三角化，结论就是有了对极约束就可以三角化将地图点恢复出来。但是在ORBSLAM中三角化的代码部分，所用到的方法要更高级。 

理论—— 

所涉及到的理论参见《Multi View Geometry in Computer Vision》第321页。这一页如下，解释的很简练，你可以再参照书中的其他内容帮助理解。  

大致意思就是，得到A矩阵，解AX=0，X即3D点位置。 求解AX=0可以用SVD方法。   

 

 

实现——

/**
 * @brief 给定投影矩阵P1,P2和图像上的点kp1,kp2，从而恢复3D坐标
 *
 * @param kp1 特征点, in reference frame
 * @param kp2 特征点, in current frame
 * @param P1  投影矩阵P1
 * @param P2  投影矩阵P２
 * @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 &am