GTSAM | gtsam::PriorFactor

基于gtsam的平面约束可以通过使用gtsam中的Plane2类来实现。Plane2类表示一个二维平面，可以使用该类的构造函数初始化平面的法向量和偏移量，例如： ```c++ gtsam::Plane2 plane(gtsam::Point2(1.0, 0.0), 0.0); ``` 其中，Point2类表示一个二维点，可以使用该类的构造函数初始化点的坐标。 接下来，可以使用gtsam中的PriorFactor类来定义平面的先验约束，例如： ```c++ gtsam::PriorFactor<gtsam::Plane2> prior_factor(1, plane, gtsam::noiseModel::Isotropic::Sigma(2, 0.1)); ``` 其中，1表示平面的标识符，noiseModel::Isotropic::Sigma(2, 0.1)表示噪声模型，该模型假设平面的法向量和偏移量都是独立同分布的高斯分布，标准差分别为0.1。 最后，可以使用gtsam中的NonlinearFactorGraph类将先验约束添加到因子图中，例如： ```c++ gtsam::NonlinearFactorGraph graph; graph.add(prior_factor); ``` 接下来，可以使用gtsam中的GaussNewtonOptimizer类或LevenbergMarquardtOptimizer类对因子图进行优化，以得到平面的最优估计值。