因子图（Factor Graph） 是联合概率分布的图表示，变量作为节点，因子作为边。 因子图（Factor Graph） 是联合概率分布的图表示，变量作为节点，因子作为边。 每个因子 f(x) 约束一组变量，通常写成残差函数： r(x)=h(x)−z r(x) = h(x) - z r ( x ) = h ( x ) − z h(x) ：预测量（模型） z ：观测量 r(x) ：残差（约束） 每个因子 f(x) 约束一组变量，通常写成残差函数： r(x)=h(x)−z r(x) = h(x) - z r ( x ) = h ( x ) − z 目标是最小化所有残差的加权平方和（非线性最小二乘）。 在 GTSAM 中，因子类的继承关系如下： 简单 1~N 个变量 ，推荐继承 NoiseModelFactorN （如 NoiseModelFactor2<Pose3, Point3> ）。 需要更大灵活性 （可变数量变量、特殊残差形式），用 CustomFactor 。 3. CustomFactor 的核心接口 CustomFactor 本质上是 NonlinearFactor 的一个实现，它要求你传入一个 lambda/函数对象 来定义残差和 Jacobian。 noiseModel ：噪声模型（如 noiseModel::Isotropic::Sigma(dimension, sigma) ） noiseModel ：噪声模型（如 noiseModel::Isotropic::Sigma(dimension, sigma) ） keys ：因子关联的变量键（ Key 的 vector） keys ：因子关联的变量键（ Key 的 vector） errorFunction ：一个 std::function ，签名为： std :: function < Vector ( const Values & , std :: vector < Matrix > & ) > 输入： Values （变量集合），和一个 std::vector<Matrix>& H （存放 Jacobian） 输出：残差向量 Vector errorFunction ：一个 std::function ，签名为： 输入： Values （变量集合），和一个 std::vector<Matrix>& H （存放 Jacobian） r(x1,x2)=f(x1,x2)−z r(x_1, x_2) = f(x_1, x_2) - z r ( x 1 ​ , x 2 ​ ) = f ( x 1 ​ , x 2 ​ ) − z 5. 继承式因子（另一种写法） 有时希望写成一个类，便于管理： 更类型安全，直接拿到模板参数类型。 自动处理 Jacobian 维度。 各向同性 ： noiseModel::Isotropic::Sigma(dim, sigma) 对角协方差 ： noiseModel::Diagonal::Sigmas(sigmas) 满协方差 ： noiseModel::Gaussian::Covariance(cov) 鲁棒核 ： noiseModel::Robust::Create(mEstimator, baseModel) Jacobian 没有正确填 如果传入 H ，必须保证维度匹配，否则会 runtime error。 用 OptionalJacobian<dim, var_dim> 更方便。 如果传入 H ，必须保证维度匹配，否则会 runtime error。 用 OptionalJacobian<dim, var_dim> 更方便。 维度错误 残差向量的维度必须和噪声模型一致。 残差向量的维度必须和噪声模型一致。 不稳定的 logmap 位姿相关因子通常用 Pose3::between() + Pose3::Logmap() 得残差。 注意小角度时数值稳定性。 位姿相关因子通常用 Pose3::between() + Pose3::Logmap() 得残差。 性能问题 在迭代器中大量分配内存（如 std::vector<Matrix> ) 可能影响性能，建议预分配。 在迭代器中大量分配内存（如 std::vector<Matrix> ) 可能影响性能，建议预分配。 GTSAM 自带 ImuFactor ，但如果要自定义： 变量： Pose_i, Vel_i, Bias_i, Pose_j, Vel_j, Bias_j 残差：根据预积分测量 Δp,Δv,ΔR\Delta p, \Delta v, \Delta R Δ p , Δ v , Δ R 构造 Jacobian：对每个状态求导

实现时推荐继承 NoiseModelFactor6<Pose3, Vector3, Bias, Pose3, Vector3, Bias> 。 2. 自定义代价函数因子（例如点到平面的约束） r=n⊤(Rp+t−q) r = n^\top (R p + t - q) r = n ⊤ ( Rp + t − q ) 下面一个 Point2 约束因子 的例子： 自定义一个因子 MyPriorFactor ，约束某个 Point2 变量靠近观测值 (1.0, 2.0) 。 一个 二元因子 （比如约束两个 Point2 的距离 = d），展示 NoiseModelFactor2 的用法。 定义 Point2DistanceFactor ，继承自 NoiseModelFactor2<Point2, Point2> 。 约束 两个点之间的欧氏距离 ≈ d 。 用高斯牛顿优化，把两个点调到满足约束。 可以看到，优化器自动把两个点调成了 相距 2.0 ，并且 Jacobian 是自定义的。 简单场景 ：继承 NoiseModelFactorN ，实现 evaluateError 。 复杂/动态场景 ：用 CustomFactor + lambda。 核心原则 ：残差维度与噪声模型一致，Jacobian 正确。 调试技巧 ：用 GaussNewtonOptimizer 或 LevenbergMarquardtOptimizer ，开启 debugPrint 查看因子贡献。 11、自定义因子计算 Jacobian（H 矩阵）附件 r=∥p1−p2∥−d r = \| p_1 - p_2 \| - d r = ∥ p 1 ​ − p 2 ​ ∥ − d p1=(x1,y1)Tp_1 = (x_1, y_1)^T p 1 ​ = ( x 1 ​ , y 1 ​ ) T ， p2=(x2,y2)Tp_2 = (x_2, y_2)^T p 2 ​ = ( x 2 ​ , y 2 ​ ) T ， 残差是 标量 （1 维），所以返回 Vector1(err) 。 ∂r∂p1,∂r∂p2 \frac{\partial r}{\partial p_1}, \quad \frac{\partial r}{\partial p_2} ∂ p 1 ​ ∂ r ​ , ∂ p 2 ​ ∂ r ​ Δ=p1−p2=[x1−x2y1−y2] \Delta = p_1 - p_2 = \begin{bmatrix} x_1 - x_2 \\ y_1 - y_2 \end{bmatrix} Δ = p 1 ​ − p 2 ​ = [ x 1 ​ − x 2 ​ y 1 ​ − y 2 ​ ​ ] ∥Δ∥=(x1−x2)2+(y1−y2)2 \| \Delta \| = \sqrt{(x_1-x_2)^2 + (y_1-y_2)^2} ∥Δ∥ = ( x 1 ​ − x 2 ​ ) 2 + ( y 1 ​ − y 2 ​ ) 2 ​ r=∥Δ∥−d r = \| \Delta \| - d r = ∥Δ∥ − d 对 p1p_1 p 1 ​ 求导 ： ∂r∂p1=ΔT∥Δ∥ \frac{\partial r}{\partial p_1} = \frac{\Delta^T}{\|\Delta\|} ∂ p 1 ​ ∂ r ​ = ∥Δ∥ Δ T ​ 对 p2p_2 p 2 ​ 求导 ： ∂r∂p2=−ΔT∥Δ∥ \frac{\partial r}{\partial p_2} = -\frac{\Delta^T}{\|\Delta\|} ∂ p 2 ​ ∂ r ​ = − ∥Δ∥ Δ T ​ unit = diff / dist 就是 Δ/∥Δ∥\Delta / \|\Delta\| Δ/∥Δ∥ 。 unit.transpose() 是 1x2 矩阵，符合 GTSAM 要求。 残差 r 是标量 → Vector1 残差 r 是标量 → Vector1 对于 p1 （2 维变量）： Jacobian 应该是 1x2 矩阵 Jacobian 应该是 1x2 矩阵 对于 p2 也是 1x2 矩阵 对于 p2 也是 1x2 矩阵 *H1 和 *H2 的类型是 Eigen::MatrixXd （动态），但我们保证返回 1x2 。 思路 ：先写出误差函数 → 对变量求导 → 确认维度 → 写进 H 。 在二元因子里， H1 / H2 分别对应两个变量的 Jacobian。 如果是 NoiseModelFactorN ，你就得对每个输入变量都算一次。