c++ gtsam/slam/BetweenFactor.h 详细介绍

gtsam/slam/BetweenFactor.h 是 GTSAM 库中的一个头文件，定义了 BetweenFactor 类。BetweenFactor 用于表示两个状态变量之间的相对约束，通常用于 SLAM 或机器人定位问题中的相对运动或测量约束。

BetweenFactor 类概述

BetweenFactor 表示两个变量（通常是位姿或状态）之间的相对约束，描述了这两个变量之间的相对关系。这种因子在优化过程中提供了关于两个状态之间相对位置或姿态的约束。

主要功能

1. 相对约束：通过定义两个变量之间的相对关系（例如相对位姿），将测量或估计的不确定性引入因子图。这种约束通常用于描述从一个状态到另一个状态的相对运动。

2. 因子图优化：将 BetweenFactor 添加到因子图中，与其他因子结合，通过图优化来求解变量的最优值。

主要成员函数

• 构造函数

BetweenFactor(Key key1, Key key2, const Pose3& measurement, const SharedNoiseModel& model);
// 创建一个 BetweenFactor，用于将 key1 和 key2 之间的相对位姿约束为 measurement，并应用 noise model。

• key1 和 key2 是因子图中两个状态变量的键。
• measurement 是两个变量之间的相对位姿（Pose3）。
• model 是描述测量不确定性的噪声模型。
• 获取测量值

Pose3 measured() const;
// 获取两个变量之间的测量值，即相对位姿。

• 计算误差

Vector evaluateError(const Pose3& pose1, const Pose3& pose2) const;
// 计算给定变量值下的因子误差。

应用举例

1. 机器人定位

假设一个机器人在一个环境中移动，并且在每一步移动时都进行里程计测量。你可以使用 BetweenFactor 来表示机器人在两个连续时刻的相对运动。

// 定义机器人在两个时刻的位姿变量
Key pose1 = symbol('x', 1);
Key pose2 = symbol('x', 2);

// 定义相对位姿测量值（从 pose1 到 pose2 的相对运动）
Pose3 relativePose = Pose3(Rot3(), Point3(1.0, 0.0, 0.0));  // 假设前进 1 米

// 定义噪声模型（假设测量不确定性）
SharedNoiseModel noiseModel = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));

// 创建 BetweenFactor
BetweenFactor<Pose3> factor(pose1, pose2, relativePose, noiseModel);

// 将因子添加到因子图中
graph.add(factor);

2. 地图构建

在 SLAM 系统中，地图中每个特征点的位置可以通过多个相对位姿的测量来约束。例如，假设有一个特征点的测量数据，表示从一个已知位置到特征点的相对位姿，可以用 BetweenFactor 来表示这个约束。

// 定义特征点在不同位姿下的相对位置
Key poseA = symbol('x', 1);
Key landmark = symbol('l', 1);

// 定义从 poseA 到特征点的相对位姿测量值
Pose3 relativePose = Pose3(Rot3(), Point3(2.0, 1.0, 0.0));  // 特征点相对位姿

// 定义噪声模型
SharedNoiseModel noiseModel = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.2));

// 创建 BetweenFactor
BetweenFactor<Pose3> factor(poseA, landmark, relativePose, noiseModel);

// 将因子添加到因子图中
graph.add(factor);

在这两个例子中，BetweenFactor 用于描述两个变量之间的相对关系，并且与噪声模型一起定义了约束的准确度。这些因子会被添加到因子图中，通过优化算法来求解最优状态估计。