#include "pose_graph/pose_cost_function.h"
#include "slam/slam.h"
#include "filter/filter.h"

#include "pose_graph/occupied_space_cost_function.h"
#include "pose_graph/rotation_delta_cost_functor.h"
#include "pose_graph/translation_delta_cost_functor.h"
#include "pose_graph/pose_graph.h"

namespace hlzn_slam {

namespace slam {

    Slam::Slam(std::unique_ptr<Map> global_map) : 
                                is_initialize_(false),
                                global_map_(std::move(global_map)),
                                use_towermatch_(true),
                                probability_plus_(0.2), 
                                probability_up_(0.8),
                                search_range_(0.3),
                                search_angle_(0.1),
                                window_size_(20),
                                achieve_range_(0.5),
                                achieve_angle_(0.4),
                                first_search_range_(5.0),
                                first_search_angle_(M_PI),
                                v_(0.0), 
                                w_(0.0)
    {
        odom_ = std::unique_ptr<odom::odom>(new odom::odom());
        global_map_->useShadow(true);
    }

    Slam::~Slam()
    {
        while (matchThread_.joinable()) {
            usleep(1000 * 10);
        }
    }

    bool Slam::isInitialize()
    {
        return is_initialize_;
    }
    
    void Slam::init(const common::Rigid2f& init, bool is_fast, float first_search_range, float first_search_angle)
    {
        TIME_POSE_LOCK()
        if (!is_fast) {
            time_pose_.pose = init;
            is_initialize_ = false;
        }
        else {
            time_pose_.pose = init;
            time_pose_.stamp = std::chrono::system_clock::now();
            is_initialize_ = true;
            odom_pose_ = time_pose_;
        }
        first_search_range_ = first_search_range;
        first_search_angle_ = first_search_angle;
    }

    void Slam::setParam(float search_range, float search_angle, bool use_towermatch, float probability_plus, float probability_up,
                    int window_size, float achieve_range, float achieve_angle, float submap_size, float submap_resolution, bool use_odom)
    {
        probability_plus_ = probability_plus;
        use_towermatch_ = use_towermatch;
        use_odom_ = use_odom;
        probability_up_ = probability_up;
        search_range_ = search_range;
        search_angle_ = search_angle;
        window_size_ = window_size;
        achieve_range_ = achieve_range;
        achieve_angle_ = achieve_angle;
        submap_size_ = submap_size;
        submap_resolution_ = submap_resolution;
    }

    void Slam::addPointCloud(common::PointCloud point_cloud, common::laserScan scan, Eigen::Matrix<float, 4, 4> tf)   // 唯一入口
    {
        if (!matchThread_.joinable()) {
            matchThread_ = std::thread([this, point_cloud, scan, tf] {
                scan_ = scan;
                laser_tf_ = tf;
                __slam(point_cloud);
                matchThread_.detach();
            });
        }
    }

    void Slam::addOriginScan(common::laserScan& scan, Eigen::Matrix<float, 4, 4>& tf)
    {
        common::PointCloud point_cloud;
        float theta = scan.angle_max;
        float offset = scan.angle_max + scan.angle_min;

        for (const float& range : scan.ranges) {
            theta -= scan.angle_increment;

            if (range > scan.range_max) continue;
            if (range < scan.range_min) continue;
            if (theta < scan.angle_min) continue;
            if (theta > scan.angle_max) continue;
            if (std::isnan(range)) continue;

            const Eigen::Matrix<float, 4, 1> point(range * cos(theta - offset),
                                                    -range * sin(theta - offset),
                                                    0.0,
                                                    1.0);
            //将基于传感器坐标系的数据转到基于base_link坐标系
            Eigen::Matrix<float, 4, 1> base = tf * point;

            point_cloud.data.push_back(
                Eigen::Vector3f(base(0), base(1), base(2))
                );
        }
    }

    void Slam::addOdom2DVelocity(float& v, float& w)
    {
        TIME_POSE_LOCK()
        v_ = v;
        w_ = w;
    }

    void Slam::addOdom2D(float& x, float& y, float& theta)
    {
        TIME_POSE_LOCK()
        odom_->addOdometry(x, y, theta);
    }

    common::TimeRigid2f Slam::getCurrentPose()
    {
        TIME_POSE_LOCK()
        return time_pose_;
    }

    void Slam::__slam(common::PointCloud point_cloud)
    {
        common::Rigid2f global_pose = getCurrentPose().pose;
        common::Rigid2f track_pose = common::Rigid2f({0.0, 0.0}, 0.0);

        if (!is_initialize_) {
            RealTimeMatch2D matcher;
            common::PointCloud filter = filter::VoxelInterpolationFilter(point_cloud, 0.5);// global_map_->getResolution() * 10.0);

            matcher.setSearchRange(first_search_range_, first_search_angle_, 0.1, 0.17);// global_map_->getResolution() * 2.0, 0.017 * 10.0);
            matcher.match(filter, getCurrentPose().pose, global_pose, *global_map_);

            is_initialize_ = true;
        }
        if (!submaps_.empty()) { // 局部地图匹配
            static common::Time clock = std::chrono::system_clock::now();
            std::chrono::duration<float> dur = std::chrono::system_clock::now() - clock;
            common::Rigid2f perticet;
            static common::Rigid2f odom({0, 0}, 0);

            float l = v_ * dur.count();
            float w = w_ * dur.count();

            perticet = common::Rigid2f({l * cos(w), l * sin(w)}, w);
            // odom_->extrapolaOdom(clock, std::chrono::system_clock::now(), perticet);
            odom_pose_.pose = odom_pose_.pose * perticet;

            clock = std::chrono::system_clock::now();

            float range = dur.count() * 3.0; // 3m/s的速度运行
            float rad = dur.count() * 1.0;  // 3rad/s的速度旋转

            range = range > 0.3 ? 0.3 : range;
            rad = rad > 0.17 ? 0.17 : rad;

#define POSEGRAPH2 0

#if POSEGRAPH2
            for (std::shared_ptr<SubMap> submap : submaps_) {
                common::Rigid2f tmp = submap->track_pose * perticet;

                submap->track_pose = __Match(tmp, point_cloud, *submap->map, range, rad, 
                                        10.0, 2.0, 3.0, 1.0);
                LOG(INFO)<<""<<submap->track_pose.translation()(0);
                LOG(INFO)<<""<<submap->track_pose.translation()(1);
                LOG(INFO)<<""<<submap->track_pose.rotation().angle();
                LOG(INFO)<<"***********************";
            }
            LOG(INFO)<<"*************************************************";
            global_pose = submaps_.back()->global_pose * submaps_.back()->track_pose;
#else
            common::Rigid2f track_pose_perticet = submaps_.back()->track_pose * perticet;
            track_pose = __Match(track_pose_perticet, point_cloud, *submaps_.back()->map, range, rad);
            
            global_pose = submaps_.back()->global_pose * track_pose;
#endif
        }
        {   // 全局地图匹配
            global_pose = __Match(global_pose, point_cloud, *global_map_, search_range_, search_angle_, 
                1.0, 2.0, 3.0, 1.0);
        }
        bool compir_success = false;

        if (!submaps_.empty()) {
#if POSEGRAPH2
            __poseGraph2(global_pose);
#else
            common::Rigid2f error = odom_pose_.pose.inverse() * global_pose;

            if ((error.translation().norm() > search_range_ || fabs(error.rotation().angle()) > search_angle_) && use_odom_) {
                float compir_cost1 = global_map_->getCost(__transformPointCloud(point_cloud, global_pose));

                common::Rigid2f maybe = __Match(odom_pose_.pose, point_cloud, *global_map_, search_range_, search_angle_);

                float compir_cost2 = global_map_->getCost(__transformPointCloud(point_cloud, maybe));

                if (compir_cost2 > compir_cost1 * 1.2) {
                    global_pose = maybe;
                    odom_pose_.pose = maybe;
                    compir_success = true;
                }
            }
            // __poseGraph(global_pose, track_pose);
            // track_pose = submaps_.back()->track_pose;
            // __optimization(point_cloud, track_pose, global_pose);
#endif
        }
        else {
            odom_pose_.pose = global_pose;
        }
#if POSEGRAPH2
        if (!submaps_.empty()) {
            track_pose = submaps_.back()->track_pose;
        }
#endif
        if (submaps_.empty() || __frameParallaxAchieve(track_pose, achieve_range_, achieve_angle_) || compir_success) {
            __createNewSubmap(point_cloud, global_pose, track_pose);
            track_pose = common::Rigid2f({0.0, 0.0}, 0.0);

            __buildSubmap(track_pose);
            submaps_.back()->track_pose = track_pose;
        }
        else {
            submaps_.back()->track_pose = track_pose;// = submaps_.back()->global_pose.inverse() * global_pose;
            __buildSubmap(track_pose);
        }
#if 0
        global_map_->clearShadowMap();
        for (std::shared_ptr<SubMap>& submap : submaps_) {
            common::PointCloud global_point_cloud = __transformPointCloud(submap->point_cloud, submap->global_pose);
            common::PointCloud result = filter::VoxelInterpolationFilter(global_point_cloud, global_map_->getResolution());
            
            global_map_->pixelValuePlusShadow(result.data, probability_plus_, probability_up_);
        }
#else
        {
            __globalMapBeamMode(global_pose);
            __growGlobalmap(global_pose);
        }
#endif

#if 1
        {
            static int i = 0;
            
            i++;
            if (i > 10) {
                global_map_->out("/home/space/catkin_ws/100.png");
                submaps_.back()->map->out("/home/space/catkin_ws/101.png");
                i = 0;
            }
        }
#endif
        __writeTimePose({global_pose, std::chrono::system_clock::now(), 
                            global_map_->getCost(__transformPointCloud(point_cloud, global_pose))});
#if POSEGRAPH2
        if (submaps_.size() > 1) {
#else 
        if (submaps_.size() > window_size_) {
#endif
            submaps_.pop_front();
            return;
        }
        return;
    }

    void Slam::__buildSubmap(common::Rigid2f pose)
    {
        common::PointCloud point_cloud = __transformScanData2PointCloudError(0.0);
        common::PointCloud word = __transformPointCloud(point_cloud, pose);
        common::PointCloud result = filter::MapVoxelInterpolationFilter(word, *submaps_.back()->map, submaps_.back()->map->getResolution());
        
        submaps_.back()->map->pixelValuePlus2(result.data, 1.0, 1.0);

        point_cloud = __transformScanData2PointCloudError(submaps_.back()->map->getResolution());
        word = __transformPointCloud(point_cloud, pose);
        result = filter::MapVoxelInterpolationFilter(word, *submaps_.back()->map, submaps_.back()->map->getResolution());

        submaps_.back()->map->pixelValuePlus2(result.data, 0.5, 1.0);

        point_cloud = __transformScanData2PointCloudError(-submaps_.back()->map->getResolution());
        word = __transformPointCloud(point_cloud, pose);
        result = filter::MapVoxelInterpolationFilter(word, *submaps_.back()->map, submaps_.back()->map->getResolution());

        submaps_.back()->map->pixelValuePlus2(result.data, 0.5, 1.0);

        point_cloud = __transformScanData2PointCloudError(2.0 * submaps_.back()->map->getResolution());
        word = __transformPointCloud(point_cloud, pose);
        result = filter::MapVoxelInterpolationFilter(word, *submaps_.back()->map, submaps_.back()->map->getResolution());

        submaps_.back()->map->pixelValuePlus2(result.data, 0.25, 1.0);

        point_cloud = __transformScanData2PointCloudError(-2.0 * submaps_.back()->map->getResolution());
        word = __transformPointCloud(point_cloud, pose);
        result = filter::MapVoxelInterpolationFilter(word, *submaps_.back()->map, submaps_.back()->map->getResolution());

        submaps_.back()->map->pixelValuePlus2(result.data, 0.25, 1.0);

        point_cloud = __transformScanData2PointCloudError(3.0 * submaps_.back()->map->getResolution());
        word = __transformPointCloud(point_cloud, pose);
        result = filter::MapVoxelInterpolationFilter(word, *submaps_.back()->map, submaps_.back()->map->getResolution());

        submaps_.back()->map->pixelValuePlus2(result.data, 0.12, 1.0);

        point_cloud = __transformScanData2PointCloudError(-3.0 * submaps_.back()->map->getResolution());
        word = __transformPointCloud(point_cloud, pose);
        result = filter::MapVoxelInterpolationFilter(word, *submaps_.back()->map, submaps_.back()->map->getResolution());

        submaps_.back()->map->pixelValuePlus2(result.data, 0.12, 1.0);

        submaps_.back()->track_pose = pose;
    }

    void Slam::__growGlobalmap(common::Rigid2f pose)
    {
        common::PointCloud point_cloud = __transformScanData2PointCloudError(0.0);
        common::PointCloud global_point_cloud = __transformPointCloud(point_cloud, pose);
        common::PointCloud result = filter::MapVoxelInterpolationFilter(global_point_cloud, *global_map_, global_map_->getResolution());

        global_map_->pixelValuePlus3(global_point_cloud.data, probability_plus_, probability_up_);

        point_cloud = __transformScanData2PointCloudError(global_map_->getResolution());
        global_point_cloud = __transformPointCloud(point_cloud, pose);
        result = filter::MapVoxelInterpolationFilter(global_point_cloud, *global_map_, global_map_->getResolution());

        global_map_->pixelValuePlus3(global_point_cloud.data, probability_plus_ * 0.5, probability_up_* 0.5);
    
        point_cloud = __transformScanData2PointCloudError(-global_map_->getResolution());
        global_point_cloud = __transformPointCloud(point_cloud, pose);
        result = filter::MapVoxelInterpolationFilter(global_point_cloud, *global_map_, global_map_->getResolution());

        global_map_->pixelValuePlus3(global_point_cloud.data, probability_plus_* 0.5, probability_up_* 0.5);

        point_cloud = __transformScanData2PointCloudError(2.0 * global_map_->getResolution());
        global_point_cloud = __transformPointCloud(point_cloud, pose);
        result = filter::MapVoxelInterpolationFilter(point_cloud, *global_map_, global_map_->getResolution());

        global_map_->pixelValuePlus3(global_point_cloud.data, probability_plus_ * 0.25, probability_up_* 0.25);
    
        point_cloud = __transformScanData2PointCloudError(-2.0 * global_map_->getResolution());
        global_point_cloud = __transformPointCloud(point_cloud, pose);
        result = filter::MapVoxelInterpolationFilter(global_point_cloud, *global_map_, global_map_->getResolution());

        global_map_->pixelValuePlus3(global_point_cloud.data, probability_plus_* 0.25, probability_up_* 0.25);
    }

    void Slam::__optimization(common::PointCloud& point_cloud, common::Rigid2f& track_pose, common::Rigid2f& global_pose)
    {
        if (submaps_.empty()) {
            return;
        }
        static common::Time clock = std::chrono::system_clock::now();
        common::Rigid2f perticet = submaps_.back()->track_pose;
        float a = 0.05;
        float b = 0.1;
        track_pose = __Match(perticet, point_cloud, *submaps_.back()->map, a, b);
        // if (!global_map_->empty()) {
            global_pose = __Match(global_pose, point_cloud, *global_map_, search_range_, search_angle_);
        // }
        clock = std::chrono::system_clock::now();
        // pose_graph
        double vertex_translation[submaps_.size() + 1][3];
        double vertex_rotation[submaps_.size() + 1][4];
        double edge_translation[submaps_.size() + 1][3];
        double edge_rotation[submaps_.size() + 1][4];

        for (int i = 0;i < submaps_.size() + 1;i++) {
            if (i < submaps_.size()) {
                __Rigid2fTranslation2Double3(submaps_[i]->global_pose, vertex_translation[i]);
                __Rigid2fQuaterniond2Double4(submaps_[i]->global_pose, vertex_rotation[i]);
                if (submaps_[i]->next != nullptr) {
                    __Rigid2fTranslation2Double3(submaps_[i]->next->transform, edge_translation[i]);
                    __Rigid2fQuaterniond2Double4(submaps_[i]->next->transform, edge_rotation[i]);
                }
                else {
                    __Rigid2fTranslation2Double3(track_pose, edge_translation[i]);
                    __Rigid2fQuaterniond2Double4(track_pose, edge_rotation[i]);
                }
            }
            else {
                __Rigid2fTranslation2Double3(global_pose, vertex_translation[i]);
                __Rigid2fQuaterniond2Double4(global_pose, vertex_rotation[i]);
            }
        }
        ceres::Solver::Summary summary;
        ceres::Problem problem;
        ceres::LossFunction *loss_function;

        loss_function = new ceres::CauchyLoss(1.0);
        pose_graph::PoseGraphFunction *cost_f = new pose_graph::PoseGraphFunction();

        // LOG(INFO)<<"submap size: "<<submaps_.size();
        // 添加后端约束
        for (int i = 0;i < submaps_.size();i++) {
            ceres::LocalParameterization *vertex_translation_i_parameterization = new pose_graph::TranslationLocalParameterization();
            problem.AddParameterBlock(vertex_translation[i], 3, vertex_translation_i_parameterization);

            ceres::LocalParameterization *vertex_rotation_i_parameterization = new pose_graph::QuaternionLocalParameterization();
            problem.AddParameterBlock(vertex_rotation[i], 4, vertex_rotation_i_parameterization);

            if (i == submaps_.size() - 1) {
                ceres::LocalParameterization *vertex_translation_ip_parameterization = new pose_graph::TranslationLocalParameterization();
                problem.AddParameterBlock(vertex_translation[submaps_.size()], 3, vertex_translation_ip_parameterization);

                ceres::LocalParameterization *vertex_rotation_ip_parameterization = new pose_graph::QuaternionLocalParameterization();
                problem.AddParameterBlock(vertex_rotation[submaps_.size()], 4, vertex_rotation_ip_parameterization);
            }
            ceres::LocalParameterization *edge_translation_i_parameterization = new pose_graph::TranslationLocalParameterization();
            problem.AddParameterBlock(edge_translation[i], 3, edge_translation_i_parameterization);

            ceres::LocalParameterization *edge_rotation_i_parameterization = new pose_graph::QuaternionLocalParameterization();
            problem.AddParameterBlock(edge_rotation[i], 4, edge_rotation_i_parameterization);

            if (i == 0) {
                problem.SetParameterBlockConstant(vertex_translation[0]);
                problem.SetParameterBlockConstant(vertex_rotation[0]);
            }

            // problem.AddResidualBlock(cost_f, loss_function, vertex_translation[i], vertex_rotation[i], 
            //                             vertex_translation[i + 1], vertex_rotation[i + 1], edge_translation[i], edge_rotation[i]);
        }
        // 添加子地图约束
        {
            // 在之前的track pose的基础上使用惯性导航做预测
            // 用里程计做初步推算
            Eigen::Vector2d traget_translation = Eigen::Vector2d(track_pose.translation()(0), track_pose.translation()(1));
            double p4[4];

            __Rigid2fQuaterniond2Double4(track_pose, p4);

            problem.AddResidualBlock(
                CreateOccupiedSpaceCostFunction(
                    1.0 / std::sqrt(static_cast<double>(point_cloud.data.size())),
                    point_cloud, *submaps_.back()->map),
            nullptr /* loss function */, edge_translation[submaps_.size() - 1], edge_rotation[submaps_.size() - 1]);

            problem.AddResidualBlock(
                TranslationDeltaCostFunctor::CreateAutoDiffCostFunction(
                        2.0, traget_translation), //10.0
                nullptr /* loss function */, edge_translation[submaps_.size() - 1]);

            ceres_matching::RotationDeltaCostFunctor* self_rotation_costfun = 
                new ceres_matching::RotationDeltaCostFunctor(3.0, edge_rotation[submaps_.size() - 1]);

            problem.AddResidualBlock(self_rotation_costfun, // 40.0
                nullptr /* loss function */, edge_rotation[submaps_.size() - 1]);   // 稳定残差

            ceres_matching::RotationDeltaCostFunctor* traget_rotation_costfun = 
                new ceres_matching::RotationDeltaCostFunctor(3.0, p4);

            problem.AddResidualBlock(traget_rotation_costfun, // 6.0
                nullptr /* loss function */, edge_rotation[submaps_.size() - 1]);
        }

        // 添加全局地图约束
        // if (!global_map_->empty()) {
            // 在之前的track pose的基础上使用惯性导航做预测
            // 用里程计做初步推算
            Eigen::Vector2d traget_translation = Eigen::Vector2d(global_pose.translation()(0), global_pose.translation()(1));
            double p4[4];

            __Rigid2fQuaterniond2Double4(global_pose, p4);

            problem.AddResidualBlock(
                CreateOccupiedSpaceCostFunction(
                    1.0 / std::sqrt(static_cast<double>(point_cloud.data.size())),
                    point_cloud, *global_map_),
            nullptr /* loss function */, vertex_translation[submaps_.size()], vertex_rotation[submaps_.size()]);

            problem.AddResidualBlock(
                TranslationDeltaCostFunctor::CreateAutoDiffCostFunction(
                        2.0, traget_translation), //10.0
                nullptr /* loss function */, vertex_translation[submaps_.size()]);

            ceres_matching::RotationDeltaCostFunctor* self_rotation_costfun = 
                new ceres_matching::RotationDeltaCostFunctor(3.0, vertex_rotation[submaps_.size()]);

            problem.AddResidualBlock(self_rotation_costfun, // 40.0
                nullptr /* loss function */, vertex_rotation[submaps_.size()]);   // 稳定残差

            ceres_matching::RotationDeltaCostFunctor* traget_rotation_costfun = 
                new ceres_matching::RotationDeltaCostFunctor(1.0, p4);

            problem.AddResidualBlock(traget_rotation_costfun, // 6.0
                nullptr /* loss function */, vertex_rotation[submaps_.size()]);
        // }

        // 添加里程计约束
        // {
        //     common::Rigid2f odom;
        //     double odom_translation[3] = {0};
        //     double odom_rotation[4] = {0};
        //     static common::Time clock = std::chrono::system_clock::now();

        //     odom_->extrapolaOdom(clock, std::chrono::system_clock::now(), odom);
        //     __Rigid2fTranslation2Double3(odom, odom_translation);
        //     __Rigid2fQuaterniond2Double4(odom, odom_rotation);

        //     ceres::LocalParameterization *edges_o_translation_parameterization = new pose_graph::TranslationLocalParameterization();                 
        //     problem.AddParameterBlock(odom_translation, 3, edges_o_translation_parameterization);
        //     problem.SetParameterBlockConstant(odom_translation);

        //     ceres::LocalParameterization *edges_o_rotation_parameterization = new pose_graph::QuaternionLocalParameterization(); 
        //     problem.AddParameterBlock(odom_rotation, 4, edges_o_rotation_parameterization);
        //     problem.SetParameterBlockConstant(odom_rotation);

        //     problem.AddResidualBlock(cost_f, loss_function, vertex_translation[submaps_.size() - 1], 
        //                                 vertex_rotation[submaps_.size() - 1], vertex_translation[submaps_.size()], vertex_rotation[submaps_.size()], 
        //                                 odom_translation, odom_rotation);
        // }

        ceres::Solver::Options options;
        
        options.linear_solver_type = ceres::DENSE_SCHUR;
        options.trust_region_strategy_type = ceres::DOGLEG;
        options.max_num_iterations = 50;

        ceres::Solve(options, &problem, &summary);

        for (int i = 0;i < submaps_.size();i++) {
            __double34ToRigid2f(vertex_translation[i], vertex_rotation[i], submaps_[i]->global_pose);
            if (submaps_[i]->next != nullptr) {
                __double34ToRigid2f(edge_translation[i], edge_rotation[i], submaps_[i]->next->transform);
            }
        }
        __double34ToRigid2f(edge_translation[submaps_.size() - 1], edge_rotation[submaps_.size() - 1], track_pose);
        // __double34ToRigid2f(vertex_translation[submaps_.size()], vertex_rotation[submaps_.size()], global_pose);
    }

    // 更新submap的global pose和edge,计算当前最佳的global pose
    void Slam::__poseGraph(common::Rigid2f& global_pose, common::Rigid2f& track_pose)
    {
        double vertexs[submaps_.size() + 1][7];
        double edges[submaps_.size() + 1][7];

        for (int i = 0;i < submaps_.size() + 1;i++) {
            if (i < submaps_.size()) {
                __Rigid2f2Double7(submaps_[i]->global_pose, vertexs[i]);
                if (submaps_[i]->next != nullptr) {
                    __Rigid2f2Double7(submaps_[i]->next->transform, edges[i]);
                }
                else {
                    __Rigid2f2Double7(track_pose, edges[i]);
                }
            } // i == submap size 把当前帧的全局匹配位姿作为最后一个顶点
            else {
                __Rigid2f2Double7(global_pose, vertexs[i]);
            }

        }
        ceres::Solver::Summary summary;
        ceres::Problem problem;
        ceres::LossFunction *loss_function;

        loss_function = new ceres::CauchyLoss(1.0);

        // LOG(INFO)<<"submap size: "<<submaps_.size();
        for (int i = 0;i < submaps_.size();i++) {
            pose_graph::PoseCostFunction *cost_f = new pose_graph::PoseCostFunction();

            ceres::LocalParameterization *vertexs_i_parameterization = new pose_graph::PoseLocalParameterization();
            problem.AddParameterBlock(vertexs[i], 7, vertexs_i_parameterization);

            if (i == submaps_.size() - 1) {
                ceres::LocalParameterization *vertexs_ip_parameterization = new pose_graph::PoseLocalParameterization();
                problem.AddParameterBlock(vertexs[i + 1], 7, vertexs_ip_parameterization);
                problem.SetParameterBlockConstant(vertexs[i + 1]);
            }
            // else {
            //      problem.SetParameterBlockConstant(vertexs[i]);
            // }

            ceres::LocalParameterization *edges_i_parameterization = new pose_graph::PoseLocalParameterization();           
            problem.AddParameterBlock(edges[i], 7, edges_i_parameterization);
            
            // problem.SetParameterBlockConstant(edges[i]);
            
            if (i == 0) { // 将第0帧固定，不进行优化
                problem.SetParameterBlockConstant(vertexs[0]);
                problem.SetParameterBlockConstant(edges[0]);
            }

            problem.AddResidualBlock(cost_f, loss_function, vertexs[i], vertexs[i + 1], edges[i]);
        }
        ceres::Solver::Options options;
        
        options.linear_solver_type = ceres::DENSE_SCHUR;
        options.trust_region_strategy_type = ceres::DOGLEG;
        options.max_num_iterations = 50;

        ceres::Solve(options, &problem, &summary);

        for (int i = 0;i < submaps_.size();i++) {
            __double7ToRigid2f(vertexs[i], submaps_[i]->global_pose);
            if (submaps_[i]->next != nullptr) {
                __double7ToRigid2f(edges[i], submaps_[i]->next->transform);
            }
        }
        __double7ToRigid2f(vertexs[submaps_.size()], global_pose);
        __double7ToRigid2f(edges[submaps_.size() - 1], track_pose);
    }

    // 更新submap的global pose和edge,计算当前最佳的global pose
    void Slam::__poseGraph2(common::Rigid2f& global_pose)
    {
        if (submaps_.empty()) {
            return;
        }
        double vertexs[submaps_.size() + 1][7];
        double edges[submaps_.size() + 1][7];
        double hyp_edges[submaps_.size() + 1][7];

        ceres::Solver::Summary summary;
        ceres::Problem problem;
        ceres::LossFunction *loss_function;

        loss_function = new ceres::CauchyLoss(1.0);
        
        __Rigid2f2Double7(global_pose, vertexs[submaps_.size()]);

        ceres::LocalParameterization *local_parameterization = new pose_graph::PoseLocalParameterization();
        problem.AddParameterBlock(vertexs[submaps_.size()], 7, local_parameterization);
        for (int i = 0;i < submaps_.size();i++) {
            pose_graph::PoseCostFunction *cost_f = new pose_graph::PoseCostFunction();

            __Rigid2f2Double7(submaps_[i]->global_pose, vertexs[i]);
            __Rigid2f2Double7(submaps_[i]->track_pose, edges[i]);
            
            ceres::LocalParameterization *global_pose_local_parameterization = new pose_graph::PoseLocalParameterization();
            problem.AddParameterBlock(vertexs[i], 7, global_pose_local_parameterization);
            
            ceres::LocalParameterization *track_pose_local_parameterization = new pose_graph::PoseLocalParameterization();
            problem.AddParameterBlock(edges[i], 7, track_pose_local_parameterization);

            if (i < submaps_.size() - 1) {
                __Rigid2f2Double7(submaps_[i]->next->transform, hyp_edges[i]);

                ceres::LocalParameterization *hyp_edges_local_parameterization = new pose_graph::PoseLocalParameterization();
                problem.AddParameterBlock(hyp_edges[i], 7, hyp_edges_local_parameterization);

                problem.AddResidualBlock(cost_f, loss_function, vertexs[i], vertexs[i + 1], hyp_edges[i]);
                if (i == 0) { 
                    problem.SetParameterBlockConstant(hyp_edges[0]);
                }
            }
            if (i == 0) { // 将第0帧固定，不进行优化
                problem.SetParameterBlockConstant(vertexs[0]);
                problem.SetParameterBlockConstant(edges[0]);
            }

            problem.AddResidualBlock(cost_f, loss_function, vertexs[i], vertexs[submaps_.size()], edges[i]);
        }
        ceres::Solver::Options options;
        
        options.linear_solver_type = ceres::DENSE_SCHUR;
        options.trust_region_strategy_type = ceres::DOGLEG;
        options.max_num_iterations = 50;

        ceres::Solve(options, &problem, &summary);

        for (int i = 0;i < submaps_.size();i++) {
            __double7ToRigid2f(vertexs[i], submaps_[i]->global_pose);
            __double7ToRigid2f(edges[i], submaps_[i]->track_pose);

            if (submaps_[i]->next != nullptr) {
                __double7ToRigid2f(hyp_edges[i], submaps_[i]->next->transform);
            }
        }
        __double7ToRigid2f(vertexs[submaps_.size()], global_pose);
    }

    common::Rigid2f Slam::__Match(common::Rigid2f& pose, 
                                  common::PointCloud& point_cloud, 
                                  Map& map, 
                                  const float& range, 
                                  const float& rad, 
                                  const float occupied_space_weight, 
                                  const float translation_weight,
                                  const float rotation_weight,
                                  const float self_stab_weight)
    {
        RealTimeMatch2D matcher;
        common::Rigid2f real_time_match_pose, predict_pose, ceres_match_pose, odom;
        ceres::Solver::Summary summary;
        float cost = 0;
        common::PointCloud filter = filter::VoxelInterpolationFilter(point_cloud, 0.1);
        // common::PointCloud filter = __transformScanData2PointCloudError(0.0, 0.0085);

        predict_pose = pose;
        matcher.setSearchRange(range, rad, map.getResolution());

        if (use_towermatch_) {
            cost = matcher.towerMatch(filter, predict_pose, real_time_match_pose, map, 2.0);
        }
        else {
            cost = matcher.match(filter, predict_pose, real_time_match_pose, map);
        }
        if (cost < 1e-2) {
            real_time_match_pose = predict_pose;
        }
        CeresScanMatcher2D ceres_scan_matcher(occupied_space_weight, translation_weight, rotation_weight, self_stab_weight);
        ceres_scan_matcher.Match(real_time_match_pose, real_time_match_pose, point_cloud, map, &ceres_match_pose, &summary);

        common::PointCloud opt_cloud = __transformPointCloud(point_cloud, ceres_match_pose);

        std::vector<common::PointCloud> group;
        common::PointCloud element;

        //　分组
        for (int i = 0;i < opt_cloud.data.size();i++) {
            if (!element.data.empty()) {
                float dist = hypot(element.data.back()(0) - opt_cloud.data[i](0), 
                                    element.data.back()(1) - opt_cloud.data[i](1));
                
                if (dist > 0.2 || i == opt_cloud.data.size() - 1) {
                    group.push_back(element);
                    element.data.clear();
                }
            }
            element.data.push_back(opt_cloud.data[i]);
        }
        element.data.clear();
        for (common::PointCloud& cloud : group) {
            float cost = map.getCost(cloud);

            if (cloud.data.size() > 2 && cost > 0.1) {
                element.data.insert(element.data.end(), cloud.data.begin(), cloud.data.end());
            }
        }
        element = __transformPointCloud(element, ceres_match_pose.inverse());
        common::Rigid2f pr;

        CeresScanMatcher2D ceres_scan_matcher2(occupied_space_weight, translation_weight, rotation_weight, self_stab_weight);
        ceres_scan_matcher2.Match(ceres_match_pose, ceres_match_pose, element, map, &pr, &summary);

        return pr;
        // return ceres_match_pose;
    }

    common::PointCloud Slam::__transformPointCloud(
        const common::PointCloud& point_cloud, const common::Rigid2f& transform)
    {
        common::PointCloud trans_cloud;

        trans_cloud.data.resize(point_cloud.data.size());

        for (int i = 0;i < point_cloud.data.size();i++) {
            Eigen::Vector3f trans;
            common::Rigid2f::Vector vector = point_cloud.data[i].head<2>();
            
            vector = transform * vector;
            trans = Eigen::Vector3f(vector(0), vector(1), 0.0);
            trans_cloud.data[i] = trans;
        }
        trans_cloud.time = point_cloud.time;

        return trans_cloud;
    }

    void Slam::__writeTimePose(common::TimeRigid2f time_pose)
    {
        TIME_POSE_LOCK()
        time_pose_ = time_pose;
    }

    void Slam::__createNewSubmap(common::PointCloud& point_cloud, common::Rigid2f& global_pose, common::Rigid2f& track_pose)
    {
        std::shared_ptr<SubMap> ptr = std::shared_ptr<SubMap>(new SubMap(submap_size_, submap_resolution_));
        std::shared_ptr<con> con_ptr = std::shared_ptr<con>(new con());
        
        ptr->global_pose = global_pose;
        if (!submaps_.empty()) {
            con_ptr->submap = ptr;
            con_ptr->transform = track_pose;
            submaps_.back()->next = con_ptr; //　把上一个submap的next指向最新的submap
        }
        submaps_.push_back(ptr);
        submaps_.back()->point_cloud = point_cloud;
        // __buildSubmap(common::Rigid2f({0.0, 0.0}, 0.0));
        // submaps_.back()->track_pose = common::Rigid2f({0.0, 0.0}, 0.0);
    }

    void Slam::__Rigid2f2Double7(common::Rigid2f& rigid2f, double* d7)
    {
        d7[0] = rigid2f.translation()(0);
        d7[1] = rigid2f.translation()(1);
        d7[2] = 0.0;
        Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX()));
        Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY()));
        Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(rigid2f.rotation().angle(), Eigen::Vector3d::UnitZ()));

        Eigen::Quaterniond quaternion;

        quaternion = yawAngle * pitchAngle * rollAngle;
        d7[3] = quaternion.x();
        d7[4] = quaternion.y();
        d7[5] = quaternion.z();
        d7[6] = quaternion.w();
    }

    void Slam::__double7ToRigid2f(double* d7, common::Rigid2f& rigid2f)
    {
        Eigen::Quaterniond q(d7[6], d7[3], d7[4], d7[5]);

        q.normalized();
        rigid2f = common::Rigid2f({d7[0], d7[1]}, q.matrix().eulerAngles(0, 1, 2)(2));
    }

    void Slam::__Rigid2fTranslation2Double3(common::Rigid2f& rigid2f, double* d3)
    {
        d3[0] = rigid2f.translation()(0);
        d3[1] = rigid2f.translation()(1);
        d3[2] = rigid2f.translation()(2);
    }

    void Slam::__Rigid2fQuaterniond2Double4(common::Rigid2f& rigid2f, double* d4)
    {
        Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(rigid2f.rotation().angle(), Eigen::Vector3d::UnitZ()));

        Eigen::Quaterniond quaternion;

        quaternion = yawAngle;// * pitchAngle * rollAngle;
        d4[0] = quaternion.x();
        d4[1] = quaternion.y();
        d4[2] = quaternion.z();
        d4[3] = quaternion.w();
    }

    void Slam::__double34ToRigid2f(double* d3, double* d4, common::Rigid2f& rigid2f)
    {
        Eigen::Quaterniond q(d4[3], d4[0], d4[1], d4[2]);

        rigid2f = common::Rigid2f({d3[0], d3[1]}, q.matrix().eulerAngles(0, 1, 2)(2));
    }

    bool Slam::__frameParallaxAchieve(common::Rigid2f& pose, float achieve_range, float achieve_angle)
    {
        if (pose.translation().norm() > achieve_range || fabs(pose.rotation().angle()) > achieve_angle) {
            return true;
        }
        return false;
    }

    common::PointCloud Slam::__transformScanData2PointCloudError(float error, const float angle_increment)
    {
        common::PointCloud point_cloud;
        float theta = scan_.angle_max;
        float offset = scan_.angle_max + scan_.angle_min;
        float sum_angle = 0;

        for (float range : scan_.ranges) {
            theta -= scan_.angle_increment;
            sum_angle += scan_.angle_increment;

            if (sum_angle < angle_increment) continue;
            if (range > scan_.range_max) continue;
            if (range < scan_.range_min) continue;
            if (theta < scan_.angle_min) continue;
            if (theta > scan_.angle_max) continue;
            if (std::isnan(range)) continue;
            
            sum_angle = 0.;
            range += error;

            const Eigen::Matrix<float, 4, 1> point(range * cos(theta - offset),
                                                    -range * sin(theta - offset),
                                                    0.0,
                                                    1.0);
            //将基于传感器坐标系的数据转到基于base_link坐标系
            Eigen::Matrix<float, 4, 1> base = laser_tf_ * point;

            point_cloud.data.push_back(
                Eigen::Vector3f(base(0), base(1), base(2))
                );
        }
        point_cloud.time = std::chrono::system_clock::now();
        return point_cloud;
    }

    void Slam::__globalMapBeamMode(common::Rigid2f& global_tf) 
    {
        common::PointCloud point_cloud;
        float theta = scan_.angle_max;
        float offset = scan_.angle_max + scan_.angle_min;
        float r2 = global_map_->getResolution() * global_map_->getResolution() * 2.0;

        r2 = sqrt(r2) * 4.0;
        for (float range : scan_.ranges) {
            theta -= scan_.angle_increment;

            if (range > scan_.range_max) continue;
            if (range < scan_.range_min) continue;
            if (theta < scan_.angle_min) continue;
            if (theta > scan_.angle_max) continue;
            if (std::isnan(range)) continue;

            float start = range - r2;
            
            if (start < 0) {
                continue;
            }
            
            const Eigen::Matrix<float, 4, 1> point(start * cos(theta - offset),
                                                    -start * sin(theta - offset),
                                                    0.0,
                                                    1.0);
            //将基于传感器坐标系的数据转到基于base_link坐标系
            Eigen::Matrix<float, 4, 1> base = laser_tf_ * point;
            
            common::Rigid2f::Vector vector(base(0), base(1));
            common::Rigid2f::Vector laser_trans(laser_tf_(0, 3), laser_tf_(1, 3));
            
            vector = global_tf * vector;
            laser_trans = global_tf * laser_trans;

            float x = -laser_trans(0) + vector(0);
            float y = -laser_trans(1) + vector(1);

            float step = std::max(fabs(x) / global_map_->getResolution(), fabs(y) / global_map_->getResolution());
            float step_x = x / step;
            float step_y = y / step;

            x = laser_trans(0), y = laser_trans(1);
            for (float error = 0.0;error < step + 1.0;error += 1.0) {
                x += step_x;
                y += step_y;
                global_map_->weakenPixel(x, y, 0.001);
            }
        }
    }

}
}