计算机视觉对极几何之Triangulate(三角化)

2024-05-31 22:32

本文主要是介绍计算机视觉对极几何之Triangulate(三角化),希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

Overview

欢迎访问 持续更新:https://cgabc.xyz/posts/fea220cc/

求解空间点坐标

Triangulate in ORB-SLAM2

已知,两个视图下匹配点的 图像坐标 p 1 \boldsymbol{p}_1 p1 p 2 \boldsymbol{p}_2 p2,两个相机的相对位姿 T \boldsymbol{T} T ,相机内参矩阵 K \boldsymbol{K} K,求 对应的三维点坐标 P \boldsymbol{P} P,其齐次坐标为 P ~ \tilde{\boldsymbol{P}} P~

两个视图的 投影矩阵 分别为

P 1 = K ⋅ [ I 3 × 3 0 3 × 1 ] , P 1 ∈ R 3 × 4 P 2 = K ⋅ [ R 3 × 3 t 3 × 1 ] , P 2 ∈ R 3 × 4 \boldsymbol{P}_1 = \boldsymbol{K} \cdot [\boldsymbol{I}_{3 \times 3} \quad \mathbf{0}_{3 \times 1}], \quad \boldsymbol{P}_1 \in \mathbb{R}^{3 \times 4} \\ \boldsymbol{P}_2 = \boldsymbol{K} \cdot [ \boldsymbol{R}_{3 \times 3} \quad \boldsymbol{t}_{3 \times 1} ], \quad \boldsymbol{P}_2 \in \mathbb{R}^{3 \times 4} P1=K[I3×303×1],P1R3×4P2=K[R3×3t3×1],P2R3×4

T = T 21 = [ R t ] ∈ R 3 × 4 \boldsymbol{T} = \boldsymbol{T}_{21} = [ \boldsymbol{R} \quad \boldsymbol{t} ] \in \mathbb{R}^{3 \times 4} T=T21=[Rt]R3×4

由于是齐次坐标的表示形式,使用叉乘消去齐次因子

p 1 ~ × ( P 1 P ~ ) = 0 p 2 ~ × ( P 2 P ~ ) = 0 \tilde{\boldsymbol{p}_1} \times (\boldsymbol{P}_1 \tilde{\boldsymbol{P}}) = \mathbf{0} \\ \tilde{\boldsymbol{p}_2} \times (\boldsymbol{P}_2 \tilde{\boldsymbol{P}}) = \mathbf{0} p1~×(P1P~)=0p2~×(P2P~)=0

P 1 \boldsymbol{P}_1 P1 P 2 \boldsymbol{P}_2 P2 按行展开(上标代表行索引)代入,对于第一视图有

[ 0 − 1 v 1 1 0 − u 1 − v 1 u 1 0 ] ⋅ [ P 1 1 ⋅ P ~ P 1 2 ⋅ P ~ P 1 3 ⋅ P ~ ] = 0 \begin{bmatrix} 0 & -1 & v_1 \\ 1 & 0 & -u_1 \\ -v_1 & u_1 & 0 \end{bmatrix} \cdot \begin{bmatrix} \boldsymbol{P}_1^1 \cdot \tilde{\boldsymbol{P}} \\ \boldsymbol{P}_1^2 \cdot \tilde{\boldsymbol{P}} \\ \boldsymbol{P}_1^3 \cdot \tilde{\boldsymbol{P}} \end{bmatrix} = \mathbf{0} 01v110u1v1u10 P11P~P12P~P13P~ =0

u 1 ( P 1 3 ⋅ P ~ ) − ( P 1 1 ⋅ P ~ ) = 0 v 1 ( P 1 3 ⋅ P ~ ) − ( P 1 2 ⋅ P ~ ) = 0 u 1 ( P 1 2 ⋅ P ~ ) − v 1 ( P 1 1 ⋅ P ~ ) = 0 u_1 (\boldsymbol{P}_1^3 \cdot \tilde{\boldsymbol{P}}) - (\boldsymbol{P}_1^1 \cdot \tilde{\boldsymbol{P}}) = 0 \\ v_1 (\boldsymbol{P}_1^3 \cdot \tilde{\boldsymbol{P}}) - (\boldsymbol{P}_1^2 \cdot \tilde{\boldsymbol{P}}) = 0 \\ u_1 (\boldsymbol{P}_1^2 \cdot \tilde{\boldsymbol{P}}) - v_1 (\boldsymbol{P}_1^1 \cdot \tilde{\boldsymbol{P}}) = 0 \\ u1(P13P~)(P11P~)=0v1(P13P~)(P12P~)=0u1(P12P~)v1(P11P~)=0

可见第三个式子可以由上两个式子线性表示,所以只需要取前连个式子即可

[ u 1 P 1 3 − P 1 1 v 1 P 1 3 − P 1 2 ] ⋅ P ~ = 0 \begin{bmatrix} u_1 \boldsymbol{P}_1^3 - \boldsymbol{P}_1^1 \\ v_1 \boldsymbol{P}_1^3 - \boldsymbol{P}_1^2 \end{bmatrix} \cdot \tilde{\boldsymbol{P}} = \mathbf{0} [u1P13P11v1P13P12]P~=0

同样的,对于第二视图

[ u 2 P 2 3 − P 2 1 v 2 P 2 3 − P 2 2 ] ⋅ P ~ = 0 \begin{bmatrix} u_2 \boldsymbol{P}_2^3 - \boldsymbol{P}_2^1 \\ v_2 \boldsymbol{P}_2^3 - \boldsymbol{P}_2^2 \end{bmatrix} \cdot \tilde{\boldsymbol{P}} = \mathbf{0} [u2P23P21v2P23P22]P~=0

组合起来

A 4 × 4 ⋅ P ~ = [ u 1 P 1 3 − P 1 1 v 1 P 1 3 − P 1 2 u 2 P 2 3 − P 2 1 v 2 P 2 3 − P 2 2 ] ⋅ P ~ = 0 \boldsymbol{A_{4 \times 4}} \cdot \tilde{\boldsymbol{P}} = \begin{bmatrix} u_1 \boldsymbol{P}_1^3 - \boldsymbol{P}_1^1 \\ v_1 \boldsymbol{P}_1^3 - \boldsymbol{P}_1^2 \\ u_2 \boldsymbol{P}_2^3 - \boldsymbol{P}_2^1 \\ v_2 \boldsymbol{P}_2^3 - \boldsymbol{P}_2^2 \end{bmatrix} \cdot \tilde{\boldsymbol{P}} = \mathbf{0} A4×4P~= u1P13P11v1P13P12u2P23P21v2P23P22 P~=0

求解 P \boldsymbol{P} P 相当于解一个 线性最小二乘问题

SVD分解 A \boldsymbol{A} A

SVD ( A ) = U Σ V T \text{SVD}(\boldsymbol{A}) = \boldsymbol{U} \boldsymbol{\Sigma} \boldsymbol{V}^T SVD(A)=UΣVT

方程的解为 A \boldsymbol{A} A最小奇异值 对应的 奇异矢量,即 齐次坐标

P ~ = ( X , Y , Z , W ) = V 3 \tilde{\boldsymbol{P}} = (X,Y,Z,W) = \boldsymbol{V}_3 P~=(X,Y,Z,W)=V3

最终, P \boldsymbol{P} P(第一视图坐标系下三维坐标)为

P = ( X W , Y W , Z W ) \boldsymbol{P} = (\frac{X}{W}, \frac{Y}{W}, \frac{Z}{W}) P=(WX,WY,WZ)

orbslam2_cg中示例代码:

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);
}

Triangulate in PTAM

已知,两个视图下匹配点的 归一化平面(z=1)齐次坐标 p 1 \boldsymbol{p}_1 p1 p 2 \boldsymbol{p}_2 p2,两个相机的相对位姿 T \boldsymbol{T} T,求 对应的三维点坐标 P \boldsymbol{P} P(第一视图坐标系下三维坐标),其齐次坐标为 P ~ \tilde{\boldsymbol{P}} P~

方程

p 1 × ( I 3 × 4 ⋅ P ~ ) = 0 p 2 × ( T 21 ⋅ P ~ ) = 0 \boldsymbol{p}_1 \times (\boldsymbol{I}_{3 \times 4} \cdot \tilde{\boldsymbol{P}}) = \mathbf{0} \\ \boldsymbol{p}_2 \times (\boldsymbol{T}_{21} \cdot \tilde{\boldsymbol{P}}) = \mathbf{0} p1×(I3×4P~)=0p2×(T21P~)=0

T = T 21 = [ R t ] ∈ R 3 × 4 \boldsymbol{T} = \boldsymbol{T}_{21} = [ \boldsymbol{R} \quad \boldsymbol{t} ] \in \mathbb{R}^{3 \times 4} T=T21=[Rt]R3×4

矩阵形式

A 6 × 4 ⋅ P ~ = [ p 1 × I 3 × 4 p 2 × T 21 ] ⋅ P ~ = 0 \boldsymbol{A_{6 \times 4}} \cdot \tilde{\boldsymbol{P}} = \begin{bmatrix} \boldsymbol{p}_1 \times \boldsymbol{I}_{3 \times 4} \\ \boldsymbol{p}_2 \times \boldsymbol{T}_{21} \end{bmatrix} \cdot \tilde{\boldsymbol{P}} = \mathbf{0} A6×4P~=[p1×I3×4p2×T21]P~=0

求解 P \boldsymbol{P} P 相当于解一个 线性最小二乘问题

SVD分解 A \boldsymbol{A} A

SVD ( A ) = U Σ V T \text{SVD}(\boldsymbol{A}) = \boldsymbol{U} \boldsymbol{\Sigma} \boldsymbol{V}^T SVD(A)=UΣVT

方程的解为 A \boldsymbol{A} A最小奇异值 对应的 奇异矢量,即 齐次坐标

P ~ = ( X , Y , Z , W ) = V 3 \tilde{\boldsymbol{P}} = (X,Y,Z,W) = \boldsymbol{V}_3 P~=(X,Y,Z,W)=V3

最终, P \boldsymbol{P} P(第一视图坐标系下三维坐标)为

P = ( X W , Y W , Z W ) \boldsymbol{P} = (\frac{X}{W}, \frac{Y}{W}, \frac{Z}{W}) P=(WX,WY,WZ)

ptam_cg中示例代码Triangulate:

Vector<3> MapMaker::Triangulate(SE3<> se3AfromB,const Vector<2> &v2A,const Vector<2> &v2B)
{Matrix<3,4> PDash;PDash.slice<0,0,3,3>() = se3AfromB.get_rotation().get_matrix();PDash.slice<0,3,3,1>() = se3AfromB.get_translation().as_col();Matrix<4> A;A[0][0] = -1.0; A[0][1] =  0.0; A[0][2] = v2B[0]; A[0][3] = 0.0;A[1][0] =  0.0; A[1][1] = -1.0; A[1][2] = v2B[1]; A[1][3] = 0.0;A[2] = v2A[0] * PDash[2] - PDash[0];A[3] = v2A[1] * PDash[2] - PDash[1];SVD<4,4> svd(A);Vector<4> v4Smallest = svd.get_VT()[3];if(v4Smallest[3] == 0.0)v4Smallest[3] = 0.00001;return project(v4Smallest);
}

ptam_cg中示例代码TriangulateNew:

Vector<3> MapMaker::TriangulateNew(SE3<> se3AfromB,const Vector<2> &v2A,const Vector<2> &v2B)
{Vector<3> v3A = unproject(v2A);Vector<3> v3B = unproject(v2B);Matrix<3> m3A = TooN::Zeros;m3A[0][1] = -v3A[2];m3A[0][2] =  v3A[1];m3A[1][2] = -v3A[0];m3A[1][0] = -m3A[0][1];m3A[2][0] = -m3A[0][2];m3A[2][1] = -m3A[1][2];Matrix<3> m3B = TooN::Zeros;m3B[0][1] = -v3B[2];m3B[0][2] =  v3B[1];m3B[1][2] = -v3B[0];m3B[1][0] = -m3B[0][1];m3B[2][0] = -m3B[0][2];m3B[2][1] = -m3B[1][2];Matrix<3,4> m34AB;m34AB.slice<0,0,3,3>() = se3AfromB.get_rotation().get_matrix();m34AB.slice<0,3,3,1>() = se3AfromB.get_translation().as_col();SE3<> se3I;Matrix<3,4> m34I;m34I.slice<0,0,3,3>() = se3I.get_rotation().get_matrix();m34I.slice<0,3,3,1>() = se3I.get_translation().as_col();Matrix<3,4> PDashA = m3A * m34AB;Matrix<3,4> PDashB = m3B * m34I;Matrix<6,4> A;A.slice<0,0,3,4>() = PDashA;A.slice<3,0,3,4>() = PDashB;SVD<6,4> svd(A);Vector<4> v4Smallest = svd.get_VT()[3];if(v4Smallest[3] == 0.0)v4Smallest[3] = 0.00001;return project(v4Smallest);
}

求解空间点深度

已知,两个视图下匹配点的 归一化平面(z=1)齐次坐标 p 1 \boldsymbol{p}_1 p1 p 2 \boldsymbol{p}_2 p2,两个相机的相对位姿 T \boldsymbol{T} T,求解空间点深度 Z 1 Z_1 Z1 Z 2 Z_2 Z2

T = T 21 = [ R t ] ∈ R 3 × 4 \boldsymbol{T} = \boldsymbol{T}_{21} = [ \boldsymbol{R} \quad \boldsymbol{t} ] \in \mathbb{R}^{3 \times 4} T=T21=[Rt]R3×4

Z 2 ⋅ p 2 = T 21 ⋅ ( Z 1 ⋅ p 1 ) = Z 1 ⋅ R p 1 + t Z_2 \cdot \boldsymbol{p}_2 = \boldsymbol{T}_{21} \cdot ( Z_1 \cdot \boldsymbol{p}_1 ) = Z_1 \cdot \boldsymbol{R} \boldsymbol{p}_1 + \boldsymbol{t} Z2p2=T21(Z1p1)=Z1Rp1+t

矩阵形式

[ p 2 − R p 1 ] ⋅ [ Z 2 Z 1 ] = t \begin{bmatrix} \boldsymbol{p}_2 & -\boldsymbol{R} \boldsymbol{p}_1 \end{bmatrix} \cdot \begin{bmatrix} Z_2 \\ Z_1 \end{bmatrix} = \boldsymbol{t} [p2Rp1][Z2Z1]=t

Triangulate in SVO

上式即 A x = b Ax=b Ax=b 的形式,解该方程可以用 正规方程

A T A x = A T b A^T A x = A^T b ATAx=ATb

解得

x = ( A T A ) − 1 A T b x = (A^TA)^{-1} A^T b x=(ATA)1ATb

svo_cg中示例代码:

bool depthFromTriangulation(const SE3& T_search_ref,const Vector3d& f_ref,const Vector3d& f_cur,double& depth)
{Matrix<double,3,2> A; A << T_search_ref.rotation_matrix() * f_ref, f_cur;const Matrix2d AtA = A.transpose() * A;if(AtA.determinant() < 0.000001)return false;const Vector2d depth2 =- AtA.inverse()* A.transpose() * T_search_ref.translation();depth = fabs(depth2[0]);return true;
}

Triangulate in REMODE

由于解向量是二维的,对上式采用 克莱默法则 求解:

[ p 2 R p 1 ] [ p 2 − R p 1 ] [ Z 2 Z 1 ] = [ p 2 p 2 − p 2 ⋅ R p 1 p 2 ⋅ R p 1 − R p 1 ⋅ R p 1 ] [ Z 2 Z 1 ] = t \begin{bmatrix} \boldsymbol{p}_2 \\ \boldsymbol{R} \boldsymbol{p}_1 \end{bmatrix} \begin{bmatrix} \boldsymbol{p}_2 & -\boldsymbol{R} \boldsymbol{p}_1 \end{bmatrix} \begin{bmatrix} Z_2 \\ Z_1 \end{bmatrix} = \begin{bmatrix} \boldsymbol{p}_2 \boldsymbol{p}_2 & -\boldsymbol{p}_2 \cdot \boldsymbol{R} \boldsymbol{p}_1 \\ \boldsymbol{p}_2 \cdot \boldsymbol{R} \boldsymbol{p}_1 & -\boldsymbol{R} \boldsymbol{p}_1 \cdot \boldsymbol{R} \boldsymbol{p}_1 \end{bmatrix} \begin{bmatrix} Z_2 \\ Z_1 \end{bmatrix} = \boldsymbol{t} [p2Rp1][p2Rp1][Z2Z1]=[p2p2p2Rp1p2Rp1Rp1Rp1][Z2Z1]=t

REMODE中示例代码:

// Returns 3D point in reference frame
// Non-linear formulation (ref. to the book 'Autonomous Mobile Robots')
__device__ __forceinline__
float3 triangulatenNonLin(const float3 &bearing_vector_ref,const float3 &bearing_vector_curr,const SE3<float> &T_ref_curr)
{const float3 t = T_ref_curr.getTranslation();float3 f2 = T_ref_curr.rotate(bearing_vector_curr);const float2 b = make_float2(dot(t, bearing_vector_ref),dot(t, f2));float A[2*2];A[0] = dot(bearing_vector_ref, bearing_vector_ref);A[2] = dot(bearing_vector_ref, f2);A[1] = -A[2];A[3] = dot(-f2, f2);const float2 lambdavec = make_float2(A[3] * b.x - A[1] * b.y,-A[2] * b.x + A[0] * b.y) / (A[0] * A[3] - A[1] * A[2]);const float3 xm = lambdavec.x * bearing_vector_ref;const float3 xn = t + lambdavec.y * f2;return (xm + xn)/2.0f;
}

Reference

  • 三角形法恢复空间点深度

这篇关于计算机视觉对极几何之Triangulate(三角化)的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/1019219

相关文章

uva 10387 Billiard(简单几何)

题意是一个球从矩形的中点出发,告诉你小球与矩形两条边的碰撞次数与小球回到原点的时间,求小球出发时的角度和小球的速度。 简单的几何问题,小球每与竖边碰撞一次,向右扩展一个相同的矩形;每与横边碰撞一次,向上扩展一个相同的矩形。 可以发现,扩展矩形的路径和在当前矩形中的每一段路径相同,当小球回到出发点时,一条直线的路径刚好经过最后一个扩展矩形的中心点。 最后扩展的路径和横边竖边恰好组成一个直

poj 1113 凸包+简单几何计算

题意: 给N个平面上的点,现在要在离点外L米处建城墙,使得城墙把所有点都包含进去且城墙的长度最短。 解析: 韬哥出的某次训练赛上A出的第一道计算几何,算是大水题吧。 用convexhull算法把凸包求出来,然后加加减减就A了。 计算见下图: 好久没玩画图了啊好开心。 代码: #include <iostream>#include <cstdio>#inclu

uva 1342 欧拉定理(计算几何模板)

题意: 给几个点,把这几个点用直线连起来,求这些直线把平面分成了几个。 解析: 欧拉定理: 顶点数 + 面数 - 边数= 2。 代码: #include <iostream>#include <cstdio>#include <cstdlib>#include <algorithm>#include <cstring>#include <cmath>#inc

XTU 1237 计算几何

题面: Magic Triangle Problem Description: Huangriq is a respectful acmer in ACM team of XTU because he brought the best place in regional contest in history of XTU. Huangriq works in a big compa

计算机毕业设计 大学志愿填报系统 Java+SpringBoot+Vue 前后端分离 文档报告 代码讲解 安装调试

🍊作者:计算机编程-吉哥 🍊简介:专业从事JavaWeb程序开发,微信小程序开发,定制化项目、 源码、代码讲解、文档撰写、ppt制作。做自己喜欢的事,生活就是快乐的。 🍊心愿:点赞 👍 收藏 ⭐评论 📝 🍅 文末获取源码联系 👇🏻 精彩专栏推荐订阅 👇🏻 不然下次找不到哟~Java毕业设计项目~热门选题推荐《1000套》 目录 1.技术选型 2.开发工具 3.功能

poj 3304 几何

题目大意:给出n条线段两个端点的坐标,问所有线段投影到一条直线上,如果这些所有投影至少相交于一点就输出Yes!,否则输出No!。 解题思路:如果存在这样的直线,过投影相交点(或投影相交区域中的点)作直线的垂线,该垂线(也是直线)必定与每条线段相交,问题转化为问是否存在一条直线和所有线段相交。 若存在一条直线与所有线段相交,此时该直线必定经过这些线段的某两个端点,所以枚举任意两个端点即可。

POJ 2318 几何 POJ 2398

给出0 , 1 , 2 ... n 个盒子, 和m个点, 统计每个盒子里面的点的个数。 const double eps = 1e-10 ;double add(double x , double y){if(fabs(x+y) < eps*(fabs(x) + fabs(y))) return 0 ;return x + y ;}struct Point{double x , y

poj 2653 几何

按顺序给一系列的线段,问最终哪些线段处在顶端(俯视图是完整的)。 const double eps = 1e-10 ;double add(double x , double y){if(fabs(x+y) < eps*(fabs(x) + fabs(y))) return 0 ;return x + y ;}struct Point{double x , y ;Point(){}Po

计算机视觉工程师所需的基本技能

一、编程技能 熟练掌握编程语言 Python:在计算机视觉领域广泛应用,有丰富的库如 OpenCV、TensorFlow、PyTorch 等,方便进行算法实现和模型开发。 C++:运行效率高,适用于对性能要求严格的计算机视觉应用。 数据结构与算法 掌握常见的数据结构(如数组、链表、栈、队列、树、图等)和算法(如排序、搜索、动态规划等),能够优化代码性能,提高算法效率。 二、数学基础

java计算机毕设课设—停车管理信息系统(附源码、文章、相关截图、部署视频)

这是什么系统? 资源获取方式在最下方 java计算机毕设课设—停车管理信息系统(附源码、文章、相关截图、部署视频) 停车管理信息系统是为了提升停车场的运营效率和管理水平而设计的综合性平台。系统涵盖用户信息管理、车位管理、收费管理、违规车辆处理等多个功能模块,旨在实现对停车场资源的高效配置和实时监控。此外,系统还提供了资讯管理和统计查询功能,帮助管理者及时发布信息并进行数据分析,为停车场的科学