三角化求三维坐标 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
from numpy import *
import numpy as np

def getInt_para(xmlfile):
	dom = xml.dom.minidom.parse(xmlfile)
	#得到文档元素对象
	root = dom.documentElement
	# ImageWidth = float(root.getElementsByTagName('Width')[0].childNodes[0].data)
	FocalLengthPixels = float(root.getElementsByTagName('FocalLengthPixels')[0].childNodes[0].data)
	# SensorSize = float(root.getElementsByTagName('SensorSize')[0].childNodes[0].data)
	# f = ImageWidth*FocalLength/SensorSize
	f = FocalLengthPixels
	PrincipalPoint = root.getElementsByTagName('PrincipalPoint')[0]
	x0 = float(PrincipalPoint.getElementsByTagName('x')[0].childNodes[0].data)
	y0 = float(PrincipalPoint.getElementsByTagName('y')[0].childNodes[0].data)
	# 相机内参
	K = mat([[f,0,x0],[0,f,y0],[0,0,1]]).reshape(3,3)
	return K, x0, y0

def getExt_para(xmlfile, imgname):
	dom = xml.dom.minidom.parse(xmlfile)
	#得到文档元素对象
	root = dom.documentElement
	ImagePathlist = root.getElementsByTagName('ImagePath')

	for i, Path in enumerate(ImagePathlist):
		ImagePath = Path.childNodes[0].data
		ImageName = ImagePath.split('\\')[-1]

		if ImageName == imgname:
			M_00 = float(root.getElementsByTagName('M_00')[i].childNodes[0].data)
			M_01 = float(root.getElementsByTagName('M_01')[i].childNodes[0].data)
			M_02 = float(root.getElementsByTagName('M_02')[i].childNodes[0].data)
			M_10 = float(root.getElementsByTagName('M_10')[i].childNodes[0].data)
			M_11 = float(root.getElementsByTagName('M_11')[i].childNodes[0].data)
			M_12 = float(root.getElementsByTagName('M_12')[i].childNodes[0].data)
			M_20 = float(root.getElementsByTagName('M_20')[i].childNodes[0].data)
			M_21 = float(root.getElementsByTagName('M_21')[i].childNodes[0].data)
			M_22 = float(root.getElementsByTagName('M_22')[i].childNodes[0].data)
			# 旋转矩阵
			R = mat([[M_00,M_01,M_02],[M_10,M_11,M_12],[M_20,M_21,M_22]]).reshape(3,3)

			x = float(root.getElementsByTagName('x')[i+1].childNodes[0].data)
			y = float(root.getElementsByTagName('y')[i+1].childNodes[0].data)
			z = float(root.getElementsByTagName('z')[i].childNodes[0].data)
			# 相机中心		
			C_center=mat([x,y,z]).reshape(3,1)
	return R, C_center

def calculate_3DX(kp1, kp2, Proj1, Proj2):
	A0 = mat(kp1[0] * Proj1[2,:] - Proj1[0,:])
	A1 = mat(kp1[1] * Proj1[2,:] - Proj1[1,:])
	A2 = mat(kp2[0] * Proj2[2,:] - Proj2[0,:])
	A3 = mat(kp2[1] * Proj2[2,:] - Proj2[1,:])
	train_data = mat(vstack((A0,A1,A2,A3)))
	U,sigma,VT = np.linalg.svd(train_data)
	posx = VT[3,:].T
	posx_ = posx / posx[3][0]
	position = posx_[0:3]
	return position

if __name__ == '__main__':
	# get camera intrinsic parameters K, C
	xmlfile = 'D:/experiments/TS-Reconstruct/Block_1 - AT -export.xml'
	imgname1 = 'DSC00041.jpg'
	imgname2 = 'DSC00044.jpg'
	K, x0, y0 = getInt_para(xmlfile)
	# get camera external parameters
	R1, C_center1 = getExt_para(xmlfile, imgname1)
	R2, C_center2 = getExt_para(xmlfile, imgname2)

	t1 = -R1 * C_center1
	t2 = -R2 * C_center2

	Proj1 = mat(K*hstack((R1,t1))) #投影矩阵P1
	Proj2 = mat(K*hstack((R2,t2))) #投影矩阵P2

	img_kp1 =(6672, 2716)
	img_kp2 =(6633, 2848)
	position = calculate_3DX(img_kp1, img_kp2, Proj1, Proj2)

注意：

1. python奇异矩阵分解结果是转置了的。

2. 参数通过空三解算，再读取XML文件。