#include "particle_method.h"
#include "set_hh_eqn.h"
#include <chrono>
#include <boost/program_options.hpp>
#define MATLAB_VISUALIZE
struct Run_options {
    std::string ICfilename;
    std::string out_filename;
    double time_stepsize;
    int stepsize_count;
    double tau;
    double lambda;
    double alpha;
    double linear_tolerance;
    double split_relax_weight;
};
struct Plot_options {
    int plot_interval = 0;
    std::vector<bool> projection_dimension = std::vector<bool>(4, false);
    std::vector<bool> density_projection_dimension = std::vector<bool>(4, false);
    bool generate_video = false;
    int plot_flag = 1;
    bool display_density = true;
    bool output_intermediate = false;
    bool silence = false;
};
void run_HH_model(const Model_options& mo, const Run_options& ro, const Plot_options& po) {
//Setting models:
    Population_density_with_equation* a_ptr = NULL;
    int plot_stepcount;
    if (po.display_density) {
        plot_stepcount = ro.stepsize_count / po.plot_interval;
    }
    Advection_diffusion_eqn* HH = set_Hodgkin_Huxley_eqn(mo);
    double plot_xlb, plot_xub, plot_ylb, plot_yub;
    a_ptr = new Population_density_with_equation(*HH,4, ro.tau,ro.lambda,ro.alpha);//NOTE: 4 is the dimension of HH model. Change this to match your model.
    a_ptr->input_all_particles(ro.ICfilename.c_str());
    if (po.display_density)
    {
        passplotcommand(global_matlab_engine, "clear");
        passplotcommand(global_matlab_engine, "subplot(2,1,1)");
        a_ptr->plot("axis([-0.2,1.1,0,1])");
        passplotcommand(global_matlab_engine, "subplot(2,1,2)");
        a_ptr->plot_density(po.density_projection_dimension, -0.2, 1.1, 0.0, 1.0);
    }
    const double average_V = a_ptr->average_in_index(0);
    std::string matlabarg;
    if (po.display_density)
    {
        matlabarg = "title('average voltage : " + std::to_string(average_V * 100) + "mV')";
        passplotcommand(global_matlab_engine, matlabarg.c_str());
        if (!po.silence)
        {
            std::cout << "Problem setup finished.\n";
        }
    }
//Setting timings:
    std::chrono::high_resolution_clock::time_point t0 = std::chrono::high_resolution_clock::now();
    auto t_update = t0 - t0;
    double t = 0.0;
    typedef std::chrono::duration<double, std::ratio<1, 1>> seconds;
//Setting output of average voltage and coupling_strength
    MATFile* output_matptr;
    std::string mat_filename;
    mat_filename = "coupling_strength_diff_" + std::to_string(int(1e6 * mo.diffusion_coeff)) + "_coup_" + std::to_string(int(10 * mo.coupling_strength_coefficient)) + ".mat";
    output_matptr = matOpen(mat_filename.c_str(), "w");
    mxArray *avg_potential_matlabarray;
    mxArray *coupling_porprotion_matlabarray;
    mxArray *t_matlabarray;
    mxArray *particle_count_matlabarray;
    avg_potential_matlabarray = mxCreateDoubleMatrix(1, ro.stepsize_count, mxREAL);
    coupling_porprotion_matlabarray = mxCreateDoubleMatrix(1, ro.stepsize_count, mxREAL);
    t_matlabarray = mxCreateDoubleMatrix(1, ro.stepsize_count, mxREAL);
    particle_count_matlabarray = mxCreateDoubleMatrix(1, ro.stepsize_count, mxREAL);
//Setting generation of video:
    if (po.display_density && po.generate_video)
    {
        matlabarg = "step_count = " + std::to_string(plot_stepcount) + ";";
        passplotcommand(global_matlab_engine, matlabarg.c_str());
        passplotcommand(global_matlab_engine, "plot_itr = 1");
        passplotcommand(global_matlab_engine, "F(step_count) = struct('cdata',[],'colormap',[]);");
    }
//Loops for distribution update:
    for (size_t i = 0; i < ro.stepsize_count + 1; i++)
    {
        a_ptr->update_ODE_adaptive_split(ro.time_stepsize, 1, ro.linear_tolerance);
        t += ro.time_stepsize;
        if (!po.silence) {
            std::cout << "time t = " << t << std::endl;
            std::cout << "coupling: " << a_ptr->coupling_at_previous_timestep() << std::endl;
            std::cout << "Particle count (before combine): " << a_ptr->size() << std::endl;
        }

        //Copy data to mat files:
        const double coupling_porprotion = a_ptr->coupling_at_previous_timestep()/mo.coupling_strength_coefficient * ro.time_stepsize;
        const double particle_count = double(a_ptr->size());//convert to double
        const double average_V = a_ptr->average_in_index(0);
        memcpy(static_cast<double*>(mxGetPr(avg_potential_matlabarray)) + i, &average_V, sizeof(double));
        memcpy(static_cast<double*>(mxGetPr(coupling_porprotion_matlabarray)) + i, &coupling_porprotion, sizeof(double));
        memcpy(static_cast<double*>(mxGetPr(t_matlabarray)) + i, &t, sizeof(double));
        memcpy(static_cast<double*>(mxGetPr(particle_count_matlabarray)) + i, &particle_count, sizeof(double));
        if (po.display_density && (i - 1) % po.plot_interval == 0)
        {
            passplotcommand(global_matlab_engine, "subplot(2,1,1)");
            a_ptr->plot(po.projection_dimension, "axis([-0.2,1.1,0,1,0,1])");
            if (std::count(po.projection_dimension.begin(), po.projection_dimension.end(), true) >= 3) {
                passplotcommand(global_matlab_engine, ";view(10,20);");
            }
            if (po.generate_video)
            {
                matlabarg = "title('t = " + std::to_string(t) + " ms')";
                passplotcommand(global_matlab_engine, matlabarg.c_str());
                passplotcommand(global_matlab_engine, "xlabel('Membrane Potential (*100 mV)')");
                passplotcommand(global_matlab_engine, "ylabel('Sodium activation subunit')");
                passplotcommand(global_matlab_engine, "zlabel('Potassium subunit')");
                passplotcommand(global_matlab_engine, "subplot(2,1,2)");
                char filename[128];
                sprintf(filename, "HHTestatstep%ic%id%i.mat", i * 20 + 1, int(10 * mo.coupling_strength_coefficient), int(100000 * mo.diffusion_coeff));
                a_ptr->output_all_particles(filename);
            }
            a_ptr->plot_density(po.density_projection_dimension, -0.2, 1.1, 0.0, 1.0);
            const double average_V = a_ptr->average_in_index(0);
            if (po.generate_video && std::count(po.density_projection_dimension.begin(), po.density_projection_dimension.end(), true) == 1 && po.density_projection_dimension[0] == true)
            {
                matlabarg = "title('average voltage : " + std::to_string(average_V * 100) + "mV')";
                passplotcommand(global_matlab_engine, matlabarg.c_str());
                passplotcommand(global_matlab_engine, "ylim([0 0.35])");
                passplotcommand(global_matlab_engine, "xlabel('Membrane Potential (*100 mV)')");
                passplotcommand(global_matlab_engine, "ylabel('Density')");
            }
            else if (po.generate_video && std::count(po.density_projection_dimension.begin(), po.density_projection_dimension.end(), true) == 2 && po.density_projection_dimension[0] == true && po.density_projection_dimension[1] == true) {
                matlabarg = "title('average voltage : " + std::to_string(average_V * 100) + "mV')";
                passplotcommand(global_matlab_engine, matlabarg.c_str());
                passplotcommand(global_matlab_engine, "xlabel('Membrane Potential (*100 mV)')");
                passplotcommand(global_matlab_engine, "ylabel('Sodium activation subunit')");
            }
            if (po.generate_video)
            {
                passplotcommand(global_matlab_engine, "drawnow\nF(plot_itr) = getframe(gcf);plot_itr = plot_itr + 1;");
            }
        }
        a_ptr->combine_particles();
        if (!po.silence) {
            std::cout << "Particle count (after combine): " << a_ptr->size() << std::endl;
            std::chrono::high_resolution_clock::time_point t1 = std::chrono::high_resolution_clock::now();
            std::cout << "Computation time: " << seconds(t1 - t0).count() << " seconds." << std::endl;
        }

    }
    if (po.display_density && po.generate_video) {
        matlabarg = "v = VideoWriter('HH_from_delta_c_" + std::to_string(int(10 * mo.coupling_strength_coefficient)) + "_d_" +
            std::to_string(int(100000 * mo.diffusion_coeff)) + "','Motion JPEG 2000');v.FrameRate = 15;open(v);\nfor i = 1:" + std::to_string(plot_stepcount) + "\nwriteVideo(v,F(i))\nend\nclose(v);";
        passplotcommand(global_matlab_engine, matlabarg.c_str());
    }

    a_ptr->output_all_particles("HHTestatend.mat");
    std::chrono::high_resolution_clock::time_point t1 = std::chrono::high_resolution_clock::now();
    const double seconds_elapsed = seconds(t1 - t0).count();
    if (!po.silence)
    {
        std::cout << "Total Computation time: " << seconds_elapsed << " seconds." << std::endl;
    }
    matPutVariable(output_matptr, "avg_potential", avg_potential_matlabarray);
    matPutVariable(output_matptr, "coupling_porprotion", coupling_porprotion_matlabarray);
    matPutVariable(output_matptr, "t", t_matlabarray);
    matPutVariable(output_matptr, "particle_count", particle_count_matlabarray);
    mxDestroyArray(avg_potential_matlabarray);
    mxDestroyArray(coupling_porprotion_matlabarray);
    mxDestroyArray(t_matlabarray);
    mxDestroyArray(particle_count_matlabarray);
    mxArray* t_computation_matlabarray;
    t_computation_matlabarray = mxCreateDoubleMatrix(1, 1, mxREAL);
    memcpy(static_cast<double*>(mxGetPr(t_computation_matlabarray)), &seconds_elapsed, sizeof(double));
    matPutVariable(output_matptr, "t_computation", t_computation_matlabarray);
    mxDestroyArray(t_computation_matlabarray);
}
int main(int argc, char **argv) {
    Model_options mo;
    Run_options ro;
    Plot_options po;
    std::string projection_dimension_str;
    std::string density_projection_dimension_str;
    boost::program_options::options_description params("Parameters");
    params.add_options()
        ("help,h", "produce help message")
        ("diffusion_coeff,d", boost::program_options::value<double>(&mo.diffusion_coeff)->default_value(1e-5), "diffusion coefficient")
        ("coupling_strength,c", boost::program_options::value<double>(&mo.coupling_strength_coefficient)->default_value(0.4), "coupling strength")
        ("coupling_potential,V", boost::program_options::value<double>(&mo.coupling_potential)->default_value(35), "mV - coupling potential")
        ("tau", boost::program_options::value<double>(&ro.tau)->default_value(0.0001), "parameter tau. affects std deviation cutoff for combine, and diffusion regularization")
        ("lambda", boost::program_options::value<double>(&ro.lambda)->default_value(1e-6), "Tikhnov regularization factor for diffusion velocity")
        ("alpha", boost::program_options::value<double>(&ro.alpha)->default_value(0.2), "velocity reference distance factor, small closer to center")
        ("tol", boost::program_options::value<double>(&ro.linear_tolerance)->default_value(0.05), "Tolerance for the linear approximation to deviate")
        ("stepsize,s", boost::program_options::value<double>(&ro.time_stepsize)->default_value(0.05), "ms - maximum_stepsize")
        ("stepcount,N", boost::program_options::value<int>(&ro.stepsize_count)->default_value(1000), "number of timesteps")
        ("plot_interval,i", boost::program_options::value<int>(&po.plot_interval)->default_value(0), "timesteps between each plot (0 if no plot)")
        ("silence",boost::program_options::bool_switch(&po.silence)->default_value(false),"silent output to std::cout during computation")
        ("projection_dimensions,p", boost::program_options::value<std::string>(&projection_dimension_str)->default_value("0,1,2"), "index of dimensions to project to, can have 1, 2 or 3 entries, separated by ,")
        ("density_projection_dimensions,q", boost::program_options::value<std::string>(&density_projection_dimension_str)->default_value("0,1"), "index of dimensions to project in density map, 1 or 2 entries, separated by ,")
        ("video,v", boost::program_options::bool_switch(&po.generate_video)->default_value(true), "generates video of all plots")
        ("uICfile,I", boost::program_options::value<std::string>(&ro.ICfilename)->default_value("HH_init_cond.mat"), "filename of initial concentration conditions")
    ;
    boost::program_options::variables_map vm;
    boost::program_options::store(boost::program_options::parse_command_line(argc, argv, params), vm);
    boost::program_options::notify(vm);
    if (vm.count("help")) {
        std::cout << params << "\n";
        return 0;
    }
    //Parsing projection dimensions:
    std::string delimiter = ",";
    const int N = 4;
    std::vector<bool> projection_dimension(N,false);

    int cur_dimension;
    size_t pos = 0;
    std::string current_number;
    while ((pos = projection_dimension_str.find(delimiter)) != std::string::npos) {
        current_number = projection_dimension_str.substr(0, pos);
        projection_dimension_str.erase(0, pos + delimiter.length());
        std::stringstream parse_stream(current_number);
        parse_stream >> cur_dimension;
        projection_dimension[cur_dimension] = true;
    }
    {
        std::stringstream parse_stream(projection_dimension_str);
        parse_stream >> cur_dimension;
        projection_dimension[cur_dimension] = true;
    }
    {
        std::stringstream parse_stream(projection_dimension_str);
        parse_stream >> cur_dimension;
        projection_dimension[cur_dimension] = true;
    }
    std::vector<bool> density_projection_dimension(N, false);
    while ((pos = density_projection_dimension_str.find(delimiter)) != std::string::npos) {
        current_number = density_projection_dimension_str.substr(0, pos);
        density_projection_dimension_str.erase(0, pos + delimiter.length());
        std::stringstream parse_stream(current_number);
        parse_stream >> cur_dimension;
        density_projection_dimension[cur_dimension] = true;
    }
    {
        std::stringstream parse_stream(density_projection_dimension_str);
        parse_stream >> cur_dimension;
        density_projection_dimension[cur_dimension] = true;
    }
    if (po.plot_interval == 0) {
        po.display_density = false;
    }
    run_HH_model(mo,ro,po);
    return 0;
}


//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//% Code implementing the paper "Accelerated Quadratic Proxy for Geometric Optimization", SIGGRAPH 2016.
//% Disclaimer: The code is provided as-is for academic use only and without any guarantees.
//%             Please contact the author to report any bugs.
//% Written by Shahar Kovalsky (http://www.wisdom.weizmann.ac.il/~shaharko/)
//%            Meirav Galun (http://www.wisdom.weizmann.ac.il/~/meirav/)
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

#include "mex.h"
#include <Eigen/Dense>
#include <Eigen/SparseCore>

using namespace Eigen;

void mapSparseMatrixToMex(const SparseMatrix<double>& mat, mxArray **out)
{
	*out = mxCreateSparse(mat.rows(), mat.cols(), mat.nonZeros(), mxREAL);

	mwIndex *Ir, *Jc;
	double *Pr;
	Ir = mxGetIr(*out);
	Jc = mxGetJc(*out);
	Pr = mxGetPr(*out);

	for (int k = 0; k < mat.nonZeros(); k++)
	{
		Pr[k] = (mat.valuePtr())[k];
		Ir[k] = (mat.innerIndexPtr())[k];
	}
	for (int k = 0; k <= mat.cols(); k++)
	{
		Jc[k] = (mat.outerIndexPtr())[k];
	}
}

void mapDenseMatrixToMex(const MatrixXd& mat, mxArray **out)
{
	*out = mxCreateDoubleMatrix(mat.rows(), mat.cols(), mxREAL);
	Map<MatrixXd> temp(mxGetPr(*out), mat.rows(), mat.cols());
	temp = mat; // copy
}


//---------------------------------------------------------------------------//
// Copyright (c) 2020-2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020-2021 Ilias Khairullin <ilias@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE secret_sharing_test

#include <algorithm>
#include <iterator>
#include <functional>
#include <utility>

#include <boost/test/unit_test.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/data/monomorphic.hpp>

#include <nil/crypto3/algebra/curves/bls12.hpp>

#include <nil/crypto3/pubkey/secret_sharing/shamir.hpp>
#include <nil/crypto3/pubkey/secret_sharing/feldman.hpp>
#include <nil/crypto3/pubkey/secret_sharing/pedersen.hpp>
#include <nil/crypto3/pubkey/secret_sharing/weighted_shamir.hpp>

#include <nil/crypto3/pubkey/algorithm/deal_shares.hpp>
#include <nil/crypto3/pubkey/algorithm/verify_share.hpp>
#include <nil/crypto3/pubkey/algorithm/reconstruct_secret.hpp>
#include <nil/crypto3/pubkey/algorithm/deal_share.hpp>
// #include <nil/crypto3/pubkey/algorithm/recover_polynomial.hpp>

using namespace nil::crypto3::algebra;
using namespace nil::crypto3::pubkey;

template<typename FieldParams>
void print_field_element(std::ostream &os, const typename fields::detail::element_fp<FieldParams> &e) {
    os
        // << std::hex
        << e.data;
}

template<typename FieldParams>
void print_field_element(std::ostream &os, const typename fields::detail::element_fp2<FieldParams> &e) {
    os
        // << std::hex
        << e.data[0].data << ", " << e.data[1].data;
}

template<typename CurveGroupElement>
void print_projective_curve_group_element(std::ostream &os, const CurveGroupElement &e) {
    os << "( ";
    print_field_element(e.X);
    os << " : ";
    print_field_element(e.Y);
    os << " : ";
    print_field_element(e.Z);
    os << " )";
}

template<typename CurveGroupElement>
void print_jacobian_with_a4_0_curve_group_element(std::ostream &os, const CurveGroupElement &e) {
    print_projective_curve_group_element(os, e);
}

template<typename CurveGroupElement>
void print_extended_curve_group_element(std::ostream &os, const CurveGroupElement &e) {
    os << "( ";
    print_field_element(e.X);
    os << " : ";
    print_field_element(e.Y);
    os << " : ";
    print_field_element(e.T);
    os << " : ";
    print_field_element(e.Z);
    os << " )";
}

template<typename CurveGroupElement>
void print_affine_curve_group_element(std::ostream &os, const CurveGroupElement &e) {
    os << "( ";
    print_field_element(e.X);
    os << " : ";
    print_field_element(e.Y);
    os << " )";
}

namespace boost {
    namespace test_tools {
        namespace tt_detail {
            template<typename FieldParams>
            struct print_log_value<typename fields::detail::element_fp<FieldParams>> {
                void operator()(std::ostream &os, typename fields::detail::element_fp<FieldParams> const &e) {
                    print_field_element(os, e);
                }
            };

            template<typename CurveParams>
            struct print_log_value<curves::detail::curve_element<CurveParams, curves::forms::short_weierstrass,
                                                                 curves::coordinates::jacobian_with_a4_0>> {
                void operator()(std::ostream &os,
                                curves::detail::curve_element<CurveParams, curves::forms::short_weierstrass,
                                                              curves::coordinates::jacobian_with_a4_0> const &p) {
                    print_projective_curve_group_element(os, p);
                }
            };

            template<typename CurveParams>
            struct print_log_value<curves::detail::curve_element<CurveParams, curves::forms::short_weierstrass,
                                                                 curves::coordinates::affine>> {
                void operator()(std::ostream &os,
                                curves::detail::curve_element<CurveParams, curves::forms::short_weierstrass,
                                                              curves::coordinates::affine> const &p) {
                    print_affine_curve_group_element(os, p);
                }
            };

            template<template<typename, typename> class P, typename K, typename V>
            struct print_log_value<P<K, V>> {
                void operator()(std::ostream &, P<K, V> const &) {
                }
            };
        }    // namespace tt_detail
    }        // namespace test_tools
}    // namespace boost
template<typename T>
class TD;

// TODO: add verification of wrong values
BOOST_AUTO_TEST_SUITE(secret_sharing_base_functional_self_tests)

BOOST_AUTO_TEST_CASE(feldman_sss) {
    using curve_type = curves::bls12_381;
    using group_type = typename curve_type::g1_type<>;
    using scheme_type = nil::crypto3::pubkey::feldman_sss<group_type>;

    using shares_dealing_isomorphic_mode =
        typename modes::isomorphic<scheme_type>::template bind<shares_dealing_policy<scheme_type>>::type;
    using share_verification_isomorphic_mode =
        typename modes::isomorphic<scheme_type>::template bind<share_verification_policy<scheme_type>>::type;
    using secret_reconstructing_isomorphic_mode =
        typename modes::isomorphic<scheme_type>::template bind<secret_reconstructing_policy<scheme_type>>::type;

    using shares_dealing_acc_set = shares_dealing_accumulator_set<shares_dealing_isomorphic_mode>;
    using shares_dealing_acc = typename boost::mpl::front<typename shares_dealing_acc_set::features_type>::type;
    using share_verification_acc_set = share_verification_accumulator_set<share_verification_isomorphic_mode>;
    using share_verification_acc = typename boost::mpl::front<typename share_verification_acc_set::features_type>::type;
    using secret_reconstructing_acc_set = reconstructing_accumulator_set<secret_reconstructing_isomorphic_mode>;
    using secret_reconstructing_acc =
        typename boost::mpl::front<typename secret_reconstructing_acc_set::features_type>::type;

    auto t = 5;
    auto n = 10;

    //===========================================================================
    // shares dealing

    // TODO: add public functions for that operations
    auto coeffs = scheme_type::get_poly(t, n);
    auto pub_coeffs = scheme_type::get_public_coeffs(coeffs);

    // deal_shares(rng)
    typename shares_dealing_isomorphic_mode::result_type shares = nil::crypto3::deal_shares<scheme_type>(coeffs, n);
    // deal_shares(first, last)
    typename shares_dealing_isomorphic_mode::result_type shares1 =
        nil::crypto3::deal_shares<scheme_type>(coeffs.begin(), coeffs.end(), n);
    // deal_shares(rng, acc)
    shares_dealing_acc_set deal_shares_acc(n, nil::crypto3::accumulators::threshold_value = t);
    nil::crypto3::deal_shares<scheme_type>(coeffs, deal_shares_acc);
    typename shares_dealing_isomorphic_mode::result_type shares2 =
        boost::accumulators::extract_result<shares_dealing_acc>(deal_shares_acc);
    // deal_shares(first, last, acc)
    shares_dealing_acc_set deal_shares_acc1(n, nil::crypto3::accumulators::threshold_value = t);
    nil::crypto3::deal_shares<scheme_type>(coeffs.begin(), coeffs.end(), deal_shares_acc1);
    typename shares_dealing_isomorphic_mode::result_type shares3 =
        boost::accumulators::extract_result<shares_dealing_acc>(deal_shares_acc1);
    // deal_shares(rng, out)
    // TODO: output shares in output iterator by elements, do not write vector of shares in output iterator
    std::vector<typename shares_dealing_isomorphic_mode::result_type> shares_out;
    nil::crypto3::deal_shares<scheme_type>(coeffs, n, std::back_inserter(shares_out));
    // deal_shares(first, last, out)
    // TODO: output shares in output iterator by elements, do not write vector of shares in output iterator
    std::vector<typename shares_dealing_isomorphic_mode::result_type> shares_out1;
    nil::crypto3::deal_shares<scheme_type>(coeffs.begin(), coeffs.end(), n, std::back_inserter(shares_out1));

    BOOST_CHECK(shares == shares1);
    BOOST_CHECK(shares == shares2);
    BOOST_CHECK(shares == shares3);
    BOOST_CHECK(shares == shares_out.back());
    BOOST_CHECK(shares == shares_out1.back());

    //===========================================================================
    // each participant check its share using accumulator

    std::size_t i = 1;
    for (const auto &s_i : shares) {
        // verify_share(rng)
        BOOST_CHECK(static_cast<bool>(
            nil::crypto3::verify_share<scheme_type>(pub_coeffs, static_cast<public_share_sss<scheme_type>>(s_i))));
        // verify_share(first, last)
        BOOST_CHECK(
            static_cast<bool>(nil::crypto3::verify_share<scheme_type>(pub_coeffs.begin(), pub_coeffs.end(), s_i)));
        // verify_share(rng, acc)
        share_verification_acc_set verify_share_acc(s_i);
        BOOST_CHECK(boost::accumulators::extract_result<share_verification_acc>(
            nil::crypto3::verify_share<scheme_type>(pub_coeffs, verify_share_acc)));
        // verify_share(first, last, acc)
        share_verification_acc_set verify_share_acc1(s_i);
        BOOST_CHECK(boost::accumulators::extract_result<share_verification_acc>(
            nil::crypto3::verify_share<scheme_type>(pub_coeffs.begin(), pub_coeffs.end(), verify_share_acc1)));
        // verify_share(rng, out)
        std::vector<bool> res_out;
        nil::crypto3::verify_share<scheme_type>(pub_coeffs, s_i, std::back_inserter(res_out));
        BOOST_CHECK(res_out.back());
        // verify_share(first, last, out)
        std::vector<bool> res_out1;
        nil::crypto3::verify_share<scheme_type>(pub_coeffs.begin(), pub_coeffs.end(), s_i,
                                                std::back_inserter(res_out1));
        BOOST_CHECK(res_out1.back());
    }

    //===========================================================================
    // reconstructing secret using accumulator

    // reconstruct_secret(rng)
    secret_sss<scheme_type> secret = nil::crypto3::reconstruct_secret<scheme_type>(shares);
    // reconstruct_secret(first, last)
    secret_sss<scheme_type> secret1 = nil::crypto3::reconstruct_secret<scheme_type>(shares.begin(), shares.end());
    // reconstruct_secret(rng, acc)
    secret_reconstructing_acc_set reconstruct_secret_acc;
    secret_sss<scheme_type> secret_acc = boost::accumulators::extract_result<secret_reconstructing_acc>(
        nil::crypto3::reconstruct_secret<scheme_type>(shares, reconstruct_secret_acc));
    // reconstruct_secret(first, last, acc)
    secret_reconstructing_acc_set reconstruct_secret_acc1;
    secret_sss<scheme_type> secret_acc1 = boost::accumulators::extract_result<secret_reconstructing_acc>(
        nil::crypto3::reconstruct_secret<scheme_type>(shares.begin(), shares.end(), reconstruct_secret_acc1));
    // reconstruct_secret(rng, out)
    std::vector<secret_sss<scheme_type>> secret_out;
    nil::crypto3::reconstruct_secret<scheme_type>(shares, std::back_inserter(secret_out));
    // reconstruct_secret(first, last, out)
    std::vector<secret_sss<scheme_type>> secret_out1;
    nil::crypto3::reconstruct_secret<scheme_type>(shares.begin(), shares.end(), std::back_inserter(secret_out1));
    BOOST_CHECK(coeffs.front() == secret.get_value());
    BOOST_CHECK(secret == secret1);
    BOOST_CHECK(secret1 == secret_acc);
    BOOST_CHECK(secret_acc == secret_acc1);
    BOOST_CHECK(secret_acc1 == secret_out.back());
    BOOST_CHECK(secret_out.back() == secret_out1.back());

    //===========================================================================
    // check impossibility of secret recovering with group weight less than threshold value

    secret_sss<scheme_type> wrong_secret =
        nil::crypto3::reconstruct_secret<scheme_type>(shares.begin(), [t, &shares]() {
            auto it = shares.begin();
            for (auto i = 0; i < t - 1; i++) {
                it++;
            }
            return it;
        }());
    BOOST_CHECK(coeffs.front() != wrong_secret.get_value());
}

BOOST_AUTO_TEST_CASE(shamir_weighted_sss) {
    using curve_type = curves::bls12_381;
    using group_type = typename curve_type::g1_type<>;
    using scheme_type = nil::crypto3::pubkey::weighted_shamir_sss<group_type>;

    using shares_dealing_isomorphic_mode =
        typename modes::isomorphic<scheme_type>::template bind<shares_dealing_policy<scheme_type>>::type;
    using secret_reconstructing_isomorphic_mode =
        typename modes::isomorphic<scheme_type>::template bind<secret_reconstructing_policy<scheme_type>>::type;

    using shares_dealing_acc_set = shares_dealing_accumulator_set<shares_dealing_isomorphic_mode>;
    using shares_dealing_acc = typename boost::mpl::front<typename shares_dealing_acc_set::features_type>::type;
    using secret_reconstructing_acc_set = reconstructing_accumulator_set<secret_reconstructing_isomorphic_mode>;
    using secret_reconstructing_acc =
        typename boost::mpl::front<typename secret_reconstructing_acc_set::features_type>::type;

    auto t = 10;
    auto n = 20;

    //===========================================================================
    // participants weights generation

    auto j = 1;
    typename scheme_type::weights_type weights;
    for (auto i = 0; i < n; ++i) {
        j = (j >= t ? 1 : j);
        weights.emplace(i + 1, j++);
    }

    typename scheme_type::weights_type weights_one;
    for (auto i = 0; i < n; ++i) {
        weights_one.emplace(i + 1, 1);
    }

    //===========================================================================
    // polynomial generation

    // TODO: add public functions for that operations
    auto coeffs = scheme_type::get_poly(t, n);
    auto pub_coeffs = scheme_type::get_public_coeffs(coeffs);

    //===========================================================================
    // default shares dealing

    // deal_shares(rng)
    typename shares_dealing_isomorphic_mode::result_type shares_one =
        nil::crypto3::deal_shares<scheme_type>(coeffs, n, weights_one);
    // deal_shares(first, last)
    typename shares_dealing_isomorphic_mode::result_type shares_one1 =
        nil::crypto3::deal_shares<scheme_type>(coeffs.begin(), coeffs.end(), n, weights_one);
    // deal_shares(rng, acc)
    shares_dealing_acc_set deal_shares_one_acc(n, nil::crypto3::accumulators::threshold_value = t,
                                               nil::crypto3::accumulators::weights = weights_one);
    nil::crypto3::deal_shares<scheme_type>(coeffs, deal_shares_one_acc);
    typename shares_dealing_isomorphic_mode::result_type shares_one2 =
        boost::accumulators::extract_result<shares_dealing_acc>(deal_shares_one_acc);
    // deal_shares(first, last, acc)
    shares_dealing_acc_set deal_shares_one_acc1(n, nil::crypto3::accumulators::threshold_value = t,
                                                nil::crypto3::accumulators::weights = weights_one);
    nil::crypto3::deal_shares<scheme_type>(coeffs.begin(), coeffs.end(), deal_shares_one_acc1);
    typename shares_dealing_isomorphic_mode::result_type shares_one3 =
        boost::accumulators::extract_result<shares_dealing_acc>(deal_shares_one_acc1);
    // deal_shares(rng, out)
    std::vector<typename shares_dealing_isomorphic_mode::result_type> shares_one_out;
    nil::crypto3::deal_shares<scheme_type>(coeffs, n, weights_one, std::back_inserter(shares_one_out));
    // deal_shares(first, last, out)
    std::vector<typename shares_dealing_isomorphic_mode::result_type> shares_one_out1;
    nil::crypto3::deal_shares<scheme_type>(coeffs.begin(), coeffs.end(), n, weights_one,
                                           std::back_inserter(shares_one_out1));

    BOOST_CHECK(shares_one == shares_one1);
    BOOST_CHECK(shares_one == shares_one2);
    BOOST_CHECK(shares_one == shares_one3);
    BOOST_CHECK(shares_one == shares_one_out.back());
    BOOST_CHECK(shares_one == shares_one_out1.back());

    //===========================================================================
    // weighted shares dealing

    // deal_shares(rng)
    typename shares_dealing_isomorphic_mode::result_type shares =
        nil::crypto3::deal_shares<scheme_type>(coeffs, n, weights);
    // deal_shares(first, last)
    typename shares_dealing_isomorphic_mode::result_type shares1 =
        nil::crypto3::deal_shares<scheme_type>(coeffs.begin(), coeffs.end(), n, weights);
    // deal_shares(rng, acc)
    shares_dealing_acc_set deal_shares_acc(n, nil::crypto3::accumulators::threshold_value = t,
                                           nil::crypto3::accumulators::weights = weights);
    nil::crypto3::deal_shares<scheme_type>(coeffs, deal_shares_acc);
    typename shares_dealing_isomorphic_mode::result_type shares2 =
        boost::accumulators::extract_result<shares_dealing_acc>(deal_shares_acc);
    // deal_shares(first, last, acc)
    shares_dealing_acc_set deal_shares_acc1(n, nil::crypto3::accumulators::threshold_value = t,
                                            nil::crypto3::accumulators::weights = weights);
    nil::crypto3::deal_shares<scheme_type>(coeffs.begin(), coeffs.end(), deal_shares_acc1);
    typename shares_dealing_isomorphic_mode::result_type shares3 =
        boost::accumulators::extract_result<shares_dealing_acc>(deal_shares_acc1);
    // deal_shares(rng, out)
    std::vector<typename shares_dealing_isomorphic_mode::result_type> shares_out;
    nil::crypto3::deal_shares<scheme_type>(coeffs, n, weights, std::back_inserter(shares_out));
    // deal_shares(first, last, out)
    std::vector<typename shares_dealing_isomorphic_mode::result_type> shares_out1;
    nil::crypto3::deal_shares<scheme_type>(coeffs.begin(), coeffs.end(), n, weights, std::back_inserter(shares_out1));

    BOOST_CHECK(shares == shares1);
    BOOST_CHECK(shares == shares2);
    BOOST_CHECK(shares == shares3);
    BOOST_CHECK(shares == shares_out.back());
    BOOST_CHECK(shares == shares_out1.back());

    //===========================================================================
    // reconstructing secret

    // reconstruct_secret(rng)
    secret_sss<scheme_type> secret_one = nil::crypto3::reconstruct_secret<scheme_type>(shares_one);
    // reconstruct_secret(first, last)
    secret_sss<scheme_type> secret_one1 =
        nil::crypto3::reconstruct_secret<scheme_type>(shares_one.begin(), shares_one.end());
    // reconstruct_secret(rng, acc)
    secret_reconstructing_acc_set reconstruct_secret_one_acc;
    secret_sss<scheme_type> secret_one_acc = boost::accumulators::extract_result<secret_reconstructing_acc>(
        nil::crypto3::reconstruct_secret<scheme_type>(shares_one, reconstruct_secret_one_acc));
    // reconstruct_secret(first, last, acc)
    secret_reconstructing_acc_set reconstruct_secret_one_acc1;
    secret_sss<scheme_type> secret_one_acc1 =
        boost::accumulators::extract_result<secret_reconstructing_acc>(nil::crypto3::reconstruct_secret<scheme_type>(
            shares_one.begin(), shares_one.end(), reconstruct_secret_one_acc1));
    // reconstruct_secret(rng, out)
    std::vector<secret_sss<scheme_type>> secret_one_out;
    nil::crypto3::reconstruct_secret<scheme_type>(shares_one, std::back_inserter(secret_one_out));
    // reconstruct_secret(first, last, out)
    std::vector<secret_sss<scheme_type>> secret_one_out1;
    nil::crypto3::reconstruct_secret<scheme_type>(shares_one.begin(), shares_one.end(),
                                                  std::back_inserter(secret_one_out1));
    BOOST_CHECK(coeffs.front() == secret_one.get_value());
    BOOST_CHECK(secret_one == secret_one1);
    BOOST_CHECK(secret_one1 == secret_one_acc);
    BOOST_CHECK(secret_one_acc == secret_one_acc1);
    BOOST_CHECK(secret_one_acc1 == secret_one_out.back());
    BOOST_CHECK(secret_one_out.back() == secret_one_out1.back());

    // reconstruct_secret(rng)
    secret_sss<scheme_type> secret = nil::crypto3::reconstruct_secret<scheme_type>(shares);
    // reconstruct_secret(first, last)
    secret_sss<scheme_type> secret1 = nil::crypto3::reconstruct_secret<scheme_type>(shares.begin(), shares.end());
    // reconstruct_secret(rng, acc)
    secret_reconstructing_acc_set reconstruct_secret_acc;
    secret_sss<scheme_type> secret_acc = boost::accumulators::extract_result<secret_reconstructing_acc>(
        nil::crypto3::reconstruct_secret<scheme_type>(shares, reconstruct_secret_acc));
    // reconstruct_secret(first, last, acc)
    secret_reconstructing_acc_set reconstruct_secret_acc1;
    secret_sss<scheme_type> secret_acc1 = boost::accumulators::extract_result<secret_reconstructing_acc>(
        nil::crypto3::reconstruct_secret<scheme_type>(shares.begin(), shares.end(), reconstruct_secret_acc1));
    // reconstruct_secret(rng, out)
    std::vector<secret_sss<scheme_type>> secret_out;
    nil::crypto3::reconstruct_secret<scheme_type>(shares, std::back_inserter(secret_out));
    // reconstruct_secret(first, last, out)
    std::vector<secret_sss<scheme_type>> secret_out1;
    nil::crypto3::reconstruct_secret<scheme_type>(shares.begin(), shares.end(), std::back_inserter(secret_out1));
    BOOST_CHECK(coeffs.front() == secret.get_value());
    BOOST_CHECK(secret == secret1);
    BOOST_CHECK(secret1 == secret_acc);
    BOOST_CHECK(secret_acc == secret_acc1);
    BOOST_CHECK(secret_acc1 == secret_out.back());
    BOOST_CHECK(secret_out.back() == secret_out1.back());

    //===========================================================================
    // check impossibility of secret recovering with group weight less than threshold value

    secret_sss<scheme_type> wrong_secret =
        nil::crypto3::reconstruct_secret<scheme_type>(shares.begin(), [t, &shares]() {
            auto it = shares.begin();
            auto weight = 0;
            while (true) {
                weight += it->get_value().size();
                if (weight >= t) {
                    break;
                }
                it++;
            }
            return it;
        }());
    BOOST_CHECK(coeffs.front() != wrong_secret.get_value());
}

BOOST_AUTO_TEST_CASE(pedersen_dkg) {
    using curve_type = curves::bls12_381;
    using group_type = typename curve_type::g1_type<>;
    using scheme_type = nil::crypto3::pubkey::pedersen_dkg<group_type>;

    using shares_dealing_isomorphic_mode =
        typename modes::isomorphic<scheme_type>::template bind<shares_dealing_policy<scheme_type>>::type;
    using share_verification_isomorphic_mode =
        typename modes::isomorphic<scheme_type>::template bind<share_verification_policy<scheme_type>>::type;
    using secret_reconstructing_isomorphic_mode =
        typename modes::isomorphic<scheme_type>::template bind<secret_reconstructing_policy<scheme_type>>::type;
    using share_dealing_isomorphic_mode =
        typename modes::isomorphic<scheme_type>::template bind<share_dealing_policy<scheme_type>>::type;

    using shares_dealing_acc_set = shares_dealing_accumulator_set<shares_dealing_isomorphic_mode>;
    using shares_dealing_acc = typename boost::mpl::front<typename shares_dealing_acc_set::features_type>::type;
    using share_verification_acc_set = share_verification_accumulator_set<share_verification_isomorphic_mode>;
    using share_verification_acc = typename boost::mpl::front<typename share_verification_acc_set::features_type>::type;
    using secret_reconstructing_acc_set = reconstructing_accumulator_set<secret_reconstructing_isomorphic_mode>;
    using secret_reconstructing_acc =
        typename boost::mpl::front<typename secret_reconstructing_acc_set::features_type>::type;
    using share_dealing_acc_set = share_dealing_accumulator_set<share_dealing_isomorphic_mode>;
    using share_dealing_acc = typename boost::mpl::front<typename share_dealing_acc_set::features_type>::type;

    auto t = 5;
    auto n = 10;

    //===========================================================================
    // every participant generates polynomial

    // TODO: add public functions for that operations
    std::vector<typename scheme_type::coeffs_type> P_polys;
    std::generate_n(std::back_inserter(P_polys), n, [t, n]() { return scheme_type::get_poly(t, n); });

    //===========================================================================
    // each participant calculates public values representing coefficients of its polynomial,
    // then he broadcasts these values

    // TODO: add public functions for that operations
    std::vector<typename scheme_type::public_coeffs_type> P_public_polys;
    std::transform(P_polys.begin(), P_polys.end(), std::back_inserter(P_public_polys),
                   [](const auto &poly_i) { return scheme_type::get_public_coeffs(poly_i); });

    //===========================================================================
    // every participant generates shares for each participant in group,
    // which he then transmits to the intended parties

    std::vector<typename shares_dealing_isomorphic_mode::result_type> P_generated_shares;
    std::transform(P_polys.begin(), P_polys.end(), std::back_inserter(P_generated_shares), [n](const auto &poly_i) {
        return static_cast<typename shares_dealing_isomorphic_mode::result_type>(
            nil::crypto3::deal_shares<scheme_type>(poly_i, n));
    });

    //===========================================================================
    // each participant verify shares received from other parties

    for (auto i = 1; i <= n; i++) {
        for (const auto &j_share : P_generated_shares[i - 1]) {
            BOOST_CHECK(static_cast<bool>(nil::crypto3::verify_share<scheme_type>(P_public_polys[i - 1], j_share)));
            auto wrong_P_public_polys = P_public_polys[i - 1];
            wrong_P_public_polys[0] = scheme_type::public_coeff_type::zero();
            BOOST_CHECK(!static_cast<bool>(nil::crypto3::verify_share<scheme_type>(wrong_P_public_polys, j_share)));
        }
    }

    //===========================================================================
    // each participant calculate its share as sum of shares generated by others for him

    std::vector<share_dealing_acc_set> P_shares_acc;
    for (std::size_t i = 1; i <= n; ++i) {
        P_shares_acc.emplace_back(share_dealing_acc_set(i));
    }
    for (const auto &i_generated_shares : P_generated_shares) {
        for (const auto &j_share : i_generated_shares) {
            nil::crypto3::deal_share<scheme_type>(std::vector {j_share}, P_shares_acc[j_share.get_index() - 1]);
        }
    }
    std::vector<share_sss<scheme_type>> P_shares;
    for (auto &P_share_acc : P_shares_acc) {
        P_shares.emplace_back(
            nil::crypto3::pubkey::accumulators::extract::deal_share<share_dealing_isomorphic_mode>(P_share_acc));
    }

    //===========================================================================
    // calculation of public values representing coefficients of real polynomial

    // TODO: implement public interface
    std::vector<typename scheme_type::public_coeff_type> P_public_poly(t, scheme_type::public_coeff_type::zero());
    for (const auto &i_poly : P_public_polys) {
        auto it1 = P_public_poly.begin();
        auto it2 = i_poly.begin();
        while (it1 != P_public_poly.end() && it2 != i_poly.end()) {
            *it1 = *it1 + *it2;
            ++it1;
            ++it2;
        }
    }

    //===========================================================================
    // verification of participants shares

    for (const auto &i_share : P_shares) {
        BOOST_CHECK(static_cast<bool>(nil::crypto3::verify_share<scheme_type>(P_public_poly, i_share)));
        auto wrong_P_public_polys = P_public_poly;
        wrong_P_public_polys[0] = scheme_type::public_coeff_type::zero();
        BOOST_CHECK(!static_cast<bool>(nil::crypto3::verify_share<scheme_type>(wrong_P_public_polys, i_share)));
    }

    //===========================================================================
    // calculation of actual secret
    // (which is not calculated directly by the parties in real application)

    typename scheme_type::coeff_type secret = scheme_type::coeff_type::zero();
    for (const auto &i_poly : P_polys) {
        secret = secret + i_poly.front();
    }

    secret_sss<scheme_type> reconstructed_secret =
        nil::crypto3::reconstruct_secret<scheme_type>(P_shares.begin(), P_shares.begin() + t);
    BOOST_CHECK_EQUAL(reconstructed_secret.get_value(), secret);

    //===========================================================================
    // check impossibility of secret recovering with group weight less than threshold value

    secret_sss<scheme_type> wrong_secret =
        nil::crypto3::reconstruct_secret<scheme_type>(P_shares.begin(), P_shares.begin() + t - 1);
    BOOST_CHECK_NE(wrong_secret.get_value(), secret);
}

BOOST_AUTO_TEST_SUITE_END()


/**
* This file is part of ORB-SLAM2.
*
* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
* For more information see <https://github.com/raulmur/ORB_SLAM2>
*
* ORB-SLAM2 is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ORB-SLAM2 is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
*/

#include "Map.h"
#include "Optimizer.h"

#include "g2o/core/block_solver.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/solvers/eigen/linear_solver_eigen.h"
#include "g2o/solvers/cholmod/linear_solver_cholmod.h"
#include "g2o/types/sba/types_six_dof_expmap.h"
#include "g2o/core/robust_kernel_impl.h"
#include "g2o/solvers/dense/linear_solver_dense.h"
#include "g2o/types/sim3/types_seven_dof_expmap.h"

#include <Eigen/StdVector>

#include "Converter.h"

#include <mutex>
#include <chrono>

namespace ORB_SLAM2
{

void Optimizer::InitialGlobalBundleAdjustemnt(Map* pMap, int nIterations)
{
    vector<KeyFrame*> vpKFs = pMap->GetAllKeyFrames();
    vector<MapPoint*> vpMP = pMap->GetAllMapPoints();

    vector<bool> vbNotIncludedMP;
    vbNotIncludedMP.resize(vpMP.size());

    g2o::SparseOptimizer optimizer;
    g2o::BlockSolver_6_3::LinearSolverType * linearSolver;

    linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();

    g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);

    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    optimizer.setAlgorithm(solver);

    long unsigned int maxKFid = 0;

    // Set KeyFrame vertices
    for(size_t i=0; i<vpKFs.size(); i++)
    {
        KeyFrame* pKF = vpKFs[i];
        if(pKF->isBad())
            continue;
        g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
        vSE3->setEstimate(Converter::toSE3Quat(pKF->GetPose()));
        vSE3->setId(pKF->mnId);
        vSE3->setFixed(true);
        optimizer.addVertex(vSE3);
        if(pKF->mnId>maxKFid)
            maxKFid=pKF->mnId;
    }

    const float thHuber2D = sqrt(5.99);
    const float thHuber3D = sqrt(7.815);

    // Set MapPoint vertices
    for(size_t i=0; i<vpMP.size(); i++)
    {
        MapPoint* pMP = vpMP[i];
        if(pMP->isBad())
            continue;
        g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
        vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
        const int id = pMP->mnId+maxKFid+1;
        vPoint->setId(id);
        vPoint->setMarginalized(true);
        optimizer.addVertex(vPoint);

       const map<KeyFrame*,size_t> observations = pMP->GetObservations();

        int nEdges = 0;
        //SET EDGES
        for(map<KeyFrame*,size_t>::const_iterator mit=observations.begin(); mit!=observations.end(); mit++)
        {

            KeyFrame* pKF = mit->first;
            if(pKF->isBad() || pKF->mnId>maxKFid)
                continue;

            nEdges++;

            const cv::KeyPoint &kpUn = pKF->mvKeysUn[mit->second];

            Eigen::Matrix<double,2,1> obs;
            obs << kpUn.pt.x, kpUn.pt.y;

            g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ();

            e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
            e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKF->mnId)));
            e->setMeasurement(obs);
            const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];
            e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);

            if(true)
            {
                g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
                e->setRobustKernel(rk);
                rk->setDelta(thHuber2D);
            }

            e->fx = pKF->fx;
            e->fy = pKF->fy;
            e->cx = pKF->cx;
            e->cy = pKF->cy;

            optimizer.addEdge(e);
        }

        if(nEdges==0)
        {
            optimizer.removeVertex(vPoint);
            vbNotIncludedMP[i]=true;
        }
        else
        {
            vbNotIncludedMP[i]=false;
        }
    }

    // Optimize!
    optimizer.initializeOptimization();
    optimizer.optimize(nIterations);

    // Recover optimized data

    //Keyframes
    for(size_t i=0; i<vpKFs.size(); i++)
    {
        KeyFrame* pKF = vpKFs[i];
        if(pKF->isBad())
            continue;
        g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));
        g2o::SE3Quat SE3quat = vSE3->estimate();
        pKF->SetPose(Converter::toCvMat(SE3quat));
    }

    //Points
    for(size_t i=0; i<vpMP.size(); i++)
    {
        if(vbNotIncludedMP[i])
            continue;

        MapPoint* pMP = vpMP[i];

        if(pMP->isBad())
            continue;
        g2o::VertexSBAPointXYZ* vPoint = static_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(pMP->mnId+maxKFid+1));

        pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate()));
        pMP->UpdateNormalAndDepth();
    }
}

void Optimizer::GlobalBundleAdjustemnt(Map* pMap, int nIterations, bool* pbStopFlag, const unsigned long nLoopKF, const bool bRobust)
{
    vector<KeyFrame*> vpKFs = pMap->GetAllKeyFrames();
    vector<MapPoint*> vpMP = pMap->GetAllMapPoints();
    BundleAdjustment(vpKFs,vpMP,nIterations,pbStopFlag, nLoopKF, bRobust);
}

void Optimizer::BundleAdjustment(const vector<KeyFrame *> &vpKFs, const vector<MapPoint *> &vpMP,
                                 int nIterations, bool* pbStopFlag, const unsigned long nLoopKF, const bool bRobust)
{
    vector<bool> vbNotIncludedMP;
    vbNotIncludedMP.resize(vpMP.size());

    g2o::SparseOptimizer optimizer;
    g2o::BlockSolver_6_3::LinearSolverType * linearSolver;

    // std::unique_ptr<g2o::BlockSolver_6_3::LinearSolverType> linearSolver;

    // linearSolver = g2o::make_unique<g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>>();

    // g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(
    //                                                       g2o::make_unique<g2o::BlockSolver_6_3>(std::move(linearSolver)));
    linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();

    g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);

    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    optimizer.setAlgorithm(solver);

    if(pbStopFlag)
        optimizer.setForceStopFlag(pbStopFlag);

    long unsigned int maxKFid = 0;

    // Set KeyFrame vertices
    for(size_t i=0; i<vpKFs.size(); i++)
    {
        KeyFrame* pKF = vpKFs[i];
        if(pKF->isBad())
            continue;
        g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
        vSE3->setEstimate(Converter::toSE3Quat(pKF->GetPose()));
        vSE3->setId(pKF->mnId);
        vSE3->setFixed(pKF->mnId==0);// || pKF->mnId==1);
        optimizer.addVertex(vSE3);
        if(pKF->mnId>maxKFid)
            maxKFid=pKF->mnId;
    }

    const float thHuber2D = sqrt(5.99);
    const float thHuber3D = sqrt(7.815);

    // Set MapPoint vertices
    for(size_t i=0; i<vpMP.size(); i++)
    {
        MapPoint* pMP = vpMP[i];
        if(pMP->isBad())
            continue;
        g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
        vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
        const int id = pMP->mnId+maxKFid+1;
        vPoint->setId(id);
        vPoint->setMarginalized(true);
        optimizer.addVertex(vPoint);

       const map<KeyFrame*,size_t> observations = pMP->GetObservations();

        int nEdges = 0;
        //SET EDGES
        for(map<KeyFrame*,size_t>::const_iterator mit=observations.begin(); mit!=observations.end(); mit++)
        {

            KeyFrame* pKF = mit->first;
            if(pKF->isBad() || pKF->mnId>maxKFid)
                continue;

            nEdges++;

            const cv::KeyPoint &kpUn = pKF->mvKeysUn[mit->second];

            Eigen::Matrix<double,2,1> obs;
            obs << kpUn.pt.x, kpUn.pt.y;

            g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ();

            e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
            e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKF->mnId)));
            e->setMeasurement(obs);
            const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];
            e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);

            if(bRobust)
            {
                g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
                e->setRobustKernel(rk);
                rk->setDelta(thHuber2D);
            }

            e->fx = pKF->fx;
            e->fy = pKF->fy;
            e->cx = pKF->cx;
            e->cy = pKF->cy;

            optimizer.addEdge(e);
        }

        if(nEdges==0)
        {
            optimizer.removeVertex(vPoint);
            vbNotIncludedMP[i]=true;
        }
        else
        {
            vbNotIncludedMP[i]=false;
        }
    }

    // Optimize!
    optimizer.initializeOptimization();
    optimizer.optimize(nIterations);

    // Recover optimized data

    //Keyframes
    for(size_t i=0; i<vpKFs.size(); i++)
    {
        KeyFrame* pKF = vpKFs[i];
        if(pKF->isBad())
            continue;
        g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));
        g2o::SE3Quat SE3quat = vSE3->estimate();
        if(nLoopKF==0)
        {
            pKF->SetPose(Converter::toCvMat(SE3quat));
        }
        else
        {
            pKF->mTcwGBA.create(4,4,CV_32F);
            Converter::toCvMat(SE3quat).copyTo(pKF->mTcwGBA);
            pKF->mnBAGlobalForKF = nLoopKF;
        }
    }

    //Points
    for(size_t i=0; i<vpMP.size(); i++)
    {
        if(vbNotIncludedMP[i])
            continue;

        MapPoint* pMP = vpMP[i];

        if(pMP->isBad())
            continue;
        g2o::VertexSBAPointXYZ* vPoint = static_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(pMP->mnId+maxKFid+1));
        pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate()));
        pMP->UpdateNormalAndDepth();
    }
}

int Optimizer::PoseOptimization(Frame *pFrame)
{
    g2o::SparseOptimizer optimizer;

    // std::unique_ptr<g2o::BlockSolver_6_3::LinearSolverType> linearSolver;

    // linearSolver = g2o::make_unique<g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>>();

    // g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(
    //                                                       g2o::make_unique<g2o::BlockSolver_6_3>(std::move(linearSolver)));

    g2o::BlockSolver_6_3::LinearSolverType * linearSolver;

    linearSolver = new g2o::LinearSolverDense<g2o::BlockSolver_6_3::PoseMatrixType>();

    g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);

    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    optimizer.setAlgorithm(solver);

    int nInitialCorrespondences=0;

    // Set Frame vertex
    g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
    vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));
    vSE3->setId(0);
    vSE3->setFixed(false);
    optimizer.addVertex(vSE3);

    // Set MapPoint vertices
    const int N = pFrame->N;

    vector<g2o::EdgeSE3ProjectXYZOnlyPose*> vpEdgesMono;
    vector<size_t> vnIndexEdgeMono;
    vpEdgesMono.reserve(N);
    vnIndexEdgeMono.reserve(N);

    const float deltaMono = sqrt(5.991);

    {
    unique_lock<mutex> lock(MapPoint::mGlobalMutex);

    for(int i=0; i<N; i++)
    {
        MapPoint* pMP = pFrame->mvpMapPoints[i];
        if(pMP)
        {
            // Monocular observation
            nInitialCorrespondences++;
            pFrame->mvbOutlier[i] = false;

            Eigen::Matrix<double,2,1> obs;
            const cv::KeyPoint &kpUn = pFrame->mvKeysUn[i];
            obs << kpUn.pt.x, kpUn.pt.y;

            g2o::EdgeSE3ProjectXYZOnlyPose* e = new g2o::EdgeSE3ProjectXYZOnlyPose();

            e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
            e->setMeasurement(obs);
            const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave];
            e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);

            g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
            e->setRobustKernel(rk);
            rk->setDelta(deltaMono);

            e->fx = pFrame->fx;
            e->fy = pFrame->fy;
            e->cx = pFrame->cx;
            e->cy = pFrame->cy;
            cv::Mat Xw = pMP->GetWorldPos();
            e->Xw[0] = Xw.at<float>(0);
            e->Xw[1] = Xw.at<float>(1);
            e->Xw[2] = Xw.at<float>(2);

            optimizer.addEdge(e);

            vpEdgesMono.push_back(e);
            vnIndexEdgeMono.push_back(i);
        }

    }
    }

    if(nInitialCorrespondences<3)
        return 0;

    // We perform 4 optimizations, after each optimization we classify observation as inlier/outlier
    // At the next optimization, outliers are not included, but at the end they can be classified as inliers again.
    const float chi2Mono[4]={5.991,5.991,5.991,5.991};
    const int its[4]={10,10,10,10};

    int nBad=0;
    for(size_t it=0; it<4; it++)
    {

        vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));
        optimizer.initializeOptimization(0);
        optimizer.optimize(its[it]);

        nBad=0;
        for(size_t i=0, iend=vpEdgesMono.size(); i<iend; ++i)
        {
            g2o::EdgeSE3ProjectXYZOnlyPose* e = vpEdgesMono[i];

            const size_t idx = vnIndexEdgeMono[i];

            if(pFrame->mvbOutlier[idx])
            {
                e->computeError();
            }

            const float chi2 = e->chi2();

            if(chi2>chi2Mono[it])
            {
                pFrame->mvbOutlier[idx]=true;
                e->setLevel(1);
                nBad++;
            }
            else
            {
                pFrame->mvbOutlier[idx]=false;
                e->setLevel(0);
            }

            if(it==2)
                e->setRobustKernel(0);
        }

        if(optimizer.edges().size()<10)
            break;
    }

    // Recover optimized pose and return number of inliers
    g2o::VertexSE3Expmap* vSE3_recov = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(0));
    g2o::SE3Quat SE3quat_recov = vSE3_recov->estimate();
    cv::Mat pose = Converter::toCvMat(SE3quat_recov);
    pFrame->SetPose(pose);

    return nInitialCorrespondences-nBad;
}

void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap,
                                      pcl::IterativeClosestPoint7dof *icp_,
                                      double dist_coeff, bool use_icp, int use_icp2)
{
    // Local KeyFrames: First Breath Search from Current Keyframe
    list<KeyFrame*> lLocalKeyFrames;

    lLocalKeyFrames.push_back(pKF);
    pKF->mnBALocalForKF = pKF->mnId;

    const vector<KeyFrame*> vNeighKFs = pKF->GetVectorCovisibleKeyFrames();
    for(int i=0, iend=vNeighKFs.size(); i<iend; ++i)
    {
        KeyFrame* pKFi = vNeighKFs[i];
        pKFi->mnBALocalForKF = pKF->mnId;
        if(!pKFi->isBad())
            lLocalKeyFrames.push_back(pKFi);
    }

    // Local MapPoints seen in Local KeyFrames
    list<MapPoint*> lLocalMapPoints;
    for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; ++lit)
    {
        vector<MapPoint*> vpMPs = (*lit)->GetMapPointMatches();
        for(vector<MapPoint*>::iterator vit=vpMPs.begin(), vend=vpMPs.end(); vit!=vend; ++vit)
        {
            MapPoint* pMP = *vit;
            if(pMP)
                if(!pMP->isBad())
                    if(pMP->mnBALocalForKF!=pKF->mnId)
                    {
                        lLocalMapPoints.push_back(pMP);
                        pMP->mnBALocalForKF=pKF->mnId;
                    }
        }
    }

    // Fixed Keyframes. Keyframes that see Local MapPoints but that are not Local Keyframes
    list<KeyFrame*> lFixedCameras;
    for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; ++lit)
    {
        map<KeyFrame*,size_t> observations = (*lit)->GetObservations();
        for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; ++mit)
        {
            KeyFrame* pKFi = mit->first;

            if(pKFi->mnBALocalForKF!=pKF->mnId && pKFi->mnBAFixedForKF!=pKF->mnId)
            {
                pKFi->mnBAFixedForKF=pKF->mnId;
                if(!pKFi->isBad())
                    lFixedCameras.push_back(pKFi);
            }
        }
    }

    std::cout << "############## KeyFrame Bundle: " << lLocalKeyFrames.size() << " ############\n";
    std::cout << "############## Fixed KeyFrame: " << lFixedCameras.size() << " ############\n";

    list<MapPoint*> lFixedMapPoints;
    lFixedMapPoints.clear();
    // for (auto&&  kf: lFixedCameras)
    // {
    //     vector<MapPoint*> vpMPs = kf->GetMapPointMatches();
    //     for (auto&& p : vpMPs)
    //     {
    //         if (p)
    //         {
    //             if (!p->isBad())
    //             {
    //               if (p->mnBALocalForKF != kf->mnId)
    //               {
    //                   lFixedMapPoints.push_back(p);
    //                   p->mnBALocalForKF = p->mnId;
    //               }
    //             }
    //         }
    //     }
    // }

    // Setup optimizer
    g2o::SparseOptimizer optimizer;

    g2o::BlockSolver_6_3::LinearSolverType * linearSolver;

    linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();

    g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);

    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    optimizer.setAlgorithm(solver);

    if(pbStopFlag)
        optimizer.setForceStopFlag(pbStopFlag);

    unsigned long maxKFid = 0;

    // Set Local KeyFrame vertices
    for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++)
    {
        KeyFrame* pKFi = *lit;
        g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
        vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));
        vSE3->setId(pKFi->mnId);
        vSE3->setFixed(pKFi->mnId==0 || pKFi->mnId==1);
        optimizer.addVertex(vSE3);
        if(pKFi->mnId>maxKFid)
            maxKFid=pKFi->mnId;
    }

    // Set Fixed KeyFrame vertices
    for(list<KeyFrame*>::iterator lit=lFixedCameras.begin(), lend=lFixedCameras.end(); lit!=lend; lit++)
    {
        KeyFrame* pKFi = *lit;
        g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
        vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));
        vSE3->setId(pKFi->mnId);
        vSE3->setFixed(true);
        optimizer.addVertex(vSE3);
        if(pKFi->mnId>maxKFid)
            maxKFid=pKFi->mnId;
    }

    // Set MapPoint vertices
    const int nExpectedSize = (lLocalKeyFrames.size()+lFixedCameras.size())*lLocalMapPoints.size();

    vector<g2o::EdgeSE3ProjectXYZ*> vpEdgesMono;
    vpEdgesMono.reserve(nExpectedSize);

    vector<KeyFrame*> vpEdgeKFMono;
    vpEdgeKFMono.reserve(nExpectedSize);

    vector<MapPoint*> vpMapPointEdgeMono;
    vpMapPointEdgeMono.reserve(nExpectedSize);

    const float thHuberMono = sqrt(5.991);

    for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; ++lit)
    {
        MapPoint* pMP = *lit;
        g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
        vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
        int id = pMP->mnId+maxKFid+1;
        vPoint->setId(id);
        vPoint->setMarginalized(true);
        optimizer.addVertex(vPoint);

        const map<KeyFrame*,size_t> observations = pMP->GetObservations();

        //Set edges
        for(map<KeyFrame*,size_t>::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; ++mit)
        {
            KeyFrame* pKFi = mit->first;

            if(!pKFi->isBad())
            {
                const cv::KeyPoint &kpUn = pKFi->mvKeysUn[mit->second];

                // Monocular observation
                Eigen::Matrix<double,2,1> obs;
                obs << kpUn.pt.x, kpUn.pt.y;

                g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ();

                e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
                e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKFi->mnId)));
                e->setMeasurement(obs);
                const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];
                e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);

                g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
                e->setRobustKernel(rk);
                rk->setDelta(thHuberMono);

                e->fx = pKFi->fx;
                e->fy = pKFi->fy;
                e->cx = pKFi->cx;
                e->cy = pKFi->cy;

                optimizer.addEdge(e);
                vpEdgesMono.push_back(e);
                vpEdgeKFMono.push_back(pKFi);
                vpMapPointEdgeMono.push_back(pMP);
            }
        }
    }

    if(pbStopFlag)
        if(*pbStopFlag)
            return;

    optimizer.initializeOptimization();
    optimizer.optimize(5);

    bool bDoMore = true;

    if(pbStopFlag)
        if(*pbStopFlag)
            bDoMore = false;

    if(bDoMore)
    {
      // Check inlier observations
      for(size_t i=0, iend=vpEdgesMono.size(); i<iend; i++)
      {
          g2o::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
          MapPoint* pMP = vpMapPointEdgeMono[i];

          if(pMP->isBad())
              continue;

          if(e->chi2()>5.991 || !e->isDepthPositive())
          {
              e->setLevel(1);
          }

          e->setRobustKernel(0);
      }

      // Optimize again without the outliers
      optimizer.initializeOptimization(0);
      optimizer.optimize(10);
    }

    vector<pair<KeyFrame*,MapPoint*> > vToErase;
    vToErase.reserve(vpEdgesMono.size());

    // Check inlier observations
    for(size_t i=0, iend=vpEdgesMono.size(); i<iend; i++)
    {
        g2o::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
        MapPoint* pMP = vpMapPointEdgeMono[i];

        if(pMP->isBad())
            continue;

        if(e->chi2()>5.991 || !e->isDepthPositive())
        {
            KeyFrame* pKFi = vpEdgeKFMono[i];
            vToErase.push_back(make_pair(pKFi,pMP));
        }
    }

    // icp matching using map point
    pcl::PointCloud<pcl::PointXYZ>::Ptr l_points (new pcl::PointCloud<pcl::PointXYZ>);
    for (auto&& p : lLocalMapPoints)
    {
        g2o::VertexSBAPointXYZ* vPoint = static_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(p->mnId+maxKFid+1));
        cv::Mat pos = Converter::toCvMat(vPoint->estimate());
        pcl::PointXYZ point;
        point.x = pos.at<float>(0);
        point.y = pos.at<float>(1);
        point.z = pos.at<float>(2);
        l_points->push_back(point);
    }

    for (auto&& p : lFixedMapPoints)
    {
        // cv::Mat pos = Converter::toCvMat(p->estimate());
        cv::Mat pos = p->GetWorldPos();
        pcl::PointXYZ point;
        point.x = pos.at<float>(0);
        point.y = pos.at<float>(1);
        point.z = pos.at<float>(2);
        l_points->push_back(point);
    }

    Eigen::Matrix4d toRef; // = Converter::toMatrix4d(mpCurrentKeyFrame->GetPose());

    for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; ++lit)
    {
        KeyFrame* pKF = *lit;
        g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));
        g2o::SE3Quat SE3quat = vSE3->estimate();
        toRef = Converter::toMatrix4d(Converter::toCvMat(SE3quat));
        break;
    }

    std::cout << "Bofore:\n" << toRef.inverse() << "\n";

    static int iteration_count = 0;
    iteration_count += 1;

    if (use_icp && use_icp2 && iteration_count % 1 == 0 & iteration_count > 10)
    {
        std::cout << "######## Use ICP or NDT method ########\n";
        pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_points (new pcl::PointCloud<pcl::PointXYZ>);
        *filtered_points = *l_points;

        icp_->transformCloudPublic(*filtered_points, *filtered_points, toRef);

        pcl::PassThrough<pcl::PointXYZ> pass;
        // pass.setInputCloud (filtered_points);
        // pass.setFilterFieldName ("x");
        // pass.setFilterLimits (-12.0, 12.0);
        // pass.filter (*filtered_points);
        // pass.setInputCloud (filtered_points);
        // pass.setFilterFieldName ("z");
        // pass.setFilterLimits (-80, 50.0);
        // pass.filter (*filtered_points);
        pass.setInputCloud (filtered_points);
        pass.setFilterFieldName ("y");
        pass.setFilterLimits (-3, 3);
        pass.filter (*filtered_points);

        // double filter_res = 0.1f;
        // pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_filter;
        // voxel_grid_filter.setLeafSize(filter_res, filter_res, filter_res);
        // voxel_grid_filter.setInputCloud(filtered_points);
        // voxel_grid_filter.filter(*filtered_points);

        icp_->transformCloudPublic(*filtered_points, *filtered_points, toRef.inverse());

        // pcl::console::setVerbosityLevel(pcl::console::L_DEBUG);
        icp_->setInputSource(filtered_points);
        icp_->setMaximumIterations(10);
        icp_->setDistThreshold(dist_coeff);
        pcl::PointCloud<pcl::PointXYZ> output;
        Eigen::Matrix4d transformation;
        Eigen::Matrix4d output_pos;
        double out_scale;

        cv::Mat cv_invToRef; // = cv_toRef.inv();
        cv::Mat cv_toRef = Converter::toCvMat(toRef);

        icp_->align(output, Eigen::Matrix4d::Identity(), toRef, output_pos, out_scale);

        transformation = icp_->getFinalTransformation();

        // double scale = pow(cv::determinant(cv_transformation), 1.0/3.0);
        double scale = out_scale;

        // cv::Mat cv_transformation = Converter::toCvMat(transformation);

        // Update map point and keyframes

        // Update points using PCL
        pcl::PointCloud<pcl::PointXYZ>::Ptr converted_points (new pcl::PointCloud<pcl::PointXYZ>);
        *converted_points = *l_points;

        icp_->transformCloudPublic(*converted_points, *converted_points, toRef);
        icp_->transformCloudPublic(*converted_points, *converted_points, transformation);
        icp_->transformCloudPublic(*converted_points, *converted_points, toRef.inverse()); //updateToGlobal);
        // Eigen::Matrix4d output_pos_inv = output_pos.inverse();
        // output_pos_inv.block(0, 0, 3, 3) *= scale;
        // icp_->transformCloudPublic(*converted_points, *converted_points, toRef);
        // icp_->transformCloudPublic(*converted_points, *converted_points, output_pos_inv);

        // icp_->transformCloudPublic(*filtered_points, *filtered_points, toRef);
        // icp_->transformCloudPublic(*filtered_points, *filtered_points, output_pos_inv);

        // Get Map Mutex
        unique_lock<mutex> lock(pMap->mMutexMapUpdate);

        // Remove outliers
        if(!vToErase.empty())
        {
            for(size_t i=0; i<vToErase.size(); ++i)
            {
                KeyFrame* pKFi = vToErase[i].first;
                MapPoint* pMPi = vToErase[i].second;
                pKFi->EraseMapPointMatch(pMPi);
                pMPi->EraseObservation(pKFi);
            }
        }

        std::cout << "#### Scale: #### : " << scale << "\n";

        for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; ++lit)
        {
            KeyFrame* pKF = *lit;
            pKF->SetPose(Converter::toCvMat(output_pos));
            cv_invToRef = pKF->GetPoseInverse();
            break;
        }

        // Update Keyframes
        for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; ++lit)
        {
            KeyFrame* pKF = *lit;
            g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));
            g2o::SE3Quat SE3quat = vSE3->estimate();
            pKF->SetPose(Converter::toCvMat(SE3quat));
            cv::Mat prev_pose = pKF->GetPoseInverse(); // GetPose();
            cv::Mat relative_pose = cv_toRef * prev_pose;
            relative_pose.rowRange(0,3).col(3) *= scale; // multiply scale to translation
            cv::Mat after_pose = cv_invToRef * relative_pose;
            pKF->SetPose(after_pose.inv());
        }

        // // Update Keyframes
        // for(list<KeyFrame*>::iterator lit=lFixedCameras.begin(), lend=lFixedCameras.end(); lit!=lend; ++lit)
        // {
        //     KeyFrame* pKF = *lit;
        //     // g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));
        //     // g2o::SE3Quat SE3quat = vSE3->estimate();
        //     // pKF->SetPose(Converter::toCvMat(SE3quat));
        //     cv::Mat prev_pose = pKF->GetPoseInverse(); // GetPose();
        //     cv::Mat relative_pose = cv_toRef * prev_pose;
        //     relative_pose.rowRange(0,3).col(3) *= scale; // multiply scale to translation
        //     cv::Mat after_pose = cv_invToRef * relative_pose;
        //     pKF->SetPose(after_pose.inv());
        // }

        vector<MapPoint*> hoge;
        hoge.clear();

        // Update points
        int i = 0;
        for (auto&& pMP: lLocalMapPoints)
        {
            if(pMP)
            {
                if(!pMP->isBad())
                {
                    pcl::PointXYZ p = converted_points->points[i];
                    cv::Mat pose = (cv::Mat_<float>(3,1) << p.x, p.y, p.z);
                    pMP->SetWorldPos(pose);
                    pMP->UpdateNormalAndDepth();
                    hoge.push_back(pMP);
                }
            }
            i++;
        }

        for (auto&& pMP: lFixedMapPoints)
        {
            if(pMP)
            {
                if(!pMP->isBad())
                {
                    pcl::PointXYZ p = converted_points->points[i];
                    cv::Mat pose = (cv::Mat_<float>(3,1) << p.x, p.y, p.z);
                    pMP->SetWorldPos(pose);
                    pMP->UpdateNormalAndDepth();
                    hoge.push_back(pMP);
                }
            }
            i++;
        }

        std::vector<cv::Mat> hoge2;
        for (auto&& p: filtered_points->points)
        {
            cv::Mat pose = (cv::Mat_<float>(3,1) << p.x, p.y, p.z);
            hoge2.push_back(pose);
        }

        pMap->SetLocalMappingPoint(hoge);
        pMap->SetUsedMatchingPoint(hoge2);

    } else
    {
        std::cout << "Not use ICP or NDT matching\n";
        if(!vToErase.empty())
        {
            for(size_t i=0;i<vToErase.size();i++)
            {
                KeyFrame* pKFi = vToErase[i].first;
                MapPoint* pMPi = vToErase[i].second;
                pKFi->EraseMapPointMatch(pMPi);
                pMPi->EraseObservation(pKFi);
            }
        }

        //Keyframes
        for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++)
        {
            KeyFrame* pKF = *lit;
            g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));
            g2o::SE3Quat SE3quat = vSE3->estimate();
            pKF->SetPose(Converter::toCvMat(SE3quat));
        }

        //Points
        vector<MapPoint*> hoge;
        hoge.clear();

        for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)
        {
            MapPoint* pMP = *lit;
            g2o::VertexSBAPointXYZ* vPoint = static_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(pMP->mnId+maxKFid+1));
            pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate()));
            pMP->UpdateNormalAndDepth();
            hoge.push_back(pMP);
        }
        pMap->SetLocalMappingPoint(hoge);
    }

}

int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &vpMatches1, g2o::Sim3 &g2oS12, const float th2, const bool bFixScale)
{
    g2o::SparseOptimizer optimizer;

    // std::unique_ptr<g2o::BlockSolver_7_3::LinearSolverType> linearSolver;

    // linearSolver = g2o::make_unique<g2o::LinearSolverCholmod<g2o::BlockSolver_7_3::PoseMatrixType>>();

    // g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(
    //                                                       g2o::make_unique<g2o::BlockSolver_7_3>(std::move(linearSolver)));
    g2o::BlockSolverX::LinearSolverType * linearSolver;

    linearSolver = new g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>();

    g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver);

    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    optimizer.setAlgorithm(solver);

    // Calibration
    const cv::Mat &K1 = pKF1->mK;
    const cv::Mat &K2 = pKF2->mK;

    // Camera poses
    const cv::Mat R1w = pKF1->GetRotation();
    const cv::Mat t1w = pKF1->GetTranslation();
    const cv::Mat R2w = pKF2->GetRotation();
    const cv::Mat t2w = pKF2->GetTranslation();

    // Set Sim3 vertex
    g2o::VertexSim3Expmap * vSim3 = new g2o::VertexSim3Expmap();
    vSim3->_fix_scale=bFixScale;
    vSim3->setEstimate(g2oS12);
    vSim3->setId(0);
    vSim3->setFixed(false);
    vSim3->_principle_point1[0] = K1.at<float>(0,2);
    vSim3->_principle_point1[1] = K1.at<float>(1,2);
    vSim3->_focal_length1[0] = K1.at<float>(0,0);
    vSim3->_focal_length1[1] = K1.at<float>(1,1);
    vSim3->_principle_point2[0] = K2.at<float>(0,2);
    vSim3->_principle_point2[1] = K2.at<float>(1,2);
    vSim3->_focal_length2[0] = K2.at<float>(0,0);
    vSim3->_focal_length2[1] = K2.at<float>(1,1);
    optimizer.addVertex(vSim3);

    // Set MapPoint vertices
    const int N = vpMatches1.size();
    const vector<MapPoint*> vpMapPoints1 = pKF1->GetMapPointMatches();
    vector<g2o::EdgeSim3ProjectXYZ*> vpEdges12;
    vector<g2o::EdgeInverseSim3ProjectXYZ*> vpEdges21;
    vector<size_t> vnIndexEdge;

    vnIndexEdge.reserve(2*N);
    vpEdges12.reserve(2*N);
    vpEdges21.reserve(2*N);

    const float deltaHuber = sqrt(th2);

    int nCorrespondences = 0;

    for(int i=0; i<N; i++)
    {
        if(!vpMatches1[i])
            continue;

        MapPoint* pMP1 = vpMapPoints1[i];
        MapPoint* pMP2 = vpMatches1[i];

        const int id1 = 2*i+1;
        const int id2 = 2*(i+1);

        const int i2 = pMP2->GetIndexInKeyFrame(pKF2);

        if(pMP1 && pMP2)
        {
            if(!pMP1->isBad() && !pMP2->isBad() && i2>=0)
            {
                g2o::VertexSBAPointXYZ* vPoint1 = new g2o::VertexSBAPointXYZ();
                cv::Mat P3D1w = pMP1->GetWorldPos();
                cv::Mat P3D1c = R1w*P3D1w + t1w;
                vPoint1->setEstimate(Converter::toVector3d(P3D1c));
                vPoint1->setId(id1);
                vPoint1->setFixed(true);
                optimizer.addVertex(vPoint1);

                g2o::VertexSBAPointXYZ* vPoint2 = new g2o::VertexSBAPointXYZ();
                cv::Mat P3D2w = pMP2->GetWorldPos();
                cv::Mat P3D2c = R2w*P3D2w + t2w;
                vPoint2->setEstimate(Converter::toVector3d(P3D2c));
                vPoint2->setId(id2);
                vPoint2->setFixed(true);
                optimizer.addVertex(vPoint2);
            }
            else
                continue;
        }
        else
            continue;

        nCorrespondences++;

        // Set edge x1 = S12*X2
        Eigen::Matrix<double,2,1> obs1;
        const cv::KeyPoint &kpUn1 = pKF1->mvKeysUn[i];
        obs1 << kpUn1.pt.x, kpUn1.pt.y;

        g2o::EdgeSim3ProjectXYZ* e12 = new g2o::EdgeSim3ProjectXYZ();
        e12->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id2)));
        e12->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
        e12->setMeasurement(obs1);
        const float &invSigmaSquare1 = pKF1->mvInvLevelSigma2[kpUn1.octave];
        e12->setInformation(Eigen::Matrix2d::Identity()*invSigmaSquare1);

        g2o::RobustKernelHuber* rk1 = new g2o::RobustKernelHuber;
        e12->setRobustKernel(rk1);
        rk1->setDelta(deltaHuber);
        optimizer.addEdge(e12);

        // Set edge x2 = S21*X1
        Eigen::Matrix<double,2,1> obs2;
        const cv::KeyPoint &kpUn2 = pKF2->mvKeysUn[i2];
        obs2 << kpUn2.pt.x, kpUn2.pt.y;

        g2o::EdgeInverseSim3ProjectXYZ* e21 = new g2o::EdgeInverseSim3ProjectXYZ();

        e21->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id1)));
        e21->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
        e21->setMeasurement(obs2);
        float invSigmaSquare2 = pKF2->mvInvLevelSigma2[kpUn2.octave];
        e21->setInformation(Eigen::Matrix2d::Identity()*invSigmaSquare2);

        g2o::RobustKernelHuber* rk2 = new g2o::RobustKernelHuber;
        e21->setRobustKernel(rk2);
        rk2->setDelta(deltaHuber);
        optimizer.addEdge(e21);

        vpEdges12.push_back(e12);
        vpEdges21.push_back(e21);
        vnIndexEdge.push_back(i);
    }

    // Optimize!
    optimizer.initializeOptimization();
    optimizer.optimize(5);

    // Check inliers
    int nBad=0;
    for(size_t i=0; i<vpEdges12.size();i++)
    {
        g2o::EdgeSim3ProjectXYZ* e12 = vpEdges12[i];
        g2o::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i];
        if(!e12 || !e21)
            continue;

        if(e12->chi2()>th2 || e21->chi2()>th2)
        {
            size_t idx = vnIndexEdge[i];
            vpMatches1[idx]=static_cast<MapPoint*>(NULL);
            optimizer.removeEdge(e12);
            optimizer.removeEdge(e21);
            vpEdges12[i]=static_cast<g2o::EdgeSim3ProjectXYZ*>(NULL);
            vpEdges21[i]=static_cast<g2o::EdgeInverseSim3ProjectXYZ*>(NULL);
            nBad++;
        }
    }

    int nMoreIterations;
    if(nBad>0)
        nMoreIterations=10;
    else
        nMoreIterations=5;

    if(nCorrespondences-nBad<10)
        return 0;

    // Optimize again only with inliers

    optimizer.initializeOptimization();
    optimizer.optimize(nMoreIterations);

    int nIn = 0;
    for(size_t i=0; i<vpEdges12.size();i++)
    {
        g2o::EdgeSim3ProjectXYZ* e12 = vpEdges12[i];
        g2o::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i];
        if(!e12 || !e21)
            continue;

        if(e12->chi2()>th2 || e21->chi2()>th2)
        {
            size_t idx = vnIndexEdge[i];
            vpMatches1[idx]=static_cast<MapPoint*>(NULL);
        }
        else
            nIn++;
    }

    // Recover optimized Sim3
    g2o::VertexSim3Expmap* vSim3_recov = static_cast<g2o::VertexSim3Expmap*>(optimizer.vertex(0));
    g2oS12= vSim3_recov->estimate();

    return nIn;
}

} //namespace ORB_SLAM


#ifndef boost_numeric_bindings_traits_algorithm_hpp
#define boost_numeric_bindings_traits_algorithm_hpp

#include <boost/numeric/bindings/traits/type_traits.hpp>

namespace boost { namespace numeric { namespace bindings { namespace traits {

  ///
  /// To be used instead of operator== for numeric types
  /// Implemented as functor instead of free function because of specialisation
  /// rationale: operator== on builtin types can not be overloaded.

  template < typename T >
  struct is_equal
  {
    is_equal(typename type_traits< T >::real_type tolerance) : tolerance_( tolerance ) {}

    bool operator()(const T& a, const T& b)
    { return std::abs( a - b ) < tolerance_ ; }

    // bool operator()(const T& a, const T& b, typename value_traits< T >::value_type tolerance)
    // { return std::abs( a - b ) < tolerance ; }

    typename type_traits< T >::real_type tolerance_ ;
  } ;

}}}}

#endif // boost_numeric_bindings_traits_algorithm_hpp


#include <boost/test/unit_test.hpp>
#include <flexcore/core/connectables.hpp>
#include <flexcore/core/connection.hpp>

using namespace fc;

BOOST_AUTO_TEST_SUITE(test_connectables)

BOOST_AUTO_TEST_CASE(test_arithmetic_and_logical)
{
	auto zero = constant(0);

	BOOST_CHECK_EQUAL((zero >> increment{})(), 1);
	BOOST_CHECK_EQUAL((zero >> decrement{})(), -1);

	BOOST_CHECK_EQUAL((zero >> identity{})(), zero());

	BOOST_CHECK_EQUAL((zero >> add(2))(), 2);

	BOOST_CHECK_EQUAL((zero >> subtract(2))(), -2);

	BOOST_CHECK_EQUAL((zero >> multiply(2))(), 0);
	BOOST_CHECK_EQUAL((zero >> increment{} >> multiply(2))(), 2);

	BOOST_CHECK_EQUAL(([](){ return 4;} >> divide(2))(), 2);

	BOOST_CHECK_EQUAL(([](){ return -1;} >> absolute{})(), 1);
	BOOST_CHECK_EQUAL(([](){ return  1;} >> absolute{})(), 1);
	BOOST_CHECK_EQUAL(([](){ return -1.5f;} >> absolute{})(), 1.5f);
	BOOST_CHECK_EQUAL(([](){ return -1.l;} >> absolute{})(), 1.l);

	BOOST_CHECK_EQUAL(([](){ return 1;} >> negate{})(), -1);

	BOOST_CHECK_EQUAL(([](){ return true;} >> logical_not{})(), false);

	BOOST_CHECK_EQUAL(([](){ return 1;} >> clamp(0,2))(), 1);
	BOOST_CHECK_EQUAL(([](){ return 3;} >> clamp(0,2))(), 2);
	BOOST_CHECK_EQUAL(([](){ return -1;} >> clamp(0,2))(), 0);
}

namespace
{
	template<class T>
	struct mutable_call
	{
		void operator()(T x)
		{
			ref = x;
		}
		T& ref;
	};
}

BOOST_AUTO_TEST_CASE(test_helpers)
{
	auto zero = constant(0);
	BOOST_CHECK_EQUAL(zero(),0);

	auto pi = constant(3.14159);
	BOOST_CHECK_EQUAL(pi(),3.14159);

	//check tee with a functor with const operator()
	int test_value = 0;
	BOOST_CHECK_EQUAL(test_value, 0);
	auto connection = constant(1) >> tee([&test_value](int in){test_value = in;});
	BOOST_CHECK_EQUAL(connection(), 1); //this call triggers the side effect from tee
	BOOST_CHECK_EQUAL(test_value, 1);

	//check tee with a functor with non-const operator()
	auto non_constconnection = constant(2) >> tee(mutable_call<int>{test_value});
	BOOST_CHECK_EQUAL(non_constconnection(), 2);
	BOOST_CHECK_EQUAL(test_value, 2);
}

BOOST_AUTO_TEST_CASE(tee_move_only)
{
	auto src = [] { return std::make_unique<int>(42); };
	std::unique_ptr<int> target{nullptr};
	int tee_target{0};
	auto sink = [&target](auto&& in){ target = std::move(in); };

	auto test_con =	src
			>> tee([&tee_target](const auto& ptr) { tee_target = *ptr; })
			>> sink;

	test_con();

	BOOST_CHECK_EQUAL(tee_target, 42);
	BOOST_CHECK_EQUAL(*target, 42);
}

BOOST_AUTO_TEST_CASE(test_print)
{
	std::ostringstream test_stream;
	auto connection = constant(1) >> print(test_stream);
	connection();

	BOOST_CHECK_EQUAL(test_stream.str(), "1\n");
}

BOOST_AUTO_TEST_CASE(test_constexpr_connectables)
{
	constexpr auto con = increment{} >> decrement{} >> identity{};
	constexpr auto val = con(0);
	static_assert(val == 0, "");
	static_assert(con(1) == 1, "");
}

BOOST_AUTO_TEST_SUITE_END()