/**
 * 设位姿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的平移以及旋转freedom
    Pose3 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, yaw
    auto 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),  // 平面 1
        OrientedPlane3(0, 0, 1, -2.0),  // 平面 2
        OrientedPlane3(0, 0, 1, -1.0),  // 平面 3
        OrientedPlane3(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;
}