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

GTSAM中gtsam::LinearContainerFactor因子详解

news 2025/8/30 5:49:15

概述

LinearContainerFactor 是 GTSAM 提供的一个 “线性因子容器”，用于把已经是线性（Gaussian）形式的因子插入到非线性因子图（NonlinearFactorGraph）中。它的主要用途包括：

将已线性化或本质为线性的因子（如 JacobianFactor、HessianFactor 或任意 GaussianFactor）以非线性因子的形式放入 NonlinearFactorGraph，从而可以与普通非线性因子一起存储、序列化、重键（rekey）等。
支持在存在**线性化点（linearization point）**时对因子进行 近似 relinearize（在满足小角度和线性假设下），从而在某些场景下避免重新生成完整的线性因子。

它是把线性世界（GaussianFactorGraph / Jacobian/HessianFactor）与非线性世界（NonlinearFactorGraph）桥接起来的“适配器”。

主要构造函数与接口（API 快览）

（下列函数名和行为均按 GTSAM doxygen 所示）

构造函数（主要形式）：
- LinearContainerFactor(const JacobianFactor& factor, const Values& linearizationPoint = Values())
- LinearContainerFactor(const HessianFactor& factor, const Values& linearizationPoint = Values())
- LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const Values& linearizationPoint = Values())
  这些构造器把线性因子（或其 shared_ptr）存储在容器中，可选地同时保存一个线性化点。
关键访问器与操作：
- GaussianFactor::shared_ptr linearize(const Values& c) const
  将容器“线性化”为一个 GaussianFactor。行为取决于是否存在线性化点（详见下节 relinearize 说明）。
- const boost::optional<Values>& linearizationPoint() const：返回可能存在的线性化点。
- double error(const Values& c) const：如果保存了线性化点，返回按（linearizationPoint → c 的局部坐标增量）代入已存线性因子后的非线性误差；否则返回 0。
- bool isJacobian() / bool isHessian()、toJacobian() / toHessian()：检查或把内部存储的 Gaussian 因子转换为具体类型（如果可能）。
- 静态工具：ConvertLinearGraph(const GaussianFactorGraph& linear_graph, const Values& linearizationPoint = Values())
  将一个线性因子图批量转换为由 LinearContainerFactor 组成的 NonlinearFactorGraph（方便把现有线性图放入非线性图中）。

`linearize()` 与 relinearize 的实现细节（关键）

这是理解 LinearContainerFactor 的核心——它并不是“简单地把线性因子原封不动放进非线性图”后就结束，而是在给出线性化点时可以基于该点做增量式近似重新线性化：

如果没有提供线性化点，linearize(c) 将直接 克隆（clone）存储的线性因子并返回（即直接返回原始 Gaussian 因子）。
如果有线性化点（linearizationPoint_），linearize(c) 会：
1. 计算 delta = linearizationPoint_.localCoordinates(c)（在变量流形上的局部差量）。
2. 用这个线性 delta 去代入到存储的线性因子中，得到一个“relinearized”版本并返回。
重要限制 / 假设：这种 relinearization 是在 小角度 & 线性变量流形 的近似下成立的。对于非交换群（例如 Pose2/Pose3 的旋转），这种替代只有在角度很小（即小角近似）时才准确。文档明确指出这是一个近似（并标注 TODO 改进），因此在大变动（large rotations）或高度非线性情况下不要依赖该近似。

直观理解：LinearContainerFactor 保存了“某个线性因子的线性化版本（可能是在某个点线性化得到的）”，当你将图的线性化点稍作移动（小增量）时，容器可以通过简单地把 delta 代入已存的线性表达式来快速得到新的线性因子，而无需重新求导或重线性化原始非线性因子。这对需要把预先计算好的线性因子注入到非线性图并在小变化下快速更新很有用。

典型用法示例（C++ 片段）

把一个已线性化的 JacobianFactor 包装成 LinearContainerFactor 并放入 NonlinearFactorGraph：

#include <gtsam/nonlinear/LinearContainerFactor.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>// 假设 jacobianFactor 是已构造好的 JacobianFactor, linPoint 是 Values
gtsam::JacobianFactor jacobianFactor = ...;
gtsam::Values linPoint = ...;gtsam::LinearContainerFactor container(jacobianFactor, linPoint);gtsam::NonlinearFactorGraph graph;
graph.add(container); // 以非线性因子形式保存该线性因子

从 NonlinearFactorGraph 中取出并重新 linearize（得到 GaussianFactor）:

gtsam::Values currentValues = ...;
auto linearized = container.linearize(currentValues); // 返回 GaussianFactor::shared_ptr
// 然后可以把 linearized 插入 GaussianFactorGraph 或线性求解器

把一个 GaussianFactorGraph 整体转为 NonlinearFactorGraph（批量包装）:

gtsam::GaussianFactorGraph gfg = ...;
gtsam::Values linearPoint = ...;
auto nonlinearFromLinear = gtsam::LinearContainerFactor::ConvertLinearGraph(gfg, linearPoint);
// nonlinearFromLinear 是 NonlinearFactorGraph，其中每个因子都是 LinearContainerFactor

（以上示例基于 GTSAM 官方 API 文档行为说明。）

适用场景（何时用它）

需要把预先构造/保存的线性因子重新注入非线性图，例如你有某个外部线性化器或想把历史线性信息序列化到磁盘并在 later 重用 / 分发给其他进程。
混合线性/非线性系统：某些因子本质上为线性（例如某些投影在某些参数化下），用容器保存可以避免重复重线性化。
性能优化场景：当线性化点只小幅改变（小角），使用 LinearContainerFactor 的 relinearize 近似比从头重线性化原始非线性因子要快。
把 GaussianFactorGraph 转换为 NonlinearFactorGraph，便于统一存储/序列化/调试（ConvertLinearGraph）。

局限与注意事项（重要）

relinearize 是近似：当变量位于非交换流形（例如旋转变量）且变化不再是“小”角度时，relinearize 的近似可能严重失真。文档明确指出仅在小角和线性假设下有效。不要在大旋转或剧烈非线性情形中盲目使用该近似。
误差计算的含义：error(c) 在存在线性化点时，计算的是将 delta = linearizationPoint_.localCoordinates(c) 代入存储的线性因子后的误差；如果没有线性化点，返回 0（因为线性因子本身并不定义直接的非线性误差）。理解这一点对于调试很重要。
序列化/IO：LinearContainerFactor 支持序列化（有 friend boost::serialization::access），适合保存到磁盘或在分布式系统中传输。
类型转换检查：可以通过 isJacobian()/isHessian() 以及 toJacobian()/toHessian() 安全地把内部 GaussianFactor 转回具体类型（如果你需要做更底层操作）。

性能与实现建议

若你的系统对速度敏感且线性化点通常只发生小幅变化，LinearContainerFactor 的 relinearize 能显著降低重线性化开销；但务必在实际数据上验证近似误差是否可接受。
如果场景中存在大量大旋转或强非线性，更保守的做法是直接保留原始非线性因子并在需要时完整线性化，而非依赖 relinearize 近似。
ConvertLinearGraph 很有用：例如把一次批处理线性求解的结果包装回 NonlinearFactorGraph 中，便于后续用 Nonlinear 工具（序列化、重键、图可视化）处理。

引用（关键依据）

GTSAM doxygen: LinearContainerFactor 类说明与成员函数（构造、linearize()、linearizationPoint()、ConvertLinearGraph() 等）。
GTSAM tests / 示例（testLinearContainerFactor.cpp）演示了 linearize 与预期 GaussianFactor 的比较，说明其行为与实现。

综合示例

示例 1：LinearContainerFactor 的基本用法 & linearize 比较

文件名：linear_container_demo.cpp

// linear_container_demo.cpp
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/LinearContainerFactor.h>
#include <gtsam/inference/Symbol.h>
#include <iostream>using namespace gtsam;int main() {// 简单非线性因子图：两个 Pose2， x1 有一个 Prior，x1-x2 有一个 BetweenNonlinearFactorGraph nfg;Values initial;Symbol x1('x', 1), x2('x', 2);Pose2 p1_init(0.0, 0.0, 0.0);Pose2 p2_init(1.0, 0.0, 0.0);// 插入初值initial.insert(x1, p1_init);initial.insert(x2, p2_init);// 先验（对 x1）noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 1e-3, 1e-3, 1e-3).finished());nfg.emplace_shared<PriorFactor<Pose2>>(x1, p1_init, priorNoise);// Between factor (x1 -> x2)Pose2 odom = Pose2(1.0, 0.0, 0.05); // slight rotationnoiseModel::Diagonal::shared_ptr betweenNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1, 0.1, 0.05).finished());nfg.emplace_shared<BetweenFactor<Pose2>>(x1, x2, odom, betweenNoise);std::cout << "Nonlinear graph:\n";nfg.print("  ");// 1) 把非线性图在线性化点 initial 上线性化 → 得到 GaussianFactorGraphstd::cout << "\nLinearizing NonlinearFactorGraph at initial values...\n";GaussianFactorGraph::shared_ptr gfg = nfg.linearize(initial); // 返回 shared_ptrif (!gfg) {std::cerr << "Linearization failed.\n";return -1;}std::cout << "GaussianFactorGraph:\n";gfg->print("  ");// 2) 将 GaussianFactorGraph 批量转换为由 LinearContainerFactor 构成的 NonlinearFactorGraphNonlinearFactorGraph containerNFG = LinearContainerFactor::ConvertLinearGraph(*gfg, initial);std::cout << "\nConverted NonlinearFactorGraph (LinearContainerFactor wrappers):\n";containerNFG.print("  ");// 3) 选取第一个 LinearContainerFactor，并演示 linearize() 在不同 values 下的行为std::shared_ptr<LinearContainerFactor> containerFactor =std::dynamic_pointer_cast<LinearContainerFactor>(containerNFG.at(0));if (!containerFactor) {std::cerr << "Failed to get LinearContainerFactor.\n";return -1;}std::cout << "\nContainer has linearization point? "<< (containerFactor->linearizationPoint() ? "yes" : "no") << "\n";// container 的 linearizationPoint 应该等于 initial（ConvertLinearGraph 时传入了 initial）if (containerFactor->linearizationPoint()) {std::cout << "Stored linearization point:\n";containerFactor->linearizationPoint()->print("  ");}// linearize at exactly the same point -> should return the same GaussianFactor (up to print)auto gaussian_at_same = containerFactor->linearize(initial);std::cout << "\nGaussian factor from linearize(initial):\n";gaussian_at_same->print("  ");// 构造一个微小变化的 currentValues（小角度、小位移），用于测试 relinearize 近似Values current = initial;Pose2 p2_perturbed(1.02, 0.01, 0.02); // slight perturbationcurrent.update(x2, p2_perturbed);std::cout << "\nLinearize at a perturbed values (small delta):\n";auto gaussian_pert = containerFactor->linearize(current);gaussian_pert->print("  ");std::cout << "\nNote: The above two GaussianFactor prints show how LinearContainerFactor reuses stored linear factor\n"<< "and applies the local coordinate delta when a linearization point is available.\n";return 0;
}

输出说明：

程序会打印非线性图、线性化后的 GaussianFactorGraph，以及由 LinearContainerFactor 生成并 linearize() 后的 GaussianFactor。
当 linearize() 在与 linearizationPoint 相同或仅有小幅度扰动的 Values 上调用时，容器会基于存储的线性因子给出一个（近似）线性因子；输出中 GaussianFactor::print() 会展示矩阵形式（Jacobian/Hessian）与常量项 b。

提示：不同 GTSAM 版本在 GaussianFactor::print() 的具体格式上略有差异，但 linearize 返回能打印出矩阵与向量信息，便于比较。

示例 2：ISAM2 中保存 LinearContainerFactor 并重建 / 重新注入的流程示例

文件名：isam2_container_workflow.cpp

// isam2_container_workflow.cpp
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/LinearContainerFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Symbol.h>
#include <iostream>
#include <memory>using namespace gtsam;int main() {// 1. 构建初始非线性图（与示例1类似）NonlinearFactorGraph nfg;Values initial;Symbol x1('x', 1), x2('x', 2);Pose2 p1_init(0.0, 0.0, 0.0), p2_init(1.0, 0.0, 0.0);initial.insert(x1, p1_init);initial.insert(x2, p2_init);auto priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 1e-3,1e-3,1e-3).finished());auto betweenNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.1,0.1,0.05).finished());nfg.emplace_shared<PriorFactor<Pose2>>(x1, p1_init, priorNoise);nfg.emplace_shared<BetweenFactor<Pose2>>(x1, x2, Pose2(1.0,0.0,0.05), betweenNoise);// 2. 线性化并把 Gaussian 因子转换为 LinearContainerFactor，方便“持久化”或重新注入GaussianFactorGraph::shared_ptr gfg = nfg.linearize(initial);NonlinearFactorGraph container_graph = LinearContainerFactor::ConvertLinearGraph(*gfg, initial);std::cout << "Container graph (to be stored / re-injected):\n";container_graph.print("  ");// 3. 构造 ISAM2 并做一次更新（用 container_graph）ISAM2Params params;ISAM2 isam(params);// We can update with the container_graph and initial valuesisam.update(container_graph, initial);Values estimate = isam.calculateEstimate();std::cout << "ISAM2 estimate after initial update:\n";estimate.print("  ");// ---- 假设某种错误发生（例如我们检测到奇异，需要 reset isam） ----// 为了示范，我们将重建 isam：释放旧的 isam 实例，创建新的 isam 实例，// 并把之前保存的 container_graph + linearization point 重新注入。std::cout << "\n--- Simulating ISAM2 reset/rebuild ---\n";// 模拟保存 container_graph 到磁盘 -> load -> 再注入的流程NonlinearFactorGraph loaded_container_graph = container_graph; // 在真实场景中你可能序列化然后读回// Create a fresh ISAM2ISAM2 isam_new(params);// Now re-inject loaded container graph and values to new isamisam_new.update(loaded_container_graph, initial);Values estimate2 = isam_new.calculateEstimate();std::cout << "ISAM2 estimate after re-injecting stored container graph:\n";estimate2.print("  ");// 说明：// - 在实际工程中，如果 ISAM2 遇到数值问题需要 reset，我们可以把现有的线性化信息（gfg）//   或 container_graph 保存下来，然后新建一个 isam 实例，并把 container_graph & linearizationValues//   重新 update 到新的 isam 中；这样避免重算整个非线性图的所有线性化步骤，且可保留历史线性信息。//// - 上面使用 ConvertLinearGraph 得到的 container_graph 是把 Gaussian 因子包装成 LinearContainerFactor，//   每个 containerFactor 中还存有 linearizationPoint（initial）。当我们给新的 isam 调用 update 时，//   isam 会把这些 LinearContainerFactor 再次线性化（通常直接使用内部的 Gaussian 因子或使用近似 relinearize）//   并插入内部线性系统中。//// - 如果 graph 很大或需要分块注入，也可以按块保存 container_graph 的子集并依次注入新 isam。//// - 如果想在 reset 时直接注入 GaussianFactorGraph 而不是 container factor，也可在新 isam 创建后将//   Gaussian 因子直接作为线性因子插入（需要使用 isam2 的线性接口或把 Gaussian 转为非线性 proxy）。//return 0;
}

说明

运行会展示 container_graph（一系列 LinearContainerFactor）以及在原 ISAM2 更新和在重建后的 ISAM2 更新后的估计值。
该示例展示了如何把线性因子保存为容器并在重建 ISAM2 时重新注入，以避免重新从原始非线性因子反复计算（这是你在工程中常用的重置/恢复模式）。

额外说明 / 注意事项

GTSAM 版本差异：不同版本的 GTSAM 具体 API 命名或返回类型可能有细微差别（例如 NonlinearFactorGraph::linearize 返回 GaussianFactorGraph::shared_ptr 或 GaussianFactorGraph，或者 LinearContainerFactor 的构造签名细节）。上面代码基于常见的 GTSAM 4.x API；若编译报错，请根据机器上 GTSAM 头文件提示作极小调整（通常仅是签名或命名空间差别）。
relinearize 的近似性：LinearContainerFactor 的 linearize 若存在线性化点，会尝试用该点对存储的线性因子做“局部代入（local coordinate delta）”来得到新的 Gaussian 因子。这是近似，只在小角度 & 近似线性区域成立。见上面演示的 perturbed 情况用于比对。若系统中存在大角度旋转或流形非线性强，请慎用该近似，或在需要时强制重新线性化原始非线性因子。
持久化（序列化）：LinearContainerFactor 支持 Boost 序列化，可以把 NonlinearFactorGraph（含 LinearContainerFactor）序列化到磁盘，然后在重启或其他进程中反序列化并重新注入。
性能考量：
- 把线性化步骤提前并以 container 形式保存能在重建图或分布式情景下节省大量重线性化开销。
- 但要权衡近似误差：若 linearization point 与实际工作 points 差距较大，因子仍需完整版重线性化以保证精度。