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

1. 因子图回顾

在 GTSAM 中：

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

r(x)=h(x)−z r(x) = h(x) - z 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。

核心构造函数：

cpp 复制代码

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

其中：

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

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

Step 1. 定义残差函数

例如，一个约束：

r(x1,x2)=f(x1,x2)−z r(x_1, x_2) = f(x_1, x_2) - z r(x1,x2)=f(x1,x2)−z

可以写成 lambda：

cpp 复制代码

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 keys
        if (H[0].rows() != 0) { 
            H[0] = ...; // dr/dx1
        }
        if (H[1].rows() != 0) { 
            H[1] = ...; // dr/dx2
        }
    }

    return r;
};

Step 2. 构造因子

cpp 复制代码

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. 插入因子图

cpp 复制代码

NonlinearFactorGraph graph;
graph.add(factor);

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

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

cpp 复制代码

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Δ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)

cpp 复制代码

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 复制代码

cmake_minimum_required(VERSION 3.10)
project(MyGTSAMFactorExample)

set(CMAKE_CXX_STANDARD 17)
set(CMAKE_BUILD_TYPE Release)

find_package(GTSAM REQUIRED)   # 确保你安装了 GTSAM

add_executable(main src/main.cpp)
target_link_libraries(main GTSAM::gtsam)

src/main.cpp

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;

	// 定义变量 key
	gtsam::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;
}

编译 & 运行

bash 复制代码

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

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_;

		// Jacobians
		if (H1 || H2) {
			// 避免除以 0
			gtsam::Vector2 unit = (dist > 1e-9) ? diff / dist : gtsam::Vector2(0.0, 0.0);

			if (H1) *H1 = unit.transpose();   // dr/dp1
			if (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.1
	auto noise = gtsam::noiseModel::Isotropic::Sigma(1, 0.1);

	// 添加约束：希望 x1, x2 之间的距离 = 2.0
	graph.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 0
  x2 = 0.5   0
Optimized:
  x1 = -1.52999989329e-17                  0
  x2 = 2 0
Final distance = 2

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

10. 总结

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

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

因子定义回顾

误差函数是：

r=∥p1−p2∥−d r = \| p_1 - p_2 \| - d r=∥p1−p2∥−d

其中

p1=(x1,y1)Tp_1 = (x_1, y_1)^Tp1=(x1,y1)T，
p2=(x2,y2)Tp_2 = (x_2, y_2)^Tp2=(x2,y2)T，
ddd 是观测距离。

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

Jacobian 推导

我们需要分别计算：

∂r∂p1,∂r∂p2 \frac{\partial r}{\partial p_1}, \quad \frac{\partial r}{\partial p_2} ∂p1∂r,∂p2∂r

定义差向量：

Δ=p1−p2=[x1−x2y1−y2] \Delta = p_1 - p_2 = \begin{bmatrix} x_1 - x_2 \\ y_1 - y_2 \end{bmatrix} Δ=p1−p2=[x1−x2y1−y2]

距离：

∥Δ∥=(x1−x2)2+(y1−y2)2 \| \Delta \| = \sqrt{(x_1-x_2)^2 + (y_1-y_2)^2} ∥Δ∥=(x1−x2)2+(y1−y2)2

残差：

r=∥Δ∥−d r = \| \Delta \| - d r=∥Δ∥−d

对 p1p_1p1 求导：

∂r∂p1=ΔT∥Δ∥ \frac{\partial r}{\partial p_1} = \frac{\Delta^T}{\|\Delta\|} ∂p1∂r=∥Δ∥ΔT

这是一个 1×2 向量。

对 p2p_2p2 求导：

∂r∂p2=−ΔT∥Δ∥ \frac{\partial r}{\partial p_2} = -\frac{\Delta^T}{\|\Delta\|} ∂p2∂r=−∥Δ∥ΔT

同样是 1×2 向量。

代码对应关系

在 C++ 里我们写：

cpp 复制代码

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

unit = diff / dist 就是 Δ/∥Δ∥\Delta / \|\Delta\|Δ/∥Δ∥。
unit.transpose() 是 1x2 矩阵，符合 GTSAM 要求。

形状确认

残差 r 是标量 → Vector1
对于 p1（2 维变量）：
- Jacobian 应该是 1x2 矩阵
对于 p2 也是 1x2 矩阵

所以在 GTSAM 里：

*H1 和 *H2 的类型是 Eigen::MatrixXd（动态），但我们保证返回 1x2。

总结

思路：先写出误差函数 → 对变量求导 → 确认维度 → 写进 H。
在二元因子里，H1 / H2 分别对应两个变量的 Jacobian。
如果是 NoiseModelFactorN，你就得对每个输入变量都算一次。

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

1. 因子图回顾

2. 因子类体系结构

选择哪种方式？

3. CustomFactor 的核心接口

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

Step 1. 定义残差函数

Step 2. 构造因子

Step 3. 插入因子图

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

6. 常见噪声模型

7. 常见坑

8. 进阶例子

1. IMU 预积分因子

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

9.综合示例

示例1-简单入门

目录结构

CMakeLists.txt

src/main.cpp

编译 & 运行

示例2-进阶

src/main.cpp

10. 总结

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

因子定义回顾

Jacobian 推导

代码对应关系

形状确认

总结

3. `CustomFactor` 的核心接口