怎么使用GTSAM库计算地平面约束的雅克比矩阵，结合激光里程计估计三维坐标系，请给出公式和C++代码

在GTSAM中，使用因子图（Factor Graph）来表示约束条件，并使用非线性优化算法来求解最优解。地平面约束可以表示为一个因子，它的误差函数为非线性函数，因此需要计算雅克比矩阵来进行优化。

首先，我们需要定义误差函数，假设我们有一个位姿变量x和一个地平面特征变量p，地平面特征变量表示地面的法向量和一个点到地面的距离。误差函数可以表示为：

$$ f(x,p) = \frac{(n^T(x-p) + d)^2}{\sigma^2} $$

其中，n是地面法向量，d是点到地面的距离，$\sigma$是噪声标准差。这个误差函数表示了地面特征点和位姿之间的误差，我们需要最小化这个误差函数来优化位姿和地平面特征。

然后，我们需要计算误差函数的雅克比矩阵。雅克比矩阵可以使用自动微分技术来计算，GTSAM库已经集成了这个功能，我们只需要定义误差函数，并将其转化为因子即可。地平面约束的雅克比矩阵可以表示为：

$$ \frac{\partial f}{\partial x} = \frac{2n(n^T(x-p)+d)}{\sigma^2} $$

$$ \frac{\partial f}{\partial p} = \frac{2(n^T(x-p)+d)n}{\sigma^2} $$

最后，我们需要将误差函数和雅克比矩阵转化为因子，使用GTSAM中的因子图进行优化。下面是使用GTSAM库计算地平面约束的C++代码：

#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
#include <gtsam/nonlinear/Expression.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/inference/Symbol.h>

using namespace gtsam;

// 定义误差函数
class GroundFactor : public NoiseModelFactor2<Pose3, Point3> {
public:
    GroundFactor(Key pose_key, Key ground_key, Vector3 ground_normal, double ground_distance, SharedNoiseModel noise_model) :
        NoiseModelFactor2<Pose3, Point3>(noise_model, pose_key, ground_key),
        ground_normal_(ground_normal),
        ground_distance_(ground_distance) {}

    Vector evaluateError(const Pose3& pose, const Point3& ground, boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
        Vector3 error;
        error[0] = (ground_normal_.transpose() * (pose.translation() - ground.vector()) + ground_distance_) / sqrt(noiseModel()->R()(0, 0));
        if (H1) {
            *H1 = (ground_normal_ * 2 * sqrt(noiseModel()->R()(0, 0))) / (ground_normal_.transpose() * (pose.translation() - ground.vector()) + ground_distance_);
        }
        if (H2) {
            *H2 = (-ground_normal_ * 2 * sqrt(noiseModel()->R()(0, 0))) / (ground_normal_.transpose() * (pose.translation() - ground.vector()) + ground_distance_);
        }
        return error;
    }

private:
    Vector3 ground_normal_;
    double ground_distance_;
};

int main() {
    // 创建因子图
    NonlinearFactorGraph graph;

    // 添加激光里程计因子
    Key pose_key = Symbol('x', 1);
    Pose3 initial_pose(Rot3::Ypr(0.1, 0.2, 0.3), Point3(1, 2, 3));
    noiseModel::Diagonal::shared_ptr pose_noise = noiseModel::Diagonal::Sigmas((Vector(6) << 0.1, 0.1, 0.1, 0.01, 0.01, 0.01).finished());
    graph.add(PriorFactor<Pose3>(pose_key, initial_pose, pose_noise));
    Pose3 last_pose = initial_pose;

    // 添加地平面约束因子
    Key ground_key = Symbol('p', 1);
    Vector3 ground_normal(0, 0, 1);
    double ground_distance = 1;
    noiseModel::Diagonal::shared_ptr ground_noise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));
    graph.add(boost::make_shared<GroundFactor>(pose_key, ground_key, ground_normal, ground_distance, ground_noise));

    // 定义变量初始值
    Values initial_values;
    initial_values.insert(pose_key, initial_pose);
    initial_values.insert(ground_key, Point3(0, 0, ground_distance));

    // 运行优化
    LevenbergMarquardtOptimizer optimizer(graph, initial_values);
    Values result_values = optimizer.optimize();

    // 获取优化结果
    Pose3 pose = result_values.at<Pose3>(pose_key);
    Point3 ground = result_values.at<Point3>(ground_key);

    return 0;
}

这段代码中，我们定义了一个GroundFactor类，它继承自NoiseModelFactor2，并重写了evaluateError方法来定义误差函数和雅克比矩阵。然后，我们创建了一个因子图，并添加了激光里程计因子和地平面约束因子。接着，我们定义了初始变量值，并使用LevenbergMarquardtOptimizer算法进行优化。最后，我们获取优化结果，包括位姿和地平面特征。