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

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

news 2025/8/16 9:58:57

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) = \| X_i - X_{prior} \|_{\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, rollgtsam::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. 常见坑点

只能加一个强先验
- 通常只在第一个节点加先验，如果给多个节点加强先验，可能会导致系统过约束或优化失败。
噪声不要太小
- 如果噪声过小（权重过大），优化会强行锁定该节点，导致误差无法被传播修正。
注意 Key 对应的节点存在性
- 必须先 insert 或在优化前确保 Key 存在，否则会报错。
单位换算
- 平移是米，旋转是弧度。经常有人把角度直接放进去导致优化结果异常。

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); // 移除这个先验}
}