利用gtsam的C++库在激光里程计中添加地平面约束完成因子图优化，请给出公式和C++代码

在激光里程计中添加地平面约束的因子图模型包括以下因子：

1. 里程计测量因子（odometry factor）：用来衡量两个相邻时刻机器人在里程计上的运动变化，假设机器人在时刻 $k$ 和时刻 $k+1$ 之间的运动变化是 $\delta x_{k,k+1}$，则里程计测量因子的公式为：

$$
x_{k+1} = f(x_k, \delta x_{k,k+1}) + \epsilon
$$

其中，$f$ 是运动学模型，$\epsilon$ 是噪声。

2. 地平面约束因子（plane factor）：用来衡量机器人当前位姿下的地面与激光点云之间的误差，假设机器人当前位姿是 $x_k$，地面的方程为 $ax+by+cz+d=0$，激光点云中的点云坐标为 $(p_{x,i}, p_{y,i}, p_{z,i})$，则地平面约束因子的公式为：

$$
p_{z,i} - \frac{a}{c}p_{x,i} - \frac{b}{c}p_{y,i} - \frac{d}{c} = \epsilon
$$

其中，$\epsilon$ 是噪声。

下面是利用 gtsam 库实现激光里程计中添加地平面约束的因子图优化的 C++ 代码：

#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/plane.h>

using namespace gtsam;

int main(int argc, char** argv) {
    // 加载数据集
    NonlinearFactorGraph graph;
    Values initialEstimate;
    std::tie(graph, initialEstimate) = load2D(argv[1]);

    // 添加先验因子
    noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));
    graph.add(PriorFactor<Pose3>(Symbol('x', 0), Pose3(), priorNoise));

    // 添加里程计测量因子
    noiseModel::Diagonal::shared_ptr odomNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
    for (int i = 1; i < initialEstimate.size(); i++) {
        Pose3 delta = initialEstimate.at<Pose3>(Symbol('x', i-1)).between(initialEstimate.at<Pose3>(Symbol('x', i)));
        graph.add(BetweenFactor<Pose3>(Symbol('x', i-1), Symbol('x', i), delta, odomNoise));
    }

    // 添加地平面约束因子
    noiseModel::Diagonal::shared_ptr planeNoise = noiseModel::Diagonal::Sigmas(Vector1(0.1));
    double a = 0.0, b = 0.0, c = 1.0, d = 0.0; // 地面方程：z = 0
    Point3 planePoint(a/c, b/c, d/c);
    Plane3 plane(a, b, c, d);
    for (int i = 0; i < initialEstimate.size(); i++) {
        auto pointCloud = boost::dynamic_pointer_cast<Point3Vector>(initialEstimate.at(Symbol('l', i)));
        for (int j = 0; j < pointCloud->size(); j++) {
            Point3 point = pointCloud->at(j);
            graph.add(PlaneFactor(Symbol('x', i), point, plane, planeNoise));
        }
    }

    // 执行因子图优化
    GaussNewtonOptimizer optimizer(graph, initialEstimate);
    Values result = optimizer.optimize();

    return 0;
}

需要注意的是，在上述代码中，我们假设机器人起始时刻的位姿为原点，即 $x_0=0$。如果机器人起始时刻的位姿不为原点，则需要添加相应的先验因子。另外，我们假设地面的法向量为 $(a, b, c)$，则为了避免除数为零，我们需要将地面方程写成 $ax+by+cz+d=0$ 的形式。