做非线性优化的朋友应该都有体会Ceres里最常用的就是CostFunction、LossFunction、LocalParameterization这三件套。但如果你遇到过“残差已经算好了我还想在它进入优化器之前再做一层加工”这种需求多少会有点别扭。改CostFunction里的Evaluate吧数学逻辑和工程细节全搅在一起不改吧某些场景又确实需要这一步。我在读Ceres源码的时候发现它其实提供了一个现成的包装类ConditionedCostFunction。这篇文章就对着源码把它的实现拆开讲清楚包括它到底解决了什么问题、Evaluate里那个雅可比矩阵是怎么被巧妙改写的以及在什么场景下你才值得用它。下面直接进入正文。1. 为什么需要ConditionedCostFunction残差后处理的典型场景1.1 初看类名第一反应是“多包一层有什么用”我第一次在Ceres头文件里看到ConditionedCostFunction时内心其实是有点不以为然的既然我能手写Evaluate那在函数最后一句话把residuals改一改不就行了吗为什么要专门包一层这种想法在上手写了几个自定义CostFunction之后就会被现实打脸。最核心的问题是耦合。你写好的量测模型比如点到平面距离、重投影误差这些函数本质上是“传感器物理模型 几何关系”的数学表达。但实际优化时你往往还想对残差做别的事情某个分量要从米制改成毫米制、某个分量要往一个目标值上拉、某个分量在超过一定范围后要减弱它对优化方向的牵引力。如果这些都塞进同一个Evaluate量测模型和后处理策略就彻底绑死了。而ConditionedCostFunction的思路非常直接它把“残差后处理”单独抽出来做成一个独立的包装器内部持有一个普通的CostFunction和一组Condition。CostFunction负责算原始残差和雅可比Condition负责把每个残差分量再做一次标量变换。1.2 它和LossFunction的分工是不一样的很多读者会想残差加权不是有LossFunction吗HuberLoss、SoftLOneLoss这些不是现成的吗这里要先把两者的边界划清楚。LossFunction的作用点在整个残差块的平方范数上。Ceres算完一个残差块的平方和之后用rho(f(x)^T f(x))乘一下从整体上改变这个残差块对梯度方向的贡献。它不改变残差向量里的具体数值。ConditionedCostFunction则完全不同。它改变的是残差向量本身每个残差分量经过Condition变换后数值被替换成新的值雅可比矩阵也会按照链式法则同步更新。之后再进入LossFunction做整体加权。所以两者是“先变换、后加权”的串联关系不是同一层作用。1.3 三个值得用它而不是手写Evaluate的场景场景不用它时的做法用它时的做法不同残差分量量纲不一致角度和米混在一起在CostFunction里硬乘固定系数散落各处的magic number对指定残差分量套一个ScaleCondition希望某个残差分量尽量逼近一个先验目标值额外开一个残差项和原残差形成两段逻辑对已有残差项套一个EqualityCondition目标值作为构造参数传入残差超过一定范围后需要弱化牵引力软截断在Evaluate里加if分支导数也要跟着分情况套一个分段线性或soft clamping的Condition这三种需求有一个共同点它们都是对残差的“后处理”而不是对参数化模型的修改。所以用一个包装类来做反而比改CostFunction本身更干净。2. 源码骨架解读构造、析构与成员隐藏约定2.1 头文件里藏着的那点成员不管手头是哪个小版本头文件里这个类的主体结构基本都长这样namespace ceres { class CERES_EXPORT ConditionedCostFunction : public CostFunction { public: ConditionedCostFunction(CostFunction* wrapped_cost_function, const std::vectorCondition* conditions); ~ConditionedCostFunction() override; bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const override; private: std::unique_ptrCostFunction wrapped_cost_function_; std::vectorCondition* conditions_; }; } // namespace ceres注意这里有几个信息量很大的点。第一它本身是一个CostFunction的子类所以你可以直接把它的实例交给Problem::AddResidualBlock对外完全透明。第二它没有自己的参数块所有参数块的配置全部透传给内部被包装的那个CostFunction。第三被包装的CostFunction用智能指针管理而conditions_是一个裸指针的vector。这个细节直接决定了后面要注意的所有权问题。2.2 构造函数的两个核心职责数量校验与元数据透传从源码阅读的角度看构造函数其实只做了两件事但每一件都必不可少。第一件事是防御性校验。既然Condition是逐分量绑定残差的那么conditions的数量就必须和底层CostFunction的残差维数完全一致。不一致的话程序会直接崩溃在CHECK上而不是等到优化迭代时才报一些莫名其妙的错。第二件事是把元数据透传上去。底层CostFunction有num_residuals()和parameter_block_sizes()这两个接口ConditionedCostFunction不能只是转发调用因为它自己也是CostFunctionProblem在AddResidualBlock阶段会直接读它自身的元数据来分配内存、初始化雅可比矩阵。ConditionedCostFunction::ConditionedCostFunction( CostFunction* wrapped_cost_function, const std::vectorCondition* conditions) : wrapped_cost_function_(wrapped_cost_function), conditions_(conditions) { CHECK(wrapped_cost_function_ ! nullptr); CHECK_EQ(wrapped_cost_function_-num_residuals(), conditions_.size()); set_num_residuals(wrapped_cost_function_-num_residuals()); *mutable_parameter_block_sizes() wrapped_cost_function_-parameter_block_sizes(); }这个透传逻辑是我读了源码之后才注意到的重要细节。如果漏掉这一步外部看起来这个CostFunction的残差数是0参数块列表是空的后面优化器在分配内存的时候直接就是灾难。2.3 析构函数与所有权约定新版本里wrapped_cost_function_是智能指针析构时自动释放。而conditions_是裸指针数组需要手动遍历释放。两个加起来意味着一个最重要的所有权约定一旦你把一个Condition对象传给了ConditionedCostFunction它的生命周期就交给这个包装类了外面不能再手动delete。我见过好几次“双重释放”和“悬垂指针”都是从这来的。典型错误是在函数栈上建了一个Condition局部对象然后传给包装类包装类析构时对一个栈对象做delete直接崩溃或者两个ConditionedCostFunction共享同一个Condition*第一个析构把对象释放了第二个析构时重复释放。从工程实践角度我的习惯是让ConditionedCostFunction成为唯一的所有者并且尽可能用一个函数把所有new出来的对象一次性组装好减少中途出错的概率。3. Evaluate第一步原始残差是怎么被Condition逐个替换的3.1 一切从调用底层CostFunction开始Evaluate是整个包装器的心脏也是源码阅读的重点。它的第一步非常朴素先把参数原封不动传给内部那个被包装的CostFunction让它计算原始残差和原始雅可比。bool ConditionedCostFunction::Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { bool success wrapped_cost_function_-Evaluate(parameters, residuals, jacobians); if (!success) { return false; } // 接下来是残差逐分量变换的循环 // ... }这个设计有一个很好的副作用底层CostFunction内部对雅可比矩阵的布局要求完全不用变ConditionedCostFunction只是站在外面“加工”它的输出。所以你可以拿着任何已有的CostFunction直接包一层而不需要改原类。3.2 逐分量处理每个Condition只面对一个标量接下来是核心循环。源码逻辑大致是这样的int num_residuals wrapped_cost_function_-num_residuals(); int num_parameter_blocks wrapped_cost_function_-parameter_block_sizes().size(); std::vectordouble condition_residuals(num_residuals); std::vectordouble condition_jacobians(num_residuals); for (int i 0; i num_residuals; i) { bool alloc_jacobian (jacobians ! nullptr); if (conditions_[i]-Evaluate(residuals[i], condition_residuals[i], alloc_jacobian ? condition_jacobians[i] : nullptr)) { return false; } residuals[i] condition_residuals[i]; }这里面的结构非常清晰。对第i个残差分量把旧的residuals[i]作为Condition的输入Condition计算出新的残差值先存进condition_residuals[i]最后再统一写回residuals[i]。这里有两个细节值得停下来想一下。细节一是为什么要用临时数组。因为Condition是逐个执行的如果每算完一个就直接写回residuals[i]后面的Condition拿到的就不是原始残差而是被前面处理过的残差了。源码这里的“先全部算完再统一写回”其实是一种很典型的规约模式避免前后条件互相污染。细节二是第三个参数jacobian的非空判断。如果调用方传入的jacobians是nullptr说明优化器此刻不要求解析雅可比那Condition实现里可以跳过导数计算省掉一次不必要的数学运算。这个细节在后面的雅可比处理里非常重要。3.3 失败传播语义Condition返回true就是出错了还有一个容易踩坑的细节Condition::Evaluate的返回值和CostFunction::Evaluate的返回值语义是反的。CostFunction返回false表示失败而Condition返回true表示失败。为什么会有这种不一致因为CostFunction沿袭了Ceres主接口的约定而Condition接口在早期设计时参考了另一套内部习惯。所以读源码的时候不要靠直觉判断一定要看清楚注释里“true is failure”的说明。一旦某个Condition返回了true整个Evaluate立刻返回false优化器会认为这个残差块不可用进而终止迭代或者跳过这一步。这其实是一个很有用的防御机制如果你的Condition在某个输入位置没有定义比如根号下出现了负数你可以返回true让优化器知道这里出了问题而不是给一个非法数值把它带偏。4. Evaluate第二步雅可比矩阵的行缩放与链式法则落地4.1 链式法则公式推导ConditionedCostFunction最精华的部分也是我读完后觉得“这个类真不简单”的部分在于它对雅可比矩阵的处理。假设原始残差是r_i经过Condition变换得到新残差r_ir_i C_i(r_i)其中r_i f_i(p)是底层CostFunction关于某些参数块p的原始残差。根据链式法则∂r_i / ∂p C_i(r_i) * ∂r_i / ∂p这里C_i(r_i)是一个标量∂r_i / ∂p是一行向量。乘法的效果就是原始雅可比矩阵的第i行整体乘以C_i(r_i)。把所有残差分量放在一起看整个雅可比矩阵的变换等价于左乘一个对角矩阵J_new diag(C_1(r_1), C_2(r_2), ..., C_n(r_n)) * J这就是源码里所谓“行缩放”的由来。你不需要重新构建什么复杂的矩阵乘积只需要对旧雅可比矩阵的每一行做一次标量乘法。4.2 源码里那个行缩放循环有了公式之后再看源码一切都顺理成章了if (jacobians ! nullptr) { for (int j 0; j num_parameter_blocks; j) { if (jacobians[j] ! nullptr) { const int num_parameters wrapped_cost_function_-parameter_block_sizes()[j]; for (int i 0; i num_residuals; i) { VectorRef(jacobians[j] i * num_parameters, num_parameters) * condition_jacobians[i]; } } } } return true;这个双层循环的顺序很有讲究。外层先遍历参数块。一个残差块可能关联多个参数块每个参数块在自己的雅可比数组里占据一段连续内存。内层再遍历残差维度。Ceres的雅可比矩阵布局是行优先的jacobians[j]这一段内存里第i个残差分量对应的导数存放在下标i * num_parameters开始的位置。所以VectorRef(jacobians[j] i * num_parameters, num_parameters)这行代码本质上是把“第i行”包装成一个Eigen的Map对象然后整个乘上condition_jacobians[i]。用*的好处是原地更新不会为新矩阵额外分配内存。4.3 一个容易被忽视的推论雅可比为0的语义既然雅可比每一行都要乘condition_jacobians[i]那么一个很自然的推论是如果你写了一个Condition它的导数在某段输入范围内恒等于0那么对应残差行的雅可比也会变成0。这意味着什么意味着这个残差分量在当前迭代点附近不提供任何梯度信息。但注意残差本身可能并不是0。在LM算法里这会表现为信息矩阵的某个方向降秩优化器可能认为这个约束已经“被满足”了从而放弃对它进行校正。我在实际项目里踩过一次这种坑写了一个硬截断的Condition残差超过阈值直接给一个常数导数写0结果优化进程在边界处卡了很久。后来把导数从0改成一个小斜坡比如1e-6问题立刻消失。后面避坑章节还会再展开。5. Condition接口拆解与自定义一个非线性条件5.1 Condition的纯虚接口看ConditionedCostFunction源码的时候顺带要把Condition接口也读了。它比想象中简单class CERES_EXPORT Condition { public: virtual ~Condition() {} virtual bool Evaluate(const double* parameters, double* residuals, double* jacobian) const 0; virtual int num_parameters() const 0; };这里parameters是这个Condition的输入residuals是输出两者都是标量指针。jacobian指向一个长度至少为num_parameters()的数组存放“输出对每个输入的偏导数”。在ConditionedCostFunction的使用上下文里Condition的输入被绑定到某个残差分量上所以实际上num_parameters()几乎总是1。少数特例可能是0代表一个不依赖输入、直接输出常量的条件。5.2 从源码看官方实现的套路Ceres源码里给出的Condition实现大体上有两条路线。一条是数值微分路线。把残差变换函数写成一个模板函数然后用中心差分对输入求导。优点是实现成本低你只需要关心数学表达式不需要手搓导数缺点是每次Evaluate要多算两次函数值而且差分布长选得不合适时导数精度会受影响。另一条是解析雅可比路线。要求实现者同时给出函数值和函数对输入的导数。这里有个常见的误解需要澄清Condition的雅可比是对“输入残差”求导而不是对“优化参数块”求导。对优化参数块的导数是由ConditionedCostFunction通过行缩放完成的。如果你在自定义Condition时把这两者搞反包装后的整个雅可比矩阵都会是错的而且很难排查。5.3 手写一个ScaleThenClampCondition理解了接口写一个自定义Condition就不难了。我以一个“缩放后再截断”的条件为例这个在工程里很实用class ScaleThenClampCondition final : public ceres::Condition { public: ScaleThenClampCondition(double scale, double lower, double upper) : scale_(scale), lower_(lower), upper_(upper) {} int num_parameters() const override { return 1; } bool Evaluate(const double* parameters, double* residuals, double* jacobian) const override { const double r parameters[0]; const double scaled scale_ * r; if (scaled lower_) { *residuals lower_; if (jacobian ! nullptr) { jacobian[0] 0.0; } } else if (scaled upper_) { *residuals upper_; if (jacobian ! nullptr) { jacobian[0] 0.0; } } else { *residuals scaled; if (jacobian ! nullptr) { jacobian[0] scale_; } } return true; } private: double scale_; double lower_; double upper_; };这个类本身很直观在非饱和区输出是scale乘输入导数就是scale在饱和区输出恒定导数就是0。但正如前面说的jacobian[0] 0.0的做法要非常谨慎。如果你真的要做硬截断可以考虑把饱和区的导数设成一个极小的正数而不是严格的0这样至少优化器不会在边界处完全失去方向。6. 完整实战给一个点到平面距离残差加软截断6.1 底层CostFunction简单的点到平面距离为了把上面的源码讲通这里用一个最简单的例子串一遍。假设你在做一个点云对齐问题需要优化一个三维点p的位置让它尽量落在某个平面n·p d 0上。残差定义为一个标量class PlaneDistanceCostFunction final : public ceres::CostFunction { public: PlaneDistanceCostFunction(const Eigen::Vector3d n, double d) : n_(n), d_(d) { set_num_residuals(1); mutable_parameter_block_sizes()-push_back(3); } bool Evaluate(const double* const* parameters, double* residuals, double** jacobians) const override { const Eigen::Mapconst Eigen::Vector3d p(parameters[0]); residuals[0] n_.dot(p) d_; if (jacobians ! nullptr jacobians[0] ! nullptr) { Eigen::MapEigen::RowVector3d(jacobians[0]) n_.transpose(); } return true; } private: Eigen::Vector3d n_; double d_; };这个类非常简单残差维数是1参数块是1个三维向量雅可比就是平面的法向量。6.2 用Condition做软截断现在设想一个真实场景初始位姿特别差点到平面的距离可能达到10米甚至20米。如果直接用线性距离残差优化初期会产生非常大的梯度LM迭代容易“冲过头”。这时可以用一个tanh软截断让距离超过一定范围后对梯度的贡献慢慢减小class SoftTanhCondition final : public ceres::Condition { public: explicit SoftTanhCondition(double scale) : scale_(scale) {} int num_parameters() const override { return 1; } bool Evaluate(const double* parameters, double* residuals, double* jacobian) const override { const double r parameters[0]; *residuals scale_ * std::tanh(r / scale_); if (jacobian ! nullptr) { const double c std::cosh(r / scale_); jacobian[0] 1.0 / (c * c); } return true; } private: double scale_; };当输入r绝对值很小时tanh(r)近似等于r此时约等于没变换当r绝对值很大时tanh趋向于1导数趋向于0。这样优化初期就不会被一个巨大距离残差带偏方向。6.3 组装并加入优化Problem组装方式如下std::vectorceres::Condition* conditions; conditions.push_back(new SoftTanhCondition(2.0)); ceres::CostFunction* cost new ceres::ConditionedCostFunction( new PlaneDistanceCostFunction(n, d), conditions); ceres::Problem problem; problem.AddResidualBlock(cost, nullptr, point_param);point_param是一个长度为3的double数组。把所有初始点放在距离平面很远的地方分别跑一次“不加Condition”和“加Condition”的优化能明显看到前者在头几步迭代的步长非常夸张后者则更温和。用表格对比一下这个差异方案迭代初期的行为收敛稳定性实现成本直接线性残差梯度大步长容易过大依赖信赖域策略兜底最低加SoftTanhCondition距离越大牵引越小步长平滑差初值下表现更稳定低只换LossFunction整体加权但残差数值不变比线性好但对单个分量控制力弱最低这里也回应了上文说的边界LossFunction是整体权重缩放而Condition是对单个残差分量做非线性映射。它是另一个维度的工具。7. 读完后必须提醒的几个坑7.1 所有权与double delete这是用这个类最容易踩的坑没有之一。ConditionedCostFunction析构时会释放内部被包装的CostFunction也会释放所有Condition对象。所以不要把栈上创建的Condition对象塞进去不要让两个包装类共享同一个Condition*如果自己又额外delete了一份那就离崩溃不远了。我自己的实践是在建图优化、多传感器标定这类项目里统一用一个工厂函数创建整个残差块所有权链非常清晰。7.2 饱和区雅可比为0的数值风险上面已经提到过硬截断会让导数在某段区间为0。这种设计在数值上非常危险残差不为0但梯度为0优化器会认定这个约束已经满足从而不再移动参数。很多时候表现为优化进程卡在某个不太好的位置怎么迭代都不动。如果确实需要硬截断有几种变通方案把饱和区的导数设成一个很小的斜坡比如1e-6改用tanh这类平滑函数把“截断”软化在Condition内部做一次带死区的线性映射保证整个定义域内导数不为0。7.3 别拿它做跨残差的耦合变换ConditionedCostFunction的哲学是“一个Condition处理一个残差分量”。每个Condition的输入都只是一个标量天然无法看到其他残差分量的值。如果有人想用它做残差白化、把二维向量投影到切平面、或者对不同残差分量做线性组合那这个类就不适合。跨维度的耦合变换应该直接写一个新的CostFunction在Evaluate里自己控制多维残差和完整的雅可比矩阵。7.4 调试技巧从雅可比入手最后分享一个排查问题的实用方法。如果你发现套了Condition之后优化结果不对第一步先验证雅可比。方法很简单写一个中心差分的小脚本把一个参数分量扰动1e-6计算残差变化量再对比ConditionedCostFunction返回的解析雅可比。这个检查往往能很快定位到问题根源八成是自定义Condition里的jacobian写错了或者对输入求导和对参数块求导搞混了。因为ConditionedCostFunction本身的行缩放逻辑是纯粹的线性代数只要头尾两个环节底层CostFunction雅可比和Condition雅可比是对的结果就一定是对的。读这套源码给我最大的收获并不是那个行缩放循环本身有多精妙而是Ceres把“残差后处理”抽象成独立组件的思路。后来我在做多传感器标定的时候把不同传感器残差的量纲归一化全部挪到Condition里实现主CostFunction保持纯粹的量测模型代码结构一下子就清晰了。如果你也想在自己的项目里引入它建议先从一个最简单的ScaleCondition跑通整体流程再逐步加重非线性变换。另外千万别忽略Condition接口那第三个形参jacobian——在源码里它被一个vector收集后直接决定了雅可比矩阵每一行的缩放因子。把这一点吃透ConditionedCostFunction的源码阅读就算真正到位了。 
长河君 · 2026/10/16 21:13:00 · 阅读 1.2k Ceres ConditionedCostFunction源码解析：残差后处理与雅可比行缩放
做非线性优化的朋友应该都有体会Ceres里最常用的就是CostFunction、LossFunction、LocalParameterization这三件套。但如果你遇到过“残差已经算好了我还想在它进入优化器之前再做一层加工”这种需求多少会有点别扭。改CostFunction里的Evaluate吧数学逻辑和工程细节全搅在一起不改吧某些场景又确实需要这一步。我在读Ceres源码的时候发现它其实提供了一个现成的包装类ConditionedCostFunction。这篇文章就对着源码把它的实现拆开讲清楚包括它到底解决了什么问题、Evaluate里那个雅可比矩阵是怎么被巧妙改写的以及在什么场景下你才值得用它。下面直接进入正文。1. 为什么需要ConditionedCostFunction残差后处理的典型场景1.1 初看类名第一反应是“多包一层有什么用”我第一次在Ceres头文件里看到ConditionedCostFunction时内心其实是有点不以为然的既然我能手写Evaluate那在函数最后一句话把residuals改一改不就行了吗为什么要专门包一层这种想法在上手写了几个自定义CostFunction之后就会被现实打脸。最核心的问题是耦合。你写好的量测模型比如点到平面距离、重投影误差这些函数本质上是“传感器物理模型 几何关系”的数学表达。但实际优化时你往往还想对残差做别的事情某个分量要从米制改成毫米制、某个分量要往一个目标值上拉、某个分量在超过一定范围后要减弱它对优化方向的牵引力。如果这些都塞进同一个Evaluate量测模型和后处理策略就彻底绑死了。而ConditionedCostFunction的思路非常直接它把“残差后处理”单独抽出来做成一个独立的包装器内部持有一个普通的CostFunction和一组Condition。CostFunction负责算原始残差和雅可比Condition负责把每个残差分量再做一次标量变换。1.2 它和LossFunction的分工是不一样的很多读者会想残差加权不是有LossFunction吗HuberLoss、SoftLOneLoss这些不是现成的吗这里要先把两者的边界划清楚。LossFunction的作用点在整个残差块的平方范数上。Ceres算完一个残差块的平方和之后用rho(f(x)^T f(x))乘一下从整体上改变这个残差块对梯度方向的贡献。它不改变残差向量里的具体数值。ConditionedCostFunction则完全不同。它改变的是残差向量本身每个残差分量经过Condition变换后数值被替换成新的值雅可比矩阵也会按照链式法则同步更新。之后再进入LossFunction做整体加权。所以两者是“先变换、后加权”的串联关系不是同一层作用。1.3 三个值得用它而不是手写Evaluate的场景场景不用它时的做法用它时的做法不同残差分量量纲不一致角度和米混在一起在CostFunction里硬乘固定系数散落各处的magic number对指定残差分量套一个ScaleCondition希望某个残差分量尽量逼近一个先验目标值额外开一个残差项和原残差形成两段逻辑对已有残差项套一个EqualityCondition目标值作为构造参数传入残差超过一定范围后需要弱化牵引力软截断在Evaluate里加if分支导数也要跟着分情况套一个分段线性或soft clamping的Condition这三种需求有一个共同点它们都是对残差的“后处理”而不是对参数化模型的修改。所以用一个包装类来做反而比改CostFunction本身更干净。2. 源码骨架解读构造、析构与成员隐藏约定2.1 头文件里藏着的那点成员不管手头是哪个小版本头文件里这个类的主体结构基本都长这样namespace ceres { class CERES_EXPORT ConditionedCostFunction : public CostFunction { public: ConditionedCostFunction(CostFunction* wrapped_cost_function, const std::vectorCondition* conditions); ~ConditionedCostFunction() override; bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const override; private: std::unique_ptrCostFunction wrapped_cost_function_; std::vectorCondition* conditions_; }; } // namespace ceres注意这里有几个信息量很大的点。第一它本身是一个CostFunction的子类所以你可以直接把它的实例交给Problem::AddResidualBlock对外完全透明。第二它没有自己的参数块所有参数块的配置全部透传给内部被包装的那个CostFunction。第三被包装的CostFunction用智能指针管理而conditions_是一个裸指针的vector。这个细节直接决定了后面要注意的所有权问题。2.2 构造函数的两个核心职责数量校验与元数据透传从源码阅读的角度看构造函数其实只做了两件事但每一件都必不可少。第一件事是防御性校验。既然Condition是逐分量绑定残差的那么conditions的数量就必须和底层CostFunction的残差维数完全一致。不一致的话程序会直接崩溃在CHECK上而不是等到优化迭代时才报一些莫名其妙的错。第二件事是把元数据透传上去。底层CostFunction有num_residuals()和parameter_block_sizes()这两个接口ConditionedCostFunction不能只是转发调用因为它自己也是CostFunctionProblem在AddResidualBlock阶段会直接读它自身的元数据来分配内存、初始化雅可比矩阵。ConditionedCostFunction::ConditionedCostFunction( CostFunction* wrapped_cost_function, const std::vectorCondition* conditions) : wrapped_cost_function_(wrapped_cost_function), conditions_(conditions) { CHECK(wrapped_cost_function_ ! nullptr); CHECK_EQ(wrapped_cost_function_-num_residuals(), conditions_.size()); set_num_residuals(wrapped_cost_function_-num_residuals()); *mutable_parameter_block_sizes() wrapped_cost_function_-parameter_block_sizes(); }这个透传逻辑是我读了源码之后才注意到的重要细节。如果漏掉这一步外部看起来这个CostFunction的残差数是0参数块列表是空的后面优化器在分配内存的时候直接就是灾难。2.3 析构函数与所有权约定新版本里wrapped_cost_function_是智能指针析构时自动释放。而conditions_是裸指针数组需要手动遍历释放。两个加起来意味着一个最重要的所有权约定一旦你把一个Condition对象传给了ConditionedCostFunction它的生命周期就交给这个包装类了外面不能再手动delete。我见过好几次“双重释放”和“悬垂指针”都是从这来的。典型错误是在函数栈上建了一个Condition局部对象然后传给包装类包装类析构时对一个栈对象做delete直接崩溃或者两个ConditionedCostFunction共享同一个Condition*第一个析构把对象释放了第二个析构时重复释放。从工程实践角度我的习惯是让ConditionedCostFunction成为唯一的所有者并且尽可能用一个函数把所有new出来的对象一次性组装好减少中途出错的概率。3. Evaluate第一步原始残差是怎么被Condition逐个替换的3.1 一切从调用底层CostFunction开始Evaluate是整个包装器的心脏也是源码阅读的重点。它的第一步非常朴素先把参数原封不动传给内部那个被包装的CostFunction让它计算原始残差和原始雅可比。bool ConditionedCostFunction::Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { bool success wrapped_cost_function_-Evaluate(parameters, residuals, jacobians); if (!success) { return false; } // 接下来是残差逐分量变换的循环 // ... }这个设计有一个很好的副作用底层CostFunction内部对雅可比矩阵的布局要求完全不用变ConditionedCostFunction只是站在外面“加工”它的输出。所以你可以拿着任何已有的CostFunction直接包一层而不需要改原类。3.2 逐分量处理每个Condition只面对一个标量接下来是核心循环。源码逻辑大致是这样的int num_residuals wrapped_cost_function_-num_residuals(); int num_parameter_blocks wrapped_cost_function_-parameter_block_sizes().size(); std::vectordouble condition_residuals(num_residuals); std::vectordouble condition_jacobians(num_residuals); for (int i 0; i num_residuals; i) { bool alloc_jacobian (jacobians ! nullptr); if (conditions_[i]-Evaluate(residuals[i], condition_residuals[i], alloc_jacobian ? condition_jacobians[i] : nullptr)) { return false; } residuals[i] condition_residuals[i]; }这里面的结构非常清晰。对第i个残差分量把旧的residuals[i]作为Condition的输入Condition计算出新的残差值先存进condition_residuals[i]最后再统一写回residuals[i]。这里有两个细节值得停下来想一下。细节一是为什么要用临时数组。因为Condition是逐个执行的如果每算完一个就直接写回residuals[i]后面的Condition拿到的就不是原始残差而是被前面处理过的残差了。源码这里的“先全部算完再统一写回”其实是一种很典型的规约模式避免前后条件互相污染。细节二是第三个参数jacobian的非空判断。如果调用方传入的jacobians是nullptr说明优化器此刻不要求解析雅可比那Condition实现里可以跳过导数计算省掉一次不必要的数学运算。这个细节在后面的雅可比处理里非常重要。3.3 失败传播语义Condition返回true就是出错了还有一个容易踩坑的细节Condition::Evaluate的返回值和CostFunction::Evaluate的返回值语义是反的。CostFunction返回false表示失败而Condition返回true表示失败。为什么会有这种不一致因为CostFunction沿袭了Ceres主接口的约定而Condition接口在早期设计时参考了另一套内部习惯。所以读源码的时候不要靠直觉判断一定要看清楚注释里“true is failure”的说明。一旦某个Condition返回了true整个Evaluate立刻返回false优化器会认为这个残差块不可用进而终止迭代或者跳过这一步。这其实是一个很有用的防御机制如果你的Condition在某个输入位置没有定义比如根号下出现了负数你可以返回true让优化器知道这里出了问题而不是给一个非法数值把它带偏。4. Evaluate第二步雅可比矩阵的行缩放与链式法则落地4.1 链式法则公式推导ConditionedCostFunction最精华的部分也是我读完后觉得“这个类真不简单”的部分在于它对雅可比矩阵的处理。假设原始残差是r_i经过Condition变换得到新残差r_ir_i C_i(r_i)其中r_i f_i(p)是底层CostFunction关于某些参数块p的原始残差。根据链式法则∂r_i / ∂p C_i(r_i) * ∂r_i / ∂p这里C_i(r_i)是一个标量∂r_i / ∂p是一行向量。乘法的效果就是原始雅可比矩阵的第i行整体乘以C_i(r_i)。把所有残差分量放在一起看整个雅可比矩阵的变换等价于左乘一个对角矩阵J_new diag(C_1(r_1), C_2(r_2), ..., C_n(r_n)) * J这就是源码里所谓“行缩放”的由来。你不需要重新构建什么复杂的矩阵乘积只需要对旧雅可比矩阵的每一行做一次标量乘法。4.2 源码里那个行缩放循环有了公式之后再看源码一切都顺理成章了if (jacobians ! nullptr) { for (int j 0; j num_parameter_blocks; j) { if (jacobians[j] ! nullptr) { const int num_parameters wrapped_cost_function_-parameter_block_sizes()[j]; for (int i 0; i num_residuals; i) { VectorRef(jacobians[j] i * num_parameters, num_parameters) * condition_jacobians[i]; } } } } return true;这个双层循环的顺序很有讲究。外层先遍历参数块。一个残差块可能关联多个参数块每个参数块在自己的雅可比数组里占据一段连续内存。内层再遍历残差维度。Ceres的雅可比矩阵布局是行优先的jacobians[j]这一段内存里第i个残差分量对应的导数存放在下标i * num_parameters开始的位置。所以VectorRef(jacobians[j] i * num_parameters, num_parameters)这行代码本质上是把“第i行”包装成一个Eigen的Map对象然后整个乘上condition_jacobians[i]。用*的好处是原地更新不会为新矩阵额外分配内存。4.3 一个容易被忽视的推论雅可比为0的语义既然雅可比每一行都要乘condition_jacobians[i]那么一个很自然的推论是如果你写了一个Condition它的导数在某段输入范围内恒等于0那么对应残差行的雅可比也会变成0。这意味着什么意味着这个残差分量在当前迭代点附近不提供任何梯度信息。但注意残差本身可能并不是0。在LM算法里这会表现为信息矩阵的某个方向降秩优化器可能认为这个约束已经“被满足”了从而放弃对它进行校正。我在实际项目里踩过一次这种坑写了一个硬截断的Condition残差超过阈值直接给一个常数导数写0结果优化进程在边界处卡了很久。后来把导数从0改成一个小斜坡比如1e-6问题立刻消失。后面避坑章节还会再展开。5. Condition接口拆解与自定义一个非线性条件5.1 Condition的纯虚接口看ConditionedCostFunction源码的时候顺带要把Condition接口也读了。它比想象中简单class CERES_EXPORT Condition { public: virtual ~Condition() {} virtual bool Evaluate(const double* parameters, double* residuals, double* jacobian) const 0; virtual int num_parameters() const 0; };这里parameters是这个Condition的输入residuals是输出两者都是标量指针。jacobian指向一个长度至少为num_parameters()的数组存放“输出对每个输入的偏导数”。在ConditionedCostFunction的使用上下文里Condition的输入被绑定到某个残差分量上所以实际上num_parameters()几乎总是1。少数特例可能是0代表一个不依赖输入、直接输出常量的条件。5.2 从源码看官方实现的套路Ceres源码里给出的Condition实现大体上有两条路线。一条是数值微分路线。把残差变换函数写成一个模板函数然后用中心差分对输入求导。优点是实现成本低你只需要关心数学表达式不需要手搓导数缺点是每次Evaluate要多算两次函数值而且差分布长选得不合适时导数精度会受影响。另一条是解析雅可比路线。要求实现者同时给出函数值和函数对输入的导数。这里有个常见的误解需要澄清Condition的雅可比是对“输入残差”求导而不是对“优化参数块”求导。对优化参数块的导数是由ConditionedCostFunction通过行缩放完成的。如果你在自定义Condition时把这两者搞反包装后的整个雅可比矩阵都会是错的而且很难排查。5.3 手写一个ScaleThenClampCondition理解了接口写一个自定义Condition就不难了。我以一个“缩放后再截断”的条件为例这个在工程里很实用class ScaleThenClampCondition final : public ceres::Condition { public: ScaleThenClampCondition(double scale, double lower, double upper) : scale_(scale), lower_(lower), upper_(upper) {} int num_parameters() const override { return 1; } bool Evaluate(const double* parameters, double* residuals, double* jacobian) const override { const double r parameters[0]; const double scaled scale_ * r; if (scaled lower_) { *residuals lower_; if (jacobian ! nullptr) { jacobian[0] 0.0; } } else if (scaled upper_) { *residuals upper_; if (jacobian ! nullptr) { jacobian[0] 0.0; } } else { *residuals scaled; if (jacobian ! nullptr) { jacobian[0] scale_; } } return true; } private: double scale_; double lower_; double upper_; };这个类本身很直观在非饱和区输出是scale乘输入导数就是scale在饱和区输出恒定导数就是0。但正如前面说的jacobian[0] 0.0的做法要非常谨慎。如果你真的要做硬截断可以考虑把饱和区的导数设成一个极小的正数而不是严格的0这样至少优化器不会在边界处完全失去方向。6. 完整实战给一个点到平面距离残差加软截断6.1 底层CostFunction简单的点到平面距离为了把上面的源码讲通这里用一个最简单的例子串一遍。假设你在做一个点云对齐问题需要优化一个三维点p的位置让它尽量落在某个平面n·p d 0上。残差定义为一个标量class PlaneDistanceCostFunction final : public ceres::CostFunction { public: PlaneDistanceCostFunction(const Eigen::Vector3d n, double d) : n_(n), d_(d) { set_num_residuals(1); mutable_parameter_block_sizes()-push_back(3); } bool Evaluate(const double* const* parameters, double* residuals, double** jacobians) const override { const Eigen::Mapconst Eigen::Vector3d p(parameters[0]); residuals[0] n_.dot(p) d_; if (jacobians ! nullptr jacobians[0] ! nullptr) { Eigen::MapEigen::RowVector3d(jacobians[0]) n_.transpose(); } return true; } private: Eigen::Vector3d n_; double d_; };这个类非常简单残差维数是1参数块是1个三维向量雅可比就是平面的法向量。6.2 用Condition做软截断现在设想一个真实场景初始位姿特别差点到平面的距离可能达到10米甚至20米。如果直接用线性距离残差优化初期会产生非常大的梯度LM迭代容易“冲过头”。这时可以用一个tanh软截断让距离超过一定范围后对梯度的贡献慢慢减小class SoftTanhCondition final : public ceres::Condition { public: explicit SoftTanhCondition(double scale) : scale_(scale) {} int num_parameters() const override { return 1; } bool Evaluate(const double* parameters, double* residuals, double* jacobian) const override { const double r parameters[0]; *residuals scale_ * std::tanh(r / scale_); if (jacobian ! nullptr) { const double c std::cosh(r / scale_); jacobian[0] 1.0 / (c * c); } return true; } private: double scale_; };当输入r绝对值很小时tanh(r)近似等于r此时约等于没变换当r绝对值很大时tanh趋向于1导数趋向于0。这样优化初期就不会被一个巨大距离残差带偏方向。6.3 组装并加入优化Problem组装方式如下std::vectorceres::Condition* conditions; conditions.push_back(new SoftTanhCondition(2.0)); ceres::CostFunction* cost new ceres::ConditionedCostFunction( new PlaneDistanceCostFunction(n, d), conditions); ceres::Problem problem; problem.AddResidualBlock(cost, nullptr, point_param);point_param是一个长度为3的double数组。把所有初始点放在距离平面很远的地方分别跑一次“不加Condition”和“加Condition”的优化能明显看到前者在头几步迭代的步长非常夸张后者则更温和。用表格对比一下这个差异方案迭代初期的行为收敛稳定性实现成本直接线性残差梯度大步长容易过大依赖信赖域策略兜底最低加SoftTanhCondition距离越大牵引越小步长平滑差初值下表现更稳定低只换LossFunction整体加权但残差数值不变比线性好但对单个分量控制力弱最低这里也回应了上文说的边界LossFunction是整体权重缩放而Condition是对单个残差分量做非线性映射。它是另一个维度的工具。7. 读完后必须提醒的几个坑7.1 所有权与double delete这是用这个类最容易踩的坑没有之一。ConditionedCostFunction析构时会释放内部被包装的CostFunction也会释放所有Condition对象。所以不要把栈上创建的Condition对象塞进去不要让两个包装类共享同一个Condition*如果自己又额外delete了一份那就离崩溃不远了。我自己的实践是在建图优化、多传感器标定这类项目里统一用一个工厂函数创建整个残差块所有权链非常清晰。7.2 饱和区雅可比为0的数值风险上面已经提到过硬截断会让导数在某段区间为0。这种设计在数值上非常危险残差不为0但梯度为0优化器会认定这个约束已经满足从而不再移动参数。很多时候表现为优化进程卡在某个不太好的位置怎么迭代都不动。如果确实需要硬截断有几种变通方案把饱和区的导数设成一个很小的斜坡比如1e-6改用tanh这类平滑函数把“截断”软化在Condition内部做一次带死区的线性映射保证整个定义域内导数不为0。7.3 别拿它做跨残差的耦合变换ConditionedCostFunction的哲学是“一个Condition处理一个残差分量”。每个Condition的输入都只是一个标量天然无法看到其他残差分量的值。如果有人想用它做残差白化、把二维向量投影到切平面、或者对不同残差分量做线性组合那这个类就不适合。跨维度的耦合变换应该直接写一个新的CostFunction在Evaluate里自己控制多维残差和完整的雅可比矩阵。7.4 调试技巧从雅可比入手最后分享一个排查问题的实用方法。如果你发现套了Condition之后优化结果不对第一步先验证雅可比。方法很简单写一个中心差分的小脚本把一个参数分量扰动1e-6计算残差变化量再对比ConditionedCostFunction返回的解析雅可比。这个检查往往能很快定位到问题根源八成是自定义Condition里的jacobian写错了或者对输入求导和对参数块求导搞混了。因为ConditionedCostFunction本身的行缩放逻辑是纯粹的线性代数只要头尾两个环节底层CostFunction雅可比和Condition雅可比是对的结果就一定是对的。读这套源码给我最大的收获并不是那个行缩放循环本身有多精妙而是Ceres把“残差后处理”抽象成独立组件的思路。后来我在做多传感器标定的时候把不同传感器残差的量纲归一化全部挪到Condition里实现主CostFunction保持纯粹的量测模型代码结构一下子就清晰了。如果你也想在自己的项目里引入它建议先从一个最简单的ScaleCondition跑通整体流程再逐步加重非线性变换。另外千万别忽略Condition接口那第三个形参jacobian——在源码里它被一个vector收集后直接决定了雅可比矩阵每一行的缩放因子。把这一点吃透ConditionedCostFunction的源码阅读就算真正到位了。