企业官网建设流程全解析

1. 从一次作业说起：为什么第四讲是SLAM学习的第一道分水岭

如果你正在自学视觉SLAM，或者正在跟着某门课程啃《SLAM十四讲》，那你大概率会在第四讲附近经历一次明显的“卡顿感”。前三讲还在讲数学基础、三维空间变换、李群李代数，虽然抽象，但至少还能靠纸笔推导跟上节奏。到了第四讲，画风突然一变——开始出现相机模型、非线性优化、状态估计，作业里甚至要你动手推导、写代码、调库、跑数据。很多人就是在这个位置第一次感受到：SLAM不是“看懂”的，是“算出来”的。

第四讲讲后作业的核心，通常围绕三个东西展开：相机成像模型与畸变校正、非线性优化基础（尤其是高斯牛顿法和列文伯格-马夸尔特法）、以及基于ceres或g2o的曲线拟合/位姿优化实践。这三个东西看起来是独立的，实际上是一条线：相机把三维世界投影成二维像素，优化算法负责从带噪声的观测里反推出最可能的参数，而作业就是让你亲手把这条线走一遍。

这篇文章适合谁看？如果你是刚学完第三讲、正准备做第四讲作业但不知道从哪下手的人，这篇可以直接当参考；如果你已经做完但结果对不上、误差下不去、代码跑不通，这篇里的排查思路能帮你定位问题；如果你是想复习SLAM优化基础的老手，也可以看看我在参数选择和工程实现上的经验总结。我不打算复述教材，而是把作业里真正会卡住你的地方一个个拆开讲。

2. 第四讲作业的整体设计逻辑与核心考点拆解

2.1 作业到底在考什么：三个层次的能力

很多人拿到作业第一反应是“题目好多”，但如果把第四讲的作业按能力层次拆开，其实就三层。

第一层是建模能力。相机模型、畸变模型、观测方程，这些本质上都是把物理过程写成数学表达。作业里让你推导像素坐标和空间点的关系，不是为难你，而是让你明白：SLAM里的每一个残差项，背后都是一个物理假设。你写不出观测方程，后面优化就无从谈起。

第二层是算法能力。高斯牛顿、LM这些优化方法，教材上讲的是迭代公式，但作业要你真正理解每一步在干什么。比如雅可比矩阵到底是对谁求导、增量方程怎么解、阻尼因子怎么调，这些在纸面上看是一回事，自己推一遍又是另一回事。

第三层是工程能力。用ceres或g2o把曲线拟合跑通，看着简单，但实际会碰到模板报错、自动求导失败、初值敏感、迭代不收敛等一堆问题。这一层才是真正区分“学过”和“做过”的地方。

提示：第四讲作业的价值不在于答案对不对，而在于你是否能独立把“建模—推导—编码—调参—验证”这条链路走完。只抄答案的人，第五讲会更痛苦。

2.2 为什么选曲线拟合作为优化入门案例

作业里经常用“曲线拟合”作为非线性优化的入门例子，比如给定一堆带噪声的观测点，去拟合一条形如 y = exp(a·x² + b·x + c) 的曲线。很多人会问：SLAM不是做位姿估计吗，为什么拿曲线拟合练手？

原因很实际。曲线拟合具备非线性优化的全部核心要素：有残差、有雅可比、有迭代、有初值问题，但它的状态量少（就三个参数a、b、c），没有复杂的矩阵运算和坐标系变换。你可以把全部注意力放在优化算法本身，而不用同时处理李代数、位姿更新这些干扰项。等你在曲线拟合上把高斯牛顿和LM吃透了，再去看BA（Bundle Adjustment），会发现结构完全一样，只是状态量从3个变成了几百上千个。

这也是我在实际学习中体会最深的一点：优化算法的理解，必须在一个足够简单的问题上先跑通，再迁移到复杂问题。直接上手BA的人，往往连残差对谁求导都搞不清楚。

2.3 作业里几个容易被忽略的隐藏考点

除了明面上的题目，第四讲作业其实埋了几个隐藏考点，这些在评分里不一定体现，但对后续学习影响很大。

第一个是初值敏感性。曲线拟合里如果初值给得离谱，高斯牛顿可能直接发散。作业里通常会给一个合理初值，但你应该自己试试把初值改偏，观察迭代过程，理解为什么非线性优化是“局部”的。

第二个是雅可比矩阵的推导方式。作业可能只要求你用ceres的自动求导，但你应该手推一遍解析雅可比，对比两者的结果和速度。这个习惯在后续做视觉BA时非常关键，因为自动求导在复杂残差下会变慢甚至出错。

第三个是误差评估方式。很多人跑完优化看到cost下降就结束了，但你应该把优化后的曲线和真实曲线画在一起，看看参数误差是多少、残差分布是否合理。这才是“验证”，而不是“跑通”。

3. 相机模型与畸变校正：作业里最容易算错的部分

3.1 针孔模型：从三维点到像素的完整链路

相机模型这部分，作业通常会让你实现世界坐标到像素坐标的转换，或者反过来做去畸变。针孔模型的完整链路是这样的：世界坐标点 P_w 先经过外参（旋转R和平移t）变到相机坐标系 P_c = R·P_w + t，然后通过内参矩阵K投影到归一化平面，再经过畸变模型得到实际像素坐标。

内参矩阵K的形式是：

K = [fx 0 cx] [0 fy cy] [0 0 1]

其中fx、fy是焦距（以像素为单位），cx、cy是主点坐标。这里最容易出错的地方是归一化平面的理解。很多人直接把P_c的x、y除以z就完事了，但忘了这一步得到的是归一化坐标，还需要乘fx、fy再加cx、cy才是像素坐标。

我在批改类似作业时发现，超过一半的错误都出在坐标变换的顺序上。有人先加畸变再投影，有人把内外参顺序搞反，结果像素坐标差了几百个像素。建议你在写代码前，先在纸上把链路画一遍，标清楚每一步的输入输出维度。

3.2 畸变模型：径向畸变和切向畸变的处理细节

畸变模型一般考虑径向畸变（k1、k2、k3）和切向畸变（p1、p2）。径向畸变来自透镜形状，切向畸变来自透镜和成像平面不平行。完整的畸变公式是：

x_distorted = x(1 + k1·r² + k2·r⁴ + k3·r⁶) + 2·p1·x·y + p2·(r² + 2x²) y_distorted = y(1 + k1·r² + k2·r⁴ + k3·r⁶) + p1·(r² + 2y²) + 2·p2·x·y

其中r² = x² + y²，x、y是归一化平面坐标。

作业里常见的坑是去畸变和加畸变的方向搞反。加畸变是已知理想归一化坐标求畸变后坐标，去畸变是已知畸变后像素坐标求理想坐标。去畸变没有解析解，通常用迭代法或者直接对整张图做重映射。如果你在作业里要做去畸变，建议用OpenCV的initUndistortRectifyMap加remap，自己手写迭代容易不收敛。

注意：畸变系数k1、k2、k3的符号和量级很关键。一般k1是负值（桶形畸变）或正值（枕形畸变），量级在10⁻¹到10⁻³之间。如果你算出来的畸变位移超过几十个像素，大概率是系数用错了或者r²没算对。

3.3 内参标定与作业数据的对应关系

作业里给的数据通常包括一组世界坐标点和对应的像素坐标，让你估计内参或者验证投影关系。这里要区分两种情况：如果题目给了内参，你只需要验证投影；如果题目让你标定，那就需要用多组对应点解DLT或者用张正友标定法。

实际操作中，我建议先用给定的内参跑一遍投影，看看重投影误差有多大。如果误差在1个像素以内，说明你的模型实现是对的；如果误差几十个像素，那肯定是某个环节错了。这个“先验证再标定”的思路能帮你快速定位问题，而不是一上来就怀疑标定算法。

另外，作业数据里的像素坐标通常是整数，但真实投影是浮点数。做误差评估时要用浮点计算，不要提前取整，否则会引入额外误差。

4. 非线性优化：高斯牛顿与LM的推导和实现要点

4.1 从最小二乘到高斯牛顿：一步步推导不跳步

非线性优化的目标是最小化残差平方和：F(x) = ½·Σ||f_i(x)||²。高斯牛顿的核心思想是对残差做一阶泰勒展开，然后求解增量方程。

设当前估计为x，增量Δx，则f(x+Δx) ≈ f(x) + J·Δx，其中J是雅可比矩阵。代入目标函数并对Δx求导令其为零，得到正规方程：

Jᵀ·J·Δx = -Jᵀ·f

这就是高斯牛顿的增量方程。作业里经常让你推导这一步，很多人卡在“为什么是Jᵀ·J”上。其实很简单：目标函数对Δx求导，f(x+Δx)的平方展开后，二次项是Δxᵀ·Jᵀ·J·Δx，一次项是2·fᵀ·J·Δx，求导后就是2·Jᵀ·J·Δx + 2·Jᵀ·f = 0，约掉2就得到上面的方程。

推导不难，但雅可比矩阵J的具体形式才是作业的重点。对于曲线拟合 y = exp(a·x² + b·x + c)，残差是 f = y_obs - exp(a·x² + b·x + c)，那么J对a、b、c的偏导分别是：

• ∂f/∂a = -x²·exp(a·x² + b·x + c)
• ∂f/∂b = -x·exp(a·x² + b·x + c)
• ∂f/∂c = -exp(a·x² + b·x + c)

这个推导必须自己动手写一遍，否则用ceres自动求导时你根本不知道它在算什么。

4.2 LM算法：阻尼因子到底怎么调

LM算法在高斯牛顿的基础上引入了阻尼因子λ，增量方程变成：

(Jᵀ·J + λ·I)·Δx = -Jᵀ·f

λ的作用是控制迭代步长：λ大时接近梯度下降，λ小时接近高斯牛顿。作业里通常要求你实现一个简单的λ调整策略，比如：如果新残差比旧残差小，就接受这次迭代并减小λ（比如λ /= 10）；如果残差变大，就拒绝迭代并增大λ（比如λ *= 10）。

这里的关键经验是：λ的初始值和调整倍率对收敛速度影响很大。我一般初始λ取1e-3，调整倍率取10。如果发现迭代震荡，就把初始λ调大；如果收敛太慢，就调小。作业里如果只要求跑通，用固定λ也能收敛，但理解自适应λ才是LM的精髓。

提示：LM的阻尼项加在Jᵀ·J的对角线上，不是随便加的。当某个参数方向梯度很小时，Jᵀ·J对应元素接近零，加上λ能保证矩阵可逆。这也是LM比高斯牛顿更稳的原因。

4.3 手写优化与调库的取舍：什么时候该自己实现

作业里通常会有两部分：一部分让你手写高斯牛顿或LM，另一部分让你用ceres或g2o。很多人觉得调库就行了，手写没必要。但我的经验是：手写一遍，你才知道调库时那些参数在干什么。

手写优化的好处是你能完全控制迭代过程，打印每一步的残差、增量、λ值，观察收敛行为。调库的好处是工程上更高效，ceres的自动求导和鲁棒核函数能省很多事。两者不是替代关系，而是互补。

具体到作业，我建议的顺序是：先手写高斯牛顿跑通曲线拟合，再手写LM对比收敛速度，最后用ceres实现同样的残差，对比结果是否一致。如果ceres的结果和你手写的差很多，那大概率是你的雅可比推错了或者残差定义不一致。

5. 基于ceres和g2o的实操过程与参数配置

5.1 ceres曲线拟合：从残差定义到求解器配置

用ceres做曲线拟合，核心是定义一个CostFunctor。以 y = exp(a·x² + b·x + c) 为例，残差是预测值减观测值。代码结构大致如下：

struct CurveFittingCost { CurveFittingCost(double x, double y) : x_(x), y_(y) {} template <typename T> bool operator()(const T* const abc, T* residual) const { residual[0] = T(y_) - ceres::exp(abc[0]*T(x_)*T(x_) + abc[1]*T(x_) + abc[2]); return true; } double x_, y_; };

然后构建Problem，添加残差块，配置求解器：

ceres::Problem problem; for (int i = 0; i < N; ++i) { problem.AddResidualBlock( new ceres::AutoDiffCostFunction<CurveFittingCost, 1, 3>( new CurveFittingCost(x_data[i], y_data[i])), nullptr, abc); } ceres::Solver::Options options; options.linear_solver_type = ceres::DENSE_QR; options.minimizer_progress_to_stdout = true; options.max_num_iterations = 100;

这里有几个参数值得注意。linear_solver_type对于小规模问题用DENSE_QR就够了，大规模BA要用SPARSE_NORMAL_CHOLESKY。max_num_iterations默认是50，曲线拟合一般20次以内就收敛了。minimizer_progress_to_stdout打开后能看到每次迭代的cost变化，方便调试。

5.2 g2o曲线拟合：顶点、边与优化器配置

g2o的写法比ceres繁琐一些，需要定义顶点（优化变量）和边（残差）。曲线拟合里只有一个顶点，维度是3（a、b、c），边是一元边，连接这个顶点。

顶点定义：

class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW virtual void setToOriginImpl() override { _estimate << 0, 0, 0; } virtual void oplusImpl(const double* update) override { _estimate += Eigen::Vector3d(update); } virtual bool read(std::istream&) { return false; } virtual bool write(std::ostream&) const { return false; } };

边的定义需要实现computeError和linearizeOplus：

class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW CurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {} virtual void computeError() override { const CurveFittingVertex* v = static_cast<const CurveFittingVertex*>(_vertices[0]); const Eigen::Vector3d abc = v->estimate(); _error(0,0) = _measurement - std::exp(abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0)); } virtual void linearizeOplus() override { const CurveFittingVertex* v = static_cast<const CurveFittingVertex*>(_vertices[0]); const Eigen::Vector3d abc = v->estimate(); double y = std::exp(abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0)); _jacobianOplusXi[0] = -_x*_x*y; _jacobianOplusXi[1] = -_x*y; _jacobianOplusXi[2] = -y; } double _x; };

g2o的坑在于雅可比必须自己写，写错了优化就不收敛。而且g2o的更新方式是_estimate += update，对于向量空间没问题，但对于旋转矩阵就需要用流形上的更新，这也是为什么后续位姿优化要用Sophus。

5.3 两种库的结果对比与误差评估

跑完ceres和g2o后，你应该对比两者的结果。正常情况下，两者的优化参数应该非常接近，差异在1e-6量级。如果差很多，检查以下几点：残差定义是否一致、雅可比符号是否正确、初值是否相同、迭代次数是否足够。

误差评估我一般看三个指标：最终cost值、参数误差（和真值比）、残差分布。最终cost值反映拟合程度，参数误差反映估计精度，残差分布反映是否存在系统性偏差。如果残差分布有明显趋势，说明模型选错了，不是优化的问题。

6. 常见问题与排查技巧实录

6.1 优化不收敛：从初值、雅可比到阻尼因子逐项排查

优化不收敛是第四讲作业最高频的问题。我的排查顺序是这样的：

我踩过最坑的一次是雅可比符号写反了，结果cost先降后升，折腾了半天才发现。后来养成的习惯是：手写雅可比后，用数值求导验证一遍，误差在1e-6以内才继续。

6.2 编译报错：ceres和g2o的模板错误怎么读

ceres和g2o都是重度模板库，编译报错往往几百行，看着吓人。我的经验是：只看第一个错误，而且只看和你的代码相关的部分。

ceres常见错误是CostFunctor的operator()签名不对，比如忘了template、residual维度写错、参数指针类型不对。g2o常见错误是顶点维度不匹配、边没有实现linearizeOplus、EIGEN_MAKE_ALIGNED_OPERATOR_NEW没加。

提示：如果报错里出现“no matching function for call”，先检查你的operator()参数类型和数量是否和模板参数一致。如果出现“static assertion failed”，检查维度是否匹配。

6.3 结果对不上：数值精度、坐标系和单位问题

有时候代码跑通了，但结果和参考答案差很多。这种情况通常不是算法错，而是细节错。我遇到过的问题包括：角度和弧度混用、像素坐标和归一化坐标混用、单精度和双精度混用、数据读取时行列搞反。

排查这类问题，我建议用“小数据验证法”：自己构造一组无噪声数据，用已知参数生成观测，然后看优化能否恢复参数。如果无噪声都恢复不了，那肯定是代码有bug；如果无噪声能恢复，有噪声不行，那是正常现象，调调初值和迭代次数就行。

6.4 独家避坑清单：我踩过的五个典型坑

第一个坑：ceres自动求导对std::exp支持没问题，但对某些自定义函数不支持，必须用ceres::exp。第二个坑：g2o的_measurement类型要和边模板参数一致，否则编译能过但运行结果错。第三个坑：LM的阻尼因子如果一直增大不减小，说明残差一直在变大，检查雅可比。第四个坑：曲线拟合的x数据如果范围很大（比如0到1000），exp会溢出，需要先归一化。第五个坑：优化后的参数要用double输出，不要用float，否则精度不够。

7. 从作业到实战：第四讲内容在后续SLAM中的延伸

第四讲作业里的曲线拟合，看起来和SLAM没关系，但它的结构就是BA的简化版。曲线拟合里的参数a、b、c，对应BA里的位姿和路标点；曲线拟合里的残差，对应BA里的重投影误差；曲线拟合里的雅可比，对应BA里的雅可比矩阵。你在曲线拟合上花的时间，会在做BA时成倍省回来。

再往后走，第五讲会讲特征点法和直接法，第六讲会讲非线性优化在视觉里程计里的应用。到那时候你会发现，第四讲的高斯牛顿和LM是贯穿始终的工具。我个人的体会是：第四讲没吃透的人，后面每一讲都会卡；第四讲吃透的人，后面只是换状态量和残差形式，优化框架是不变的。

如果你现在正在做第四讲作业，我的建议是：不要急着交，把曲线拟合手写一遍，把雅可比推一遍，把ceres和g2o都跑一遍，把不收敛的情况故意制造出来再解决掉。这个过程比答案本身值钱得多。等你做到第八讲、第九讲回头再看，会发现第四讲是整个SLAM学习里性价比最高的一讲。