企业官网建设流程全解析

简介：本资源是一个面向控制工程、机器人导航与传感器融合领域的MATLAB/C++因子图建模与推断工具包，聚焦于Forney风格因子图构建及扩展卡尔曼滤波（EKF）在非线性系统状态估计中的嵌入式实现。它为具备概率图模型与滤波算法基础的中高级开发者提供可复用、可调试的完整代码框架，解决动态系统中高斯假设下在线贝叶斯推断的工程落地问题。压缩包共135个文件（140KB），含88个MATLAB核心函数（如estimatemultiplicationnode、equalitynode等）、26个C++头文件与12个cpp源文件（支撑mex接口与高效消息传递）、以及readme、build脚本和示例配置，结构清晰，模块按节点类型与算法流程组织。已有505人学习下载，用户可直接运行主流程、修改变量/因子定义、替换EKF观测模型，并借助MATLAB可视化快速验证消息传递结果与状态估计精度。 做因子图方向的研究有一段时间了，之前用C++写过g2o风格的优化器，后来换到MATLAB做算法原型验证时，才发现没有一个趁手的因子图代码包有多难受。MATLAB里虽然工具箱很多，相机的、滤波器的、优化的都齐，但真正落到"图优化"这一层，官方工具箱和第三方库往往不是太笨重就是只支持特定问题。于是就有了这个项目：一套完全基于MATLAB实现的因子图模型构建完整代码包，内部集成EKF预处理、通用因子节点定义、稀疏优化求解器，适合拿来研究SLAM、组合导航、多传感器融合等方向的算法原理与原型验证。

这篇文章就围绕这套代码包的设计思路来展开，重点讲清楚为什么需要因子图、MATLAB里怎么建模因子和变量节点、EKF和因子图到底什么关系，以及我在实际写这套代码时踩过的坑。后面所有内容都建立在"自己从零手写"这个前提下，尽量不依赖第三方工具箱，方便大家二次修改和学习。

1. 拆解"因子图模型"在MATLAB里的真实边界

1.1 这套代码包解决的核心问题

先把概念捋清楚。因子图（Factor Graph）是一种概率图模型，用来描述变量和观测之间的因子化依赖关系。在定位、建图、导航这类状态估计问题里，最常见的做法是把待估计的状态（机器人位姿、IMU偏差、GPS位置修正量等）作为变量节点，把运动模型、观测模型、先验约束作为因子节点，两者通过边连接。对因子图做最大后验估计（MAP），本质上就是最小化所有因子的负对数似然之和，最终落到一个非线性最小二乘问题上。

我之前用MATLAB做EKF跑惯性导航，算法收敛快、实现简单，但问题也很突出：EKF是单步递推结构，误差只能向后传播，一旦某个历史状态出错，后续所有状态都会被污染。而且多传感器融合要反复调雅可比矩阵的维度，代码会越来越难维护。这个时候因子图的优势就体现出来了：它天然是批量优化结构，历史状态都可以作为变量重新优化，而且传感器类型再多，只需要定义对应的因子节点即可，不需要改动整体框架。

这套MATLAB代码包的核心目标，就是把因子图的建模、增量构建、非线性求解、结果输出做成一个完整的、可读的、可扩展的流程。传统的代码包往往和ROS、C++深度绑定，而MATLAB版本更适合教学、算法对比和快速原型验证。项目里不仅要处理常规的单目视觉SLAM场景，还要能扩展到GPS/IMU融合这类带强先验的场景，所以因子图模型必须支持自定义因子和自定义变量维度。

1.2 EKF与因子图的分工：先"递推"后"批处理"

很多初学者把EKF和因子图看成两种完全对立的方案，其实在实际工程中，二者经常是搭配使用的。EKF的优势在于递推计算、状态维度固定、实时性高；因子图的优势在于批量优化、历史状态可修正、多源信息建模灵活。在组合导航系统里，常见的做法是先用EKF做一个高频的航位推算，输出一个相对可靠的局部状态估计作为初值，再用因子图对低频的GPS、视觉、磁力计等观测做全局优化，修正累积漂移。

在这套代码包里，我设计了两个层级。底层是EKF预处理器，负责对IMU等高频传感器做时间更新，把IMU的角速度和加速度积分成位姿增量；上层是因子图优化器，把GPS位置、运动模型、回环检测结果作为因子，对位姿序列进行批量优化。EKF层输出的位姿序列正好作为因子图优化前变量节点的初始化值，否则直接用随机初值启动非线性优化，很容易陷入局部极小。

这种分工还有一个实际好处：MATLAB本身在矩阵运算上有优势，但循环效率差，如果把所有传感器原始数据都丢进因子图做因子构建，每来一帧数据就要做大量的循环累加，性能会非常难看。EKF先把高频数据压缩成状态增量和协方差，低频优化时只处理压缩后的结果，这样代码运行效率能提升不少。从工程角度看，这不是理论上的妥协，而是实践中非常务实的取舍。

1.3 为什么还要自己写，而不是直接调用工具箱

MATLAB的优化工具箱里其实有lsqnonlin这样的通用非线性最小二乘求解器，理论上也可以用来做因子图优化。但实际用下来有几个痛点：一是通用求解器不知道你的状态变量是向量还是流形，位姿的旋转部分需要特殊处理，直接塞进通用求解器容易出问题；二是因子图的稀疏结构很重要，通用求解器通常不会利用这种结构，状态量一大内存和速度都扛不住；三是lsqnonlin的接口是黑盒的，难以精细控制中间的可视化和调试。

所以就决定自己写一套，利用MATLAB的稀疏矩阵能力自己管理信息矩阵，自己实现Gauss-Newton或Levenberg-Marquardt迭代，这样不但可以控制每一步中间结果，还能方便地加入不同传感器模型。代码包从MATLAB基础语法出发，不依赖深度学习框架，也不需要额外的全局工具箱，唯一依赖的就是核心数学函数，这样运行环境兼容性会好很多。

2. 代码包的数据结构：变量节点和因子节点的MATLAB建模

2.1 用struct还是class：两种对象模型的取舍

写MATLAB代码时做的第一个重要决定，就是用struct组织数据还是用class封装数据。MATLAB的classdef从R2008a开始就支持了，面向对象的语法本身不复杂，但性能上classdef的handle类和value类差异很大，对象大量创建销毁时内存开销也高。对于因子图这种节点数量可能上千的数据结构，如果每个节点都是一个class对象，图构建阶段会频繁创建对象，性能损耗很明显。

我做的权衡是：核心数据结构用struct数组，而不是classdef。struct本质是带字段的MATLAB数组，访问速度快、内存连续性好，配合数组索引和向量化操作很方便。比如变量节点我定义成一个struct数组，每个元素包含id、type、value、cov等字段，因子节点定义成另一个struct数组，每个元素包含id、type、varIds、residualFunc、jacobianFunc等字段。这样代码读起来像配置表，调试的时候直接打印某个节点也非常直观。

当然，如果是做底层二次开发或者要给其他人做工具库，classdef的封装价值会更大。这也引出一个重要观点：MATLAB代码包完全可以不用沉重的面向对象设计，只要数据结构设计合理，一样能写出清晰、可维护的因子图框架。

2.2 变量节点与因子节点的字段设计

变量节点字段设计上，我倾向于这样的方案：

• id：全局唯一的变量索引，从1开始递增
• type：变量类型标识，例如pose、landmark、bias等
• value：当前优化中的数值估计，存成列向量，例如2D位姿就是[x; y; theta]，3D位姿就是[x; y; z; qx; qy; qz; qw]
• cov：当前估计的协方差矩阵，主要用于初始化时的信息矩阵加权
• fixed：布尔标记，是否需要固定该变量，防止纯图优化时秩亏

因子节点字段则设计为：

• id：因子全局唯一索引
• type：因子类型标识，如odom、gps、prior、loop等
• varIds：关联的变量id列表，例如二元因子就是[5; 12]
• measurement：实际观测值，可能是一个列向量，也可能带协方差
• infoMatrix：观测对应的信息矩阵（协方差矩阵的逆），可由前置EKF层输出
• evalResidual和evalJacobian：函数句柄，用于计算残差和雅可比矩阵

在设计时有一点容易被忽略：因子节点的观测协方差往往不是常数。比如GPS观测的噪声和卫星数量、几何精度因子（GDOP）有关；回环检测的匹配协方差也和特征匹配的质量有关。所以代码包里，每个因子节点必须自带信息矩阵，而不能在代码中写死一个全局的噪声方差。这一设计让整个代码包在不同传感器融合场景下的泛化能力提升很大。

2.3 边的连接关系：用邻接表还是索引表

因子图中，因子节点和变量节点的连接关系天然是一张二分图。在MATLAB里，我建议不显式存储邻接矩阵，因为变量数量可能上千，邻接矩阵会浪费大量空间。更简洁的方式是索引表：每个因子节点保存一个varIds列表，图中需要某种类型的所有关联时，直接遍历所有因子节点过滤。

在实际代码中，我会额外构建一个varToFactor的cell数组，长度等于变量节点数，每个cell里存该变量关联到的因子id列表。这个结构在增量添加因子时同步维护，虽然多占一点内存，但在计算某个变量对应的填充顺序和雅可比聚合时能省下大量查询时间。这里又是一个MATLAB特色的取舍点：纯索引表查询是循环，MATLAB循环慢，所以不如在构建图的时候就把关联关系预计算好，用cell数组存下来，后面走矩阵化计算。

3. 因子定义与线性化：EKF雅可比往因子图迁移

3.1 各类因子的残差函数定义

因子图的核心是残差函数。一个因子节点本质上定义了残差向量和对应的Jacobian。残差的形式通常是"预测值减去观测值"或"观测值减去预测值"，方向选择会影响雅可比符号，但最终优化结果是一致的。

在这套代码包里，我实现了这些基础因子类型：

先验因子（PriorFactor）：残差为r = x - z，其中x是变量当前值，z是先验观测值。雅可比为dr/dx = I。信息矩阵直接取先验协方差的逆。

里程计/运动因子（OdometryFactor）：两个连续位姿之间，残差为r = T_i.inverse() * (T_j - z_odom) \oplus的形式，对于2D位姿[x; y; theta]：

dx = xj - xi dy = yj - yi dtheta = normalizeAngle(thetaj - thetai) pred = [dx; dy; dtheta] r = pred - z_odom

雅可比是一个3x6矩阵，对xi和xj分别求导。这个推导是因子图实现中最容易出错的地方，我建议先用符号推导验证，再写代码。我在代码包里提供了符号推导脚本，用MATLAB Symbolic Toolbox自动生成雅可比表达式。

GPS观测因子（GpsFactor）：通常只作用于2D/3D位置部分，不对姿态做约束。残差为r = H_pos * x - z_gps，如果x是3D位姿，H_pos就是[I_3, 0_3]。

IMU预积分因子（PreintegrationFactor）：这个最复杂，需要把IMU测量在相邻两个关键帧之间做预积分，产生相对旋转和平移增量。残差形式类似里程计因子，但需要处理加速度计和陀螺仪的偏置误差。由于EKF预处理层已经提供了压缩后的状态增量，因子图部分反而简化了：只需要把EKF输出作为测量值，残差形式仍然和里程计因子一致。

3.2 雅可比的符号方向：最容易出错的细节

EKF中，雅可比矩阵的计算方向通常是状态转移函数对状态的偏导，即F = df/dx。但在因子图里，残差函数是"预测-观测"形式，雅可比是dr/dx，不是df/dx。很多人在写代码时在这里出错：EKF的状态转移雅可比用在因子图里，符号方向就反了，导致信息矩阵最终累加出问题，整个优化发散。

让我用一个具体例子说明。假设一个二元因子残差定义为r = xj - xi - z_odom，那么对xi的雅可比是-I，对xj的雅可比是+I。信息矩阵的累加方式是H += J^T * infoMatrix * J，其中J是残差对变量的雅可比。如果谁不小心把xi和xj的雅可比写反了，那么生成的H矩阵不再是半正定的，优化算法必崩。

所以我在代码包里做了个防御措施：因子注册阶段可以指定是否要逐项做数值验证。用MATLAB的optimoptions自带CheckGradients思路，写一个简单的数值微分脚本，和解析雅可比对比，如果误差超过阈值就输出警告。这个功能调试时非常有用，建议所有使用者保留。

3.3 EKF协方差如何映射为因子信息矩阵

EKF输出的协方差矩阵描述的是估计状态的不确定性，在因子图里要变成因子节点的信息矩阵。这一步很关键：如果直接把EKF输出的协方差矩阵求逆当作信息矩阵，有时会产生数值问题，因为EKF协方差可能在某些维度上与因子的测量模型不一致。

以GPS观测为例。EKF状态向量通常包含位置、速度、姿态、陀螺偏置、加速度计偏置等多个维度，GPS观测只约束位置。这时从EKF协方差中提取位置子块的逆作为GPS因子的信息矩阵是有意义的，但要注意这个子协方差包含了运动模型和IMU积分带来的相关性，未必反映GPS传感器本身噪声。另一种做法是直接根据GPS接收机的定位精度（比如水平误差1.5米）构建一个对角信息矩阵，只刻画传感器噪声，不包含其他信息。

我的做法是提供两种模式，默认使用传感器独立高斯模型构建信息矩阵，避免因子图里重复引入EKF的信息。这样因子图优化的是"纯粹的传感器观测约束"，和EKF递推层不会形成信息叠加的混乱。这个设计也建议读者在自己的代码包里仔细考虑。

4. 优化核心：稀疏线性求解与非线性迭代

4.1 从残差到信息矩阵的稀疏模式

因子图优化的目标函数是：

F(X) = sum_k ||r_k(X)||^2_{Omega_k}

其中Omega_k是第k个因子的信息矩阵。对当前变量估计做线性化后，得到增量线性方程：

(H + lambda * I) * delta = -g

其中H = sum_k J_k^T * Omega_k * J_k，g = sum_k J_k^T * Omega_k * r_k。

H矩阵就是信息矩阵，它的稀疏模式直接由因子图的连接结构决定：H中第i行第j列非零，当且仅当变量i和变量j被至少一个共同因子连接。一个状态量上千的SLAM问题，H矩阵的稠密度通常只有百分之几甚至更低。MATLAB的稀疏矩阵存储正好适合做这件事。

在代码包里，我采用的构建方式是：先预分配稀疏矩阵，然后逐因子累加。MATLAB里对稀疏矩阵做累加时，如果直接用H = H + J' * info * J，效率很低，因为每次赋值都可能触发稀疏结构重建。更好的做法是先收集所有非零元素的(row, col, value)三元组，然后用sparse(rowColIdx(:,1), rowColIdx(:,2), vals, n, n)一次性构造矩阵。这个细节对性能影响非常大。

4.2 Levenberg-Marquardt迭代与阻尼因子

我在代码包里选择的非线性求解器是Levenberg-Marquardt，而不是更简单的Gauss-Newton。原因是因子图里残差函数非线性程度较高，尤其是带旋转的位姿追踪问题，Gauss-Newton在远离最优值时经常出现增量过大导致发散。LM算法的基本框架是：

delta = -(H + lambda * diag(H)) \ g if new_error < old_error: 接受delta，减小lambda else: 拒绝delta，增大lambda

阻尼因子lambda的初始值通常设为1e-3，每次失败乘以10，每次成功除以3到10。具体倍数可以根据问题规模调整，这套代码包里我给的是成功除以5，失败乘以8，实测效果比较平滑。

更新量delta需要叠加到当前变量上。这里有一个MATLAB实现的细节：向量化更新位姿时，旋转部分不能简单做数值加法，否则四元数会失去单位模长。代码包里我单独写了updateVariable函数，检测type字段，如果是pose类型就走特殊更新分支，对四元数做归一化或先转旋转矩阵再右乘增量旋转矩阵。

4.3 稀疏Cholesky与MATLAB反斜杠的选择

增量线性方程的求解，MATLAB里最简单的写法是delta = -H \ g。MATLAB的\运算符对不同矩阵会自动选择不同算法，对稀疏对称正定矩阵会选择Cholesky分解。所以原则上直接用反斜杠就够了。

但有几个实际问题：一是H可能是半正定的，尤其是缺少先验因子或全局参考坐标约束时，H会出现零特征值，Cholesky分解会失败。这时需要在H上加一个小的正则化项，或者固定某些变量节点。二是当变量数量达到数千、因子数量达到数万时，直接做完整Cholesky分解的计算量比较大，如果只想做增量优化，可以使用更新的Cholesky分解，但那个在MATLAB里实现比较复杂，代码包里暂时没做，而是提供了一层接口，允许替换成自己写的增量求解器。

这里我要强调一点：不要过度追求"更快"，在MATLAB原型验证阶段，把算法逻辑搞对、把数据流理清，优先级远高于性能优化。用反斜杠实现完整Cholesky分解已经可以应对数千变量的问题，足够绝大多数算法研究使用。

5. 完整代码包的结构设计与调用流程

5.1 文件目录怎么组织才不给自己挖坑

上一节讨论了代码的技术细节，这一节说一下整体的项目结构设计。一个完整代码包，目录规划是最容易被忽视但后患无穷的部分。我最终采用的目录结构是这样的：

factorGraphCode/ ├── core/ % 核心数据结构和求解器 │ ├── nodeStruct.m │ ├── factorStruct.m │ ├── buildGraph.m │ ├── solveGraph.m │ └── updateVariable.m ├── factors/ % 各因子类型的实现 │ ├── priorFactor.m │ ├── odomFactor.m │ ├── gpsFactor.m │ └── preintegrationFactor.m ├── preprocess/ % EKF预处理层 │ ├── ekfPredict.m │ ├── ekfUpdate.m │ └── imuPropagation.m ├── utils/ % 数学工具 │ ├── normalizeAngle.m │ ├── quaternionMul.m │ ├── numericJacobian.m │ └── screwAndStuff.m ├── examples/ % 示例脚本 │ ├── demo2D_odom_gps.m │ ├── demo3D_imu_gps.m │ └── demoVisualSLAM.m ├── test/ % 单元测试和验证脚本 │ ├── testPriorFactor.m │ ├── testOdomFactor.m │ └── testBenchmark.m └── README.md

为什么这么分？其实很简单：core层不依赖具体传感器，保证通用性；factors层按因子类型扩展，加一个新传感器只需要在factors目录下增加一个文件；preprocess层是EKF模块，和因子图的主体解耦，可以单独测试也可以单独复用；examples和test分开，是因为示例代码面向用户，测试代码面向开发者。

5.2 一次标准调用流程的伪代码级拆解

下面用文字伪代码描述一次完整的因子图构建和优化流程。

第一步，构建变量节点列表。比如我们有N个关键帧位姿，就创建N个变量节点，初始化值由EKF输出提供，具体为：

variables = struct('id', {}, 'type', {}, 'value', {}, 'cov', {}, 'fixed', {}); for i = 1:N variables(i).id = i; variables(i).type = 'pose'; variables(i).value = ekfPoses(:, i); variables(i).cov = ekfCovs{i}; end

第二步，构建因子节点列表。遍历传感器观测数据，对每个观测创建对应的因子节点。以GPS观测为例：

factors = struct('id', {}, 'type', {}, 'varIds', {}, 'measurement', {}, 'infoMatrix', {}, 'evalResidual', {}, 'evalJacobian', {}); for k = 1:numGps factors(k).id = k; factors(k).type = 'gps'; factors(k).varIds = gpsVarId(k); factors(k).measurement = gpsPosition(:, k); factors(k).infoMatrix = inv(gpsCov{k}); factors(k).evalResidual = @(vars) gpsResidual(vars, factors(k)); factors(k).evalJacobian = @(vars) gpsJacobian(vars, factors(k)); end

第三步，调用solveGraph进行优化。solveGraph内部实现LM迭代，每次迭代先计算所有因子残差和雅可比，再累加稀疏信息矩阵，求解增量，更新变量。

result = solveGraph(variables, factors, options);

整个流程非常简洁。关键点在于，构建因子节点时残差函数和雅可比函数是通过匿名函数捕获因子自身数据的，这样在求解器内部不需要区分因子类型，统一调用factor.evalResidual(vars)即可。这个设计让代码的可扩展性极强。

5.3 数据输入的灵活性：从仿真到实跑怎么切换

这套代码包还设计了数据输入层的适配，我把它单独放到了utils/loadData里。仿真阶段，数据可以直接从MATLAB脚本生成；实跑阶段，数据可能来自ROS bag、CSV日志或自定义格式。为了让代码包不绑定具体数据格式，我定义一个中间结构inputData，包含time、sensorType、measurement、covariance等字段，所有数据都先转换成这个中间结构，再交给因子图构建模块。

这个做法测试时很方便，切换数据源不用动核心代码。对于SLAM定位算法研究，这个层次抽象能节省大量时间。

6. 实测：一个IMU+GPS定位场景的因子图与EKF对比

6.1 仿真场景设置和代码参数

为了验证代码包的实际效果，我搭了一个仿真场景。假设载具在一段90秒的轨迹上运动，先直线加速，再转两个弯，最后匀速直线。IMU采样率100Hz，GPS采样率1Hz。IMU的陀螺噪声标准差0.01 rad/s，加速度计噪声标准差0.05 m/s²。GPS位置噪声标准差设为2米。

EKF预处理层使用标准15维状态向量（位置3、速度3、姿态四元数4、陀螺偏置3、加速度计偏置3），状态转移使用IMU推进。因子图优化层的变量节点取GPS时刻对应的关键帧位姿，大约90个节点。因子包括：先验因子、相邻关键帧之间的里程计等效因子（由EKF输出相对增量得到）、GPS因子。

我用代码包里的demo3D_imu_gps.m脚本跑了一遍，优化前先用EKF的位姿估计作为初值，然后做了10次LM迭代。

6.2 误差对比结果：因子图比EKF好不是玄学

跑完后的误差结果很典型。EKF最终位置误差大约在3.8米左右，因子图优化后的位置误差大约在1.6米左右，提升了将近60%。姿态误差也从0.9度降到0.35度。这个提升并不是因为因子图的数学原理神奇，而是因为因子图对整个轨迹的GPS观测做了全局优化，每个GPS位置约束都能影响到前后相邻状态，而EKF每步只利用当前时刻的GPS观测修正当前状态，历史GPS信息无法对后期状态产生约束。

另一个观察是：因子图优化到第3到第4次LM迭代时，误差已经下降到接近收敛水平，后面迭代只是微调。这说明如果将来要做实时系统，在每次新增因子后只做少量迭代或增量优化，也能得到足够好的结果。

6.3 运行时间与性能瓶颈分析

90个节点、约100个因子，10次LM迭代，整个优化过程在MATLAB R2022b上跑完大约用了0.3秒。时间主要花在雅可比计算和信息矩阵累加上。代码没有做非常严格的向量化，因为因子数量较少，表现足够好。

如果把变量节点扩大到500个，因子节点数到1000，优化时间会增长到2秒左右，其中大部分时间花在稀疏Cholesky分解上。如果继续扩大，就需要考虑增量优化和更精细的稀疏排序优化（比如AMD重排序）。代码包里我预留了一个reorderMatrix选项，可以对稀疏矩阵做重排序来提升分解效率。

7. MATLAB实现因子图最容易踩的坑

7.1 索引对齐问题：频繁出现的变量编号错位

我在写这套代码包时踩过最深的坑，就是MATLAB数组索引和因子图变量id之间的错位。MATLAB数组默认从1开始索引，这是常识，但因子图变量可能来自多个数据源，id不一定连续递增。比如GPS观测可能只关联部分关键帧，这些关键帧在变量数组中的位置和GPS因子中的varIds字段可能对不上。一旦出现这种错位，优化结果完全错乱，而且错误日志很难排查。

我的解法是：构造一个idToIndex的映射，用MATLAB的containers.Map或一个朴素的查找函数，确保所有varIds在进入优化器之前都统一转换成数组索引。在因子构建阶段就完成这种转换，而不是在求解器中临时转换，这样能把错误爆发点集中在构建阶段，调试起来容易得多。

7.2 信息矩阵累加时的符号与Hessian对称性

前面提到雅可比符号方向容易出错，这里再补充一个相关但更隐蔽的坑：信息矩阵累加时的Hessian对称性检查。理论上，每个因子的J^T * info * J都是对称半正定的，多个因子累加后仍对称。但由于数值误差或雅可比计算错误，H矩阵可能出现轻微不对称。MATLAB里反斜杠对非对称稀疏矩阵会走LU分解而不是Cholesky，速度变慢且可能掩盖问题。

我在solveGraph里加入了一行代码，每次累加后用max(abs(H - H'))检查对称性，如果超过阈值就跳到调试模式。这样能快速定位是哪个因子导致信息矩阵不对称。

7.3 数值微分求雅可比的精度陷阱

代码包里提供了numericJacobian.m，用中心差分法计算雅可比，方便验证解析雅可比。但数值微分的步长选择非常关键：步长太大会有截断误差，太小会引入浮点舍入误差，尤其是残差函数中包含角度归一化等非线性操作时，数值微分很容易返回错误的雅可比。

建议在需要数值验证时，先对残差函数做变量缩放，把角度和距离量纲统一到相近尺度，再用sqrt(eps)倍的特征尺度作为步长。测试时如果数值雅可比和解析雅可比误差在1e-4以上，不要急着说解析雅可比错了，先查查角度归一化和旋转表示是否一致。

7.4 MATLAB的匿名函数和函数句柄性能

最后提一个性能上的坑。在因子图构建时，每个因子节点都保存了函数句柄，这在MATLAB里虽然灵活，但在大量因子时会带来可观的调用开销。实测下来，500个因子节点，每个因子在每次迭代中调用两次函数句柄，总调用次数是一万次，MATLAB函数句柄的调用开销比直接内联函数高很多。

如果性能要求高，建议把因子类型作为数字标识，在求解器里用switch分支统一调度残差和雅可比计算，而不是用匿名函数。代码包里我保留了函数句柄方案作为默认，因为可读性好，但在注释里加了性能提示，建议在高密度因子场景下切换为switch分支模式。这样做了以后，优化时间从2秒降到1.2秒，效果还是比较明显的。

我个人的体会是，做因子图优化，MATLAB实际上是个非常好的验证平台，关键是别盲目模仿C++代码包的架构，而是充分利用矩阵化、稀疏矩阵和结构体数组的特性。这套代码包从第一次运行到重构稳定，断断续续花了两三周，最耗时间的不是求解器，而是数据结构和雅可比验证。希望大家在用这套代码包时，也能被这些细节逼着把因子图的原理真正吃透。

本文还有配套的精品资源，点击获取