GTSAM中先验位姿（Prior Factor）详解应用

GTSAM先验位姿的值应该是什么

1. 先验位姿的来源

在因子图优化里，通常需要给第一个位姿节点 X0 加一个 PriorFactor（先验因子），防止图在优化中漂移。
这个先验的值是你人为指定的，比如：

poseNoise = noiseModel::Diagonal::Sigmas(Vector6::Constant(1e-6));
graph.add(PriorFactor<Pose3>(0, Pose3(), poseNoise));

这里 Pose3() 是默认构造函数，确实是单位矩阵（平移为零，旋转为单位旋转）。
这就意味着 如果你没有特别指定，单位矩阵会作为第一个位姿的先验。

2. 为什么常用单位矩阵作为先验？

• 因为第一帧通常设为坐标系原点（世界坐标系）。
• 这样后续优化结果是相对于第一帧的世界坐标输出的，方便可视化与处理。
• 如果你输入的先验不是单位矩阵，整个优化结果会相对于你给的初始先验发生刚体变换。

3. 可以不是单位矩阵

• 如果你的第一帧并不是坐标系原点，比如用 GPS/其他定位系统给出了初始估计：

Pose3 init_pose(Rot3::RzRyRx(yaw, pitch, roll), Point3(x, y, z));
graph.add(PriorFactor<Pose3>(0, init_pose, poseNoise));

• 优化会以这个先验作为参考原点，而不是单位矩阵。

4. 注意事项

• 如果完全不加先验位姿因子（PriorFactor），因子图会有自由度，优化结果会漂移（整个图的绝对位置/朝向不确定）。
• 如果第一帧已经被其他强约束（如 GPS 因子）锁定，也可以不加单位矩阵先验。

5.小结

在 GTSAM 中，“先验位姿”并不是固定单位矩阵，但大多数 SLAM / LiDAR-IMU 系统为了简化，会把第一帧的先验设为 单位矩阵 Pose3()，这样优化结果就是相对第一帧的坐标系。
总结一句就是：在 GTSAM 中，优化时的“先验位姿”是否是单位矩阵，取决于你的因子图建模方式，并不是固定为单位矩阵。

GTSAM中添加先验位姿

在 GTSAM 中添加先验位姿（Prior Factor），其实就是往因子图 NonlinearFactorGraph 里插入一个 PriorFactor<Pose3>。

1. 先验位姿在 GTSAM 中的意义

• 因子图优化 本质上需要一个参考系原点，否则问题是“悬空的”（无绝对参考），优化会出现无穷多解（例如整体平移、旋转自由度）。

• 先验位姿（Prior Factor） 就是给某个节点（通常是第一帧）加一个绝对约束，定义它在全局坐标系下的位置和姿态。

• 数学形式：

  $$f_{\text{prior}}(X_i)=\Vert X_i-X_{prior}{\Vert }_{\Sigma }$$

  其中：

  • $X_i$ 是第 $i$ 个位姿变量（Pose3）
  • $X_{prior}$ 是已知先验位姿
  • $\Sigma$ 是噪声协方差矩阵（噪声模型）

2. 添加先验位姿的基本步骤

2.1 准备数据结构

#include <gtsam/slam/PriorFactor.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/noiseModel/Diagonal.h>

1.2 创建因子图与变量

gtsam::NonlinearFactorGraph graph;
gtsam::Values initialEstimate;

2.3 定义先验噪声模型

auto priorNoise = gtsam::noiseModel::Diagonal::Sigmas(
(gtsam::Vector(6) <<
0.1, 0.1, 0.1, // 平移方向 x,y,z 方差（米）
0.1, 0.1, 0.1 // 旋转方向 roll, pitch, yaw 方差（弧度）
).finished()
);

• 注意这里的单位：

  • 平移单位：米
  • 旋转单位：弧度

2.4 创建先验位姿并加入因子图

假设我们要对 第 0 个节点 添加先验：

gtsam::Pose3 priorPose(
gtsam::Rot3::RzRyRx(0.0, 0.0, 0.0), // yaw, pitch, roll
gtsam::Point3(0.0, 0.0, 0.0) // x, y, z
);

graph.add(gtsam::PriorFactor<gtsam::Pose3>(0, priorPose, priorNoise));

• 第一个参数 0 是变量的 Key（位姿 ID）。
• priorPose 是已知的先验位姿。
• priorNoise 控制先验约束的权重。

2.5 给变量一个初始值

initialEstimate.insert(0, gtsam::Pose3(
gtsam::Rot3::RzRyRx(0.05, –0.02, 0.01), // 初始估计值（稍微有偏差）
gtsam::Point3(0.1, –0.1, 0.05)
));

2.6 完整优化示例

#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <iostream>

int main() {
gtsam::NonlinearFactorGraph graph;
gtsam::Values initialEstimate;

auto priorNoise = gtsam::noiseModel::Diagonal::Sigmas(
(gtsam::Vector(6) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1).finished()
);

gtsam::Pose3 priorPose(gtsam::Rot3::RzRyRx(0, 0, 0),
gtsam::Point3(0, 0, 0));

graph.add(gtsam::PriorFactor<gtsam::Pose3>(0, priorPose, priorNoise));

initialEstimate.insert(0, gtsam::Pose3(
gtsam::Rot3::RzRyRx(0.05, –0.02, 0.01),
gtsam::Point3(0.1, –0.1, 0.05)
));

gtsam::LevenbergMarquardtOptimizer optimizer(graph, initialEstimate);
gtsam::Values result = optimizer.optimize();

std::cout << "优化结果:\\n" << result.at<gtsam::Pose3>(0) << std::endl;
}

3. 常见坑点

GTSAM删除多余的先验位姿

在 GTSAM 里，“删除多余的先验位姿” 本质上是移除因子图里的多余 PriorFactor，因为先验是作为一个因子加入 NonlinearFactorGraph 的。

下面我们分为两种情况说明：

1. 如果是自己代码里误加了多余先验

最简单的方法：

• 不要往因子图里添加多余的 PriorFactor，或者在添加时用条件判断只加一次。

示例：

if (pose_id == 0) { // 只在第一帧加先验
graph.add(PriorFactor<Pose3>(0, Pose3(), poseNoise));
}

这样根本不会有多余的先验进入因子图。

2. 如果因子图已经有多余先验（运行中动态删除）

在 NonlinearFactorGraph 中，每个因子都有索引，可以按条件删除。

#include <gtsam/slam/PriorFactor.h>

for (size_t i = 0; i < graph.size(); ++i) {
auto factor = boost::dynamic_pointer_cast<gtsam::PriorFactor<gtsam::Pose3>>(graph[i]);
if (factor && factor->key() != 0) { // 只保留第一个先验
graph.remove(i); // 移除这个先验
}
}

注意：

• graph.remove(i) 会把对应因子移除（实际是置空 nullptr，不影响索引）。
• 如果你要彻底压缩索引，可以用 graph.erase(std::remove(…)) 方式，但一般没必要。

3. 为什么要删除多余先验？

• 多余先验 会过度约束图（尤其是多机器人、多传感器融合时），可能导致优化发散或强制对齐错误位置。
• LiDAR-IMU 系统中，一般只对第一个时间节点加先验，其他位置的绝对参考用回环约束、GPS因子等替代。

4. 实战建议

如果是 LiDAR-Inertial-Mapping 或 SLAM：

• 在建图时，只在第一帧（或地图初始关键帧）添加一个先验。
• 其他帧的绝对位置约束，用回环检测、GPS、地图匹配等因子实现。
• 如果后处理要移除某个先验，建议在保存因子图（graph.save()）前就清理。