<think>我们参考提供的引用内容，特别是引用[1]中提到的二维和三维空间刚体变换中的
雅克比矩阵推导，以及引用[3]和[4]中关于GTSAM因子图构建和
优化的代码示例。用户的问题是关于在GTSAM中自定义地平面约束因子（包括法向量和距离）并计算雅可比
矩阵。
地平面约束可以视为一个一元因子（因为它是针对单个位姿的约束）或二元因子（如果涉及两个位姿之间的约束），但这里我们考虑的是单个位姿与地平面之间的约束。具体来说，我们假设地平面由法向量n和距离d定义（平面方程为n^T * x = d，其中x是平面上的点）。对于机器人位姿T（通常表示为SE(3)），我们关心的是机器人位置（即平移部分t）是否满足地平面约束。然而，更常见的是，我们约束机器人的高度（即z坐标）与地平面一致。但这里用户要求的是更一般的地平面约束（法向量和距离）。
我们可以这样定义
误差函数：
设机器人的位姿为T，其位置为t（即T的平移向量）。则位置t到地平面（法向量n，距离d）的距离
误差为：
error = n^T * t - d
注意：这里我们假设地平面方程是n^T * x = d，其中||n||=1。那么点t到平面的距离就是|n^T t - d|，但作为
误差，我们通常使用线性
误差（即n^T t - d）或者平方
误差（用于高斯牛顿法）。在因子图中，我们通常使用
误差的平方和
形式，但
误差函数本身是线性的。
然而，用户可能希望约束机器人的姿态，使得机器人的某个平面（例如底盘）与地平面平行。这就会涉及到旋转部分。但用户问题中只提到了法向量和距离，所以我们先考虑位置约束。
如果只考虑位置约束，那么
误差函数为：
e(t) = n^T * t - d
这个
误差函数关于位姿T的雅可比
矩阵如何求？注意，位姿T是6自由度（旋转和平移各3）。但是，上述
误差函数仅与平移t有关。因此，关于平移t的导数为：
de/dt = n^T (1x3向量)
而关于旋转部分的导数呢？由于
误差函数不依赖于旋转，所以旋转部分的导数为0。因此，整个
误差关于位姿T（李代数ξ）的雅可比
矩阵为：
J = [0, 0, 0, n^T] (1x6
矩阵，前3维为旋转，后3维为平移)
但是，我们通常使用李代数上的导数。在GTSAM中，位姿是SE(3)，其李代数为6维向量（前3维旋转，后3维平移）。所以，
误差关于李代数的导数就是上述1x6
矩阵。
然而，用户可能希望约束机器人的姿态，使得机器人的z轴与地平面法向量平行？这并没有明确。根据问题，我们只考虑位置约束。如果需要姿态约束，则需要另外定义
误差函数。
现在，我们按照GTSAM自定义因子的步骤来编写代码：
1. 从gtsam::NoiseModelFactor1<gtsam::Pose3>继承（因为是一元因子，约束一个位姿）。
2. 在构造
函数中传入地平面的法向量n和距离d，以及该因子关联的变量键（key）和噪声模型。
3. 重写evaluateError
函数，计算
误差并返回。同时，如果提供了雅可比
矩阵的指针，则计算雅可比
矩阵。
注意：在GTSAM中，自定义因子需要提供
误差函数以及其导数（雅可比
矩阵）。我们使用自动微分（通过GTSAM提供的衍生工具）或者手动
推导。这里我们手动
推导。
代码示例：
```cpp
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
using namespace gtsam;
// 地平面约束因子，约束单个位姿的位置满足地平面方程
class GroundPlaneFactor : public NoiseModelFactor1<Pose3> {
private:
Vector3 n_; // 地平面法向量（单位向量）
double d_; // 距离
public:
// 构造
函数：传入关联的位姿key，法向量n，距离d，噪声模型
GroundPlaneFactor(Key key, const Vector3& n, double d, const SharedNoiseModel& model) :
NoiseModelFactor1<Pose3>(model, key), n_(n.normalized()), d_(d) {}
// 重写evaluateError
函数
Vector evaluateError(const Pose3& T, boost::optional<Matrix&> H = boost::none) const override {
// 获取位姿的平移部分
Point3 t = T.translation();
// 计算
误差：e = n^T * t - d
double error = n_.dot(t) - d_;
// 如果要求计算雅可比
矩阵
if (H) {
//
误差关于位姿T的导数：由于
误差只与平移t有关，所以关于位姿的李代数ξ的导数为：
// 前3维（旋转部分）为0，后3维（平移部分）为n^T
Matrix16 H_; // 1x6
矩阵
H_.setZero();
H_.block<1,3>(0,3) = n_.transpose(); // 平移部分的导数
*H = H_; // 将雅可比
矩阵赋值给H
}
// 返回
误差向量（这里是一维）
return Vector1(error);
}
};
```
注意：上述因子只约束了位置，没有约束姿态。如果用户希望同时约束姿态（例如要求机器人的z轴与地平面法向量平行），那么我们需要定义更复杂的
误差函数。这里我们仅按用户要求（法向量和距离）实现了位置约束。
在因子图中使用该因子的示例：
```cpp
// 创建因子图
NonlinearFactorGraph graph;
// 定义地平面：法向量(0,0,1)，距离0（即z=0的平面）
Vector3 n(0, 0, 1);
double d = 0;
// 噪声模型：假设
误差的标准差为0.1米
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(1, 0.1);
// 创建地平面因子，关联到位姿key为1
Key key = 1;
graph.add(boost::make_shared<GroundPlaneFactor>(key, n, d, model));
// 然后添加其他因子，并设置初始值，进行
优化（如引用[3]中的
优化过程）
```
注意：上面的代码中，我们假设地平面是xy平面（z=0）。实际应用中，地平面参数需要根据实际情况确定。
如果用户希望约束姿态，我们可以定义另一个因子，例如约束机器人的z轴（在机器人坐标系中，通常为(0,0,1)）与地平面法向量（在世界坐标系中为n）之间的夹角。但根据用户问题，我们只实现了位置约束。
此外，在三维激光里程计中，地平面约束通常用于
SLAM中的高度约束，以减少高度方向的漂移。因此，上述因子可以有效地约束机器人的高度。
§§相关问题§§
1. 在GTSAM中自定义因子时，除了地平面约束，还有哪些常见的自定义约束因子？
2. 如何
推导三维空间中点面距离约束的雅可比
矩阵？
3. GTSAM中如何
优化包含自定义因子的因子图？
4. 地平面约束在三维激光
SLAM中的作用是什么？</think>### 在GTSAM中自定义地平面约束因子
在三维激光里程计中，地平面约束（法向量和距离）可通过自定义一元因子实现，用于约束机器人位姿的高度和平行度。以下是完整的实现方案：
#### 1. 地平面约束的数学定义
设地平面方程为：
$$ \mathbf{n}^\top \mathbf{p} + d = 0 $$
其中：
- $\mathbf{n}$ 为平面单位法向量（世界坐标系）
- $d$ 为平面到原点的距离
- $\mathbf{p}$ 为机器人位置（平移向量）
误差函数定义为位置到平面的距离：
$$ e(\mathbf{T}) = \mathbf{n}^\top (\mathbf{R}\mathbf{p}_b + \mathbf{t}) + d $$
其中：
- $\mathbf{T} = (\mathbf{R}, \mathbf{t})$ 为位姿（$\mathbf{R}$为旋转
矩阵，$\mathbf{t}$为平移向量）
- $\mathbf{p}_b$ 为机器人坐标系下的参考点（通常取零向量）
#### 2. 雅可比
矩阵推导
误差函数对位姿李代数 $\xi$ 的导数为：
$$ \frac{\partial e}{\partial \xi} =
\begin{bmatrix}
\mathbf{n}^\top (\mathbf{R}[\mathbf{p}_b]_\times) & \mathbf{n}^\top \mathbf{R}
\end{bmatrix} $$
其中 $[\cdot]_\times$ 为叉
乘的反对称
矩阵。
#### 3. C++代码实现
```cpp
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/Matrix.h>
class GroundPlaneFactor : public gtsam::NoiseModelFactor1<gtsam::Pose3> {
gtsam::Vector3 n_; // 平面法向量 (世界坐标系)
double d_; // 平面距离
gtsam::Point3 p_b_; // 机器人坐标系下的参考点
public:
GroundPlaneFactor(gtsam::Key key,
const gtsam::Vector3& n,
double d,
const gtsam::Point3& p_b,
const gtsam::SharedNoiseModel& model) :
NoiseModelFactor1<gtsam::Pose3>(model, key),
n_(n.normalized()), d_(d), p_b_(p_b) {}
gtsam::Vector evaluateError(
const gtsam::Pose3& T,
boost::optional<gtsam::Matrix&> H = boost::none) const override
{
using namespace gtsam;
// 坐标系转换: p_w = R * p_b + t
Point3 p_w = T.rotation() * p_b_ + T.translation();
// 计算
误差: e = n^T * p_w + d
double error = n_.dot(p_w) + d_;
// 计算雅可比
矩阵
if (H) {
Matrix16 H_pose = Matrix::Zero(1, 6);
// 对平移的导数: ∂e/∂t = n^T
H_pose.block<1, 3>(0, 3) = n_.transpose();
// 对旋转的导数: ∂e/∂θ = n^T * R * [p_b]_×
Matrix3 R = T.rotation().matrix();
Matrix3 skew_p_b = skewSymmetric(p_b_); // 反对称
矩阵
H_pose.block<1, 3>(0, 0) = n_.transpose() * R * skew_p_b;
*H = H_pose;
}
return Vector1(error);
}
private:
// 生成反对称
矩阵
gtsam::Matrix3 skewSymmetric(const gtsam::Vector3& v) const {
return (gtsam::Matrix3() <<
0, -v.z(), v.y(),
v.z(), 0, -v.x(),
-v.y(), v.x(), 0).finished();
}
};
```
#### 4. 在因子图中集成
```cpp
// 创建因子图
gtsam::NonlinearFactorGraph graph;
// 定义地平面参数 (示例值)
gtsam::Vector3 n(0, 0, 1); // Z轴法向量
double d = -1.5; // 高度1.5米
gtsam::Point3 p_b(0, 0, 0); // 机器人坐标系原点
// 创建噪声模型 (等方差高斯噪声)
auto noise = gtsam::noiseModel::Isotropic::Sigma(1, 0.1); //
10cm标准差
// 添加地平面约束 (约束位姿key=1)
graph.add(boost::make_shared<GroundPlaneFactor>(1, n, d, p_b, noise));
// 添加其他约束 (里程计、回环等)...
//
优化 (参考引用[3])
gtsam::Values initial;
initial.insert(1, gtsam::Pose3());
gtsam::LevenbergMarquardtOptimizer optimizer(graph, initial);
gtsam::Values result = optimizer.optimize();
```
#### 关键点说明
1. **坐标系定义**：
- 法向量 $\mathbf{n}$ 需在世界坐标系定义
- $\mathbf{p}_b$ 是机器人本体坐标系下的固定参考点
- 默认取 $\mathbf{p}_b = \mathbf{0}$ 约束机器人原点高度
2. **雅可比计算**：
- 旋转部分使用李代数 $\mathfrak{so}(3)$ 的导数
形式
- 通过反对称
矩阵实现旋转导数的链式法则
- 平移导数直接为法向量转置
3. **实际应用**：
- 在激光
SLAM中可作为先验约束减少Z轴漂移
- 与IMU数据融合时需协调坐标系
- 适用于室内结构化环境或地面机器人[^1]
[^1]: 二维空间和三维空间刚体变换中的
雅克比矩阵的
推导
[^3]: GTSAM
优化流程参考激光里程计实现
本文详述了在学习《视觉SLAM十四讲》第10讲时，关于位姿图优化中右乘形式雅克比矩阵的推导过程。作者分享了自己的尝试，并提供了相关文章链接，帮助读者理解从第二步到最终推导的步骤。适合对SLAM和图优化感兴趣的读者深入研究。
扫一扫
907
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
