当前位置：首页 > news >正文

GTSAM 中自定义因子（Custom Factor）的详解和实战示例

news 2025/9/26 7:35:05

1. 因子图回顾

在 GTSAM 中：

因子图（Factor Graph） 是联合概率分布的图表示，变量作为节点，因子作为边。
每个因子 f(x) 约束一组变量，通常写成残差函数：

$r (x) = h (x) - z$
- h(x)：预测量（模型）
- z：观测量
- r(x)：残差（约束）

目标是最小化所有残差的加权平方和（非线性最小二乘）。

2. 因子类体系结构

在 GTSAM 中，因子类的继承关系如下：

Factor└── NonlinearFactor├── NoiseModelFactor (常用基类，带噪声模型)│       ├── NoiseModelFactor1│       ├── NoiseModelFactor2│       └── ...└── CustomFactor (通用接口)

选择哪种方式？

简单 1~N 个变量，推荐继承 NoiseModelFactorN（如 NoiseModelFactor2<Pose3, Point3>）。
需要更大灵活性（可变数量变量、特殊残差形式），用 CustomFactor。

3. `CustomFactor` 的核心接口

CustomFactor 本质上是 NonlinearFactor 的一个实现，它要求你传入一个 lambda/函数对象 来定义残差和 Jacobian。

核心构造函数：

CustomFactor::CustomFactor(const SharedNoiseModel& noiseModel,const KeyVector& keys,const ErrorFunction& errorFunction);

其中：

noiseModel：噪声模型（如 noiseModel::Isotropic::Sigma(dimension, sigma)）
keys：因子关联的变量键（Key 的 vector）
errorFunction：一个 std::function，签名为：
```
std::function< Vector(const Values&, std::vector<Matrix>&) >
```
- 输入：Values（变量集合），和一个 std::vector<Matrix>& H（存放 Jacobian）
- 输出：残差向量 Vector

4. 实现自定义因子的步骤

Step 1. 定义残差函数

例如，一个约束：

$r(x_1, x_2) = f(x_1, x_2) - z$

可以写成 lambda：

auto errorFun = [](const Values& values, std::vector<Matrix>& H) {Pose3 x1 = values.at<Pose3>(X1);Pose3 x2 = values.at<Pose3>(X2);// 残差（比如相对位姿约束）Pose3 between = x1.between(x2);Vector6 r = between.logmap(Z); // 假设观测是 Pose3 Z// Jacobian（如果 H 非空，才填充）if (!H.empty()) {// H.size() == number of keysif (H[0].rows() != 0) { H[0] = ...; // dr/dx1}if (H[1].rows() != 0) { H[1] = ...; // dr/dx2}}return r;
};

Step 2. 构造因子

auto noise = noiseModel::Diagonal::Sigmas((Vector(6) << 0.1,0.1,0.1, 0.1,0.1,0.1).finished());
auto factor = boost::make_shared<CustomFactor>(noise, KeyVector{X1, X2}, errorFun);

Step 3. 插入因子图

NonlinearFactorGraph graph;
graph.add(factor);

5. 继承式因子（另一种写法）

有时希望写成一个类，便于管理：

class MyFactor : public NoiseModelFactor2<Pose3, Point3> {Point3 measured_;
public:MyFactor(Key key1, Key key2, Point3 measured,const SharedNoiseModel& model): NoiseModelFactor2<Pose3, Point3>(model, key1, key2),measured_(measured) {}Vector evaluateError(const Pose3& pose, const Point3& point,boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const override {Point3 pred = pose.transformFrom(point, H1, H2);return pred - measured_;}
};

特点：

更类型安全，直接拿到模板参数类型。
自动处理 Jacobian 维度。
推荐用于固定变量数目的因子。

6. 常见噪声模型

各向同性：noiseModel::Isotropic::Sigma(dim, sigma)
对角协方差：noiseModel::Diagonal::Sigmas(sigmas)
满协方差：noiseModel::Gaussian::Covariance(cov)
鲁棒核：noiseModel::Robust::Create(mEstimator, baseModel)

7. 常见坑

Jacobian 没有正确填
- 如果传入 H，必须保证维度匹配，否则会 runtime error。
- 用 OptionalJacobian<dim, var_dim> 更方便。
维度错误
- 残差向量的维度必须和噪声模型一致。
不稳定的 logmap
- 位姿相关因子通常用 Pose3::between() + Pose3::Logmap() 得残差。
- 注意小角度时数值稳定性。
性能问题
- 在迭代器中大量分配内存（如 std::vector<Matrix>) 可能影响性能，建议预分配。

8. 进阶例子

1. IMU 预积分因子

GTSAM 自带 ImuFactor，但如果要自定义：

变量：Pose_i, Vel_i, Bias_i, Pose_j, Vel_j, Bias_j
残差：根据预积分测量 $Δp,Δv,ΔR\Delta p, \Delta v, \Delta R$ 构造
Jacobian：对每个状态求导

实现时推荐继承 NoiseModelFactor6<Pose3, Vector3, Bias, Pose3, Vector3, Bias>。

2. 自定义代价函数因子（例如点到平面的约束）

$n^\top (R p + t - q)$

class PointPlaneFactor : public NoiseModelFactor1<Pose3> {Point3 point_;Unit3 normal_;Point3 planePoint_;
public:PointPlaneFactor(Key poseKey, Point3 point, Unit3 normal, Point3 planePoint,const SharedNoiseModel& model): NoiseModelFactor1<Pose3>(model, poseKey),point_(point), normal_(normal), planePoint_(planePoint) {}Vector evaluateError(const Pose3& pose, OptionalMatrixType H) const override {Point3 p_world = pose.transformFrom(point_, H);double dist = normal_.point3().dot(p_world - planePoint_);return (Vector(1) << dist).finished();}
};

9.综合示例

示例1-简单入门

下面一个 Point2 约束因子 的例子：

目标：

自定义一个因子 MyPriorFactor，约束某个 Point2 变量靠近观测值 (1.0, 2.0)。
用高斯牛顿优化得到最优结果。

目录结构

my_gtsam_project/
├── CMakeLists.txt
└── src/└── main.cpp

CMakeLists.txt

cmake_minimum_required(VERSION 3.10)
project(MyGTSAMFactorExample)set(CMAKE_CXX_STANDARD 17)
set(CMAKE_BUILD_TYPE Release)find_package(GTSAM REQUIRED)   # 确保你安装了 GTSAMadd_executable(main src/main.cpp)
target_link_libraries(main GTSAM::gtsam)

src/main.cpp

#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/inference/Symbol.h>
#include <iostream>using namespace gtsam;// ============ 自定义因子 ============
// 继承 NoiseModelFactor1，表示这个因子依赖 1 个变量：Point2
class MyPriorFactor:public gtsam::NoiseModelFactor1<gtsam::Point2>
{
public:MyPriorFactor(gtsam::Key key,const gtsam::Point2&measured,const gtsam::SharedNoiseModel&model):gtsam::NoiseModelFactor1<gtsam::Point2>(model,key), m_measured(measured){}// 残差函数：预测值 - 观测值gtsam::Vector evaluateError(const  gtsam::Point2& x, boost::optional<gtsam::Matrix&> H) const override{if (H){*H= (gtsam::Matrix(2, 2) << 1, 0, 0, 1).finished(); // dr/dp = I}return x - m_measured;}
protected:
private:gtsam::Point2 m_measured;//观测值
};// ============ 主函数 ============
int main() {gtsam::NonlinearFactorGraph graph;// 定义变量 keygtsam::Key x = gtsam::Symbol('x', 0);// 构造噪声模型（2维，sigma=0.1）auto noise = gtsam::noiseModel::Isotropic::Sigma(2, 0.1);// 添加自定义因子 (约束 x ~ (1,2))graph.add(boost::make_shared<MyPriorFactor>(x, gtsam::Point2(1.0, 2.0), noise));// 初始值gtsam::Values initial;initial.insert(x, gtsam::Point2(0.0, 0.0));// 优化gtsam::GaussNewtonParams params;params.setVerbosity("ERROR");gtsam::GaussNewtonOptimizer optimizer(graph, initial, params);gtsam::Values result = optimizer.optimize();// 输出结果std::cout << "Initial estimate: " << initial.at<gtsam::Point2>(x).transpose() << std::endl;std::cout << "Optimized result: " << result.at<gtsam::Point2>(x).transpose() << std::endl;
}return 0;
}

编译 & 运行

mkdir build && cd build
cmake ..
make
./main

输出示例：

Initial error: 250
newError: 0
errorThreshold: 0 < 0
iterations: 1 >? 100
Initial estimate: 0 0
Optimized result: 1 2

示例2-进阶

一个 二元因子（比如约束两个 Point2 的距离 = d），展示 NoiseModelFactor2 的用法。

目标：

定义 Point2DistanceFactor，继承自 NoiseModelFactor2<Point2, Point2>。
约束 两个点之间的欧氏距离 ≈ d。
用高斯牛顿优化，把两个点调到满足约束。

src/main.cpp

#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/inference/Symbol.h>
#include <iostream>
#include <cmath>using namespace gtsam;// ============ 二元因子：约束两点距离 ============
// 残差 r = ||p1 - p2|| - d
class Point2DistanceFactor : public gtsam::NoiseModelFactor2<gtsam::Point2, gtsam::Point2> {double measured_d_;  // 观测的距离public:Point2DistanceFactor(gtsam::Key key1, gtsam::Key key2, double measured_d, const gtsam::SharedNoiseModel& model): gtsam::NoiseModelFactor2<gtsam::Point2, Point2>(model, key1, key2), measured_d_(measured_d) {}Vector evaluateError(const gtsam::Point2& p1, const gtsam::Point2& p2,boost::optional<gtsam::Matrix&> H1, boost::optional<gtsam::Matrix&> H2) const override {gtsam::Vector2 diff = p1 - p2;double dist = diff.norm();   // ||p1 - p2||double err = dist - measured_d_;// Jacobiansif (H1 || H2) {// 避免除以 0gtsam::Vector2 unit = (dist > 1e-9) ? diff / dist : gtsam::Vector2(0.0, 0.0);if (H1) *H1 = unit.transpose();   // dr/dp1if (H2) *H2 = -unit.transpose();  // dr/dp2}return gtsam::Vector1(err);}
};// ============ 主函数 ============
int main() {gtsam::NonlinearFactorGraph graph;gtsam::Key x1 = gtsam::Symbol('x', 1);gtsam::Key x2 = gtsam::Symbol('x', 2);// 噪声模型：1 维残差，sigma=0.1auto noise = gtsam::noiseModel::Isotropic::Sigma(1, 0.1);// 添加约束：希望 x1, x2 之间的距离 = 2.0graph.add(boost::make_shared<Point2DistanceFactor>(x1, x2, 2.0, noise));// 约束 x1 在 (0,0)，噪声较小（sigma=1e-3 表示强约束）auto priorNoise = gtsam::noiseModel::Isotropic::Sigma(2, 1e-2);graph.add(boost::make_shared<gtsam::PriorFactor<gtsam::Point2>>(x1, gtsam::Point2(0.0, 0.0), priorNoise));// 初始值：两个点很接近，不满足约束gtsam::Values initial;initial.insert(x1, gtsam::Point2(0.0, 0.0));initial.insert(x2, gtsam::Point2(0.5, 0.0));// 优化gtsam::LevenbergMarquardtParams params;params.setVerbosity("ERROR");gtsam::LevenbergMarquardtOptimizer optimizer(graph, initial, params);gtsam::Values result = optimizer.optimize();// 输出结果std::cout << "Initial:\n";std::cout << "  x1 = " << initial.at<gtsam::Point2>(x1).transpose() << "\n";std::cout << "  x2 = " << initial.at<gtsam::Point2>(x2).transpose() << "\n";std::cout << "Optimized:\n";std::cout << "  x1 = " << result.at<gtsam::Point2>(x1).transpose() << "\n";std::cout << "  x2 = " << result.at<gtsam::Point2>(x2).transpose() << "\n";double final_dist = (result.at<gtsam::Point2>(x1) - result.at<gtsam::Point2>(x2)).norm();std::cout << "Final distance = " << final_dist << "\n";return 0;
}

运行结果

Initial error: 112.5
newError: 1.13625e-12
errorThreshold: 1.13625e-12 > 0
absoluteDecrease: 112.5 >= 1e-05
relativeDecrease: 1 >= 1e-05
newError: 1.21964775949e-28
errorThreshold: 1.21964775949e-28 > 0
absoluteDecrease: 1.13624977089e-12 < 1e-05
relativeDecrease: 1 >= 1e-05
converged
errorThreshold: 1.21964775949e-28 <? 0
absoluteDecrease: 1.13624977089e-12 <? 1e-05
relativeDecrease: 1 <? 1e-05
iterations: 2 >? 100
Initial:x1 = 0 0x2 = 0.5   0
Optimized:x1 = -1.52999989329e-17                  0x2 = 2 0
Final distance = 2

可以看到，优化器自动把两个点调成了 相距 2.0，并且 Jacobian 是自定义的。

10. 总结

简单场景：继承 NoiseModelFactorN，实现 evaluateError。
复杂/动态场景：用 CustomFactor + lambda。
核心原则：残差维度与噪声模型一致，Jacobian 正确。
调试技巧：用 GaussNewtonOptimizer 或 LevenbergMarquardtOptimizer，开启 debugPrint 查看因子贡献。

11、自定义因子计算 Jacobian（H 矩阵）附件

因子定义回顾

误差函数是：

$r = \| p_1 - p_2 \| - d$

其中

$p_1 = (x_1, y_1)^T$ ，
$p_2 = (x_2, y_2)^T$ ，
$d$ 是观测距离。

残差是标量（1 维），所以返回 Vector1(err)。

Jacobian 推导

我们需要分别计算：

$\frac{\partial r}{\partial p_1}, \quad \frac{\partial r}{\partial p_2}$

定义差向量：

$\Delta = p_1 - p_2 = \begin{bmatrix} x_1 - x_2 \\ y_1 - y_2 \end{bmatrix}$

距离：

$\| \Delta \| = \sqrt{(x_1-x_2)^2 + (y_1-y_2)^2}$

残差：

$\| \Delta \| - d$

对 $p_1$ 求导：

$\frac{\partial r}{\partial p_1} = \frac{\Delta^T}{\|\Delta\|}$

这是一个 1×2 向量。

对 $p_2$ 求导：

$\frac{\partial r}{\partial p_2} = -\frac{\Delta^T}{\|\Delta\|}$

同样是 1×2 向量。

代码对应关系

在 C++ 里我们写：

Vector2 diff = p1 - p2;
double dist = diff.norm();  // ||p1 - p2||Vector2 unit = (dist > 1e-9) ? diff / dist : Vector2(0.0, 0.0);if (H1) *H1 = unit.transpose();   // dr/dp1 : 1x2
if (H2) *H2 = -unit.transpose();  // dr/dp2 : 1x2