三角化公式推导手撕代码

前言

三角测量是在已知相机参数和图像中匹配点的情况下，求解这些匹配点对应的空间点三维坐标的方法。针对单目与双目系统，三角测量的使用方法有所不同。双目视觉测距原理参见：双目视觉测距原理深度剖析：一个被忽略的小问题，与双目相机相比，单目相机拍摄的两张图片没有固定的位姿，也就没有准确的基线，所以其三角化是指根据相机的运动来估计特征点的深度信息。

这里主要介绍直接线性变换法（Direct Linear Transform，DLT）。

直接线性变换法

设三维空间点 P P P在世界坐标系下的齐次坐标为 X = [ x , y , z , 1 ] T X=[x,y,z,1]^T X=[x,y,z,1]T，相应的在两个视角的投影点分别为 p 1 p1 p1和 p 2 p2 p2，它们在相机坐标系下的坐标为：
x 1 → = [ x 1 , y 1 , 1 ] T , x 2 → = [ x 2 , y 2 , 1 ] T \overrightarrow {x_1}=[x_1,y_1,1]^T,\overrightarrow {x_2}=[x_2,y_2,1]^T x1 =[x1,y1,1]T,x2 =[x2,y2,1]T

两幅图像对应的相机投影矩阵分别为 P 1 、 P 2 P_1、P_2 P1、P2（3*4维），
P 1 = [ P 11 , P 12 , P 13 ] T , P 2 = [ P 21 , P 22 , P 23 ] T P_1=[P_{11},P_{12},P_{13}]^T,P_2=[P_{21},P_{22},P_{23}]^T P1=[P11,P12,P13]T,P2=[P21,P22,P23]T其中， P 11 、 P 12 、 P 13 P_{11}、P_{12}、P_{13} P11、P12、P13分别是投影矩阵 P 1 P_1 P1的第1-3行； P 21 、 P 22 、 P 23 P_{21}、P_{22}、P_{23} P21、P22、P23分别是投影矩阵 P 2 P_2 P2的第1-3行；在理想情况下，有：
x 1 → = P 1 X , x 2 → = P 2 X \overrightarrow {x_1}=P_1X,\overrightarrow {x_2}=P_2X x1 =P1X,x2 =P2X对于 x 1 → \overrightarrow {x_1} x1 ，在其两侧分别叉乘其本身，可知：
(1) x 1 → × ( P 1 X ) = 0 \overrightarrow {x_1}\times(P_1X)=0 \tag1 x1 ×(P1X)=0(1)即：
[ x 1 y 1 1 ] × [ P 11 X P 12 X P 13 X ] = [ 0 − 1 y 1 1 0 − x 1 − y 1 x 1 0 ] [ P 11 X P 12 X P 13 X ] = 0 \begin{bmatrix} x_1 \\ y_1 \\ 1 \end{bmatrix}\times\begin{bmatrix} P_{11}X \\P_{12}X \\ P_{13}X \end{bmatrix}=\begin{bmatrix} 0 & -1 & y_1 \\ 1 & 0 & -x_1 \\ -y_1 & x_1& 0\end{bmatrix}\begin{bmatrix} P_{11}X \\P_{12}X \\ P_{13}X \end{bmatrix}=0 ⎣⎡x1y11⎦⎤×⎣⎡P11XP12XP13X⎦⎤=⎣⎡01−y1−10x1y1−x10⎦⎤⎣⎡P11XP12XP13X⎦⎤=0可以得到：
(2) [ − P 12 X + y 1 P 13 X P 11 X − x 1 P 13 X x 1 P 12 X − y 1 P 11 X ] = 0 \begin{bmatrix} -P_{12}X+y_1P_{13}X \\P_{11}X-x_1P_{13}X \\ x_1P_{12}X-y_1P_{11}X \end{bmatrix} =0 \tag2 ⎣⎡−P12X+y1P13XP11X−x1P13Xx1P12X−y1P11X⎦⎤=0(2)

其中，第三个方程可以由前两个进行线性变换得到，因此每个视角可以提供两个约束条件，联合第二个视角，可以得到: (3) A X = 0 AX=0\tag3 AX=0(3)

其中， A = [ x 1 P 13 − P 11 y 1 P 13 − P 12 x 2 P 23 − P 21 y 2 P 23 − P 22 ] A=\begin{bmatrix} x_1P_{13}- P_{11} \\ y_1P_{13}-P_{12} \\ x_2P_{23}- P_{21} \\ y_2P_{23}-P_{22} \end{bmatrix} A=⎣⎢⎢⎡x1P13−P11y1P13−P12x2P23−P21y2P23−P22⎦⎥⎥⎤

针对上述方程，当视角点数较少且不存在外点时可直接对矩阵 A A A进行SVD分解来求解，得出 X X X坐标；当存在外点（错误的匹配点），则采用RANSAC（随机一致性采样）的方法进行估计。

当点数多于2个时，还可以采用最小二乘法进行求解；由于每次观测可以提供两个约束条件，因此当存在 n n n次观测时， A ∈ R 2 n × 4 A \in R^{2n\times4} A∈R2n×4，使用最小二乘法求解 A X = 0 AX=0 AX=0，即是求：
(4) min ⁡ X ∣ ∣ A X ∣ ∣ 2 2 , s . t . ∣ ∣ X ∣ ∣ = 1 \min_{X}||AX||^2_2, \quad s.t.||X||=1 \tag4 Xmin∣∣AX∣∣22,s.t.∣∣X∣∣=1(4)

对 A T A A^TA ATA进行SVD分解：
(5) A T A = ∑ i = 1 4 σ i 2 u i u j T A^TA=\sum_{i=1}^4\sigma^2_iu_iu_j^T \tag5 ATA=i=1∑4σi2uiujT(5)

其中， σ i \sigma_i σi为奇异值，且由大到小排列， u i 、 u j u_i、u_j ui、uj正交。
则可知，当 X = u 4 X=u_4 X=u4时，公式(4)将取得最小值，且最小值为 σ 4 2 \sigma_4^2 σ42。

上述DLT求解中，是已知 x 1 → \overrightarrow {x_1} x1 和投影矩阵 P P P求解对应点的三维空间坐标 X X X；除此之外，DLT还可以用来求解PnP问题，只不过已知条件变成了 x 1 → \overrightarrow {x_1} x1 和 X X X，推导过程也与上述过程类似。

代码

如下代码是直接对矩阵 A A A进行SVD分解的方式来求解。有关最小二乘法求解及代码，可参考：《视觉SLAM十四讲》ch13.3：三角化公式推导及代码详解

#include <iostream>
#include <math/matrix_svd.h>
#include "math/vector.h"
#include "math/matrix.h"using namespace std;int main(int argc, char* argv[])
{math::Vec2f p1;p1[0] = 0.289986; p1[1] = -0.0355493;math::Vec2f p2;p2[0] = 0.316154; p2[1] = 0.0898488;math::Matrix<double, 3, 4> P1, P2;P1(0, 0) = 0.919653;    P1(0, 1) = -0.000621866; P1(0, 2) = -0.00124006; P1(0, 3) = 0.00255933;P1(1, 0) = 0.000609954; P1(1, 1) = 0.919607; P1(1, 2) = -0.00957316; P1(1, 3) = 0.0540753;P1(2, 0) = 0.00135482;  P1(2, 1) = 0.0104087; P1(2, 2) = 0.999949;    P1(2, 3) = -0.127624;P2(0, 0) = 0.920039;    P2(0, 1) = -0.0117214;  P2(0, 2) = 0.0144298;   P2(0, 3) = 0.0749395;P2(1, 0) = 0.0118301;   P2(1, 1) = 0.920129;  P2(1, 2) = -0.00678373; P2(1, 3) = 0.862711;P2(2, 0) = -0.0155846;  P2(2, 1) = 0.00757181; P2(2, 2) = 0.999854;   P2(2, 3) = -0.0887441;/* 构造A矩阵 */math::Matrix<double, 4, 4> A;for (int i = 0; i<4; i++){// p1A(0, i) = p1[0] * P1(2, i) - P1(0, i);A(1, i) = p1[1] * P1(2, i) - P1(1, i);// p2A(2, i) = p2[0] * P2(2, i) - P2(0, i);A(3, i) = p2[1] * P2(2, i) - P2(1, i);}// SVD分解math::Matrix<double, 4, 4> V;math::matrix_svd<double, 4, 4>(A, nullptr, nullptr, &V);math::Vec3f X;X[0] = V(0, 3) / V(3, 3);X[1] = V(1, 3) / V(3, 3);X[2] = V(2, 3) / V(3, 3);std::cout << " trianglede point is :" << X[0] << " " << X[1] << " " << X[2] << std::endl;std::cout << " the result should be " << "2.14598 -0.250569 6.92321\n" << std::endl;system("pause");return 0;
}

OpenCV也对三角测量进行了封装（triangulatePoints()），原理大致相同，使用起来更加方便、简洁。

// 归一化，将像素坐标转换到相机坐标(非齐次坐标)
Point2d pixel2cam(const Point2d& p, const Mat& K)
{/* // 等价Mat x = (Mat_<double>(3, 1) << p.x, p.y, 1);x = K.inv()*x;return Point2d(x.at<double>(0,0),x.at<double>(1,0));*/return Point2d((p.x - K.at<double>(0, 2)) / K.at<double>(0, 0), // 像素坐标系->图像坐标系->相机坐标系(p.y - K.at<double>(1, 2)) / K.at<double>(1, 1));
}
// 返回的是三维空间点
void triangulation(const vector<KeyPoint>& keypoint_1, const vector<KeyPoint>& keypoint_2, const vector<DMatch>& matches, const Mat& R, const Mat& t, vector<Point3d>& space_points)
{// T1、T2为外参矩阵，视第一个相机坐标系为世界坐标系，则其R = I, T = 0Mat T1 = (Mat_<double>(3, 4) <<1, 0, 0, 0,0, 1, 0, 0,0, 0, 1, 0);Mat T2 = (Mat_<double>(3, 4) <<R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2), t.at<double>(0, 0),R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2), t.at<double>(1, 0),R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2), t.at<double>(2, 0));// 定义相机的内参矩阵Mat K = (Mat_<double>(3, 3) <<529.0, 0, 325.1,0, 521.0, 249.9,0, 0, 1);// 将所有匹配的特征点转化为归一化坐标vector<Point2d> pts_1, pts_2;for (DMatch m : matches){// 将像素坐标转换至相机坐标pts_1.push_back(pixel2cam(keypoint_1[m.queryIdx].pt, K));pts_2.push_back(pixel2cam(keypoint_2[m.trainIdx].pt, K));}Mat pts_4d;cv::triangulatePoints(T1, T2, pts_1, pts_2, pts_4d);// 转换为非齐次坐标for (int i = 0; i < pts_4d.cols; i++){Mat x = pts_4d.col(i);  // 第i个三维点对应的齐次坐标x /= x.at<double>(3, 0);   space_points.push_back(Point3d(x.at<double>(0, 0), x.at<double>(1, 0), x.at<double>(2, 0)));}
}

参考

《视觉SLAM14讲》-chapter7
深蓝学院《基于图像的三维模型重建》

三角化公式推导手撕代码相关推荐

【数字IC手撕代码】Verilog奇数分频|题目|原理|设计|仿真（三分频，五分频，奇数分频及特殊占空比）
芯片设计验证社区·芯片爱好者聚集地·硬件相关讨论社区·数字verifier星球四社区联合力荐!近500篇数字IC精品文章收录! [数字IC精品文章收录]学习路线·基础知识·总线·脚本语言·芯片求职· ...
Interview：算法岗位面试—11.06早上上海某智能驾驶科技公司(创业)笔试+面试之手撕代码、项目考察、比赛考察、图像算法的考察等
Interview:算法岗位面试-11.06早上上海某智能驾驶科技公司(创业)笔试+面试之手撕代码.项目考察.比赛考察.图像算法的考察等导读:该公司是在同济某次大型招聘会上投的,当时和HR聊了半个多 ...
秋招总结：遇到的手撕代码题
2020年秋招总结:遇到的手撕代码题跟谁学一面:求连续子数组的最大和(力扣 53) [思路:力扣系列略,题解区都比我讲得好] 二面:翻转字符串中的每个单词(简单题,比较常见,没去找对应的原题) [ ...
2023华为OD面试手撕代码经验分享
我们先来看下这个同学的面试经历吧,非常有借鉴的意义. [22届考研渣渣的od求职之旅,推荐一下两个人,德科hr和牛客的老哥] "*********",hr给了机会吧,一开始我都没想 ...
【数字IC手撕代码】Verilog偶数分频|题目|原理|设计|仿真（二分频，四分频，六分频，八分频，偶数分频及特殊占空比）
芯片设计验证社区·芯片爱好者聚集地·硬件相关讨论社区·数字verifier星球四社区联合力荐!近500篇数字IC精品文章收录! [数字IC精品文章收录]学习路线·基础知识·总线·脚本语言·芯片求职· ...
数字IC手撕代码-流水握手（利用握手解决流水线断流、反压问题）
前言: 本专栏旨在记录高频笔面试手撕代码题,以备数字前端秋招,本专栏所有文章提供原理分析.代码及波形,所有代码均经过本人验证. 目录如下: 1.数字IC手撕代码-分频器(任意偶数分频) 2.数字IC手 ...
【数字IC手撕代码】Verilog固定优先级仲裁器|题目|原理|设计|仿真
芯片设计验证社区·芯片爱好者聚集地·硬件相关讨论社区·数字verifier星球四社区联合力荐!近500篇数字IC精品文章收录! [数字IC精品文章收录]学习路线·基础知识·总线·脚本语言·芯片求职· ...
华为手撕代码c语言题目,想去面试？这10道最高频的手撕代码题都会了吗？
原标题:想去面试?这10道最高频的手撕代码题都会了吗? 来源:Python与算法之美 ID:Python_Ai_Road 作者:梁云1991 想去看机会?下面这10道最高频的手撕代码面试题都会了吗? ...
数字IC手撕代码-同步FIFO
前言: 本专栏旨在记录高频笔面试手撕代码题,以备数字前端秋招,本专栏所有文章提供原理分析.代码及波形,所有代码均经过本人验证. 目录如下: 1.数字IC手撕代码-分频器(任意偶数分频) 2.数字IC手 ...

三角化公式推导手撕代码

前言

直接线性变换法

代码

参考

三角化公式推导手撕代码相关推荐

最新文章

热门文章