三角测量计算点的三维坐标

三角测量（triangulation), 就是在不同的姿态下观察同一个对象，根据图像上的同名点的像素坐标，计算对象的三维坐标。

在双目视觉中，分左右相机，同时可以获得左视图和右视图，根据匹配的同名点和左右相机的投影矩阵，就可以计算出匹配同名点的世界坐标。

如图： O1和O2相机的投影的像素点为p， p’；P为世界坐标点的齐次坐标。
首先对两个相机进行标定，并获得投影矩阵分别是T1 和 T2， T1和T2的大小为3x4的矩阵。

p=[uv1]p′=[u′v′1]p = \left[\begin{matrix} u \\ v \\ 1 \end{matrix}\right] p^{'} = \left[\begin{matrix} u ^{'}\\ v ^{'}\\ 1 \end{matrix}\right] p=⎣⎡uv1⎦⎤p′=⎣⎡u′v′1⎦⎤
根据：

s1p=T1Ps2p′=T2Ps_1p = T_1P \\ s_2p'=T_2P s1p=T1Ps2p′=T2P

s1和s2 为尺度。把T1分解成行向量T11、T12、T13。
s1[uv1]=[T11T12T13]Ps_1\left[\begin{matrix} u \\ v \\ 1 \end{matrix}\right] = \left[\begin{matrix} T_{11} \\ T_{12} \\ T_{13} \end{matrix}\right]P s1⎣⎡uv1⎦⎤=⎣⎡T11T12T13⎦⎤P
分解
s1v=T11Ps1u=T12Ps1=T13Ps_1v=T_{11}P \\ s_1u = T_{12}P \\ s_1 = T_{13}P \\ s1v=T11Ps1u=T12Ps1=T13P
把第三式带入，消去s1s_1s1
vT13P=T11PuT13P=T12P=>(vT13−T11)P=0(uT13−T12)P=0vT_{13}P=T_{11}P \\ u T_{13}P= T_{12}P \\ =>\\ (vT_{13}- T_{11})P=0 \\ (uT_{13}-T_{12})P=0\\ vT13P=T11PuT13P=T12P=>(vT13−T11)P=0(uT13−T12)P=0
可以看出，一对点可以有4个方程，P有3个未知数。可以把等式写成AP=0形式。
即：
[vT13−T11uT13−T12v′T23−T21u′T23−T22]P=0\left[\begin{matrix} vT_{13}- T_{11} \\ uT_{13}-T_{12}\\ v^{'}T_{23}- T_{21} \\ u^{'}T_{23}-T_{22} \end{matrix}\right]P=0 ⎣⎢⎢⎡vT13−T11uT13−T12v′T23−T21u′T23−T22⎦⎥⎥⎤P=0

然后对A进行SVD分解，最小二乘法就可以计算出P。

代码实现：


/*** * @param T1 相机O1的投影矩阵* @param T2 相机O2的投影矩阵* @param p1 相机O1的图像的像素点* @param p2 相机O2的图像的像素点* @param out 输出的三维坐标*/
void triangulatePoint(Matrix<double, 3, 4>& T1, Matrix<double, 3, 4>& T2, Vector2d& p1, Vector2d&p2, Vector3d& out){// 构建Ax = 0;Matrix<double, 4, 4> A;A.row(0) = p1(0)*T1.row(2) - T1.row(0);A.row(1) = p1(1)*T1.row(2) - T1.row(1);A.row(2) = p2(0)*T2.row(2) - T2.row(0);A.row(3) = p2(1)*T2.row(2) - T2.row(1);// SVD 分解Eigen::JacobiSVD<MatrixXd> solver(A, Eigen::ComputeFullU|Eigen::ComputeThinV);MatrixXd V = solver.matrixV();VectorXd X = V.rightCols(1);out[0] = X(0)/X(3);out[1] = X(1)/X(3);out[2] = X(2)/X(3);
}

OpenCV库，也已经实现了cv::triangulatePoints
这里也对其进行调用

void triangulateByOpenCV(Matrix<double, 3, 4>& T1, Matrix<double, 3, 4>& T2, Vector2d& p1, Vector2d&p2, Vector3d& out){Mat T_mat1, T_mat2;cv::eigen2cv(T1, T_mat1);cv::eigen2cv(T2, T_mat2);std::vector<Point2d> p1s,p2s;p1s.push_back(Point2d(p1.x(), p1.y()));p2s.push_back(Point2d(p2.x(), p2.y()));/**  @param projMatr1 3x4 projection matrix of the first camera.@param projMatr2 3x4 projection matrix of the second camera.@param projPoints1 2xN array of feature points in the first image. In case of c++ version it canbe also a vector of feature points or two-channel matrix of size 1xN or Nx1.@param projPoints2 2xN array of corresponding points in the second image. In case of c++ versionit can be also a vector of feature points or two-channel matrix of size 1xN or Nx1.@param points4D 4xN array of reconstructed points in homogeneous coordinates.**/Mat pts_4d;cv::triangulatePoints(T_mat1, T_mat2, p1s, p2s, pts_4d);Mat X = pts_4d.col(0);X /=X.at<double>(3,0);out[0] = X.at<double>(0, 0);out[1] = X.at<double>(1, 0);out[2] = X.at<double>(2, 0);}

对两个函数进行测试，数据来自网络。


int main(){Matrix<double, 3, 4> T1, T2;T1(0, 0) = 0.919653;    T1(0, 1)=-0.000621866; T1(0, 2)= -0.00124006; T1(0, 3) = 0.00255933;T1(1, 0) = 0.000609954; T1(1, 1)=0.919607    ; T1(1, 2)= -0.00957316; T1(1, 3) = 0.0540753;T1(2, 0) = 0.00135482;  T1(2, 1) =0.0104087  ; T1(2, 2)= 0.999949;    T1(2, 3) = -0.127624;T2(0, 0) = 0.920039;    T2(0, 1)=-0.0117214;  T2(0, 2) = 0.0144298;   T2(0, 3)   = 0.0749395;T2(1, 0) = 0.0118301;   T2(1, 1)=0.920129  ;  T2(1, 2) = -0.00678373; T2(1, 3) = 0.862711;T2(2, 0) = -0.0155846;  T2(2, 1) =0.00757181; T2(2, 2) = 0.999854 ;   T2(2, 3)   = -0.0887441;Vector2d p1, p2;p1(0) = 0.289986; p1(1) = -0.0355493;p2(0) = 0.316154; p2(1) =  0.0898488;Vector3d out1, out2;triangulatePoint(T1, T2, p1, p2, out1);triangulateByOpenCV(T1, T2, p1, p2, out2);std::cout<< "result 1 >> " << out1.transpose() << std::endl;std::cout<< "result 2 >> " << out2.transpose() << std::endl;return 0;
}

两个函数输出的结果一致：

result 1 >>   2.14598 -0.250569   6.92321
result 2 >>   2.14598 -0.250569   6.92321

参考：
http://users.cecs.anu.edu.au/~hartley/Papers/triangulation/triangulation.pdf
http://vision.stanford.edu/teaching/cs231a_autumn1112/lecture/lecture10_multi_view_cs231a_old.pdf

三角测量计算点的三维坐标相关推荐

三维重建（5）之三角测量计算双目相机坐标系下三维坐标
三角测量计算三维坐标注:三角测量须在双目相机进行立体匹配之后进行计算,针对极线校正后图像关于极线校正可以参考我的上篇博文:相机成像+单应性变换+相机标定+立体校正 1.双目相机成像原理详解参考: ...
项目需求 | PC VR头戴显示器实时三维坐标（项目经费20万）
项目需求:PC VR头戴显示器实时三维坐标需求内容:Windows环境下,通过双目摄像头+IMU+orb-slam获取实时三维坐标. 1.头部跟踪支持双目(四目)鱼眼摄像头的SLAM定位的算法,输出 ...
Intel Realsense D435 如何通过图像识别出的比例点（x, y）获得实际点相对于摄像机原点的三维坐标（x, y, z）?
自己也因为这个问题咨询了Intel的官方工作人员,以下是我的提问和他们的答复: My ask Before opening a new issue, we wanted to provide you ...
python 三维数据绘图_Python中三维坐标空间绘制的实现
在三维空间绘制点,线,面 1.绘制点用scatter()散点绘制三维坐标点from matplotlib import pyplot as plt from mpl_toolkits.mplot3d ...
静止的单摄像机无法得到像点的三维坐标详解
我们知道在机器视觉中通常要使用的搭建的视觉测量系统对一个物体的尺寸.形变.以及三维形貌进行测量.一般按照摄像机的个数以及组成部分分为三类测量方法.分别为单目测量.双目(大于2为多目)测量.以及结构光测 ...
三维坐标转经纬度_Three.js 地理坐标和三维空间坐标的转换
编者按:本文作者高峰,360奇舞团前端工程师,W3C性能工作组/WOT工作组成员. 一.引言在实现3D地球时,球面是通过地理贴图渲染的.所以我们所说的地理坐标和三维空间坐标的转换,是指将地理贴图上的 ...
三维坐标偏转_三维坐标变换原理-平移, 旋转, 缩放
给定一个二维点(x, y),那么形如(kx, ky, k)的所有三元组就都是等价的,它们就是这个点的齐次坐标(homogeneous).齐次坐标就是将一个原本是n维的向量用一个n+1维向量来表示,是指 ...
OpenGL 获取当前屏幕坐标对应的三维坐标
转自原文 OpenGL 获取当前屏幕坐标对应的三维坐标,使用很简单glu库中的一个函数 #include <GL/glut.h> #include <stdlib.h> #in ...
利用双目摄像头来进行空间点的三维坐标定位
一.三维重建概述三维重建主要是研究如何从得到的匹配点中计算出相机的投影矩阵(如果是外部标定的话,就是求出相机的外部参数)以及如何计算出匹配点的三维坐标. 目前研究的进展与之还相差较远.研究人员为了能 ...

三角测量计算点的三维坐标

三角测量计算点的三维坐标相关推荐

最新文章

热门文章