1. 从二维照片到三维世界:三角化到底在做什么?
你有没有想过,为什么我们拍的照片是平的,但手机地图却能告诉你前面那栋楼有多高?或者,为什么一些AR应用能把虚拟的恐龙稳稳地放在你家的地板上,好像它真的在那里一样?这背后,其实藏着一个从二维“照片”变出三维“世界”的魔法,而这个魔法的核心,就是三角化。
简单来说,三角化就是用几何里的三角形关系,从几张不同角度拍到的二维照片里,算出物体在真实三维空间中的位置。这和我们用两只眼睛来判断距离是一个道理。你闭上一只眼睛去拿桌上的水杯,可能会有点犹豫,因为单眼很难精确判断距离。但睁开双眼,大脑通过对比左右眼看到的细微差异,瞬间就能知道杯子离你有多远。三角化就是让计算机也拥有这种“双眼立体视觉”。
在计算机视觉和机器人领域,这个技术太重要了。比如,你想让扫地机器人自己建个家里的地图,它就需要一边走,一边用摄像头“看”,然后通过三角化,把看到的墙角、桌腿这些特征点,从像素坐标(照片上的位置)还原成三维坐标(房间里真实的位置),最后拼出一张三维地图。再比如,电影里做特效,需要把虚拟角色合成到实拍场景中,也得先通过三角化计算出摄像机怎么运动的、场景里物体的三维结构,虚拟角色才能“站”对地方,光影也才能匹配得上。
所以,三角化不是什么遥不可及的学术概念,它正是连接我们看到的二维图像和计算机理解的三维世界的那座关键桥梁。今天,我就带你深入这座桥的内部,看看它到底是怎么搭建起来的,并且用实际的代码告诉你,怎么亲手实现这个“从二维到三维”的魔法。
2. 三角化的数学心脏:对极几何与投影模型
要理解三角化,我们得先搞清楚相机是怎么“看”世界的。相机就像一个特殊的投影仪,它把三维空间中的点,投射到二维的成像平面上,这个过程会丢失一个最关键的维度信息——深度,也就是这个点离相机到底有多远。三角化要做的,正是把这个丢失的深度给找回来。
2.1 相机如何“拍扁”世界:小孔成像模型
我们用一个最经典的模型——小孔成像模型来理解。你可以把相机想象成一个暗箱,前面有一个针孔。三维世界中的点P,穿过这个小孔,在后面的感光元件(比如CMOS)上形成了一个倒立的像点p。这个过程用数学来描述,就是投影。
一个三维点P的坐标是 [X, Y, Z],这里的Z就是它相对于相机的深度。投影后,它在照片上的像素坐标是 [u, v]。它们之间的关系,可以用一个矩阵乘法来表示:
s * [u, v, 1]^T = K * [R | t] * [X, Y, Z, 1]^T
我拆开解释一下:
s:这是一个神秘的尺度因子,你可以把它理解为那个丢失的深度Z的“影子”。因为投影后深度信息没了,我们需要引入这个s来平衡等式。[u, v, 1]^T:这是照片上的像素坐标,用齐次坐标表示。K:这是相机内参矩阵。它包含了相机的“个性”信息,比如焦距(f)、像素的物理尺寸(dx, dy)以及图像中心点(cx, cy)。它负责把物理成像平面上的点换算成像素坐标。[R | t]:这是相机外参矩阵,由旋转矩阵R和平移向量t组成。它描述了相机在世界坐标系下的姿态和位置。简单说,就是相机朝哪儿看、站在哪儿。[X, Y, Z, 1]^T:这是三维点在世界坐标系下的齐次坐标。
这个等式是整个三维视觉的基石。三角化的问题,本质上就是:我们知道了在两个不同位置(不同R, t)拍到的两张照片上,同一个点P的像素坐标 [u1, v1] 和 [u2, v2],也知道了这两个时刻相机的内参K和外参(R1, t1; R2, t2),我们想要求出那个丢失的深度,也就是恢复出P点的三维坐标 [X, Y, Z]。
2.2 两张照片的约束:对极几何
当同一个三维点被两个相机看到时,这两个相机光心(可以理解为针孔的位置)、三维点P、以及两个像点p1和p2,这五点必然共面。这个几何关系,就是对极几何。
想象一下:你左眼(相机1)和右眼(相机2)看同一个手指(点P)。你左眼看到的指尖位置(p1)、右眼看到的指尖位置(p2)、你的左眼和右眼本身,还有你的手指,这五个东西都在同一个平面上。这个平面叫做极平面。
从这个几何关系里,可以推导出一个非常重要的约束方程,叫做对极约束:
p2^T * F * p1 = 0
这里的F叫做基础矩阵。这个方程的美妙之处在于,它不依赖于场景的三维结构,只和像素坐标以及相机之间的相对运动有关。它告诉我们,对于匹配的点对(p1, p2),它们的坐标必须满足这个方程。我们通常先用这个约束来求解相机之间的运动(R, t),或者用来校验找到的匹配点对不对。
有了相机运动,我们再回到投影方程,就能列出关于三维点P的方程了。一个点P,在两幅图像中给出两个投影方程,而未知数其实就是它的三维坐标(三个数)或者说是两个深度因子(s1和s2)。方程数多于未知数,我们就可以求解了。这就是三角化的基本思路。
3. 实战:三种主流三角化算法详解与代码实现
理论说了一堆,到底怎么算呢?这里我分享三种最常用、也最实用的三角化方法,并且配上我实际在项目中用过的代码片段。咱们不玩虚的,直接看怎么动手做。
3.1 方法一:线性三角化法(DLT)
这是最直接、最常用的一种方法,思路非常“暴力美学”:直接利用投影方程构建线性方程组,然后用最小二乘法求解。还记得投影方程 s * p = K * [R|t] * P 吗?我们可以把这个方程稍微变个形。
把内参K和外参[R|t]乘在一起,得到一个3x4的投影矩阵 M = K * [R|t]。那么投影方程可以写成:
s * [u, v, 1]^T = M * [X, Y, Z, 1]^T
我们把尺度因子s消掉。用第三行去除前两行,可以得到两个方程:
u = (M[0]·P) / (M[2]·P)
v = (M[1]·P) / (M[2]·P)
这里 M[i] 表示矩阵M的第i行向量,· 表示点积,P是三维点的齐次坐标 [X, Y, Z, 1]。
把分母乘到左边,整理一下,对于一个观测点,我们可以得到两个线性方程:
(u * M[2] - M[0]) · P = 0
(v * M[2] - M[1]) · P = 0
现在,我们从两个视角观测同一个点,就能得到四个这样的线性方程,构成一个齐次线性方程组 A * P = 0。这里的A是一个4x4的矩阵。求解这个方程组,就是在求矩阵A的零空间向量。通常,我们对A做奇异值分解(SVD),取最小奇异值对应的右奇异向量作为解,然后再将齐次坐标归一化(除以第四维),就得到了三维坐标 [X, Y, Z]。
这个方法在ORB-SLAM2和VINS-Mono里都有应用。我们来看看VINS-Mono里的实现,非常清晰:
// 仿照 VINS-Mono 风格的线性三角化
void linearTriangulation(const Eigen::Matrix3d& R1, const Eigen::Vector3d& t1,
const Eigen::Matrix3d& R2, const Eigen::Vector3d& t2,
const Eigen::Vector2d& pt1, const Eigen::Vector2d& pt2,
const Eigen::Matrix3d& K, // 相机内参
Eigen::Vector3d& point_3d) {
// 构造投影矩阵 P = K * [R | t]
Eigen::Matrix<double, 3, 4> P1, P2;
P1.block<3, 3>(0, 0) = R1;
P1.col(3) = t1;
P1 = K * P1; // 实际代码中,有时特征点已经是归一化平面坐标,则无需乘K
P2.block<3, 3>(0, 0) = R2;
P2.col(3) = t2;
P2 = K * P2;
// 构建设计矩阵 A
Eigen::Matrix4d A = Eigen::Matrix4d::Zero();
A.row(0) = pt1[0] * P1.row(2) - P1.row(0); // u1 * P1第三行 - P1第一行
A.row(1) = pt1[1] * P1.row(2) - P1.row(1); // v1 * P1第三行 - P1第二行
A.row(2) = pt2[0] * P2.row(2) - P2.row(0); // u2 * P2第三行 - P2第一行
A.row(3) = pt2[1] * P2.row(2) - P2.row(1); // v2 * P2第三行 - P2第二行
// 对A进行奇异值分解 (SVD)
Eigen::JacobiSVD<Eigen::Matrix4d> svd(A, Eigen::ComputeFullV);
Eigen::Vector4d triangulated_point = svd.matrixV().col(3); // 取最后一列,即最小奇异值对应的右奇异向量
// 齐次坐标归一化,得到三维坐标
point_3d(0) = triangulated_point(0) / triangulated_point(3);
point_3d(1) = triangulated_point(1) / triangulated_point(3);
point_3d(2) = triangulated_point(2) / triangulated_point(3);
}
代码解读:这段代码的核心就是构建那个4x4的矩阵A,然后做SVD。svd.matrixV().col(3) 取的是矩阵V的最后一列,在SVD中,这对应着最小的奇异值,其对应的右奇异向量就是方程组 A*x=0 在最小二乘意义下的解。最后一步归一化至关重要,因为SVD求解的是齐次坐标,其尺度是任意的,除以第四维就恢复了真实的尺度。
3.2 方法二:中点法(Mid-point Method)
线性三角化法虽然简单,但它最小化的误差是在“代数距离”上,并不是最符合几何意义的误差。中点法提供了一个更直观的几何解释。
它的思想是:从两个相机光心出发,分别作两条指向各自图像观测点的射线。理论上,这两条射线应该在三维空间相交于目标点。但由于噪声的存在,它们几乎永远不会完美相交。那怎么办?一个很自然的想法是:找一条线段,同时垂直于这两条射线,这条线段的中点,就可以作为三维点位置的估计。
具体步骤如下:
- 根据相机位姿和像素坐标,反投影得到两条射线在各自相机坐标系下的方向向量
d1和d2。 - 两条射线可以用参数方程表示:
L1: C1 + λ1 * d1,L2: C2 + λ2 * d2。其中C1和C2是两个相机的光心位置。 - 我们想找到参数
λ1和λ2,使得点P1 = C1 + λ1 * d1和P2 = C2 + λ2 * d2之间的距离最短。连接P1和P2的线段,应该同时垂直于d1和d2。 - 根据垂直关系,可以列出一个关于
λ1和λ2的二元一次方程组,求解即可。 - 最后,三维点坐标取
P = (P1 + P2) / 2。
我在一些对精度要求不是极端苛刻、但需要快速计算的移动端项目里用过这个方法,它的计算量比SVD小,而且几何意义非常清晰。
bool midPointTriangulation(const Eigen::Vector3d& cam_center1, const Eigen::Vector3d& ray_dir1,
const Eigen::Vector3d& cam_center2, const Eigen::Vector3d& ray_dir2,
Eigen::Vector3d& point_3d) {
// 确保方向向量是单位向量
Eigen::Vector3d d1 = ray_dir1.normalized();
Eigen::Vector3d d2 = ray_dir2.normalized();
// 计算连接两个相机光心的向量
Eigen::Vector3d c2c1 = cam_center1 - cam_center2;
// 根据几何关系,构建方程组系数
// 我们希望找到 lambda1 和 lambda2,使得 (C1 + lambda1*d1) 和 (C2 + lambda2*d2) 的连线垂直于 d1 和 d2
// 这等价于解方程: [ d1·d1, -d1·d2 ] [lambda1] = [ d1·c2c1 ]
// [ d1·d2, -d2·d2 ] [lambda2] [ d2·c2c1 ]
// 注意符号,这里推导时把C2移到左边了。
double a = d1.dot(d1); // 实际上就是1,因为是单位向量
double b = -d1.dot(d2);
double c = d1.dot(c2c1);
double d = d1.dot(d2); // 注意和b的关系
double e = -d2.dot(d2); // 就是-1
double f = d2.dot(c2c1);
// 使用克莱姆法则解二元一次方程组
double det = a * e - b * d;
if (fabs(det) < 1e-6) {
// 两射线近乎平行,三角化结果不可靠
return false;
}
double lambda1 = (c * e - b * f) / det;
double lambda2 = (a * f - c * d) / det;
// 计算两个射线上的最近点
Eigen::Vector3d p1 = cam_center1 + lambda1 * d1;
Eigen::Vector3d p2 = cam_center2 + lambda2 * d2;
// 取中点作为最终的三维点估计
point_3d = 0.5 * (p1 + p2);
return true;
}
踩坑提醒:这个方法对射线方向非常敏感。如果两视图的视角变化很小(基线短),或者特征点定位不准导致反投影射线方向误差大,那么两条射线可能近乎平行,此时 det 接近零,解会非常不稳定,算出来的点可能跑到无穷远处。所以实际使用时,一定要加上对 det 的判断,并考虑三角化的角度。
3.3 方法三:迭代优化法(非线性优化)
前面两种方法都是“一步到位”的线性求解。但理论上,最准确的方法应该是直接最小化重投影误差。什么是重投影误差?就是我估计出一个三维点P,然后把它按照相机模型重新投影到两个图像上,得到预测的像素坐标 p1_pred 和 p2_pred。然后我计算预测坐标和实际观测到的坐标 p1_obs, p2_obs 之间的像素距离。这个距离就是重投影误差。我们的目标就是找到那个三维点P,让这个误差的总和最小。
这显然不是一个线性问题了,需要用非线性优化的方法,比如高斯-牛顿法或列文伯格-马夸尔特法。在SLAM系统中,我们经常把很多个点的三角化和相机位姿一起放进一个大的优化问题里求解,这就是所谓的Bundle Adjustment(光束法平差)。
虽然完整的BA比较复杂,但我们可以看看针对单个点的重投影误差优化是什么样子:
// 定义重投影误差的结构(用于Ceres Solver等优化库)
struct ReprojectionError {
ReprojectionError(double observed_u, double observed_v,
const Eigen::Matrix3d& K,
const Eigen::Matrix3d& R, const Eigen::Vector3d& t)
: observed_u(observed_u), observed_v(observed_v), K(K), R(R), t(t) {}
template <typename T>
bool operator()(const T* const point_3d, // 三维点参数块,[x, y, z]
T* residuals) const { // 残差输出
// 将三维点变换到相机坐标系
Eigen::Matrix<T, 3, 1> P_cam = R.cast<T>() * Eigen::Matrix<T, 3, 1>(point_3d[0], point_3d[1], point_3d[2]) + t.cast<T>();
// 检查深度,必须为正
if (P_cam(2) < T(1e-3)) {
return false;
}
// 投影到归一化平面
T xp = P_cam(0) / P_cam(2);
T yp = P_cam(1) / P_cam(2);
// 应用内参,得到预测的像素坐标
T predicted_u = K(0,0) * xp + K(0,2);
T predicted_v = K(1,1) * yp + K(1,2);
// 计算残差:观测值 - 预测值
residuals[0] = T(observed_u) - predicted_u;
residuals[1] = T(observed_v) - predicted_v;
return true;
}
double observed_u, observed_v;
Eigen::Matrix3d K;
Eigen::Matrix3d R;
Eigen::Vector3d t;
};
// 使用示例(伪代码):
void optimizeTriangulation(const Eigen::Vector2d& pt1, const Eigen::Vector2d& pt2,
const Eigen::Matrix3d& R1, const Eigen::Vector3d& t1,
const Eigen::Matrix3d& R2, const Eigen::Vector3d& t2,
const Eigen::Matrix3d& K,
Eigen::Vector3d& initial_guess) { // initial_guess可由线性三角化提供
// 创建优化问题
ceres::Problem problem;
// 为第一个视图添加残差块
ceres::CostFunction* cost_function1 =
new ceres::AutoDiffCostFunction<ReprojectionError, 2, 3>(
new ReprojectionError(pt1(0), pt1(1), K, R1, t1));
problem.AddResidualBlock(cost_function1, nullptr, initial_guess.data());
// 为第二个视图添加残差块
ceres::CostFunction* cost_function2 =
new ceres::AutoDiffCostFunction<ReprojectionError, 2, 3>(
new ReprojectionError(pt2(0), pt2(1), K, R2, t2));
problem.AddResidualBlock(cost_function2, nullptr, initial_guess.data());
// 配置并运行求解器
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
options.minimizer_progress_to_stdout = false;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
// 优化后的结果就在 initial_guess 中
}
经验之谈:非线性优化精度最高,但计算量也最大,而且需要一个好的初始值(通常就用线性三角化的结果)。在实际的SLAM系统里,对于新检测到的特征点,我们往往先用线性三角化得到一个初始深度,然后随着相机运动,收集更多观测帧,再用滤波或优化的方式不断迭代更新这个深度值,这就是“深度滤波器”的工作。VINS和ORB-SLAM中的三角化线程干的就是这个事。
4. 工程实践中的挑战与应对策略
纸上谈兵总是容易的,但把三角化真正用到机器人、AR这些实时系统里,你会遇到一堆坑。我结合自己调试SLAM系统的经验,聊聊几个最常见的挑战和解决办法。
4.1 深度值的“正负”与“尺度”问题
这是一个初学者最容易懵的地方。你用线性三角化算出一个点 [0.1, 0.2, -0.5],这个Z坐标是-0.5,这合理吗?在相机坐标系下,相机光心通常是原点,光轴是Z轴正方向。所以,一个有效的三维点,其Z坐标(深度)必须是正数。如果算出来是负的,那说明这个点其实在相机的后面,这通常意味着特征点匹配错了,或者相机位姿估计错了。
怎么处理?在三角化之后,必须检查深度值的正负。通常我们会检查在两个相机坐标系下的深度是否都为正。在ORB-SLAM的初始化阶段,这个检查非常严格,因为初始的位姿和地图如果建错了,后面整个系统都会跑偏。
另一个问题是“尺度”。在单目SLAM中,从图像恢复的三维结构,其绝对尺度是未知的。你的三角化结果告诉你两点距离是1.0,但这个“1.0”可能对应现实中的1厘米,也可能是1米。这个尺度因子需要通过其他信息来确定,比如引入一个已知真实尺寸的物体(比如一张A4纸),或者通过IMU传感器(因为加速度计测量的是有物理单位的加速度)。在纯视觉初始化时,我们通常固定第一个相机到第二个相机的平移向量的模长为1,从而确立一个初始尺度,后续再通过优化和回环检测进行尺度修正。
4.2 三角化的“视差角”陷阱
三角化的精度严重依赖于视差角,也就是两个相机光心与三维点连线之间的夹角。我打个比方:你要用三角测量法测一个塔的高度,如果你站在塔底很近的两个点去测,那么算出来的高度会非常不准确,因为两条视线几乎平行。但如果你走到很远的地方,从两个差异很大的角度去测,结果就会准很多。
在视觉里也一样。如果两帧图像之间相机移动很小(比如高速运动的无人机,相邻帧间隔时间极短),基线短,视差角就小。这时候,像素观测的微小噪声(比如特征点提取差一个像素),会被放大成巨大的深度误差。你可能算出来深度是10米,实际可能是100米或者1米。
工程上的策略:
- 延迟三角化:不要一匹配上特征点就立刻三角化。等这个特征点被跟踪了足够多的帧,并且相机已经运动产生足够大的视差(比如超过5度)时,再进行三角化。ORB-SLAM就是这么干的。
- 设置视差阈值:在三角化前,计算两帧之间的旋转平移,估算视差角,如果小于阈值(例如3度),就认为三角化不可靠,放弃或等待后续帧。
- 使用多帧信息:不仅仅用两帧,而是用多帧观测一起进行三角化(比如用SVD构建一个更大的矩阵),这可以在一定程度上平摊噪声,提高鲁棒性。
4.3 应对噪声与误匹配:RANSAC的威力
真实图像中,特征点匹配不可能100%正确。总会有一些“外点”混进来,它们要么是匹配错了,要么是图像中的动态物体(比如走过的人)。如果用这些外点去三角化,不仅这个点本身是错的,还可能污染后续的优化。
怎么办?RANSAC 是解决这个问题的神器。它的思想很巧妙:我不指望所有数据都是好的,我假设数据里既有“内点”(正确的匹配)也有“外点”(错误的匹配)。我随机从匹配点对中抽选一小部分(比如8对),用它们来计算一个模型(比如基础矩阵F或者单应矩阵H),然后用这个模型去测试所有的匹配点,看看有多少点符合这个模型(误差小于某个阈值),这些就是“内点”。重复这个过程很多次,最后选择那个拥有最多“内点”的模型。
在三角化的上下文中,我们通常先用RANSAC结合对极几何(八点法求F或E矩阵)来筛选出正确的匹配点对。只有那些被判定为“内点”的匹配,才会被送去进行三角化。这一步过滤,能极大地提升初始地图点的质量。
// 一个简化的RANSAC三角化流程思路
std::vector<bool> inlier_mask;
int num_inliers = findFundamentalMatrixRANSAC(keypoints1, keypoints2, inlier_mask);
std::vector<Eigen::Vector3d> triangulated_points;
for (size_t i = 0; i < keypoints1.size(); ++i) {
if (inlier_mask[i]) {
Eigen::Vector3d pt3d;
if (triangulateWithCheck(R1, t1, R2, t2, keypoints1[i], keypoints2[i], K, pt3d)) {
// 检查深度为正、重投影误差小等
if (checkReprojectionError(pt3d, ...) < threshold) {
triangulated_points.push_back(pt3d);
}
}
}
}
4.4 三角化失败的处理与深度滤波
不是所有匹配点都能成功三角化。除了前面说的视差不足、深度为负,还可能因为数值不稳定导致SVD求解失败。一个健壮的系统必须有完善的处理机制。
更高级的策略是使用深度滤波器。对于一个新的特征点,我们不急于给它一个确定的深度值,而是给它一个深度估计的概率分布(通常用高斯分布表示,有一个均值和一个方差)。当这个点被后续新的帧观测到时,我们就用新的观测来更新这个分布(类似于卡尔曼滤波)。随着观测次数的增加,深度的估计会越来越准(方差越来越小)。当方差小到一定程度时,我们才认为这个点的深度已经收敛,可以把它正式地加入到三维地图中。SVO(半直接视觉里程计)这套方案就把深度滤波器用到了极致。
这就像我们对一个陌生人的距离进行猜测。第一眼看去,你大概觉得他在5到15米之间(方差很大)。你走几步换个角度看,发现他在8到12米之间(方差缩小)。又看了几眼后,你确信他在10米左右(方差很小,深度收敛了)。这个不断迭代更新的过程,比只用前两帧“猜”一次要稳健得多。
338



被折叠的 条评论
为什么被折叠?



