结合C++代码说明hdl_graph_slam如何在因子图中添加的地平面约束

在hdl_graph_slam的C++代码中，地平面约束是通过添加一个平面因子到因子图中来实现的。具体来说，可以在`hdl_graph_slam::OdometryMapping::optimize()`函数中找到添加地平面约束的代码。

首先，在`hdl_graph_slam::OdometryMapping::optimize()`函数中，会检查是否启用了地面移除功能和地平面约束：

if (enable_ground_removal_) {
    ...
    if (enable_ground_constraint_) {
        add_ground_factor();
    }
}

在这段代码中，`enable_ground_removal_`和`enable_ground_constraint_`分别表示是否启用了地面移除功能和地平面约束。如果启用了地面移除功能，将执行相应的地面提取算法，并在提取出地面后执行以下代码以添加地平面约束：

void hdl_graph_slam::OdometryMapping::add_ground_factor() {
    for (auto &cell : ground_cells_) {
        gtsam::Point3 plane_normal(cell.plane_coeffs.x(), cell.plane_coeffs.y(), cell.plane_coeffs.z());
        gtsam::Point3 plane_point = plane_normal * cell.plane_coeffs.w();
        gtsam::Unit3 plane_orientation = gtsam::Unit3::FromVector(plane_normal);

        gtsam::SharedNoiseModel model = gtsam::noiseModel::Isotropic::Sigma(3, ground_sigma_);

        for (const auto &scan : cell.scans) {
            const auto &pt = cloud_->points[scan.i];

            gtsam::Point3 p(pt.x, pt.y, pt.z);

            gtsam::PinholeCamera<gtsam::Cal3_S2> camera = camera_model_;

            gtsam::Pose3 sensor_pose = sensor_poses_[scan.ring];

            gtsam::Pose3 odom_pose = odom_poses_[scan.ring];
            gtsam::Pose3 body_pose = body_poses_[scan.ring];

            gtsam::Pose3 point_pose = body_pose * sensor_pose.inverse() * odom_pose.inverse();

            gtsam::Point3 point_w = point_pose.transform_to(p);

            gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3> factor(point_w, model, camera, plane_point, plane_orientation);

            optimizer_->add(factor);
        }
    }
}

在这段代码中，首先遍历所有的地面cell，然后根据该cell的平面参数创建一个平面因子。接下来，将该cell中的所有点与该平面因子进行匹配，并将平面因子添加到因子图中。

总的来说，hdl_graph_slam在C++代码中通过执行地面提取算法并添加平面因子的方式来实现地平面约束。通过在`hdl_graph_slam::OdometryMapping::optimize()`函数中设置相应的参数，可以启用地面移除功能并添加地平面约束。