三角化到底是个啥玩意儿？

先说个Zui基本的——你有两台相机，拍到同一个点。

理论上，两条光线在空间里应该交叉。

可现实是噪声、误差把它们弄得不碰头。

于是我们就得“找个Zui佳点”，让这两条线的距离Zui小。

哈哈，这事儿就叫三角化。

Zui常见的Zuo法是把投影方程线性化。

把相机矩阵左乘一个齐次坐标，得到Ax=0这种形式。

然后用SVD求解Zui小奇异值对应的向量。

说实话，这一步算是给你一个“粗糙初始值”。

但别指望它太精确——误差还在背后暗暗捣鼓呢。

要想把误差压到Zui低，得把重投影误差直接丢进优化器里。

也就是让每个相机kan到的点和真实观测点的距离平方和Zui小化。

这玩意儿叫非线性Zui小二乘问题，听着高大上，其实核心就是梯度下降或者牛顿类算法。

假设三维点X在世界坐标系下用相机i的位姿投影到图像上得到ui。

残差ri = ui - π。

π是针孔模型，把三维变成二维。

我们要Zuo的，就是让所有ri的范数平方和降到Zui低。

先算雅可比J，也就是残差对X的导数矩阵。

然后构造近似Hessian：JᵀJ。

解线性系统 ΔX = -Jᵀr，得到geng新量ΔX。

geng新X←X+ΔX，再迭代。

害，这里有个坑——Ru果JᵀJ奇异或者条件太差，求解会出问题。

L‑M其实就是在JᵀJ上加个λI，让矩阵geng“好玩”。

λ大时像梯度下降，稳但慢；λ小则靠近牛顿法，快但可Neng不稳。

不对不对，我刚才说错了——其实L‑M会根据每次迭代的效果自动调节λ，大赞！

现实里总有几个特征匹配错了那残差会炸掉整个优化。

M‑估计用个Huber或Cauchy核，把大残差压平，让它们不那么抢眼。

这样一来优化geng靠谱，结果geng稳健。你懂的，那种“被坏点拖垮”的感觉真的hen尴尬啊。

- 步骤一： 用DLT或SVD算出粗糙X₀。哈哈，这一步几乎所有库dou自带实现啦。

- 步骤二： 把X₀投回每帧，kan残差大小。Ru果全dou在像素级别，那Yi经够好了。

- 步骤三： 若残差偏大，就启动非线性优化。

- 步骤四： 选LM作为默认求解器，因为它兼顾速度和鲁棒。

- 步骤五： 设定Zui大迭代次数和收敛阈值，例如1e‑6。

- 小技巧：Ru果相机数量超过两台，就把所有观测一起放进同一个残差向量里一次搞定。

- 注意：Ru果某帧视角极端，那对应Jacobian会退化，需要手动剔除或加权。

- Zui后一定记得把结果转回齐次坐标，否则后面会出现怪异错误。

- 好了这套流程基本Neng应付大多数SLAM/ SfM 项目了。

- 还有一点没说完——Ru果你用了GPU加速，记得同步显存，否则梯度算出来可Neng是旧值。 不对，我这句有点跑题，不过提醒一下还是不错呀！

// 假设Yi有相机矩阵K、R,t以及特征匹配points
Eigen::Vector4d X = linearTriangulation;
for{
    // 计算重投影误差 & 雅可比
    computeResidualAndJacobian;
    // LM 阻尼项
    double lambda = computeLambda;
    Eigen::MatrixXd A = J.transpose*J + lambda*Eigen::MatrixXd::Identity;
    Eigen::VectorXd b = -J.transpose*r;
    // 求解增量
    Eigen::VectorXd dX = A.ldlt.solve;
    // geng新
    X.head<3> += dX;
    // 检查收敛
    if 
常见坑点与调参经验

- 有时候Jacobian算错了导致优化永远不收敛。别忘了检查除以Z坐标时是否出现除零。 
- λ调节太激进，会导致“跳来跳去”甚至发散。建议使用“增大×10、减小÷10”这种指数策略。 
- 当特征匹配噪声极大时即使LM收敛，也可Neng得到漂移大的结果。这时候一定要加鲁棒核或者先剔除异常匹配。 
- 多视图情况下Ru果某些相机姿态估计本身不准，会把错误传播到三角化结果里。Zui好先用PNP把姿态再优化一次再来三角化。 
- 在稀疏SLAM里你会发现hen多三角化点其实只用一次就被抹掉了。这时候Ke以考虑只保留关键帧之间的点，以降低计算负担。 
- Zui后提醒一句：别总盯着数值误差低就满意，有时候视觉效果才是真正评判好坏的标准——比如重建出来的模型kan起来扁扁的不自然那说明还有隐藏bias呢！哈哈哈~

P.S. 这篇文章虽然随意，但每一句dou是我掰着指头数过来的经验碎片。
C++、Python、MATLABdou有现成实现，你挑自己喜欢的撸起来吧！
# Zui后祝你玩转三角化，一路顺风～ 🎉