在激光SLAM框架LIO-SAM中加入地平面约束，使用GTSAM构造地平面因子，雅克比矩阵在哪调用，用C++代码实现

在LIO-SAM中加入地平面约束，需要在因子图中加入地平面因子，然后在求解器中调用雅克比矩阵。在GTSAM中，可以通过添加 `PriorFactor` 或 `PlaneFactor` 来引入地平面因子。

具体实现步骤如下：

1. 首先需要定义地平面因子的类型，可以使用 `PlaneFactor` 类型。例如：

using namespace gtsam;

// 定义地平面因子的类型
typedef PlaneFactor<Pose3, Plane3> PlaneFactorType;

2. 在因子图中添加地平面因子。例如：

// 定义因子图
NonlinearFactorGraph graph;

// 添加位姿节点和地平面节点
graph.add(PriorFactor<Pose3>(1, Pose3()));
graph.add(PriorFactor<Plane3>(2, Plane3()));

// 添加地平面因子
graph.add(PlaneFactorType(1, 2, Plane3(), noiseModel::Unit::Create(3)));

其中，`1` 和 `2` 分别是位姿节点和地平面节点的编号，`Plane3()` 是地平面的初始值，`noiseModel::Unit::Create(3)` 是噪声模型。

3. 在求解器中调用雅克比矩阵。例如：

// 定义求解器
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate);

// 设置优化参数
optimizer.setAbsoluteErrorTol(1e-5);
optimizer.setRelativeErrorTol(1e-5);
optimizer.setMaxIterations(100);

// 优化
Values result = optimizer.optimize();

// 获取雅克比矩阵
Matrix H = optimizer.sparseJacobian();

其中，`initialEstimate` 是初始估计值，`optimizer.sparseJacobian()` 可以获取求解器的雅克比矩阵。

以上是一个简单的实现示例，具体实现需要根据具体的应用场景进行调整和优化。