#include <omp.h>
#include <chrono>
#include <queue>
#include <thread>

#include <Eigen/StdVector>
#include <ceres/ceres.h>
#include <glog/logging.h>

#include "ct_icp.hpp"
#include "cost_functions.h"

#ifdef CT_ICP_WITH_VIZ

#include "utils.hpp"

#include <viz3d/engine.hpp>
#include <colormap/colormap.hpp>
#include <colormap/color.hpp>

#endif
namespace ct_icp {

    /* -------------------------------------------------------------------------------------------------------------- */
    // Subsample to keep one random point in every voxel of the current frame
    void sub_sample_frame(std::vector<Point3D> &frame, double size_voxel) {
        std::unordered_map<Voxel, std::vector<Point3D>> grid;
        for (int i = 0; i < (int) frame.size(); i++) {
            auto kx = static_cast<short>(frame[i].pt[0] / size_voxel);
            auto ky = static_cast<short>(frame[i].pt[1] / size_voxel);
            auto kz = static_cast<short>(frame[i].pt[2] / size_voxel);
            grid[Voxel(kx, ky, kz)].push_back(frame[i]);
        }
        frame.resize(0);
        int step = 0; //to take one random point inside each voxel (but with identical results when lunching the SLAM a second time)
        for (const auto &n: grid) {
            if (n.second.size() > 0) {
                //frame.push_back(n.second[step % (int)n.second.size()]);
                frame.push_back(n.second[0]);
                step++;
            }
        }
    }

    /* -------------------------------------------------------------------------------------------------------------- */
    void
    grid_sampling(const std::vector<Point3D> &frame, std::vector<Point3D> &keypoints, double size_voxel_subsampling) {
        // TODO Replace std::list by a vector ?
        keypoints.resize(0);
        std::vector<Point3D> frame_sub;
        frame_sub.resize(frame.size());
        for (int i = 0; i < (int) frame_sub.size(); i++) {
            frame_sub[i] = frame[i];
        }
        sub_sample_frame(frame_sub, size_voxel_subsampling);
        keypoints.reserve(frame_sub.size());
        for (int i = 0; i < (int) frame_sub.size(); i++) {
            keypoints.push_back(frame_sub[i]);
        }
    }

    /* -------------------------------------------------------------------------------------------------------------- */

    struct Neighborhood {
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        Eigen::Vector3d center = Eigen::Vector3d::Zero();

        Eigen::Vector3d normal = Eigen::Vector3d::Zero();

        Eigen::Matrix3d covariance = Eigen::Matrix3d::Identity();

        double a2D = 1.0; // Planarity coefficient
    };

    // Computes normal and planarity coefficient
    Neighborhood compute_neighborhood_distribution(const ArrayVector3d &points) {
        Neighborhood neighborhood;
        // Compute the normals
        Eigen::Vector3d barycenter(Eigen::Vector3d(0, 0, 0));
        for (auto &point: points) {
            barycenter += point;
        }
        barycenter /= (double) points.size();
        neighborhood.center = barycenter;

        Eigen::Matrix3d covariance_Matrix(Eigen::Matrix3d::Zero());
        for (auto &point: points) {
            for (int k = 0; k < 3; ++k)
                for (int l = k; l < 3; ++l)
                    covariance_Matrix(k, l) += (point(k) - barycenter(k)) *
                                               (point(l) - barycenter(l));
        }
        covariance_Matrix(1, 0) = covariance_Matrix(0, 1);
        covariance_Matrix(2, 0) = covariance_Matrix(0, 2);
        covariance_Matrix(2, 1) = covariance_Matrix(1, 2);
        neighborhood.covariance = covariance_Matrix;
        Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> es(covariance_Matrix);
        Eigen::Vector3d normal(es.eigenvectors().col(0).normalized());
        neighborhood.normal = normal;

        // Compute planarity from the eigen values
        double sigma_1 = sqrt(std::abs(
                es.eigenvalues()[2])); //Be careful, the eigenvalues are not correct with the iterative way to compute the covariance matrix
        double sigma_2 = sqrt(std::abs(es.eigenvalues()[1]));
        double sigma_3 = sqrt(std::abs(es.eigenvalues()[0]));
        neighborhood.a2D = (sigma_2 - sigma_3) / sigma_1;

        if (neighborhood.a2D != neighborhood.a2D) {
            LOG(ERROR) << "FOUND NAN!!!";
            throw std::runtime_error("error");
        }

        return neighborhood;
    }

    /* -------------------------------------------------------------------------------------------------------------- */
    // Search Neighbors with VoxelHashMap lookups
    using pair_distance_t = std::tuple<double, Eigen::Vector3d, Voxel>;

    struct Comparator {
        bool operator()(const pair_distance_t &left, const pair_distance_t &right) const {
            return std::get<0>(left) < std::get<0>(right);
        }
    };

    using priority_queue_t = std::priority_queue<pair_distance_t, std::vector<pair_distance_t>, Comparator>;

    inline ArrayVector3d
    search_neighbors(const VoxelHashMap &map,
                     const Eigen::Vector3d &point,
                     int nb_voxels_visited,
                     double size_voxel_map,
                     int max_num_neighbors,
                     int threshold_voxel_capacity = 1,
                     std::vector<Voxel> *voxels = nullptr) {

        if (voxels != nullptr)
            voxels->reserve(max_num_neighbors);

        short kx = static_cast<short>(point[0] / size_voxel_map);
        short ky = static_cast<short>(point[1] / size_voxel_map);
        short kz = static_cast<short>(point[2] / size_voxel_map);

        priority_queue_t priority_queue;

        Voxel voxel(kx, ky, kz);
        for (short kxx = kx - nb_voxels_visited; kxx < kx + nb_voxels_visited + 1; ++kxx) {
            for (short kyy = ky - nb_voxels_visited; kyy < ky + nb_voxels_visited + 1; ++kyy) {
                for (short kzz = kz - nb_voxels_visited; kzz < kz + nb_voxels_visited + 1; ++kzz) {
                    voxel.x = kxx;
                    voxel.y = kyy;
                    voxel.z = kzz;

                    auto search = map.find(voxel);
                    if (search != map.end()) {
                        const auto &voxel_block = search.value();
                        if (voxel_block.NumPoints() < threshold_voxel_capacity)
                            continue;
                        for (int i(0); i < voxel_block.NumPoints(); ++i) {
                            auto &neighbor = voxel_block.points[i];
                            double distance = (neighbor - point).norm();
                            if (priority_queue.size() == max_num_neighbors) {
                                if (distance < std::get<0>(priority_queue.top())) {
                                    priority_queue.pop();
                                    priority_queue.emplace(distance, neighbor, voxel);
                                }
                            } else
                                priority_queue.emplace(distance, neighbor, voxel);
                        }
                    }
                }
            }
        }

        auto size = priority_queue.size();
        ArrayVector3d closest_neighbors(size);
        if (voxels != nullptr) {
            voxels->resize(size);
        }
        for (auto i = 0; i < size; ++i) {
            closest_neighbors[size - 1 - i] = std::get<1>(priority_queue.top());
            if (voxels != nullptr)
                (*voxels)[size - 1 - i] = std::get<2>(priority_queue.top());
            priority_queue.pop();
        }

        return closest_neighbors;
    }

    /* -------------------------------------------------------------------------------------------------------------- */
    inline ArrayVector3d
    select_closest_neighbors(const std::vector<std::vector<Eigen::Vector3d> const *> &neighbors_ptr,
                             const Eigen::Vector3d &pt_keypoint,
                             int num_neighbors, int max_num_neighbors) {
        std::vector<std::pair<double, Eigen::Vector3d>> distance_neighbors;
        distance_neighbors.reserve(neighbors_ptr.size());
        for (auto &it_ptr: neighbors_ptr) {
            for (auto &it: *it_ptr) {
                double sq_dist = (pt_keypoint - it).squaredNorm();
                distance_neighbors.emplace_back(sq_dist, it);
            }
        }

        int real_number_neighbors = std::min(max_num_neighbors, (int) distance_neighbors.size());
        std::partial_sort(distance_neighbors.begin(),
                          distance_neighbors.begin() + real_number_neighbors,
                          distance_neighbors.end(),
                          [](const std::pair<double, Eigen::Vector3d> &left,
                             const std::pair<double, Eigen::Vector3d> &right) {
                              return left.first < right.first;
                          });

        ArrayVector3d neighbors(real_number_neighbors);
        for (auto i(0); i < real_number_neighbors; ++i)
            neighbors[i] = distance_neighbors[i].second;
        return neighbors;
    }

    /* -------------------------------------------------------------------------------------------------------------- */

    // A Builder to abstract the different configurations of ICP optimization
    class ICPOptimizationBuilder {
    public:
        using CTICP_PointToPlaneResidual = ceres::AutoDiffCostFunction<CTPointToPlaneFunctor, 1, 4, 3, 4, 3>;
        using PointToPlaneResidual = ceres::AutoDiffCostFunction<PointToPlaneFunctor, 1, 4, 3>;

        explicit ICPOptimizationBuilder(const CTICPOptions *options,
                                        const std::vector<Point3D> *points) :
                options_(options),
                keypoints(points) {
            corrected_raw_points_.resize(keypoints->size());
            for (int i(0); i < points->size(); ++i)
                corrected_raw_points_[i] = (*points)[i].raw_pt;

            max_num_residuals_ = options->max_num_residuals;
        }

        bool InitProblem(int num_residuals) {
            problem = std::make_unique<ceres::Problem>();
            parameter_block_set_ = false;

            // Select Loss function
            switch (options_->loss_function) {
                case LEAST_SQUARES::STANDARD:
                    break;
                case LEAST_SQUARES::CAUCHY:
                    loss_function = new ceres::CauchyLoss(options_->ls_sigma);
                    break;
                case LEAST_SQUARES::HUBER:
                    loss_function = new ceres::HuberLoss(options_->ls_sigma);
                    break;
                case LEAST_SQUARES::TOLERANT:
                    loss_function = new ceres::TolerantLoss(options_->ls_tolerant_min_threshold,
                                                            options_->ls_sigma);
                    break;
                case LEAST_SQUARES::TRUNCATED:
                    loss_function = new ct_icp::TruncatedLoss(options_->ls_sigma);
                    break;
            }

            // Resize the number of residuals
            vector_ct_icp_residuals_.resize(num_residuals);
            vector_cost_functors_.resize(num_residuals);
            begin_quat_ = nullptr;
            end_quat_ = nullptr;
            begin_t_ = nullptr;
            end_t_ = nullptr;

            return true;
        }

        void DistortFrame(Eigen::Quaterniond &begin_quat, Eigen::Quaterniond &end_quat,
                          Eigen::Vector3d &begin_t, Eigen::Vector3d &end_t) {
            if (options_->distance == POINT_TO_PLANE) {
                // Distorts the frame (put all raw_points in the coordinate frame of the pose at the end of the acquisition)
                Eigen::Quaterniond end_quat_I = end_quat.inverse(); // Rotation of the inverse pose
                Eigen::Vector3d end_t_I = -1.0 * (end_quat_I * end_t); // Translation of the inverse pose

                for (int i(0); i < keypoints->size(); ++i) {
                    auto &keypoint = (*keypoints)[i];
                    double alpha_timestamp = keypoint.alpha_timestamp;
                    Eigen::Quaterniond q_alpha = begin_quat.slerp(alpha_timestamp, end_quat);
                    q_alpha.normalize();
                    Eigen::Matrix3d R = q_alpha.toRotationMatrix();
                    Eigen::Vector3d t = (1.0 - alpha_timestamp) * begin_t + alpha_timestamp * end_t;

                    // Distort Raw Keypoints
                    corrected_raw_points_[i] = end_quat_I * (q_alpha * keypoint.raw_pt + t) + end_t_I;
                }
            }
        }

        inline void AddParameterBlocks(Eigen::Quaterniond &begin_quat, Eigen::
        Quaterniond &end_quat, Eigen::Vector3d &begin_t, Eigen::Vector3d &end_t) {
            CHECK(!parameter_block_set_) << "The parameter block was already set";
            auto *parameterization = new ceres::EigenQuaternionParameterization();
            begin_t_ = &begin_t.x();
            end_t_ = &end_t.x();
            begin_quat_ = &begin_quat.x();
            end_quat_ = &end_quat.x();

            switch (options_->distance) {
                case CT_POINT_TO_PLANE:
                    problem->AddParameterBlock(begin_quat_, 4, parameterization);
                    problem->AddParameterBlock(end_quat_, 4, parameterization);
                    problem->AddParameterBlock(begin_t_, 3);
                    problem->AddParameterBlock(end_t_, 3);
                    break;
                case POINT_TO_PLANE:
                    problem->AddParameterBlock(end_quat_, 4, parameterization);
                    problem->AddParameterBlock(end_t_, 3);
                    break;
            }

            parameter_block_set_ = true;
        }

        inline void SetResidualBlock(int residual_id,
                                     int keypoint_id,
                                     const Eigen::Vector3d &reference_point,
                                     const Eigen::Vector3d &reference_normal,
                                     double weight = 1.0,
                                     double alpha_timestamp = -1.0) {

            CTPointToPlaneFunctor *ct_point_to_plane_functor = nullptr;
            PointToPlaneFunctor *point_to_plane_functor = nullptr;
            void *cost_functor = nullptr;
            void *cost_function = nullptr;
            if (alpha_timestamp < 0 || alpha_timestamp > 1)
                throw std::runtime_error("BAD ALPHA TIMESTAMP !");
            switch (options_->distance) {
                case CT_POINT_TO_PLANE:
                    ct_point_to_plane_functor = new CTPointToPlaneFunctor(reference_point,
                                                                          corrected_raw_points_[keypoint_id],
                                                                          reference_normal,
                                                                          alpha_timestamp, weight);
                    cost_functor = ct_point_to_plane_functor;
                    cost_function = static_cast<void *>(new CTICP_PointToPlaneResidual(ct_point_to_plane_functor));
                    break;
                case POINT_TO_PLANE:
                    point_to_plane_functor = new PointToPlaneFunctor(reference_point,
                                                                     corrected_raw_points_[keypoint_id],
                                                                     reference_normal,
                                                                     weight);
                    cost_functor = point_to_plane_functor;
                    cost_function = static_cast<void *>(new PointToPlaneResidual(point_to_plane_functor));
                    break;
            }
            vector_ct_icp_residuals_[residual_id] = cost_function;
            vector_cost_functors_[residual_id] = cost_functor;
        }

        std::unique_ptr<ceres::Problem> GetProblem(int &out_number_of_residuals) {
            out_number_of_residuals = 0;
            for (auto &pt_to_plane_residual: vector_ct_icp_residuals_) {
                if (pt_to_plane_residual != nullptr) {
                    if (max_num_residuals_ <= 0 || out_number_of_residuals < max_num_residuals_) {

                        switch (options_->distance) {
                            case CT_POINT_TO_PLANE:
                                problem->AddResidualBlock(
                                        static_cast<CTICP_PointToPlaneResidual *>(pt_to_plane_residual), loss_function,
                                        begin_quat_, begin_t_, end_quat_, end_t_);
                                break;
                            case POINT_TO_PLANE:
                                problem->AddResidualBlock(
                                        static_cast<PointToPlaneResidual *>(pt_to_plane_residual), loss_function,
                                        end_quat_, end_t_);
                                break;
                        }
                        out_number_of_residuals++;
                    } else {
                        // Need to deallocate memory from the allocated pointers not managed by Ceres
                        CTICP_PointToPlaneResidual *ct_pt_to_pl_ptr = nullptr;
                        PointToPlaneResidual *pt_to_pl_ptr = nullptr;
                        switch (options_->distance) {
                            case CT_POINT_TO_PLANE:
                                ct_pt_to_pl_ptr = static_cast<CTICP_PointToPlaneResidual *>(pt_to_plane_residual);
                                delete ct_pt_to_pl_ptr;
                                break;
                            case POINT_TO_PLANE:
                                pt_to_pl_ptr = static_cast<PointToPlaneResidual *>(pt_to_plane_residual);
                                delete pt_to_pl_ptr;
                                break;
                        }
                    }
                }
            }

#if CT_ICP_WITH_VIZ
            // Adds to the visualizer keypoints colored by timestamp value
            if (options_->debug_viz) {
                auto palette = colormap::palettes.at("jet").rescale(0, 1);
                auto &instance = viz::ExplorationEngine::Instance();
                auto model_ptr = std::make_shared<viz::PointCloudModel>();
                auto &model_data = model_ptr->ModelData();
                model_data.xyz.reserve(keypoints->size());
                model_data.point_size = 6;
                model_data.default_color = Eigen::Vector3f(1, 0, 0);
                model_data.rgb.reserve(keypoints->size());
                std::vector<double> scalars(keypoints->size());

                double s_min = 0.0;
                double s_max = 1.0;
                std::vector<double> s_values;
                if (options_->viz_mode == WEIGHT || options_->viz_mode == TIMESTAMP) {
                    s_min = std::numeric_limits<double>::max();
                    s_max = std::numeric_limits<double>::min();
                    s_values.resize(keypoints->size());
                    for (int i(0); i < keypoints->size(); ++i) {
                        double new_s;
                        auto *ptr = vector_cost_functors_[i];
                        if (ptr != nullptr) {
                            CTPointToPlaneFunctor *ct_ptr;
                            PointToPlaneFunctor *pt_to_pl_ptr;

                            switch (options_->distance) {
                                case CT_POINT_TO_PLANE:
                                    ct_ptr = static_cast<CTPointToPlaneFunctor *>(ptr);
                                    new_s = options_->viz_mode == WEIGHT ? ct_ptr->weight_ : ct_ptr->alpha_timestamps_;
                                    break;
                                case POINT_TO_PLANE:
                                    pt_to_pl_ptr = static_cast<PointToPlaneFunctor *>(ptr);
                                    new_s = options_->viz_mode == WEIGHT ? pt_to_pl_ptr->weight_ : 1.0;
                                    break;
                            }
                            if (new_s < s_min)
                                s_min = new_s;
                            if (new_s > s_max)
                                s_max = new_s;
                            s_values[i] = new_s;
                        }

                    }
                }

                for (size_t i(0); i < keypoints->size(); ++i) {
                    void *ptr = vector_cost_functors_[i];
                    if (!ptr)
                        continue;
                    model_data.xyz.push_back((*keypoints)[i].pt.cast<float>());
                    scalars[i] = (*keypoints)[i].alpha_timestamp;
                    if (options_->viz_mode == NORMAL) {
                        switch (options_->distance) {
                            case CT_POINT_TO_PLANE:
                                model_data.rgb.push_back(
                                        static_cast<CTPointToPlaneFunctor *>(ptr)->reference_normal_.cwiseAbs().cast<float>());
                                break;
                            case POINT_TO_PLANE:
                                model_data.rgb.push_back(
                                        static_cast<PointToPlaneFunctor *>(ptr)->reference_normal_.cwiseAbs().cast<float>());
                                break;
                        }
                    } else {
                        double s = s_min == s_max ? 1.0 : (s_values[i] - s_min) / (s_max - s_min);
                        colormap::rgb value = palette(s);
                        std::uint8_t *rgb_color_ptr = reinterpret_cast<std::uint8_t *>(&value);
                        Eigen::Vector3f rgb((float) rgb_color_ptr[0] / 255.0f,
                                            (float) rgb_color_ptr[1] / 255.0f, (float) rgb_color_ptr[2] / 255.0f);
                        model_data.rgb.push_back(rgb);
                    }
                }

                instance.AddModel(-2, model_ptr);
            }
#endif
            std::fill(vector_cost_functors_.begin(), vector_cost_functors_.end(), nullptr);
            std::fill(vector_ct_icp_residuals_.begin(), vector_ct_icp_residuals_.end(), nullptr);

            return std::move(problem);
        }

    private:
        const CTICPOptions *options_;
        std::unique_ptr<ceres::Problem> problem = nullptr;
        int max_num_residuals_ = -1;

        // Parameters block pointers
        bool parameter_block_set_ = false;
        double *begin_quat_ = nullptr;
        double *end_quat_ = nullptr;
        double *begin_t_ = nullptr;
        double *end_t_ = nullptr;

        // Pointers managed by ceres
        const std::vector<Point3D> *keypoints;
        std::vector<Eigen::Vector3d> corrected_raw_points_;

        std::vector<void *> vector_cost_functors_;
        std::vector<void *> vector_ct_icp_residuals_;
        ceres::LossFunction *loss_function = nullptr;
    };

    /* -------------------------------------------------------------------------------------------------------------- */
    ICPSummary CT_ICP_CERES(const CTICPOptions &options,
                            const VoxelHashMap &voxels_map, std::vector<Point3D> &keypoints,
                            std::vector<TrajectoryFrame> &trajectory, int index_frame) {

        const short nb_voxels_visited = index_frame < options.init_num_frames ? 2 : options.voxel_neighborhood;
        const int kMinNumNeighbors = options.min_number_neighbors;
        const int kThresholdCapacity = index_frame < options.init_num_frames ? 1 : options.threshold_voxel_occupancy;

        ceres::Solver::Options ceres_options;
        ceres_options.max_num_iterations = options.ls_max_num_iters;
        ceres_options.num_threads = options.ls_num_threads;
        ceres_options.trust_region_strategy_type = ceres::TrustRegionStrategyType::LEVENBERG_MARQUARDT;

        TrajectoryFrame *previous_estimate = nullptr;
        Eigen::Vector3d previous_velocity = Eigen::Vector3d::Zero();
        Eigen::Quaterniond previous_orientation = Eigen::Quaterniond::Identity();
        if (index_frame > 0) {
            previous_estimate = &trajectory[index_frame - 1];
            previous_velocity = previous_estimate->end_t - previous_estimate->begin_t;
            previous_orientation = Eigen::Quaterniond(previous_estimate->end_R);
        }

        TrajectoryFrame &current_estimate = trajectory[index_frame];
        Eigen::Quaterniond begin_quat = Eigen::Quaterniond(current_estimate.begin_R);
        Eigen::Quaterniond end_quat = Eigen::Quaterniond(current_estimate.end_R);
        Eigen::Vector3d begin_t = current_estimate.begin_t;
        Eigen::Vector3d end_t = current_estimate.end_t;

        int number_of_residuals;

        ICPOptimizationBuilder builder(&options, &keypoints);
        if (options.point_to_plane_with_distortion) {
            builder.DistortFrame(begin_quat, end_quat, begin_t, end_t);
        }

        int num_iter_icp = index_frame < options.init_num_frames ? std::max(15, options.num_iters_icp) :
                           options.num_iters_icp;

        auto transform_keypoints = [&]() {
            // Elastically distorts the frame to improve on Neighbor estimation
            Eigen::Matrix3d R;
            Eigen::Vector3d t;
            for (auto &keypoint: keypoints) {
                if (options.point_to_plane_with_distortion || options.distance == CT_POINT_TO_PLANE) {
                    double alpha_timestamp = keypoint.alpha_timestamp;
                    Eigen::Quaterniond q = begin_quat.slerp(alpha_timestamp, end_quat);
                    q.normalize();
                    R = q.toRotationMatrix();
                    t = (1.0 - alpha_timestamp) * begin_t + alpha_timestamp * end_t;
                } else {
                    R = end_quat.normalized().toRotationMatrix();
                    t = end_t;
                }

                keypoint.pt = R * keypoint.raw_pt + t;
            }
        };

        auto estimate_point_neighborhood = [&](ArrayVector3d &vector_neighbors,
                                               Eigen::Vector3d &location,
                                               double &planarity_weight) {

            auto neighborhood = compute_neighborhood_distribution(vector_neighbors);
            planarity_weight = std::pow(neighborhood.a2D, options.power_planarity);

            if (neighborhood.normal.dot(trajectory[index_frame].begin_t - location) < 0) {
                neighborhood.normal = -1.0 * neighborhood.normal;
            }
            return neighborhood;
        };

        double lambda_weight = std::abs(options.weight_alpha);
        double lambda_neighborhood = std::abs(options.weight_neighborhood);
        const double kMaxPointToPlane = options.max_dist_to_plane_ct_icp;
        const double sum = lambda_weight + lambda_neighborhood;
        CHECK(sum > 0.0) << "Invalid requirement: weight_alpha(" << options.weight_alpha <<
                         ") + weight_neighborhood(" << options.weight_neighborhood << ") <= 0 " << std::endl;
        lambda_weight /= sum;
        lambda_neighborhood /= sum;

        for (int iter(0); iter < num_iter_icp; iter++) {
            transform_keypoints();

            builder.InitProblem(keypoints.size() * options.num_closest_neighbors);
            builder.AddParameterBlocks(begin_quat, end_quat, begin_t, end_t);

            // Add Point-to-plane residuals
            int num_keypoints = keypoints.size();
            int num_threads = options.ls_num_threads;
#pragma omp parallel for num_threads(num_threads)
            for (int k = 0; k < num_keypoints; ++k) {
                auto &keypoint = keypoints[k];
                auto &raw_point = keypoint.raw_pt;
                // Neighborhood search
                std::vector<Voxel> voxels;
                auto vector_neighbors = search_neighbors(voxels_map, keypoint.pt,
                                                         nb_voxels_visited, options.size_voxel_map,
                                                         options.max_number_neighbors, kThresholdCapacity,
                                                         options.estimate_normal_from_neighborhood ? nullptr : &voxels);

                if (vector_neighbors.size() < kMinNumNeighbors)
                    continue;

                double weight;
                auto neighborhood = estimate_point_neighborhood(vector_neighbors,
                                                                raw_point,
                                                                weight);

                weight = lambda_weight * weight +
                         lambda_neighborhood * std::exp(-(vector_neighbors[0] -
                                                          keypoint.pt).norm() / (kMaxPointToPlane * kMinNumNeighbors));

                double point_to_plane_dist;
                std::set<Voxel> neighbor_voxels;
                for (int i(0); i < options.num_closest_neighbors; ++i) {
                    point_to_plane_dist = std::abs(
                            (keypoint.pt - vector_neighbors[i]).transpose() * neighborhood.normal);
                    if (point_to_plane_dist < options.max_dist_to_plane_ct_icp) {
                        builder.SetResidualBlock(options.num_closest_neighbors * k + i, k,
                                                 vector_neighbors[i],
                                                 neighborhood.normal, weight, keypoint.alpha_timestamp);
                    }
                }
            }

            auto problem = builder.GetProblem(number_of_residuals);

            if (index_frame > 1) {
                if (options.distance == CT_POINT_TO_PLANE) {
                    // Add Regularisation residuals
                    problem->AddResidualBlock(new ceres::AutoDiffCostFunction<LocationConsistencyFunctor,
                                                      LocationConsistencyFunctor::NumResiduals(), 3>(
                                                      new LocationConsistencyFunctor(previous_estimate->end_t,
                                                                                sqrt(number_of_residuals *
                                                                                options.beta_location_consistency))),
                                              nullptr,
                                              &begin_t.x());
                    problem->AddResidualBlock(new ceres::AutoDiffCostFunction<ConstantVelocityFunctor,
                                                      ConstantVelocityFunctor::NumResiduals(), 3, 3>(
                                                      new ConstantVelocityFunctor(previous_velocity,
                                                                                sqrt(number_of_residuals * options.beta_constant_velocity))),
                                              nullptr,
                                              &begin_t.x(),
                                              &end_t.x());

                    // SMALL VELOCITY
                    problem->AddResidualBlock(new ceres::AutoDiffCostFunction<SmallVelocityFunctor,
                                                      SmallVelocityFunctor::NumResiduals(), 3, 3>(
                                                      new SmallVelocityFunctor(sqrt(number_of_residuals * options.beta_small_velocity))),
                                              nullptr,
                                              &begin_t.x(), &end_t.x());

                    // ORIENTATION CONSISTENCY
                    problem->AddResidualBlock(new ceres::AutoDiffCostFunction<OrientationConsistencyFunctor,
                                                      OrientationConsistencyFunctor::NumResiduals(), 4>(
                                                      new OrientationConsistencyFunctor(previous_orientation,
                                                                                sqrt(number_of_residuals *
                                                                                options.beta_orientation_consistency))),
                                              nullptr,
                                              &begin_quat.x());
                }
            }
            if (number_of_residuals < options.min_number_neighbors) {
                std::stringstream ss_out;
                ss_out << "[CT_ICP] Error : not enough keypoints selected in ct-icp !" << std::endl;
                ss_out << "[CT_ICP] number_of_residuals : " << number_of_residuals << std::endl;
                ICPSummary summary;
                summary.success = false;
                summary.num_residuals_used = number_of_residuals;
                summary.error_log = ss_out.str();
                if (options.debug_print) {
                    std::cout << summary.error_log;
                }
                return summary;
            }

            ceres::Solver::Summary summary;
            ceres::Solve(ceres_options, problem.get(), &summary);
            if (!summary.IsSolutionUsable()) {
                std::cout << summary.FullReport() << std::endl;
                throw std::runtime_error("Error During Optimization");
            }
            if (options.debug_print) {
                std::cout << summary.BriefReport() << std::endl;
            }

            begin_quat.normalize();
            end_quat.normalize();

            double diff_trans = (current_estimate.begin_t - begin_t).norm() + (current_estimate.end_t - end_t).norm();
            double diff_rot = AngularDistance(current_estimate.begin_R, begin_quat.toRotationMatrix()) +
                              AngularDistance(current_estimate.end_R, end_quat.toRotationMatrix());

            current_estimate.begin_t = begin_t;
            current_estimate.end_t = end_t;
            current_estimate.begin_R = begin_quat.toRotationMatrix();
            current_estimate.end_R = end_quat.toRotationMatrix();

            if (options.point_to_plane_with_distortion) {
                builder.DistortFrame(begin_quat, end_quat, begin_t, end_t);
            }

            if ((index_frame > 1) &&
                (diff_rot < options.threshold_orientation_norm &&
                 diff_trans < options.threshold_translation_norm)) {

                if (options.debug_print) {
                    std::cout << "CT_ICP: Finished with N=" << iter << " ICP iterations" << std::endl;

                }
                break;
            }
        }
        transform_keypoints();

        ICPSummary summary;
        summary.success = true;
        summary.num_residuals_used = number_of_residuals;
        return summary;
    }

    /* -------------------------------------------------------------------------------------------------------------- */
    ICPSummary CT_ICP_GN(const CTICPOptions &options,
                         const VoxelHashMap &voxels_map, std::vector<Point3D> &keypoints,
                         std::vector<TrajectoryFrame> &trajectory, int index_frame) {

        //Optimization with Traj constraints
        double ALPHA_C = options.beta_location_consistency; // 0.001;
        double ALPHA_E = options.beta_constant_velocity; // 0.001; //no ego (0.0) is not working

        // For the 50 first frames, visit 2 voxels
        const short nb_voxels_visited = index_frame < options.init_num_frames ? 2 : 1;
        int number_keypoints_used = 0;
        const int kMinNumNeighbors = options.min_number_neighbors;

        using AType = Eigen::Matrix<double, 12, 12>;
        using bType = Eigen::Matrix<double, 12, 1>;
        AType A;
        bType b;

        // TODO Remove chronos
        double elapsed_search_neighbors = 0.0;
        double elapsed_select_closest_neighbors = 0.0;
        double elapsed_normals = 0.0;
        double elapsed_A_construction = 0.0;
        double elapsed_solve = 0.0;
        double elapsed_update = 0.0;

        ICPSummary summary;

        int num_iter_icp = index_frame < options.init_num_frames ? 15 : options.num_iters_icp;
        for (int iter(0); iter < num_iter_icp; iter++) {
            A = Eigen::MatrixXd::Zero(12, 12);
            b = Eigen::VectorXd::Zero(12);

            number_keypoints_used = 0;
            double total_scalar = 0;
            double mean_scalar = 0.0;

            for (auto &keypoint: keypoints) {
                auto start = std::chrono::steady_clock::now();
                auto &pt_keypoint = keypoint.pt;

                // Neighborhood search
                ArrayVector3d vector_neighbors = search_neighbors(voxels_map, pt_keypoint,
                                                                  nb_voxels_visited, options.size_voxel_map,
                                                                  options.max_number_neighbors);
                auto step1 = std::chrono::steady_clock::now();
                std::chrono::duration<double> _elapsed_search_neighbors = step1 - start;
                elapsed_search_neighbors += _elapsed_search_neighbors.count() * 1000.0;

                if (vector_neighbors.size() < kMinNumNeighbors) {
                    continue;
                }

                auto step2 = std::chrono::steady_clock::now();
                std::chrono::duration<double> _elapsed_neighbors_selection = step2 - step1;
                elapsed_select_closest_neighbors += _elapsed_neighbors_selection.count() * 1000.0;

                // Compute normals from neighbors
                auto neighborhood = compute_neighborhood_distribution(vector_neighbors);
                double planarity_weight = neighborhood.a2D;
                auto &normal = neighborhood.normal;

                if (normal.dot(trajectory[index_frame].begin_t - pt_keypoint) < 0) {
                    normal = -1.0 * normal;
                }

                double alpha_timestamp = keypoint.alpha_timestamp;
                double weight = planarity_weight *
                                planarity_weight; //planarity_weight**2 much better than planarity_weight (planarity_weight**3 is not working)
                Eigen::Vector3d closest_pt_normal = weight * normal;

                Eigen::Vector3d closest_point = vector_neighbors[0];

                double dist_to_plane = normal[0] * (pt_keypoint[0] - closest_point[0]) +
                                       normal[1] * (pt_keypoint[1] - closest_point[1]) +
                                       normal[2] * (pt_keypoint[2] - closest_point[2]);

                auto step3 = std::chrono::steady_clock::now();
                std::chrono::duration<double> _elapsed_normals = step3 - step2;
                elapsed_normals += _elapsed_normals.count() * 1000.0;

                // std::cout << "dist_to_plane : " << dist_to_plane << std::endl;

                if (fabs(dist_to_plane) < options.max_dist_to_plane_ct_icp) {

                    double scalar = closest_pt_normal[0] * (pt_keypoint[0] - closest_point[0]) +
                                    closest_pt_normal[1] * (pt_keypoint[1] - closest_point[1]) +
                                    closest_pt_normal[2] * (pt_keypoint[2] - closest_point[2]);
                    total_scalar = total_scalar + scalar * scalar;
                    mean_scalar = mean_scalar + fabs(scalar);
                    number_keypoints_used++;

                    Eigen::Vector3d frame_idx_previous_origin_begin =
                            trajectory[index_frame].begin_R * keypoint.raw_pt;
                    Eigen::Vector3d frame_idx_previous_origin_end =
                            trajectory[index_frame].end_R * keypoint.raw_pt;

                    double cbx =
                            (1 - alpha_timestamp) * (frame_idx_previous_origin_begin[1] * closest_pt_normal[2] -
                                                     frame_idx_previous_origin_begin[2] * closest_pt_normal[1]);
                    double cby =
                            (1 - alpha_timestamp) * (frame_idx_previous_origin_begin[2] * closest_pt_normal[0] -
                                                     frame_idx_previous_origin_begin[0] * closest_pt_normal[2]);
                    double cbz =
                            (1 - alpha_timestamp) * (frame_idx_previous_origin_begin[0] * closest_pt_normal[1] -
                                                     frame_idx_previous_origin_begin[1] * closest_pt_normal[0]);

                    double nbx = (1 - alpha_timestamp) * closest_pt_normal[0];
                    double nby = (1 - alpha_timestamp) * closest_pt_normal[1];
                    double nbz = (1 - alpha_timestamp) * closest_pt_normal[2];

                    double cex = (alpha_timestamp) * (frame_idx_previous_origin_end[1] * closest_pt_normal[2] -
                                                      frame_idx_previous_origin_end[2] * closest_pt_normal[1]);
                    double cey = (alpha_timestamp) * (frame_idx_previous_origin_end[2] * closest_pt_normal[0] -
                                                      frame_idx_previous_origin_end[0] * closest_pt_normal[2]);
                    double cez = (alpha_timestamp) * (frame_idx_previous_origin_end[0] * closest_pt_normal[1] -
                                                      frame_idx_previous_origin_end[1] * closest_pt_normal[0]);

                    double nex = (alpha_timestamp) * closest_pt_normal[0];
                    double ney = (alpha_timestamp) * closest_pt_normal[1];
                    double nez = (alpha_timestamp) * closest_pt_normal[2];

                    Eigen::VectorXd u(12);
                    u << cbx, cby, cbz, nbx, nby, nbz, cex, cey, cez, nex, ney, nez;
                    for (int i = 0; i < 12; i++) {
                        for (int j = 0; j < 12; j++) {
                            A(i, j) = A(i, j) + u[i] * u[j];
                        }
                        b(i) = b(i) - u[i] * scalar;
                    }

                    auto step4 = std::chrono::steady_clock::now();
                    std::chrono::duration<double> _elapsed_A = step4 - step3;
                    elapsed_search_neighbors += _elapsed_A.count() * 1000.0;
                }
            }

            if (number_keypoints_used < 100) {
                std::stringstream ss_out;
                ss_out << "[CT_ICP]Error : not enough keypoints selected in ct-icp !" << std::endl;
                ss_out << "[CT_ICP]Number_of_residuals : " << number_keypoints_used << std::endl;

                summary.error_log = ss_out.str();
                if (options.debug_print)
                    std::cout << summary.error_log;

                summary.success = false;
                return summary;
            }

            auto start = std::chrono::steady_clock::now();

            // Normalize equation
            for (int i(0); i < 12; i++) {
                for (int j(0); j < 12; j++) {
                    A(i, j) = A(i, j) / number_keypoints_used;
                }
                b(i) = b(i) / number_keypoints_used;
            }

            //Add constraints in trajectory
            if (index_frame > 1) //no constraints for frame_index == 1
            {
                Eigen::Vector3d diff_traj = trajectory[index_frame].begin_t - trajectory[index_frame - 1].end_t;
                A(3, 3) = A(3, 3) + ALPHA_C;
                A(4, 4) = A(4, 4) + ALPHA_C;
                A(5, 5) = A(5, 5) + ALPHA_C;
                b(3) = b(3) - ALPHA_C * diff_traj(0);
                b(4) = b(4) - ALPHA_C * diff_traj(1);
                b(5) = b(5) - ALPHA_C * diff_traj(2);

                Eigen::Vector3d diff_ego = trajectory[index_frame].end_t - trajectory[index_frame].begin_t -
                                           trajectory[index_frame - 1].end_t + trajectory[index_frame - 1].begin_t;
                A(9, 9) = A(9, 9) + ALPHA_E;
                A(10, 10) = A(10, 10) + ALPHA_E;
                A(11, 11) = A(11, 11) + ALPHA_E;
                b(9) = b(9) - ALPHA_E * diff_ego(0);
                b(10) = b(10) - ALPHA_E * diff_ego(1);
                b(11) = b(11) - ALPHA_E * diff_ego(2);
            }

            //Solve
            Eigen::VectorXd x_bundle = A.ldlt().solve(b);

            double alpha_begin = x_bundle(0);
            double beta_begin = x_bundle(1);
            double gamma_begin = x_bundle(2);
            Eigen::Matrix3d rotation_begin;
            rotation_begin(0, 0) = cos(gamma_begin) * cos(beta_begin);
            rotation_begin(0, 1) =
                    -sin(gamma_begin) * cos(alpha_begin) + cos(gamma_begin) * sin(beta_begin) * sin(alpha_begin);
            rotation_begin(0, 2) =
                    sin(gamma_begin) * sin(alpha_begin) + cos(gamma_begin) * sin(beta_begin) * cos(alpha_begin);
            rotation_begin(1, 0) = sin(gamma_begin) * cos(beta_begin);
            rotation_begin(1, 1) =
                    cos(gamma_begin) * cos(alpha_begin) + sin(gamma_begin) * sin(beta_begin) * sin(alpha_begin);
            rotation_begin(1, 2) =
                    -cos(gamma_begin) * sin(alpha_begin) + sin(gamma_begin) * sin(beta_begin) * cos(alpha_begin);
            rotation_begin(2, 0) = -sin(beta_begin);
            rotation_begin(2, 1) = cos(beta_begin) * sin(alpha_begin);
            rotation_begin(2, 2) = cos(beta_begin) * cos(alpha_begin);
            Eigen::Vector3d translation_begin = Eigen::Vector3d(x_bundle(3), x_bundle(4), x_bundle(5));

            double alpha_end = x_bundle(6);
            double beta_end = x_bundle(7);
            double gamma_end = x_bundle(8);
            Eigen::Matrix3d rotation_end;
            rotation_end(0, 0) = cos(gamma_end) * cos(beta_end);
            rotation_end(0, 1) = -sin(gamma_end) * cos(alpha_end) + cos(gamma_end) * sin(beta_end) * sin(alpha_end);
            rotation_end(0, 2) = sin(gamma_end) * sin(alpha_end) + cos(gamma_end) * sin(beta_end) * cos(alpha_end);
            rotation_end(1, 0) = sin(gamma_end) * cos(beta_end);
            rotation_end(1, 1) = cos(gamma_end) * cos(alpha_end) + sin(gamma_end) * sin(beta_end) * sin(alpha_end);
            rotation_end(1, 2) = -cos(gamma_end) * sin(alpha_end) + sin(gamma_end) * sin(beta_end) * cos(alpha_end);
            rotation_end(2, 0) = -sin(beta_end);
            rotation_end(2, 1) = cos(beta_end) * sin(alpha_end);
            rotation_end(2, 2) = cos(beta_end) * cos(alpha_end);
            Eigen::Vector3d translation_end = Eigen::Vector3d(x_bundle(9), x_bundle(10), x_bundle(11));

            trajectory[index_frame].begin_R = rotation_begin * trajectory[index_frame].begin_R;
            trajectory[index_frame].begin_t = trajectory[index_frame].begin_t + translation_begin;
            trajectory[index_frame].end_R = rotation_end * trajectory[index_frame].end_R;
            trajectory[index_frame].end_t = trajectory[index_frame].end_t + translation_end;

            auto solve_step = std::chrono::steady_clock::now();
            std::chrono::duration<double> _elapsed_solve = solve_step - start;
            elapsed_solve += _elapsed_solve.count() * 1000.0;

            //Update keypoints
            for (auto &keypoint: keypoints) {
                Eigen::Quaterniond q_begin = Eigen::Quaterniond(trajectory[index_frame].begin_R);
                Eigen::Quaterniond q_end = Eigen::Quaterniond(trajectory[index_frame].end_R);
                Eigen::Vector3d t_begin = trajectory[index_frame].begin_t;
                Eigen::Vector3d t_end = trajectory[index_frame].end_t;
                double alpha_timestamp = keypoint.alpha_timestamp;
                Eigen::Quaterniond q = q_begin.slerp(alpha_timestamp, q_end);
                q.normalize();
                Eigen::Matrix3d R = q.toRotationMatrix();
                Eigen::Vector3d t = (1.0 - alpha_timestamp) * t_begin + alpha_timestamp * t_end;
                keypoint.pt = R * keypoint.raw_pt + t;
            }
            auto update_step = std::chrono::steady_clock::now();
            std::chrono::duration<double> _elapsed_update = update_step - solve_step;
            elapsed_update += _elapsed_update.count() * 1000.0;

            if ((index_frame > 1) && (x_bundle.norm() < options.threshold_orientation_norm)) {
                summary.success = true;
                summary.num_residuals_used = number_keypoints_used;

                return summary;
            }
        }

        if (options.debug_print) {
            std::cout << "Elapsed Normals: " << elapsed_normals << std::endl;
            std::cout << "Elapsed Search Neighbors: " << elapsed_search_neighbors << std::endl;
            std::cout << "Elapsed A Construction: " << elapsed_A_construction << std::endl;
            std::cout << "Elapsed Select closest: " << elapsed_select_closest_neighbors << std::endl;
            std::cout << "Elapsed Solve: " << elapsed_solve << std::endl;
            std::cout << "Elapsed Solve: " << elapsed_update << std::endl;
            std::cout << "Number iterations CT-ICP : " << options.num_iters_icp << std::endl;
        }
        summary.success = true;
        summary.num_residuals_used = number_keypoints_used;

        return summary;
    }

} // namespace Elastic_ICP


#include <vector>
#include <string>
#include <iostream>
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <visualization_msgs/Marker.h>
#include <geometry_msgs/Point.h>
#include <laser_geometry/laser_geometry.h>

#include <tf/transform_listener.h>
#include <sensor_msgs/PointCloud2.h>

#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>

// #include <pcl/point_cloud.h>
// #include <pcl/point_types.h>
// #include <pcl/io/pcd_io.h>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <Eigen/Eigen>
#include "ceres/ceres.h"
#include <csm/csm_all.h>  // csm defines min and max, but Eigen complains

// #include <ros/log.h>
// #include <cmath>
using namespace ceres;
using namespace std;
struct CostFunctor {
	CostFunctor(double k1, double k2): k1_(k1), k2_(k2)
	{}
	template <typename T>
	bool operator()(const T* const main_k, T* residual) const {
		// residual[0] = T(10.0) - x[0];

		T r1 = T(k1_) - main_k[0] ;
		T r2 = T(k1_) + T(1) / main_k[0] ;

		if (r1 * r1 < r2 * r2)
		{
			T r = T(k2_) + T(1) / main_k[0] ;

			residual[0] = r1 * r1 + r * r;
			// residual[1] = r;
		}
		else
		{
			T r = T(k1_) + T(1) / main_k[0] ;

			residual[0] = r2 * r2 + r * r;
			// T r = T(k1_) - main_k[1];
			// residual[1] = r;
		}

		return true;
	}
	double k1_ , k2_;
};
void laserScanToLDP(const sensor_msgs::LaserScan::ConstPtr& scan, LDP& ldp)
{
	unsigned int n = scan->ranges.size();
	ldp = ld_alloc_new(n);

	for (unsigned int i = 0; i < n; i++)
	{
		// Calculate position in laser frame
		double r = scan->ranges[i];
		if ((r > scan->range_min) && (r < scan->range_max))
		{
			// Fill in laser scan data
			ldp->valid[i] = 1;
			ldp->readings[i] = r;
		}
		else
		{
			ldp->valid[i] = 0;
			ldp->readings[i] = -1;  // for invalid range
		}
		ldp->theta[i] = scan->angle_min + i * scan->angle_increment;
		ldp->cluster[i] = -1;
	}
	ROS_WARN("scan size is %d", ldp->cluster[0]);

	ldp->min_theta = ldp->theta[0];
	ldp->max_theta = ldp->theta[n - 1];

	ldp->odometry[0] = 0.0;
	ldp->odometry[1] = 0.0;
	ldp->odometry[2] = 0.0;

	ldp->true_pose[0] = 0.0;
	ldp->true_pose[1] = 0.0;
	ldp->true_pose[2] = 0.0;
}
double get_laser_line(sm_params& input_)
{
	boost::shared_ptr<sensor_msgs::LaserScan const> scan_ptr;    // 有const
	while (scan_ptr == nullptr)
		scan_ptr = ros::topic::waitForMessage<sensor_msgs::LaserScan>("scan", ros::Duration(10));

	input_.max_angular_correction_deg = 90;
	input_.restart = 1;
	input_.laser[0] = 0.0;
	input_.laser[1] = 0.0;
	input_.laser[2] = 0.0;
	tf::TransformListener tfListener_;
	double main_k[2] = {1, -1};
	if (scan_ptr != NULL)
	{
		sensor_msgs::PointCloud2 cloud;
		laser_geometry::LaserProjection projector_;
		projector_.transformLaserScanToPointCloud("base_scan", *scan_ptr, cloud, tfListener_);
		// input_.min_reading = scan_ptr->range_min;
		// input_.max_reading = scan_ptr->range_max;
		LDP curr_ldp_scan, prev_ldp_scan_;
		laserScanToLDP(scan_ptr, prev_ldp_scan_);
		// prev_ldp_scan_->odometry[0] = 0.0;
		// prev_ldp_scan_->odometry[1] = 0.0;
		// prev_ldp_scan_->odometry[2] = 0.0;

		prev_ldp_scan_->estimate[0] = 0.0;
		prev_ldp_scan_->estimate[1] = 0.0;
		prev_ldp_scan_->estimate[2] = 0.0;
		input_.first_guess[0] = 0.0;
		input_.first_guess[1] = 0.0;
		// input_.first_guess[2] = 0.0;
		prev_ldp_scan_->true_pose[0] = 0.0;
		prev_ldp_scan_->true_pose[1] = 0.0;
		prev_ldp_scan_->true_pose[2] = 0.0;
		int row_step = cloud.row_step;
		int height = cloud.height;

		/*将 sensor_msgs::PointCloud2 转换为　pcl::PointCloud<T> */
		//注意要用fromROSMsg函数需要引入pcl_versions（见头文件定义）
		pcl::PointCloud<pcl::PointXYZ> rawCloud;
		pcl::fromROSMsg(cloud, rawCloud);
		double min_x = 30, min_y = 30, max_x = -1, max_y = -1;
		for (size_t i = 0; i < rawCloud.points.size(); i++) {
			// std::cout << rawCloud.points[i].x << "\t" << rawCloud.points[i].y << "\t" << rawCloud.points[i].z << std::endl;
			double x = rawCloud.points[i].x;
			double y = rawCloud.points[i].y;
			min_y = min_y < y ? min_y : y;
			min_x = min_x < x ? min_x : x;
			max_y = max_y > y ? max_y : y;
			max_x = max_x > x ? max_x : x;
		}
		ROS_INFO("fuch");
		// std::cout << rawCloud.points[0].x << "\t" << rawCloud.points[0].y << "\t" << rawCloud.points[0].z << std::endl;
		double or_x = min_x - 4;
		double or_y = min_y - 4;
		int h = (max_y - min_y + 8) / 0.05 ;
		int width = (max_x - min_x + 8) / 0.05 ;
		cv::Mat tmp_line = cv::Mat::zeros(cv::Size(width, h), CV_8UC1);
		std::vector<cv::Point> laser_pts;
		for (size_t i = 0; i < rawCloud.points.size(); i++) {
			// std::cout << rawCloud.points[i].x << "\t" << rawCloud.points[i].y << "\t" << rawCloud.points[i].z << std::endl;
			double x = rawCloud.points[i].x;
			double y = rawCloud.points[i].y;
			int cell_x = (x - or_x) / 0.05;
			int cell_y =  h -  (y - or_y) / 0.05;
			// tmp_line.at<uchar>(cell_y,cell_x) =

			laser_pts.push_back(cv::Point(cell_x, cell_y));
		}
		std::vector<Eigen::Vector3d> line_params;
		// ROS_INFO("line size:%d", laser_pts.size());

		// cv::Point last_pt ;
		int first = 1;
		int count = 0;
		Eigen::Vector3d tmp_line_param;
		std::vector<cv::Point> filter_laser_pts_;
		// std::vector<std::vector<cv::Point>> best_pts;
		cv::Mat show = tmp_line.clone();
		for (auto&pt : laser_pts)
		{
			cv::circle(show, cv::Point(pt.x, pt.y), 2, cv::Scalar(255, 255, 255), -1);

			if (filter_laser_pts_.size() <= 1 || filter_laser_pts_.size() % 2 != 0)
			{
				filter_laser_pts_.push_back(pt);
			}
			else
			{
				auto& last_pt = filter_laser_pts_.back();
				auto& line_start_pt = filter_laser_pts_[filter_laser_pts_.size() - 2];
				double A = last_pt.y - line_start_pt.y;
				double B = line_start_pt.x - last_pt.x;
				double C = last_pt.x * line_start_pt.y
				           - line_start_pt.x * last_pt.y;
				double dist = fabs(A * pt.x * 1.0 + B * pt.y * 1.0 + C)
				              / ceres::sqrt(A * A + B * B);
				// ROS_INFO("line dst:%f", dist);
				// std::cerr << "dst is " << dist * 0.05 << std::endl;
				if (dist * 0.05 <= 0.1)
				{
					// std::cerr << "pop dst is " << dist << std::endl;

					filter_laser_pts_.pop_back();
				}
				else
				{

					// if (dist < 0.789)
					// {
					// 	double pt_dst = sqrt(pow(last_pt.x - pt.x, 2) +
					// 	                     pow(last_pt.y - pt.y, 2));
					// 	if (pt_dst < 0.789)
					// 	{
					// 		filter_laser_pts_.push_back(last_pt);

					// 		filter_laser_pts_.push_back(pt);
					// 	}

					// }
				}
				filter_laser_pts_.push_back(pt);
			}
			cv::imshow("test", show);
			cv::waitKey(1);
		}
		// if (filter_laser_pts_.size() % 2 != 0)
		// {
		// 	filter_laser_pts_.push_back(laser_pts.back());
		// }
		// for (auto&pt : filter_laser_pts_)
		// {
		// 	cv::circle(tmp_line, cv::Point(pt.x, pt.y), 2, cv::Scalar(255, 255, 255), -1);
		// 	// cv::imshow("test", tmp_line);
		// 	// cv::waitKey(5);
		// }
		// std::vector<Eigen::Vector3d> line_params;
		for (int i = 0; i < filter_laser_pts_.size() - 2; i = i + 2)
		{
			// cv::line(tmp_line, filter_laser_pts_[i], filter_laser_pts_[i + 1], cv::Scalar(180), 2);
			// cv::imshow("test", tmp_line);
			// cv::waitKey(15);
			double A = filter_laser_pts_[i].y - filter_laser_pts_[i + 1].y;
			double B = filter_laser_pts_[i + 1].x - filter_laser_pts_[i].x;
			double C = filter_laser_pts_[i].x * filter_laser_pts_[i + 1].y
			           - filter_laser_pts_[i + 1].x * filter_laser_pts_[i].y;
			line_params.push_back(Eigen::Vector3d(A, B, C));
		}
		std::vector<std::vector<cv::Point> > final_lines;
		std::vector<Eigen::Vector3d> final_params;
		for (auto&line : line_params)
		{
			std::vector<cv::Point> pt_line;
			double A = line[0];
			double B = line[1];
			double C = line[2];
			auto pt = laser_pts.begin();
			while (pt != laser_pts.end())
			{
				double dist = fabs(A * pt->x * 1.0 + B * pt->y * 1.0 + C)
				              / ceres::sqrt(A * A + B * B);
				if (dist * 0.05 < 0.15)
				{
					pt_line.push_back(*pt);
					pt = laser_pts.erase(pt);
				}
				else
					pt++;
			}
			if (pt_line.size() > 30)
			{
				final_lines.push_back(pt_line);
				final_params.push_back(line);
			}
		}
		cerr << "finale line size is " << final_lines.size() << endl;

		for (auto&pts : final_lines)
		{
			for (auto&pt : pts)
			{
				cv::circle(tmp_line, cv::Point(pt.x, pt.y), 2, cv::Scalar(255, 255, 255), -1);
			}

			// cv::imshow("test", tmp_line);
			// cv::waitKey(1);
		}

		for (auto&line : final_params)
		{

		}

		// cv::imshow("test", tmp_line);
		// cv::waitKey(1);
		double main_k1 = 1;
		double main_k2 = -1;

		// double main_k[2] = { -line_params.front()[0] / line_params.front()[1], line_params.front()[1] / line_params.front()[0]};

		Problem problem;

		// Set up the only cost function (also known as residual). This uses
		// auto-differentiation to obtain the derivative (jacobian).
		// CostFunction* cost_function =
		//     new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
		// problem.AddResidualBlock(cost_function, NULL, &x);

		// // Run the solver!
		Solver::Options options;
		// options.linear_solver_type = ceres::DENSE_QR;
		options.minimizer_progress_to_stdout = false;
		Solver::Summary summary;
		for (auto&params : final_params)
		{
			if (params[1] == 0)
			{
				params[1] = 0.000001;
			}
			if (params[0] == 0)
			{
				params[0] = 0.000001;
			}
			cout << "origin angle1 is " << -params[0] / params[1] << endl;
			cout << "origin angle2 is " << params[1] / params[0] << endl;
			problem.AddResidualBlock (     // 向问题中添加误差项
			    // 使用自动求导，模板参数：误差类型，输出维度，输入维度，维数要与前面struct中一致
			    new ceres::AutoDiffCostFunction<CostFunctor, 1, 2> (
			        new CostFunctor ( -params[0] / params[1], params[1] / params[0] )
			    ),
			    new ceres::CauchyLoss(0.5),            // 核函数，这里不使用，为空
			    main_k                 // 待估计参数
			);
		}
		Solve(options, &problem, &summary);
		// cout << summary.BriefReport() << endl;
		cout << "angle1 is " << main_k[0] << endl;
		cout << "angle2 is " << main_k[1] << endl;
		input_.laser_ref = prev_ldp_scan_;

	}

	double line1 = ceres::atan(main_k[0]);
	double line2 =  ceres::atan(-1 / main_k[0]);
	double angle;
	if (fabs(line1) < fabs(line2))
	{
		angle = line1;
	}
	else
	{
		angle = line2;
	}
	return angle;
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "laser_detector");

	ros::NodeHandle nh;
	sm_params input_;

	{
		if (!nh.getParam ("max_angular_correction_deg", input_.max_angular_correction_deg))
			input_.max_angular_correction_deg = 100.0;

		// Maximum translation between scans (m)
		if (!nh.getParam ("max_linear_correction", input_.max_linear_correction))
			input_.max_linear_correction = 0.50;

		// Maximum ICP cycle iterations
		if (!nh.getParam ("max_iterations", input_.max_iterations))
			input_.max_iterations = 10;

		// A threshold for stopping (m)
		if (!nh.getParam ("epsilon_xy", input_.epsilon_xy))
			input_.epsilon_xy = 0.000001;

		// A threshold for stopping (rad)
		if (!nh.getParam ("epsilon_theta", input_.epsilon_theta))
			input_.epsilon_theta = 0.000001;

		// Maximum distance for a correspondence to be valid
		if (!nh.getParam ("max_correspondence_dist", input_.max_correspondence_dist))
			input_.max_correspondence_dist = 0.3;

		// Noise in the scan (m)
		if (!nh.getParam ("sigma", input_.sigma))
			input_.sigma = 0.010;

		// Use smart tricks for finding correspondences.
		if (!nh.getParam ("use_corr_tricks", input_.use_corr_tricks))
			input_.use_corr_tricks = 1;

		// Restart: Restart if error is over threshold
		if (!nh.getParam ("restart", input_.restart))
			input_.restart = 0;

		// Restart: Threshold for restarting
		if (!nh.getParam ("restart_threshold_mean_error", input_.restart_threshold_mean_error))
			input_.restart_threshold_mean_error = 0.01;

		// Restart: displacement for restarting. (m)
		if (!nh.getParam ("restart_dt", input_.restart_dt))
			input_.restart_dt = 1.0;

		// Restart: displacement for restarting. (rad)
		if (!nh.getParam ("restart_dtheta", input_.restart_dtheta))
			input_.restart_dtheta = 0.1;

		// Max distance for staying in the same clustering
		if (!nh.getParam ("clustering_threshold", input_.clustering_threshold))
			input_.clustering_threshold = 0.25;

		// Number of neighbour rays used to estimate the orientation
		if (!nh.getParam ("orientation_neighbourhood", input_.orientation_neighbourhood))
			input_.orientation_neighbourhood = 20;

		// If 0, it's vanilla ICP
		if (!nh.getParam ("use_point_to_line_distance", input_.use_point_to_line_distance))
			input_.use_point_to_line_distance = 1;

		// Discard correspondences based on the angles
		if (!nh.getParam ("do_alpha_test", input_.do_alpha_test))
			input_.do_alpha_test = 0;

		// Discard correspondences based on the angles - threshold angle, in degrees
		if (!nh.getParam ("do_alpha_test_thresholdDeg", input_.do_alpha_test_thresholdDeg))
			input_.do_alpha_test_thresholdDeg = 20.0;

		// Percentage of correspondences to consider: if 0.9,
		// always discard the top 10% of correspondences with more error
		if (!nh.getParam ("outliers_maxPerc", input_.outliers_maxPerc))
			input_.outliers_maxPerc = 0.90;

		// Parameters describing a simple adaptive algorithm for discarding.
		//  1) Order the errors.
		//  2) Choose the percentile according to outliers_adaptive_order.
		//     (if it is 0.7, get the 70% percentile)
		//  3) Define an adaptive threshold multiplying outliers_adaptive_mult
		//     with the value of the error at the chosen percentile.
		//  4) Discard correspondences over the threshold.
		//  This is useful to be conservative; yet remove the biggest errors.
		if (!nh.getParam ("outliers_adaptive_order", input_.outliers_adaptive_order))
			input_.outliers_adaptive_order = 0.7;

		if (!nh.getParam ("outliers_adaptive_mult", input_.outliers_adaptive_mult))
			input_.outliers_adaptive_mult = 2.0;

		// If you already have a guess of the solution, you can compute the polar angle
		// of the points of one scan in the new position. If the polar angle is not a monotone
		// function of the readings index, it means that the surface is not visible in the
		// next position. If it is not visible, then we don't use it for matching.
		if (!nh.getParam ("do_visibility_test", input_.do_visibility_test))
			input_.do_visibility_test = 0;

		// no two points in laser_sens can have the same corr.
		if (!nh.getParam ("outliers_remove_doubles", input_.outliers_remove_doubles))
			input_.outliers_remove_doubles = 1;

		// If 1, computes the covariance of ICP using the method http://purl.org/censi/2006/icpcov
		if (!nh.getParam ("do_compute_covariance", input_.do_compute_covariance))
			input_.do_compute_covariance = 0;

		// Checks that find_correspondences_tricks gives the right answer
		if (!nh.getParam ("debug_verify_tricks", input_.debug_verify_tricks))
			input_.debug_verify_tricks = 0;

		// If 1, the field 'true_alpha' (or 'alpha') in the first scan is used to compute the
		// incidence beta, and the factor (1/cos^2(beta)) used to weight the correspondence.");
		if (!nh.getParam ("use_ml_weights", input_.use_ml_weights))
			input_.use_ml_weights = 0;

		// If 1, the field 'readings_sigma' in the second scan is used to weight the
		// correspondence by 1/sigma^2
		if (!nh.getParam ("use_sigma_weights", input_.use_sigma_weights))
			input_.use_sigma_weights = 0;
	}
	ros::Publisher cmd_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 100);

	{
		LDP curr_ldp_scan;
		double angle = get_laser_line(input_);
		cerr << "tmp value is " << angle << endl;
		// double line1 = ceres::atan(main_k);
		// double line2 =  ceres::atan(-1 / main_k);
		// double angle;
		// if (fabs(line1) < fabs(line2))
		// {
		// 	angle = line1;
		// }
		// else
		// {
		// 	angle = line2;
		// }
		// double angle = (ceres::atan( -final_params.back()[0] / final_params.back()[1]));
		cerr << 180 * angle / M_PI << endl;
		geometry_msgs::Twist tmp ;
		if (angle > 0)
		{
			tmp.angular.z = -0.03;
			angle *= -1;
		}
		else
		{
			tmp.angular.z = 0.03;

			angle *= -1;
		}

		input_.first_guess[2] = angle;
		double t = fabs(angle) / 0.03 ;
		double t0 = (ros::Time::now()).toSec();
		cerr << t << endl;

		sm_result output_;
		output_.cov_x_m = 0;
		output_.dx_dy1_m = 0;
		output_.dx_dy2_m = 0;
		while (true)
		{
			// double tt = (ros::Time::now()).toSec();
			double t1 = (ros::Time::now()).toSec();
			cmd_pub_.publish(tmp);
			if (t1 - t0 > t)
			{

				cerr << t1 - t0 << endl;
				tmp.angular.z = 0;
				cmd_pub_.publish(tmp);

				boost::shared_ptr<sensor_msgs::LaserScan const> new_scan_ptr = nullptr;  // 有const
				cerr << "restart" << endl;
				while (new_scan_ptr == nullptr) {
					cerr << "wait" << endl;
					new_scan_ptr = ros::topic::waitForMessage<sensor_msgs::LaserScan>("scan", ros::Duration(10));
				}

				laserScanToLDP(new_scan_ptr, curr_ldp_scan);
				input_.laser_sens = curr_ldp_scan;
				cerr << "bug-1" << endl;

				if (output_.cov_x_m)
				{
					gsl_matrix_free(output_.cov_x_m);
					output_.cov_x_m = 0;
				}
				if (output_.dx_dy1_m)
				{
					gsl_matrix_free(output_.dx_dy1_m);
					output_.dx_dy1_m = 0;
				}
				if (output_.dx_dy2_m)
				{
					gsl_matrix_free(output_.dx_dy2_m);
					output_.dx_dy2_m = 0;
				}
				cerr << "bug-21" << endl;
				sm_icp(&input_, &output_);
				cerr << "bug-22" << endl;
				double match_angle;

				if (output_.valid)
				{
					// the correction of the laser's position, in the laser frame
					match_angle = output_.x[2];
					input_.first_guess[2] = match_angle;

					if (fabs(match_angle - angle) <= 0.01)
					{
						break;
					}
					else
					{
						if (angle > 0)
						{
							double delta_angle = angle - match_angle ;
							t = fabs(delta_angle) / 0.03;
							t0 = (ros::Time::now()).toSec();
							tmp.angular.z = delta_angle < 0 ? -0.03 : 0.03;
						}
						else
						{
							double delta_angle = angle - match_angle ;
							t = fabs(delta_angle) / 0.03;
							t0 = (ros::Time::now()).toSec();
							tmp.angular.z = delta_angle < 0 ? 0.03 : -0.03;
						}
					}
				}
				else
				{
					ROS_WARN("Error in scan matching");
					cerr << "none result" << endl;
					break;
				}

			}
			// double tt1 = (ros::Time::now()).toSec();
			// cerr <<"time is " << tt1 - tt << endl;
			// break;
		}

		// cmd_pub_.publish(tmp);
		cerr << "over" << endl;

	}

	// ros::spinOnce();

	cv::waitKey(1);
	ros::shutdown();
	return 0;
}


/**
 * trajectory generator for testing the visual ekf
 */
#include <iostream>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/TwistStamped.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "qp_generator.h"

using namespace std;
using namespace Eigen;
ros::Publisher imu_pub, visual_pub, debug_pub;

const int update_freq = 400; // 1 / dt
const double dt = 0.0025;

void pub_imu(const Vector3d& acc, const Quaterniond& orientation){
    sensor_msgs::Imu imu;
    imu.header.stamp = ros::Time::now();
    imu.orientation.w = orientation.w();
    imu.orientation.x = orientation.x();
    imu.orientation.y = orientation.y();
    imu.orientation.z = orientation.z();
    imu.linear_acceleration.x = acc[0];
    imu.linear_acceleration.y = acc[1];
    imu.linear_acceleration.z = acc[2] + 9.8;

    imu_pub.publish(imu);
}

/**
 * Change the world frame position to the camera frame, and publish in millimeter
 * @param pos
 */
void pub_visual(const Vector3d& pos) {
    geometry_msgs::TwistStamped pose;
    pose.header.stamp = ros::Time::now();

    MatrixXd imu_R_camera = MatrixXd::Identity(3, 3);
    imu_R_camera <<  0, 0, 1,
                    -1, 0, 0,
                     0,-1, 0;

    Vector3d pos_camera = imu_R_camera.transpose() * (1000 * pos);
    pose.twist.linear.x = pos_camera[0];
    pose.twist.linear.y = pos_camera[1];
    pose.twist.linear.z = pos_camera[2];

    visual_pub.publish(pose);
}

void pub_debug(const Vector3d& pos, const Vector3d& vel, const Vector3d& acc)
{
    nav_msgs::Odometry odom;
    odom.header.stamp = ros::Time::now();

    odom.child_frame_id = "world";
    odom.pose.pose.position.x = pos[0];
    odom.pose.pose.position.y = pos[1];
    odom.pose.pose.position.z = pos[2];
    odom.twist.twist.linear.x = vel[0];
    odom.twist.twist.linear.y = vel[1];
    odom.twist.twist.linear.z = vel[2];
    odom.twist.twist.angular.x= acc[0];
    odom.twist.twist.angular.y= acc[1];
    odom.twist.twist.angular.z= acc[2];

    debug_pub.publish(odom);
}

// get position from coefficient
void getPositionFromCoeff(Eigen::Vector3d &pos, const Eigen::MatrixXd &coeff,
                                             const int &index, const double &time)
{
    int s = index;
    double t = time;
    double x = coeff(s, 0) + coeff(s, 1) * t + coeff(s, 2) * pow(t, 2) + coeff(s, 3) * pow(t, 3) +
               coeff(s, 4) * pow(t, 4) + coeff(s, 5) * pow(t, 5);
    double y = coeff(s, 6) + coeff(s, 7) * t + coeff(s, 8) * pow(t, 2) + coeff(s, 9) * pow(t, 3) +
               coeff(s, 10) * pow(t, 4) + coeff(s, 11) * pow(t, 5);
    double z = coeff(s, 12) + coeff(s, 13) * t + coeff(s, 14) * pow(t, 2) + coeff(s, 15) * pow(t, 3) +
               coeff(s, 16) * pow(t, 4) + coeff(s, 17) * pow(t, 5);

    pos(0) = x;
    pos(1) = y;
    pos(2) = z;
}

// get velocity from cofficient
void getVelocityFromCoeff(Eigen::Vector3d &vel, const Eigen::MatrixXd &coeff,
                                             const int &index, const double &time)
{
    int s = index;
    double t = time;
    double vx = coeff(s, 1) + 2 * coeff(s, 2) * pow(t, 1) + 3 * coeff(s, 3) * pow(t, 2) +
                4 * coeff(s, 4) * pow(t, 3) + 5 * coeff(s, 5) * pow(t, 4);
    double vy = coeff(s, 7) + 2 * coeff(s, 8) * pow(t, 1) + 3 * coeff(s, 9) * pow(t, 2) +
                4 * coeff(s, 10) * pow(t, 3) + 5 * coeff(s, 11) * pow(t, 4);
    double vz = coeff(s, 13) + 2 * coeff(s, 14) * pow(t, 1) + 3 * coeff(s, 15) * pow(t, 2) +
                4 * coeff(s, 16) * pow(t, 3) + 5 * coeff(s, 17) * pow(t, 4);

    vel(0) = vx;
    vel(1) = vy;
    vel(2) = vz;
}

// get acceleration from coefficient
void getAccelerationFromCoeff(Eigen::Vector3d &acc, const Eigen::MatrixXd &coeff,
                                                 const int &index, const double &time)
{
    int s = index;
    double t = time;
    double ax = 2 * coeff(s, 2) + 6 * coeff(s, 3) * pow(t, 1) + 12 * coeff(s, 4) * pow(t, 2) +
               20 * coeff(s, 5) * pow(t, 3);
    double ay = 2 * coeff(s, 8) + 6 * coeff(s, 9) * pow(t, 1) + 12 * coeff(s, 10) * pow(t, 2) +
               20 * coeff(s, 11) * pow(t, 3);
    double az = 2 * coeff(s, 14) + 6 * coeff(s, 15) * pow(t, 1) + 12 * coeff(s, 16) * pow(t, 2) +
               20 * coeff(s, 17) * pow(t, 3);

    acc(0) = ax;
    acc(1) = ay;
    acc(2) = az;
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "visual_ekf_tester");
    ros::NodeHandle n("~");

    imu_pub    = n.advertise<sensor_msgs::Imu>("/visual_ekf/test_imu", 400);
    visual_pub = n.advertise<geometry_msgs::TwistStamped>("/visual_ekf/test_visual", 50);
    debug_pub  = n.advertise<nav_msgs::Odometry>("/visual_ekf_tester/odom", 50);

    ros::Rate r(update_freq);

    MatrixXd Path = MatrixXd::Zero(3, 3);
    Path.block<1, 3>(0, 0) = Vector3d(0.6, 0.2, 0.1);
    Path.block<1, 3>(1, 0) = Vector3d(0.8, 0.2, 0.1);
    Path.block<1, 3>(2, 0) = Vector3d(1, 0, 0);

    Vector3d Vel_init(0, 0, 0);
    Vector3d Acc_init(0, 0, 0);
    VectorXd time_period = MatrixXd::Zero(2, 1);
    time_period << 5.0, 5.0;
    VectorXd time_int = time_period;

    // generate a smooth curve
    TrajectoryGenerator generator;
    MatrixXd coeff = generator.PolyQPGeneration(
            Path, Vel_init, Acc_init, time_period, 1);
    cout << "polynomial " << endl << coeff << endl;

    auto t_start = ros::Time::now().toSec();
    int iterator = 0;
    auto time_period_size = (int)time_period.size();

    // provide the orientation, rotate in Z axis first
    Vector3d axis_z = Vector3d::UnitZ();
    VectorXd theta = MatrixXd::Zero(3, 1);
    theta[0] = 0.0;
    theta[1] = 0.1 * M_PI;
    theta[2] = 0.25 * M_PI;

    while (ros::ok()) {

        double dt = ros::Time::now().toSec() - t_start;

        if (iterator < time_period_size) {
            if (dt > time_period[iterator]) {
                dt -= time_period[iterator];
                t_start += time_period[iterator];
                iterator++;
                ROS_INFO("In polynomial section %d", iterator);
            }
        }

        if (iterator < time_period_size) {
            Vector3d pos, vel, acc;
            getPositionFromCoeff(pos, coeff, iterator, dt);
            getVelocityFromCoeff(vel, coeff, iterator, dt);
            getAccelerationFromCoeff(acc, coeff, iterator, dt);

            double omg = (theta[iterator+1] - theta[iterator]) / time_period[iterator];
            double angle = omg * dt + theta[iterator];
            AngleAxisd orientation(angle, axis_z);
            Quaterniond q(orientation);

            Vector3d acc_imu = q.conjugate() * acc;
            pub_imu(acc_imu, q);

            Vector3d pos_imu = q.conjugate() * pos;
            pub_visual(pos_imu);

            Vector3d vel_imu = q.conjugate() * vel;
            pub_debug(pos_imu, vel_imu, acc_imu);
        }

        r.sleep();
        ros::spinOnce();
    }
}


// Copyright (c) 2008-2016 Emil Dotchevski and Reverge Studios, Inc.

// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef UUID_5FD6A664ACC811DEAAFF8A8055D89593
#define UUID_5FD6A664ACC811DEAAFF8A8055D89593

#include <boost/qvm/inline.hpp>
#include <math.h>

namespace boost {
namespace qvm {
template <class T> T acos(T);
template <class T> T asin(T);
template <class T> T atan(T);
template <class T> T atan2(T, T);
template <class T> T cos(T);
template <class T> T sin(T);
template <class T> T tan(T);
template <class T> T cosh(T);
template <class T> T sinh(T);
template <class T> T tanh(T);
template <class T> T exp(T);
template <class T> T log(T);
template <class T> T log10(T);
template <class T> T mod(T, T);
template <class T> T pow(T, T);
template <class T> T sqrt(T);
template <class T> T ceil(T);
template <class T> T abs(T);
template <class T> T floor(T);
template <class T> T mod(T, T);
template <class T> T ldexp(T, int);
template <class T> T sign(T);

template <> BOOST_QVM_INLINE_TRIVIAL float acos<float>(float x) {
  return ::acosf(x);
}
template <> BOOST_QVM_INLINE_TRIVIAL float asin<float>(float x) {
  return ::asinf(x);
}
template <> BOOST_QVM_INLINE_TRIVIAL float atan<float>(float x) {
  return ::atanf(x);
}
template <> BOOST_QVM_INLINE_TRIVIAL float atan2<float>(float x, float y) {
  return ::atan2f(x, y);
}
template <> BOOST_QVM_INLINE_TRIVIAL float cos<float>(float x) {
  return ::cosf(x);
}
template <> BOOST_QVM_INLINE_TRIVIAL float sin<float>(float x) {
  return ::sinf(x);
}
template <> BOOST_QVM_INLINE_TRIVIAL float tan<float>(float x) {
  return ::tanf(x);
}
template <> BOOST_QVM_INLINE_TRIVIAL float cosh<float>(float x) {
  return ::coshf(x);
}
template <> BOOST_QVM_INLINE_TRIVIAL float sinh<float>(float x) {
  return ::sinhf(x);
}
template <> BOOST_QVM_INLINE_TRIVIAL float tanh<float>(float x) {
  return ::tanhf(x);
}
template <> BOOST_QVM_INLINE_TRIVIAL float exp<float>(float x) {
  return ::expf(x);
}
template <> BOOST_QVM_INLINE_TRIVIAL float log<float>(float x) {
  return ::logf(x);
}
template <> BOOST_QVM_INLINE_TRIVIAL float log10<float>(float x) {
  return ::log10f(x);
}
template <> BOOST_QVM_INLINE_TRIVIAL float mod<float>(float x, float y) {
  return ::fmodf(x, y);
}
template <> BOOST_QVM_INLINE_TRIVIAL float pow<float>(float x, float y) {
  return ::powf(x, y);
}
template <> BOOST_QVM_INLINE_TRIVIAL float sqrt<float>(float x) {
  return ::sqrtf(x);
}
template <> BOOST_QVM_INLINE_TRIVIAL float ceil<float>(float x) {
  return ::ceilf(x);
}
template <> BOOST_QVM_INLINE_TRIVIAL float abs<float>(float x) {
  return ::fabsf(x);
}
template <> BOOST_QVM_INLINE_TRIVIAL float floor<float>(float x) {
  return ::floorf(x);
}
template <> BOOST_QVM_INLINE_TRIVIAL float ldexp<float>(float x, int y) {
  return ::ldexpf(x, y);
}
template <> BOOST_QVM_INLINE_TRIVIAL float sign<float>(float x) {
  return x < 0 ? -1.f : +1.f;
}

template <> BOOST_QVM_INLINE_TRIVIAL double acos<double>(double x) {
  return ::acos(x);
}
template <> BOOST_QVM_INLINE_TRIVIAL double asin<double>(double x) {
  return ::asin(x);
}
template <> BOOST_QVM_INLINE_TRIVIAL double atan<double>(double x) {
  return ::atan(x);
}
template <> BOOST_QVM_INLINE_TRIVIAL double atan2<double>(double x, double y) {
  return ::atan2(x, y);
}
template <> BOOST_QVM_INLINE_TRIVIAL double cos<double>(double x) {
  return ::cos(x);
}
template <> BOOST_QVM_INLINE_TRIVIAL double sin<double>(double x) {
  return ::sin(x);
}
template <> BOOST_QVM_INLINE_TRIVIAL double tan<double>(double x) {
  return ::tan(x);
}
template <> BOOST_QVM_INLINE_TRIVIAL double cosh<double>(double x) {
  return ::cosh(x);
}
template <> BOOST_QVM_INLINE_TRIVIAL double sinh<double>(double x) {
  return ::sinh(x);
}
template <> BOOST_QVM_INLINE_TRIVIAL double tanh<double>(double x) {
  return ::tanh(x);
}
template <> BOOST_QVM_INLINE_TRIVIAL double exp<double>(double x) {
  return ::exp(x);
}
template <> BOOST_QVM_INLINE_TRIVIAL double log<double>(double x) {
  return ::log(x);
}
template <> BOOST_QVM_INLINE_TRIVIAL double log10<double>(double x) {
  return ::log10(x);
}
template <> BOOST_QVM_INLINE_TRIVIAL double mod<double>(double x, double y) {
  return ::fmod(x, y);
}
template <> BOOST_QVM_INLINE_TRIVIAL double pow<double>(double x, double y) {
  return ::pow(x, y);
}
template <> BOOST_QVM_INLINE_TRIVIAL double sqrt<double>(double x) {
  return ::sqrt(x);
}
template <> BOOST_QVM_INLINE_TRIVIAL double ceil<double>(double x) {
  return ::ceil(x);
}
template <> BOOST_QVM_INLINE_TRIVIAL double abs<double>(double x) {
  return ::fabs(x);
}
template <> BOOST_QVM_INLINE_TRIVIAL double floor<double>(double x) {
  return ::floor(x);
}
template <> BOOST_QVM_INLINE_TRIVIAL double ldexp<double>(double x, int y) {
  return ::ldexp(x, y);
}
template <> BOOST_QVM_INLINE_TRIVIAL double sign<double>(double x) {
  return x < 0 ? -1.0 : +1.0;
}
} // namespace qvm
} // namespace boost

#endif


#include <iostream>
#include <fstream>
#include <vector>
#include <string>
#include <map>
#include <unordered_map>
#include <cstdlib>

#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>

#include "Day4.hpp"

bool isValidPassword(int number);

void day4() {
	std::ifstream inputFile("Data/Day4.txt");
	std::string line;
	std::getline(inputFile, line);

	std::vector<std::string> numbersStr;
	boost::split(numbersStr, line, boost::is_any_of("-"), boost::token_compress_on);

	std::vector<int> numbers;
	std::transform(numbersStr.begin(), numbersStr.end(), std::back_inserter(numbers), &boost::lexical_cast<int, std::string>);

	// Cheating
	int minNumber = numbers[0];
	int maxNumber = numbers[1];

	int countValidPassword = 0;
	for (int i = minNumber; i < maxNumber; ++i) {
		if (isValidPassword(i)) {
			countValidPassword++;
		}
	}

	std::cout << countValidPassword << std::endl;
}

bool isValidPassword(int number) {
	std::string numberStr = std::to_string(number);

	//It is a six - digit number.
	//The value is within the range given in your puzzle input.
	// Handled by for loop

	//Two adjacent digits are the same(like 22 in 122345).
	if (!(
		(										numberStr.at(0) == numberStr.at(1) && numberStr.at(1) != numberStr.at(2)) ||
		(numberStr.at(0) != numberStr.at(1) &&	numberStr.at(1) == numberStr.at(2) && numberStr.at(2) != numberStr.at(3)) ||
		(numberStr.at(1) != numberStr.at(2) &&	numberStr.at(2) == numberStr.at(3) && numberStr.at(3) != numberStr.at(4)) ||
		(numberStr.at(2) != numberStr.at(3) &&	numberStr.at(3) == numberStr.at(4) && numberStr.at(4) != numberStr.at(5)) ||
		(numberStr.at(3) != numberStr.at(4) &&	numberStr.at(4) == numberStr.at(5)										)
			)) {
			return false;
	}

	//Going from left to right, the digits never decrease; they only ever increase or stay the same(like 111123 or 135679).
	if (numberStr.at(0) > numberStr.at(1) ||
		numberStr.at(1) > numberStr.at(2) ||
		numberStr.at(2) > numberStr.at(3) ||
		numberStr.at(3) > numberStr.at(4) ||
		numberStr.at(4) > numberStr.at(5)) {
		return false;
	}

	return true;
}


#include <vector>
#include <iostream>
#include "../incl/graph_creator.h"
#include "../incl/bellman_ford.h"
#include "../incl/graph_printer.h"
#include "../incl/bellman_ford_test.h"
#include <boost/program_options.hpp>

unsigned long nodes;
unsigned long edges = 0;
int times;
int minWeight;
int maxWeight;

/**
 * Prints usage message.
*/
boost::program_options::variables_map parseArguments(int argc, char *const *argv)
{
    boost::program_options::options_description options("Usage");
    options.add_options()
            ("test", "run all tests")
            ("run", "runs with specified graph")
            ("benchmark", "runs benchmark with specified graph")
            ("help", "produce help message")
            ("random", "random connected graph")
            ("grid", "modified grid graph with negative cycle (worst case for bellman ford)")
            ("nodes,n", boost::program_options::value<unsigned long>(&nodes), "number of nodes (default=100)")
            ("edges,e", boost::program_options::value<unsigned long>(&edges), "number of edges for random graphs (default 20*nodes*log10(nodes))")
            ("times,t", boost::program_options::value<int>(&times), "number of times to run the benchmark")
            ("min-weight", boost::program_options::value<int>(&minWeight), "minimum edge weight (default=-100)")
            ("max-weight", boost::program_options::value<int>(&maxWeight), "maximum edge weight (default=10000)")
            ;

    boost::program_options::variables_map vm;
    store(parse_command_line(argc, argv, options), vm);
    notify(vm);

    if (vm.count("help") || argc < 2) {
        std::cout << options << "\n";
        exit(1);
    }

    return vm;
}

int main(int argc, char **argv)
{
    const int INF = (std::numeric_limits < int >::max)();
    nodes = 100;
    times = 1;
    minWeight = -100;
    maxWeight = 10000;

    boost::program_options::variables_map args = parseArguments(argc, argv);
    if (edges==0) {
        edges = static_cast<unsigned long>(20 * nodes * log10(nodes));
    }

    if (args.count("test")) {
        testAll();
    }
    else if (args.count("benchmark") && args.count("random")) {
        Graph G = createRandomGraph(nodes, edges, minWeight, maxWeight);
        std::cout << "[i] Benchmark random graph " << nodes << "x" << edges << std::endl;
        benchmark(G, times);
    }
    else if (args.count("benchmark") && args.count("grid")) {
        Graph G = createGridGraph(static_cast<int>(nodes), minWeight, maxWeight);
        std::cout << "[i] Benchmark grid graph " << nodes << "x" << nodes << std::endl;
        benchmark(G, times);
    }
    else if (args.count("run")) {
        Graph G;
        if (args.count("random")) {
            std::cout << "[i] Random graph " << nodes << "x" << edges << std::endl;
            G = createRandomGraph(nodes, edges, minWeight, maxWeight);
        }
        else if (args.count("grid")) {
            std::cout << "[i] Grid graph " << nodes << "x" << nodes << std::endl;
            G = createGridGraph(static_cast<int>(nodes), minWeight, maxWeight);
        }
        else {
            return 0;
        }

        unsigned long  N = num_vertices(G);
        std::vector<unsigned long> pred(N);
        std::vector<long> dist(N, INF);
        WeightMap weight = get(&EdgeProperties::weight, G);
        printGraphVizToFile(G, "graph.dot");
        bool res = bellmanFord(G, 0, weight, pred, dist);
        if (!res) {
            std::cout << "negative cycle detected" << std::endl;
        }
        std::vector<int> label = CHECK_BELLMAN_FORD(G, 0, weight, pred, dist);
        std::cout << "[+] Test OK!" << std::endl;
        printGraphShortestPath(G, dist, pred, label);
        printGraphShortestPathVizToFile(G, pred, "shortest-path.dot");
        std::cout << "Run the following (if you have graphviz installed) to see the graph images:" << std::endl;
        std::cout << "  neato -Tpng graph.dot -o graph.png && xdg-open  graph.png" << std::endl;
        std::cout << "  neato -Tpng shortest-path.dot -o shortest-path.png && xdg-open shortest-path.png" << std::endl;
    }

    return 0;
}


#include <pcl/features/moment_of_inertia_estimation.h>
#include <vector>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
#include <boost/thread/thread.hpp>

int main (int argc, char** argv)
{
  if (argc != 2)
    return (0);

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());
  if (pcl::io::loadPCDFile (argv[1], *cloud) == -1)
    return (-1);

  pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
  feature_extractor.setInputCloud (cloud);
  feature_extractor.compute ();

  std::vector <float> moment_of_inertia;
  std::vector <float> eccentricity;
  pcl::PointXYZ min_point_AABB;
  pcl::PointXYZ max_point_AABB;
  pcl::PointXYZ min_point_OBB;
  pcl::PointXYZ max_point_OBB;
  pcl::PointXYZ position_OBB;
  Eigen::Matrix3f rotational_matrix_OBB;
  float major_value, middle_value, minor_value;
  Eigen::Vector3f major_vector, middle_vector, minor_vector;
  Eigen::Vector3f mass_center;

  feature_extractor.getMomentOfInertia (moment_of_inertia);
  feature_extractor.getEccentricity (eccentricity);
  feature_extractor.getAABB (min_point_AABB, max_point_AABB);
  feature_extractor.getOBB (min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
  feature_extractor.getEigenValues (major_value, middle_value, minor_value);
  feature_extractor.getEigenVectors (major_vector, middle_vector, minor_vector);
  feature_extractor.getMassCenter (mass_center);

  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
  viewer->setBackgroundColor (0, 0, 0);
  viewer->addCoordinateSystem (1.0);
  viewer->initCameraParameters ();
  viewer->addPointCloud<pcl::PointXYZ> (cloud, "sample cloud");
  viewer->addCube (min_point_AABB.x, max_point_AABB.x, min_point_AABB.y, max_point_AABB.y, min_point_AABB.z, max_point_AABB.z, 1.0, 1.0, 0.0, "AABB");

  Eigen::Vector3f position (position_OBB.x, position_OBB.y, position_OBB.z);
  Eigen::Quaternionf quat (rotational_matrix_OBB);
  viewer->addCube (position, quat, max_point_OBB.x - min_point_OBB.x, max_point_OBB.y - min_point_OBB.y, max_point_OBB.z - min_point_OBB.z, "OBB");

  pcl::PointXYZ center (mass_center (0), mass_center (1), mass_center (2));
  pcl::PointXYZ x_axis (major_vector (0) + mass_center (0), major_vector (1) + mass_center (1), major_vector (2) + mass_center (2));
  pcl::PointXYZ y_axis (middle_vector (0) + mass_center (0), middle_vector (1) + mass_center (1), middle_vector (2) + mass_center (2));
  pcl::PointXYZ z_axis (minor_vector (0) + mass_center (0), minor_vector (1) + mass_center (1), minor_vector (2) + mass_center (2));
  viewer->addLine (center, x_axis, 1.0f, 0.0f, 0.0f, "major eigen vector");
  viewer->addLine (center, y_axis, 0.0f, 1.0f, 0.0f, "middle eigen vector");
  viewer->addLine (center, z_axis, 0.0f, 0.0f, 1.0f, "minor eigen vector");

  //Eigen::Vector3f p1 (min_point_OBB.x, min_point_OBB.y, min_point_OBB.z);
  //Eigen::Vector3f p2 (min_point_OBB.x, min_point_OBB.y, max_point_OBB.z);
  //Eigen::Vector3f p3 (max_point_OBB.x, min_point_OBB.y, max_point_OBB.z);
  //Eigen::Vector3f p4 (max_point_OBB.x, min_point_OBB.y, min_point_OBB.z);
  //Eigen::Vector3f p5 (min_point_OBB.x, max_point_OBB.y, min_point_OBB.z);
  //Eigen::Vector3f p6 (min_point_OBB.x, max_point_OBB.y, max_point_OBB.z);
  //Eigen::Vector3f p7 (max_point_OBB.x, max_point_OBB.y, max_point_OBB.z);
  //Eigen::Vector3f p8 (max_point_OBB.x, max_point_OBB.y, min_point_OBB.z);

  //p1 = rotational_matrix_OBB * p1 + position;
  //p2 = rotational_matrix_OBB * p2 + position;
  //p3 = rotational_matrix_OBB * p3 + position;
  //p4 = rotational_matrix_OBB * p4 + position;
  //p5 = rotational_matrix_OBB * p5 + position;
  //p6 = rotational_matrix_OBB * p6 + position;
  //p7 = rotational_matrix_OBB * p7 + position;
  //p8 = rotational_matrix_OBB * p8 + position;

  //pcl::PointXYZ pt1 (p1 (0), p1 (1), p1 (2));
  //pcl::PointXYZ pt2 (p2 (0), p2 (1), p2 (2));
  //pcl::PointXYZ pt3 (p3 (0), p3 (1), p3 (2));
  //pcl::PointXYZ pt4 (p4 (0), p4 (1), p4 (2));
  //pcl::PointXYZ pt5 (p5 (0), p5 (1), p5 (2));
  //pcl::PointXYZ pt6 (p6 (0), p6 (1), p6 (2));
  //pcl::PointXYZ pt7 (p7 (0), p7 (1), p7 (2));
  //pcl::PointXYZ pt8 (p8 (0), p8 (1), p8 (2));

  //viewer->addLine (pt1, pt2, 1.0, 0.0, 0.0, "1 edge");
  //viewer->addLine (pt1, pt4, 1.0, 0.0, 0.0, "2 edge");
  //viewer->addLine (pt1, pt5, 1.0, 0.0, 0.0, "3 edge");
  //viewer->addLine (pt5, pt6, 1.0, 0.0, 0.0, "4 edge");
  //viewer->addLine (pt5, pt8, 1.0, 0.0, 0.0, "5 edge");
  //viewer->addLine (pt2, pt6, 1.0, 0.0, 0.0, "6 edge");
  //viewer->addLine (pt6, pt7, 1.0, 0.0, 0.0, "7 edge");
  //viewer->addLine (pt7, pt8, 1.0, 0.0, 0.0, "8 edge");
  //viewer->addLine (pt2, pt3, 1.0, 0.0, 0.0, "9 edge");
  //viewer->addLine (pt4, pt8, 1.0, 0.0, 0.0, "10 edge");
  //viewer->addLine (pt3, pt4, 1.0, 0.0, 0.0, "11 edge");
  //viewer->addLine (pt3, pt7, 1.0, 0.0, 0.0, "12 edge");

  while(!viewer->wasStopped())
  {
    viewer->spinOnce (100);
    boost::this_thread::sleep (boost::posix_time::microseconds (100000));
  }

  return (0);
}