三角化求三维坐标 python

1.  利用2张影像上的匹配点确定特征点在三维模型中的位置

ORB-SLAM里面用的Triangulate函数可以求解（C++代码）

void Initializer::Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D)
{
    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);
}

其中kp1,kp2为特征点在原始图像中的坐标，P1,P2为影像对应的投影矩阵，x3D即为求解的三维坐标

其中投影矩阵P坐标通过R（旋转矩阵）+ 平移 矩阵t 计算得到。t通过旋转矩阵R和相机中心矩阵计算得到。参考一下公式。

P=K[R|t], t=-RC

自己重新用python实现了：

# To get the 3D position(X,Y,Z) from stereo images 
import os
import cv2
import xml.dom.minidom
fro