/*** 设位姿pose存在误差，利用观测到的平面进行位姿pose优化，只优化位姿pose的z轴平移和roll,pitch,yaw四个变量，要求优化后的pose，z轴与平面的z一致。请修改代码实现这个功能* * */
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/geometry/OrientedPlane3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Symbol.h>
#include <boost/optional.hpp>
#include <iostream>using namespace gtsam;// 自定义因子类
class CustomOrientedPlane3Factor : public NoiseModelFactor1<Pose3> {OrientedPlane3 measured_;public:CustomOrientedPlane3Factor(const OrientedPlane3& measured, const SharedNoiseModel& model, Key poseKey): NoiseModelFactor1<Pose3>(model, poseKey), measured_(measured) {}// 误差评估函数Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H = boost::none) const override {OrientedPlane3 transformedPlane = measured_.transform(pose, H); // 变换平面// 单独定义误差据向量double zError = pose.translation().z() - transformedPlane.planeCoefficients()(3);Vector3 normalError = transformedPlane.planeCoefficients().head<3>() - measured_.planeCoefficients().head<3>();Vector error(4); // 定义为4维向量error << normalError, zError; // 确保合并的正确顺序if (H) {*H = Matrix::Zero(4, 6); // 初始化 4x6 矩阵(*H)(3, 5) = 1.0; // 对z方向的偏导数// 这里需要自行计算剩余的angular部分根据误差函数的特定定义}return error; // 返回误差}
};int main() {NonlinearFactorGraph graph;// 初始位姿，只允许z的平移以及旋转freedomPose3 initialPose(Rot3::Ypr(0.1, 0.1, 0.1), Point3(10.5, 20.4, 1.2));Vector6 poseConstraint;poseConstraint << 0.0, 0.0, 1.0, 1.0, 1.0, 1.0; // 仅允许调整z, roll, pitch, yawauto poseNoise = noiseModel::Diagonal::Sigmas(poseConstraint * 0.1);graph.add(PriorFactor<Pose3>(Symbol('x', 0), initialPose, poseNoise));// 输出初始位姿std::cout << "Initial Pose:\n" << initialPose << std::endl;// 观测到的平面// OrientedPlane3 observedPlane1(0, 0, 1, -1.0); // 假设z是我们的目标// 添加多个观测平面std::vector<OrientedPlane3> observedPlanes = {OrientedPlane3(0, 0, 1, -1.5),  // 平面 1OrientedPlane3(0, 0, 1, -2.0),  // 平面 2OrientedPlane3(0, 0, 1, -1.0),  // 平面 3OrientedPlane3(0, 0, 1, -0.5)   // 平面 4// 其他平面可根据需求添加};auto planeNoise = noiseModel::Isotropic::Sigma(4, 1e-3);// 添加因子// graph.add(boost::make_shared<CustomOrientedPlane3Factor>(observedPlane1, planeNoise, Symbol('x', 0)));// 为所有平面添加因子for (size_t i = 0; i < observedPlanes.size(); ++i) {graph.add(boost::make_shared<CustomOrientedPlane3Factor>(observedPlanes[i], planeNoise, Symbol('x', 0)));}Values initialEstimate;initialEstimate.insert(Symbol('x', 0), initialPose);GaussNewtonParams params;params.setMaxIterations(10);params.setRelativeErrorTol(1e-5);GaussNewtonOptimizer optimizer(graph, initialEstimate, params);Values result = optimizer.optimize();// 输出优化结果std::cout << "Optimized Pose:\n" << result.at<Pose3>(Symbol('x', 0)) << std::endl;return 0;
}