#include "Optimizer.h"
#include <glog/logging.h>
bool Optimizer::OptimizerB(const std::vector<Pose> &slams, const std::vector<Pose> &gts, g2o::SE3Quat &B)
{
    std::vector<Pose> slam_diff;
    std::vector<Pose> gt_diff;
    double time_interval = 1.0;
    int i = 0;
    int j = i + 1;

    while (j < slams.size())
    {
        if (slams[j].ts - slams[i].ts >= time_interval)
        {
            Pose p;
            p.ts = slams[j].ts;
            p.Twb = slams[i].Twb.inverse() * slams[j].Twb;
            slam_diff.push_back(p);
            Pose q;
            q.ts = gts[j].ts;
            q.Twb = gts[i].Twb.inverse() * gts[j].Twb;
            gt_diff.push_back(q);
            i++;
        }
        j++;
    }
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 6>> BlockSolverType;
    typedef g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType> LinearSolverType;
    BlockSolverType::LinearSolverType *linear_solver = new LinearSolverType();
    BlockSolverType *block_solver = new BlockSolverType(linear_solver);
    g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(block_solver);
    g2o::SparseOptimizer optimizer; // 定义图模型
    optimizer.setAlgorithm(solver);
    optimizer.setVerbose(false);
    g2o::VertexSE3Expmap *v1 = new g2o::VertexSE3Expmap();
    v1->setId(0);
    v1->setFixed(false);
    optimizer.addVertex(v1);

    const Eigen::Matrix<double, 6, 6> matlambda = Eigen::Matrix<double, 6, 6>::Identity();

    for (int i = 0; i < gt_diff.size(); i++) // 添加边
    {
        Pose gt = gt_diff[i];
        Pose slam = slam_diff[i];
        MyunaryEdgeSE3_B *e = new MyunaryEdgeSE3_B(gt.Twb);
        e->setId(i);
        e->setVertex(0, v1);
        e->setMeasurement(slam.Twb);
        e->information() = matlambda;
        optimizer.addEdge(e);
    }

    std::vector<std::vector<int>> rot_axis{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
    for (auto tx : {0.0, -0.04, -0.02, 0.02, 0.04})
    {
        for (auto ty : {0.0, -0.04, -0.02, 0.02, 0.04})
        {
            for (auto tz : {0.0, -0.04, -0.02, 0.02, 0.04})
            {
                for (auto axis : rot_axis)
                {
                    for (auto angle : {0.0, -M_PI / 2, M_PI / 2, M_PI})
                    {
                        Eigen::Quaterniond q(cos(angle / 2), axis[0] * sin(angle / 2), axis[1] * sin(angle / 2), axis[2] * sin(angle / 2));
                        Eigen::Vector3d t(tx, ty, tz);
                        g2o::SE3Quat init_B(q, t);
                        v1->setEstimate(init_B);
                        LOG(INFO) << "start optimization B";
                        LOG(INFO) << "init_B: "
                                  << "quat: " << v1->estimate().rotation() << ", trans: " << v1->estimate().translation().transpose();
                    }
                }
            }
        }
    }
}