#include "openvslam/data/keyframe.h"
#include "openvslam/data/landmark.h"
#include "openvslam/data/map_database.h"
#include "openvslam/optimize/local_bundle_adjuster.h"
#include "openvslam/optimize/g2o/landmark_vertex_container.h"
#include "openvslam/optimize/g2o/se3/shot_vertex_container.h"
#include "openvslam/optimize/g2o/se3/reproj_edge_wrapper.h"
#include "openvslam/util/converter.h"

#include <unordered_map>

#include "openvslam/IMU/g2otypes.h"
#include <opencv2/core/eigen.hpp>

#include <Eigen/StdVector>
#include <g2o/core/solver.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/core/optimization_algorithm_levenberg.h>

#include <g2o/core/optimizable_graph.h>

namespace openvslam {

/*void VertexNavStatePVR(data::keyframe* local_keyfrm, int idKF, ::g2o::SparseOptimizer& optimizer)
{
    // Vertex of PVR
    g2o::VertexNavStatePVR * vNSPVR = new g2o::VertexNavStatePVR();
    vNSPVR->setEstimate(local_keyfrm->GetNavState());
    vNSPVR->setId(idKF);
    vNSPVR->setFixed(false);
    optimizer.addVertex(vNSPVR);
}

void VertexNavStateBias(data::keyframe* local_keyfrm, int idKF, ::g2o::SparseOptimizer& optimizer)
{
    // Vertex of Bias
    g2o::VertexNavStateBias * vNSBias = new g2o::VertexNavStateBias();
    vNSBias->setEstimate(local_keyfrm->GetNavState());
    vNSBias->setId(idKF+1);
    vNSBias->setFixed(false);
    optimizer.addVertex(vNSBias);
}*/

namespace optimize {

local_bundle_adjuster::local_bundle_adjuster(const unsigned int num_first_iter,
                                             const unsigned int num_second_iter)
    : num_first_iter_(num_first_iter), num_second_iter_(num_second_iter) {}

void local_bundle_adjuster::optimize(openvslam::data::keyframe* curr_keyfrm, bool* const force_stop_flag, data::map_database* map_db, mapping_module* mapper_) const {
    // 1. local/fixed keyframes, local landmarksを集計する

    // correct local keyframes of the current keyframe
    std::unordered_map<unsigned int, data::keyframe*> local_keyfrms;

    local_keyfrms[curr_keyfrm->id_] = curr_keyfrm;
    const auto curr_covisibilities = curr_keyfrm->graph_node_->get_covisibilities();
    for (auto local_keyfrm : curr_covisibilities) {
        if (!local_keyfrm) {
            continue;
        }
        if (local_keyfrm->will_be_erased()) {
            continue;
        }

        local_keyfrms[local_keyfrm->id_] = local_keyfrm;
    }

    // correct local landmarks seen in local keyframes
    std::unordered_map<unsigned int, data::landmark*> local_lms;

    for (auto local_keyfrm : local_keyfrms) {
        const auto landmarks = local_keyfrm.second->get_landmarks();
        for (auto local_lm : landmarks) {
            if (!local_lm) {
                continue;
            }
            if (local_lm->will_be_erased()) {
                continue;
            }

            // 重複を避ける
            if (local_lms.count(local_lm->id_)) {
                continue;
            }

            local_lms[local_lm->id_] = local_lm;
        }
    }

    // fixed keyframes: keyframes which observe local landmarks but which are NOT in local keyframes
    std::unordered_map<unsigned int, data::keyframe*> fixed_keyfrms;

    for (auto local_lm : local_lms) {
        const auto observations = local_lm.second->get_observations();
        for (auto& obs : observations) {
            auto fixed_keyfrm = obs.first;
            if (!fixed_keyfrm) {
                continue;
            }
            if (fixed_keyfrm->will_be_erased()) {
                continue;
            }

            // local keyframesに属しているときは追加しない
            if (local_keyfrms.count(fixed_keyfrm->id_)) {
                continue;
            }

            // 重複を避ける
            if (fixed_keyfrms.count(fixed_keyfrm->id_)) {
                continue;
            }

            fixed_keyfrms[fixed_keyfrm->id_] = fixed_keyfrm;
        }
    }

    // 2. optimizerを構築

    auto linear_solver = ::g2o::make_unique<::g2o::LinearSolverCSparse<::g2o::BlockSolver_6_3::PoseMatrixType>>();
    auto block_solver = ::g2o::make_unique<::g2o::BlockSolver_6_3>(std::move(linear_solver));
    auto algorithm = new ::g2o::OptimizationAlgorithmLevenberg(std::move(block_solver));

    ::g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(algorithm);

    if (force_stop_flag) {
        optimizer.setForceStopFlag(force_stop_flag);
    }

    // 3. keyframeをg2oのvertexに変換してoptimizerにセットする

    // shot vertexのcontainer
    g2o::se3::shot_vertex_container keyfrm_vtx_container(0, local_keyfrms.size() + fixed_keyfrms.size());
    // vertexに変換されたkeyframesを保存しておく
    std::unordered_map<unsigned int, data::keyframe*> all_keyfrms;

    // local keyframesをoptimizerにセット
    for (auto& id_local_keyfrm_pair : local_keyfrms) {
        auto local_keyfrm = id_local_keyfrm_pair.second;

        all_keyfrms.emplace(id_local_keyfrm_pair);
        auto keyfrm_vtx = keyfrm_vtx_container.create_vertex(local_keyfrm, local_keyfrm->id_ == 0);
        optimizer.addVertex(keyfrm_vtx);
    }

    // fixed keyframesをoptimizerにセット
    for (auto& id_fixed_keyfrm_pair : fixed_keyfrms) {
        auto fixed_keyfrm = id_fixed_keyfrm_pair.second;

        all_keyfrms.emplace(id_fixed_keyfrm_pair);
        auto keyfrm_vtx = keyfrm_vtx_container.create_vertex(fixed_keyfrm, true);
        optimizer.addVertex(keyfrm_vtx);
    }

    // 4. keyframeとlandmarkのvertexをreprojection edgeで接続する

    // landmark vertexのcontainer
    g2o::landmark_vertex_container lm_vtx_container(keyfrm_vtx_container.get_max_vertex_id() + 1, local_lms.size());

    // reprojection edgeのcontainer
    using reproj_edge_wrapper = g2o::se3::reproj_edge_wrapper<data::keyframe>;
    std::vector<reproj_edge_wrapper> reproj_edge_wraps;
    reproj_edge_wraps.reserve(all_keyfrms.size() * local_lms.size());

    // 有意水準5%のカイ2乗値
    // 自由度n=2
    constexpr float chi_sq_2D = 5.99146;
    const float sqrt_chi_sq_2D = std::sqrt(chi_sq_2D);
    // 自由度n=3
    constexpr float chi_sq_3D = 7.81473;
    const float sqrt_chi_sq_3D = std::sqrt(chi_sq_3D);

    for (auto& id_local_lm_pair : local_lms) {
        auto local_lm = id_local_lm_pair.second;

        // landmarkをg2oのvertexに変換してoptimizerにセットする
        auto lm_vtx = lm_vtx_container.create_vertex(local_lm, false);
        optimizer.addVertex(lm_vtx);

        const auto observations = local_lm->get_observations();
        for (const auto& obs : observations) {
            auto keyfrm = obs.first;
            auto idx = obs.second;
            if (!keyfrm) {
                continue;
            }
            if (keyfrm->will_be_erased()) {
                continue;
            }

            const auto keyfrm_vtx = keyfrm_vtx_container.get_vertex(keyfrm);
            const auto& undist_keypt = keyfrm->undist_keypts_.at(idx);
            const float x_right = keyfrm->stereo_x_right_.at(idx);
            const float inv_sigma_sq = keyfrm->inv_level_sigma_sq_.at(undist_keypt.octave);
            const auto sqrt_chi_sq = (keyfrm->camera_->setup_type_ == camera::setup_type_t::Monocular)
                                         ? sqrt_chi_sq_2D
                                         : sqrt_chi_sq_3D;
            auto reproj_edge_wrap = reproj_edge_wrapper(keyfrm, keyfrm_vtx, local_lm, lm_vtx,
                                                        idx, undist_keypt.pt.x, undist_keypt.pt.y, x_right,
                                                        inv_sigma_sq, sqrt_chi_sq);
            reproj_edge_wraps.push_back(reproj_edge_wrap);
            optimizer.addEdge(reproj_edge_wrap.edge_);
        }
    }

    // 5. 1回目の最適化を実行

    if (force_stop_flag) {
        if (*force_stop_flag) {
            return;
        }
    }

    optimizer.initializeOptimization();
    optimizer.optimize(num_first_iter_);

    // 6. アウトライア除去をして2回目の最適化を実行

    bool run_robust_BA = true;

    if (force_stop_flag) {
        if (*force_stop_flag) {
            run_robust_BA = false;
        }
    }

    if (run_robust_BA) {
        for (auto& reproj_edge_wrap : reproj_edge_wraps) {
            auto edge = reproj_edge_wrap.edge_;

            auto local_lm = reproj_edge_wrap.lm_;
            if (local_lm->will_be_erased()) {
                continue;
            }

            if (reproj_edge_wrap.is_monocular_) {
                if (chi_sq_2D < edge->chi2() || !reproj_edge_wrap.depth_is_positive()) {
                    reproj_edge_wrap.set_as_outlier();
                }
            }
            else {
                if (chi_sq_3D < edge->chi2() || !reproj_edge_wrap.depth_is_positive()) {
                    reproj_edge_wrap.set_as_outlier();
                }
            }

            edge->setRobustKernel(nullptr);
        }

        optimizer.initializeOptimization();
        optimizer.optimize(num_second_iter_);
    }

    // 7. アウトライアを集計する

    std::vector<std::pair<data::keyframe*, data::landmark*>> outlier_observations;
    outlier_observations.reserve(reproj_edge_wraps.size());

    for (auto& reproj_edge_wrap : reproj_edge_wraps) {
        auto edge = reproj_edge_wrap.edge_;

        auto local_lm = reproj_edge_wrap.lm_;
        if (local_lm->will_be_erased()) {
            continue;
        }

        if (reproj_edge_wrap.is_monocular_) {
            if (chi_sq_2D < edge->chi2() || !reproj_edge_wrap.depth_is_positive()) {
                outlier_observations.emplace_back(std::make_pair(reproj_edge_wrap.shot_, reproj_edge_wrap.lm_));
            }
        }
        else {
            if (chi_sq_3D < edge->chi2() || !reproj_edge_wrap.depth_is_positive()) {
                outlier_observations.emplace_back(std::make_pair(reproj_edge_wrap.shot_, reproj_edge_wrap.lm_));
            }
        }
    }

    // 8. 情報を更新

    {
        std::lock_guard<std::mutex> lock(data::map_database::mtx_database_);

        if (!outlier_observations.empty()) {
            for (auto& outlier_obs : outlier_observations) {
                auto keyfrm = outlier_obs.first;
                auto lm = outlier_obs.second;
                keyfrm->erase_landmark(lm);
                lm->erase_observation(keyfrm);
            }
        }

        for (auto id_local_keyfrm_pair : local_keyfrms) {
            auto local_keyfrm = id_local_keyfrm_pair.second;

            auto keyfrm_vtx = keyfrm_vtx_container.get_vertex(local_keyfrm);
            local_keyfrm->set_cam_pose(keyfrm_vtx->estimate());
        }

        for (auto id_local_lm_pair : local_lms) {
            auto local_lm = id_local_lm_pair.second;

            auto lm_vtx = lm_vtx_container.get_vertex(local_lm);
            local_lm->set_pos_in_world(lm_vtx->estimate());
            local_lm->update_normal_and_depth();
        }
    }
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    if(mapper_)
    {
        mapper_->SetMapUpdateFlagInTracking(true);
    }
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
}

void local_bundle_adjuster::LocalBundleAdjustmentNavState(data::keyframe *pCurKF, std::list<data::keyframe*> &local_keyfrms, bool* const force_stop_flag, data::map_database* map_db, cv::Mat& gw, mapping_module* mapper_) const
{
    #ifndef NOT_UPDATE_GYRO_BIAS
    static bool dbgLBAfopen=false;
    static ofstream dbgLBAPVRErr,dbgLBABiasErr,dbgLBAPVRErr2,dbgLBABiasErr2;
    if(!dbgLBAfopen)
    {
        dbgLBAPVRErr.open("/home/jp/opensourcecode/ORB_SLAM2/tmp/dbgLBAPVRErr.txt");
        dbgLBABiasErr.open("/home/jp/opensourcecode/ORB_SLAM2/tmp/dbgLBABiasErr.txt");
        dbgLBAPVRErr2.open("/home/jp/opensourcecode/ORB_SLAM2/tmp/dbgLBAPVRErr2.txt");
        dbgLBABiasErr2.open("/home/jp/opensourcecode/ORB_SLAM2/tmp/dbgLBABiasErr2.txt");
        if(dbgLBAPVRErr.is_open() && dbgLBABiasErr.is_open() && dbgLBAPVRErr2.is_open() && dbgLBABiasErr2.is_open())
        {
            std::cerr<<"file opened."<<std::endl;
            dbgLBAfopen = true;
        }
        else
        {
            std::cerr<<"file open error in dbgLBAfopen"<<std::endl;
            dbgLBAfopen = false;
        }
        dbgLBAPVRErr<<std::fixed<<std::setprecision(6);
        dbgLBABiasErr<<std::fixed<<std::setprecision(10);
        dbgLBAPVRErr2<<std::fixed<<std::setprecision(6);
        dbgLBABiasErr2<<std::fixed<<std::setprecision(10);
    }
    #endif

    // Check current KeyFrame in local window
    if(pCurKF != local_keyfrms.back())
        std::cerr<<"pCurKF != local_keyfrms.back. check"<<std::endl;

    // Extrinsics
    Matrix4d Tbc = ConfigParam::GetEigTbc();
    Matrix3d Rbc = Tbc.topLeftCorner(3,3);
    Vector3d Pbc = Tbc.topRightCorner(3,1);
    // Gravity vector in world frame
    Vector3d GravityVec = util::converter::toVector3d(gw);

    // 1. local/fixed keyframes, local landmarksを集計する

    // correct local keyframes of the current keyframe
    //std::unordered_map<unsigned int, data::keyframe*> local_keyfrms;

    //local_keyfrms[curr_keyfrm->id_] = curr_keyfrm;
    //const auto curr_covisibilities = curr_keyfrm->graph_node_->get_covisibilities();
    for(std::list<data::keyframe*>::const_iterator lit=local_keyfrms.begin(), lend=local_keyfrms.end(); lit!=lend; lit++)
    {
        data::keyframe* pKFi = *lit;
        pKFi->mnBALocalForKF = pCurKF->id_;
    }

    // correct local landmarks seen in local keyframes
    std::list<data::landmark*> local_lms;
    //std::unordered_map<unsigned int, data::landmark*> local_lms;
    for(std::list<data::keyframe*>::const_iterator lit=local_keyfrms.begin() , lend=local_keyfrms.end(); lit!=lend; lit++)
    {
        std::vector<data::landmark*> landmarks = (*lit)->get_landmarks();
        for(std::vector<data::landmark*>::iterator vit=landmarks.begin(), vend=landmarks.end(); vit!=vend; vit++)
        {
            data::landmark* pMP = *vit;
            if(pMP)
                if(!pMP->will_be_erased())
                    if(pMP->mnBALocalForKF!=pCurKF->id_)
                    {
                        local_lms.push_back(pMP);
                        pMP->mnBALocalForKF=pCurKF->id_;
                    }
        }
    }

    // fixed keyframes: keyframes which observe local landmarks but which are NOT in local keyframes
    std::list<data::keyframe*> fixed_keyfrms;
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    data::keyframe* pKFPrevLocal = local_keyfrms.front()->GetPrevKeyFrame();
    //data::keyframe* pKFPrevLocal;// = lLocalKeyFrames.front()->GetPrevKeyFrame();
    if(pKFPrevLocal)
    {
        // Test log
        if(pKFPrevLocal->will_be_erased()) std::cerr<<"KeyFrame before local window is bad?"<<std::endl;
        if(pKFPrevLocal->mnBAFixedForKF == pCurKF->id_) std::cerr<<"KeyFrame before local, has been added to lFixedKF?"<<std::endl;
        if(pKFPrevLocal->mnBALocalForKF == pCurKF->id_) std::cerr<<"KeyFrame before local, has been added to lLocalKF?"<<std::endl;

        pKFPrevLocal->mnBAFixedForKF = pCurKF->id_;
        if(!pKFPrevLocal->will_be_erased())
        {
            fixed_keyfrms.push_back(pKFPrevLocal);
        }
        else
            std::cerr<<"pKFPrevLocal is Bad?"<<std::endl;
    }
    // Test log
    else {std::cerr<<"pKFPrevLocal is NULL?"<<std::endl;}
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    for(std::list<data::landmark*>::iterator lit=local_lms.begin(), lend=local_lms.end(); lit!=lend; lit++)
    {
        std::map<data::keyframe*,unsigned int> observations = (*lit)->get_observations();
        for(std::map<data::keyframe*,unsigned int>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
        {
            data::keyframe* fixed_keyfrm = mit->first;
            if(fixed_keyfrm->mnBALocalForKF!=pCurKF->id_ && fixed_keyfrm->mnBAFixedForKF!=pCurKF->id_)
            {
                fixed_keyfrm->mnBAFixedForKF=pCurKF->id_;
                if(!fixed_keyfrm->will_be_erased())
                    fixed_keyfrms.push_back(fixed_keyfrm);
            }
        }
    }

    // 2. optimizerを構築

    auto linear_solver = ::g2o::make_unique<::g2o::LinearSolverCSparse<::g2o::BlockSolverX::PoseMatrixType>>();
    auto block_solver = ::g2o::make_unique<::g2o::BlockSolverX>(std::move(linear_solver));
    auto algorithm = new ::g2o::OptimizationAlgorithmLevenberg(std::move(block_solver));

    ::g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(algorithm);

    if (force_stop_flag) {
        optimizer.setForceStopFlag(force_stop_flag);
    }

    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    unsigned long maxKFid = 0;
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------

    // 3. keyframeをg2oのvertexに変換してoptimizerにセットする

    // shot vertexのcontainer
    g2o::se3::shot_vertex_container keyfrm_vtx_container(0, local_keyfrms.size() + fixed_keyfrms.size());
    // vertexに変換されたkeyframesを保存しておく
    std::unordered_map<unsigned int, data::keyframe*> all_keyfrms;

    // local keyframesをoptimizerにセット
    for(std::list<data::keyframe*>::const_iterator lit=local_keyfrms.begin(), lend=local_keyfrms.end(); lit!=lend; lit++)
    {
        data::keyframe* local_keyfrm = *lit;
        //-------------------------------------------------------------------------------------------
        //-------------------------------------------------------------------------------------------
        //-------------------------------------------------------------------------------------------
        int idKF = local_keyfrm->id_*2;
        /*VertexNavStatePVR(local_keyfrm, idKF, optimizer);
        VertexNavStateBias(local_keyfrm, idKF, optimizer);*/
        // Vertex of PVR
        {
            ::g2o::VertexNavStatePVR * vNSPVR = new ::g2o::VertexNavStatePVR();
            vNSPVR->setEstimate(local_keyfrm->GetNavState());
            vNSPVR->setId(idKF);
            vNSPVR->setFixed(false);
            optimizer.addVertex(vNSPVR);
        }
        // Vertex of Bias
        {
            ::g2o::VertexNavStateBias * vNSBias = new ::g2o::VertexNavStateBias();
            vNSBias->setEstimate(local_keyfrm->GetNavState());
            vNSBias->setId(idKF+1);
            vNSBias->setFixed(false);
            optimizer.addVertex(vNSBias);
        }
        if(idKF + 1 > maxKFid)
            maxKFid = idKF + 1;
        // Test log
        if(local_keyfrm->id_ == 0) std::cerr<<"local_keyfrm->id_ == 0, shouldn't in LocalBA of NavState"<<std::endl;
        //-------------------------------------------------------------------------------------------
        //-------------------------------------------------------------------------------------------
        //-------------------------------------------------------------------------------------------

        /*all_keyfrms.emplace(id_local_keyfrm_pair);
        auto keyfrm_vtx = keyfrm_vtx_container.create_vertex(local_keyfrm, local_keyfrm->id_ == 0);
        optimizer.addVertex(keyfrm_vtx);*/
    }

    // fixed keyframesをoptimizerにセット
    for(std::list<data::keyframe*>::iterator lit=fixed_keyfrms.begin(), lend=fixed_keyfrms.end(); lit!=lend; lit++)
    {
        data::keyframe* fixed_keyfrm = *lit;
        //-------------------------------------------------------------------------------------------
        //-------------------------------------------------------------------------------------------
        //-------------------------------------------------------------------------------------------
        int idKF = fixed_keyfrm->id_*2;
        // For common fixed KeyFrames, only add PVR vertex
        {
            ::g2o::VertexNavStatePVR * vNSPVR = new ::g2o::VertexNavStatePVR();
            vNSPVR->setEstimate(fixed_keyfrm->GetNavState());
            vNSPVR->setId(idKF);
            vNSPVR->setFixed(true);
            optimizer.addVertex(vNSPVR);
        }
        // For Local-Window-Previous KeyFrame, add Bias vertex
        if(fixed_keyfrm == pKFPrevLocal)
        {
            ::g2o::VertexNavStateBias * vNSBias = new ::g2o::VertexNavStateBias();
            vNSBias->setEstimate(fixed_keyfrm->GetNavState());
            vNSBias->setId(idKF+1);
            vNSBias->setFixed(true);
            optimizer.addVertex(vNSBias);
        }

        if(idKF + 1 > maxKFid)
            maxKFid = idKF + 1;
        //-------------------------------------------------------------------------------------------
        //-------------------------------------------------------------------------------------------
        //-------------------------------------------------------------------------------------------

        /*all_keyfrms.emplace(id_fixed_keyfrm_pair);
        auto keyfrm_vtx = keyfrm_vtx_container.create_vertex(fixed_keyfrm, true);
        optimizer.addVertex(keyfrm_vtx);*/
    }

    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    // Edges between KeyFrames in Local Window
    // and
    // Edges between 1st KeyFrame of Local Window and its previous (fixed)KeyFrame - pKFPrevLocal
    std::vector<::g2o::EdgeNavStatePVR*> vpEdgesNavStatePVR;
    std::vector<::g2o::EdgeNavStateBias*> vpEdgesNavStateBias;
    // Use chi2inv() in MATLAB to compute the value corresponding to 0.95/0.99 prob. w.r.t 15DOF: 24.9958/30.5779
    // 12.592/16.812 for 0.95/0.99 6DoF
    // 16.919/21.666 for 0.95/0.99 9DoF
    //const float thHuberNavState = sqrt(30.5779);
    const float thHuberNavStatePVR = sqrt(21.666);
    const float thHuberNavStateBias = sqrt(16.812);
    // Inverse covariance of bias random walk
#ifndef NOT_UPDATE_GYRO_BIAS
    Matrix<double,6,6> InvCovBgaRW = Matrix<double,6,6>::Identity();
    InvCovBgaRW.topLeftCorner(3,3) = Matrix3d::Identity()/IMUData::getGyrBiasRW2();       // Gyroscope bias random walk, covariance INVERSE
    InvCovBgaRW.bottomRightCorner(3,3) = Matrix3d::Identity()/IMUData::getAccBiasRW2();   // Accelerometer bias random walk, covariance INVERSE
#else
    Matrix<double,3,3> InvCovBgaRW = Matrix3d::Identity()/IMUData::getAccBiasRW2();   // Accelerometer bias random walk, covariance INVERSE
#endif

    for(std::list<data::keyframe*>::const_iterator lit=local_keyfrms.begin(), lend=local_keyfrms.end(); lit!=lend; lit++)

    for (auto& local_keyfrm : local_keyfrms)
    {
        data::keyframe* pKF1 = *lit; // Current KF, store the IMU pre-integration between previous-current
        data::keyframe* pKF0 = pKF1->GetPrevKeyFrame();   // Previous KF

        // PVR edge
        {
            ::g2o::EdgeNavStatePVR * epvr = new ::g2o::EdgeNavStatePVR();
            epvr->setVertex(0, dynamic_cast<::g2o::OptimizableGraph::Vertex*>(optimizer.vertex(2*pKF0->id_)));
            epvr->setVertex(1, dynamic_cast<::g2o::OptimizableGraph::Vertex*>(optimizer.vertex(2*pKF1->id_)));
            epvr->setVertex(2, dynamic_cast<::g2o::OptimizableGraph::Vertex*>(optimizer.vertex(2*pKF0->id_+1)));
            epvr->setMeasurement(pKF1->GetIMUPreInt());

            Matrix9d InvCovPVR = pKF1->GetIMUPreInt().getCovPVPhi().inverse();
            epvr->setInformation(InvCovPVR);
            epvr->SetParams(GravityVec);

            ::g2o::RobustKernelHuber* rk = new ::g2o::RobustKernelHuber;
            epvr->setRobustKernel(rk);
            rk->setDelta(thHuberNavStatePVR);

            optimizer.addEdge(epvr);
            vpEdgesNavStatePVR.push_back(epvr);
        }
        // Bias edge
        {
            ::g2o::EdgeNavStateBias * ebias = new ::g2o::EdgeNavStateBias();
            ebias->setVertex(0, dynamic_cast<::g2o::OptimizableGraph::Vertex*>(optimizer.vertex(2*pKF0->id_+1)));
            ebias->setVertex(1, dynamic_cast<::g2o::OptimizableGraph::Vertex*>(optimizer.vertex(2*pKF1->id_+1)));
            ebias->setMeasurement(pKF1->GetIMUPreInt());

            ebias->setInformation(InvCovBgaRW/pKF1->GetIMUPreInt().getDeltaTime());

            ::g2o::RobustKernelHuber* rk = new ::g2o::RobustKernelHuber;
            ebias->setRobustKernel(rk);
            rk->setDelta(thHuberNavStateBias);

            optimizer.addEdge(ebias);
            vpEdgesNavStateBias.push_back(ebias);
        }

        // Test log
        if(pKF1->GetIMUPreInt().getDeltaTime() < 1e-3)
        {
            std::cerr<<"IMU pre-integrator delta time between 2 KFs too small: "<<pKF1->GetIMUPreInt().getDeltaTime()<<std::endl;
            std::cerr<<"No EdgeNavState added"<<std::endl;
            continue;
        }
        // Lo dejo comentado porque solo imprime por pantalla
        /*if(lit == lLocalKeyFrames.begin())
        {
            // First KF in Local Window, link (fixed) pKFPrevLocal
            if(pKF0 != pKFPrevLocal) cerr<<"pKF0 != pKFPrevLocal for 1st KF in Local Window, id: "<<pKF0->mnId<<","<<pKFPrevLocal->mnId<<endl;
        }
        else
        {
            // KFs in Local Window, link another local KF
        }*/
    }
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------

    // 4. keyframeとlandmarkのvertexをreprojection edgeで接続する

    // landmark vertexのcontainer
    g2o::landmark_vertex_container lm_vtx_container(keyfrm_vtx_container.get_max_vertex_id() + 1, local_lms.size());

    /*// reprojection edgeのcontainer
    using reproj_edge_wrapper = g2o::se3::reproj_edge_wrapper<data::keyframe>;
    std::vector<reproj_edge_wrapper> reproj_edge_wraps;
    reproj_edge_wraps.reserve(all_keyfrms.size() * local_lms.size());*/

    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    // Set MapPoint vertices
    const int nExpectedSize = (local_keyfrms.size() + fixed_keyfrms.size())*local_lms.size();

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

    std::vector<data::keyframe*> vpEdgeKFMono;
    vpEdgeKFMono.reserve(nExpectedSize);

    std::vector<data::landmark*> vpMapPointEdgeMono;
    vpMapPointEdgeMono.reserve(nExpectedSize);
    const float thHuberMono = sqrt(5.991);
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------

    // 有意水準5%のカイ2乗値
    // 自由度n=2
    constexpr float chi_sq_2D = 5.99146;
    const float sqrt_chi_sq_2D = std::sqrt(chi_sq_2D);
    // 自由度n=3
    constexpr float chi_sq_3D = 7.81473;
    const float sqrt_chi_sq_3D = std::sqrt(chi_sq_3D);

    for(std::list<data::landmark*>::iterator lit=local_lms.begin(), lend=local_lms.end(); lit!=lend; lit++)
    {
        //auto local_lm = id_local_lm_pair.second;
        //-------------------------------------------------------------------------------------------
        //-------------------------------------------------------------------------------------------
        //-------------------------------------------------------------------------------------------
        data::landmark* pMP = *lit;
        ::g2o::VertexSBAPointXYZ* vPoint = new ::g2o::VertexSBAPointXYZ();
        vPoint->setEstimate(pMP->get_pos_in_world());
        int id = pMP->id_+maxKFid+1;
        vPoint->setId(id);
        vPoint->setMarginalized(true);
        optimizer.addVertex(vPoint);

        cv::Mat Pw;
        eigen2cv(pMP->get_pos_in_world(), Pw);
        //-------------------------------------------------------------------------------------------
        //-------------------------------------------------------------------------------------------
        //-------------------------------------------------------------------------------------------
        /*
        // landmarkをg2oのvertexに変換してoptimizerにセットする
        auto lm_vtx = lm_vtx_container.create_vertex(local_lm, false);
        optimizer.addVertex(lm_vtx);
        */
        //-------------------------------------------------------------------------------------------
        //-------------------------------------------------------------------------------------------
        //-------------------------------------------------------------------------------------------

        const std::map<data::keyframe*,unsigned int> observations = pMP->get_observations();
        for(std::map<data::keyframe*,unsigned int>::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
        {
            //auto keyfrm = obs.first;
            data::keyframe* pKFi = mit->first;
            if (!pKFi->will_be_erased()) {
                const cv::KeyPoint &kpUn = pKFi->undist_keypts_[mit->second];

                // Monocular observation
                if(pKFi->stereo_x_right_[mit->second]<0)
                {
                    Eigen::Matrix<double,2,1> obs;
                    obs << kpUn.pt.x, kpUn.pt.y;

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

                    e->setVertex(0, dynamic_cast<::g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
                    e->setVertex(1, dynamic_cast<::g2o::OptimizableGraph::Vertex*>(optimizer.vertex(2*pKFi->id_)));
                    e->setMeasurement(obs);
                    const float &invSigma2 = pKFi->inv_level_sigma_sq_[kpUn.octave];

                    e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);

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

                    cv::Mat cam_param = ConfigParam::GetCamMatrix();
                    float fx = cam_param.at<float>(0,0);
                    float fy = cam_param.at<float>(1,1);
                    float cx = cam_param.at<float>(0,2);
                    float cy = cam_param.at<float>(1,2);
                    e->SetParams(fx,fy,cx,cy,Rbc,Pbc);

                    optimizer.addEdge(e);
                    vpEdgesMono.push_back(e);
                    vpEdgeKFMono.push_back(pKFi);
                    vpMapPointEdgeMono.push_back(pMP);
                }
                else
                {
                    // Test log
                   std:: cerr<<"Stereo not supported yet, why here?? check."<<std::endl;
                }
            }
            /*
            if (!keyfrm) {
                continue;
            }
            if (keyfrm->will_be_erased()) {
                continue;
            }

            const auto keyfrm_vtx = keyfrm_vtx_container.get_vertex(keyfrm);
            const auto& undist_keypt = keyfrm->undist_keypts_.at(idx);
            const float x_right = keyfrm->stereo_x_right_.at(idx);
            const float inv_sigma_sq = keyfrm->inv_level_sigma_sq_.at(undist_keypt.octave);
            const auto sqrt_chi_sq = (keyfrm->camera_->setup_type_ == camera::setup_type_t::Monocular)
                                         ? sqrt_chi_sq_2D
                                         : sqrt_chi_sq_3D;
            auto reproj_edge_wrap = reproj_edge_wrapper(keyfrm, keyfrm_vtx, local_lm, lm_vtx,
                                                        idx, undist_keypt.pt.x, undist_keypt.pt.y, x_right,
                                                        inv_sigma_sq, sqrt_chi_sq);
            reproj_edge_wraps.push_back(reproj_edge_wrap);
            optimizer.addEdge(reproj_edge_wrap.edge_);*/
        }
    }

    // 5. 1回目の最適化を実行

    if (force_stop_flag) {
        if (*force_stop_flag) {
            return;
        }
    }

    optimizer.initializeOptimization();
    optimizer.optimize(num_first_iter_);

    // 6. アウトライア除去をして2回目の最適化を実行

    bool run_robust_BA = true;

    if (force_stop_flag) {
        if (*force_stop_flag) {
            run_robust_BA = false;
        }
    }

    if (run_robust_BA) {
        for(size_t i=0, iend=vpEdgesMono.size(); i<iend;i++)
        {
            ::g2o::EdgeNavStatePVRPointXYZ* e = vpEdgesMono[i];
            data::landmark* pMP = vpMapPointEdgeMono[i];

            if(pMP->will_be_erased())
                continue;

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

            e->setRobustKernel(0);
        }

        // Check inlier observations
        int tmpcnt=0;
        for(size_t i=0, iend=vpEdgesNavStatePVR.size(); i<iend; i++)
        {
            ::g2o::EdgeNavStatePVR* e = vpEdgesNavStatePVR[i];
            if(e->chi2()>21.666)
            {
                //e->setLevel(1);
                //cout<<"1 PVRedge "<<i<<", chi2 "<<e->chi2()<<". ";
                tmpcnt++;
            }
            //e->setRobustKernel(0);

        #ifndef NOT_UPDATE_GYRO_BIAS
                e->computeError();
                Vector9d err=e->error();
                for(int n=0;n<9;n++)
                    dbgLBAPVRErr<<err[n]<<" ";
                dbgLBAPVRErr<<endl;
        #endif
        }

        if(tmpcnt>0)
            std::cout<<std::endl;

        // Optimize again without the outliers
        optimizer.initializeOptimization();
        optimizer.optimize(num_second_iter_);
        /*for (auto& reproj_edge_wrap : reproj_edge_wraps) {
            auto edge = reproj_edge_wrap.edge_;

            auto local_lm = reproj_edge_wrap.lm_;
            if (local_lm->will_be_erased()) {
                continue;
            }

            if (reproj_edge_wrap.is_monocular_) {
                if (chi_sq_2D < edge->chi2() || !reproj_edge_wrap.depth_is_positive()) {
                    reproj_edge_wrap.set_as_outlier();
                }
            }
            else {
                if (chi_sq_3D < edge->chi2() || !reproj_edge_wrap.depth_is_positive()) {
                    reproj_edge_wrap.set_as_outlier();
                }
            }

            edge->setRobustKernel(nullptr);
        }

        optimizer.initializeOptimization();
        optimizer.optimize(num_second_iter_);*/
    }

    // 7. アウトライアを集計する
    std::vector<std::pair<data::keyframe*, data::landmark*>> outlier_observations;
    //outlier_observations.reserve(reproj_edge_wraps.size());
    outlier_observations.reserve(vpEdgesMono.size());
    double PosePointchi2=0, PosePosechi2=0, BiasBiaschi2=0;

    for(size_t i=0, iend=vpEdgesMono.size(); i<iend;i++)
    {
        ::g2o::EdgeNavStatePVRPointXYZ* e = vpEdgesMono[i];
        data::landmark* pMP = vpMapPointEdgeMono[i];

        if(pMP->will_be_erased())
            continue;

        if(e->chi2()>5.991 || !e->isDepthPositive())
        {
            data::keyframe* pKFi = vpEdgeKFMono[i];
            outlier_observations.push_back(std::make_pair(pKFi,pMP));
        }

        PosePointchi2 += e->chi2();
    }

     // Debug log
    // Check inlier observations
    int tmpcnt2=0;
    for(size_t i=0, iend=vpEdgesNavStatePVR.size(); i<iend; i++)
    {
        ::g2o::EdgeNavStatePVR* e = vpEdgesNavStatePVR[i];
        if(e->chi2()>21.666)
        {
            //cout<<"2 PVRedge "<<i<<", chi2 "<<e->chi2()<<". ";
            tmpcnt2++;
        }

    #ifndef NOT_UPDATE_GYRO_BIAS
        e->computeError();
        Vector9d err=e->error();
        for(int n=0;n<9;n++)
            dbgLBAPVRErr2<<err[n]<<" ";
        dbgLBAPVRErr2<<endl;
    #endif
    }

        //cout<<endl<<"edge bias ns bad: ";
    for(size_t i=0, iend=vpEdgesNavStateBias.size(); i<iend; i++)
    {
        ::g2o::EdgeNavStateBias* e = vpEdgesNavStateBias[i];
        if(e->chi2()>16.812)
        {
            //cout<<"2 Bias edge "<<i<<", chi2 "<<e->chi2()<<". ";
            tmpcnt2++;
        }

        #ifndef NOT_UPDATE_GYRO_BIAS
            e->computeError();
            Vector6d err=e->error();
            for(int n=0;n<6;n++)
                dbgLBABiasErr2<<err[n]<<" ";
            dbgLBABiasErr2<<endl;
        #endif
    }
    if(tmpcnt2>0)
        std::cout<<std::endl;
    /*
    for (auto& reproj_edge_wrap : reproj_edge_wraps) {
        auto edge = reproj_edge_wrap.edge_;

        auto local_lm = reproj_edge_wrap.lm_;
        if (local_lm->will_be_erased()) {
            continue;
        }

        if (reproj_edge_wrap.is_monocular_) {
            if (chi_sq_2D < edge->chi2() || !reproj_edge_wrap.depth_is_positive()) {
                outlier_observations.emplace_back(std::make_pair(reproj_edge_wrap.shot_, reproj_edge_wrap.lm_));
            }
        }
        else {
            if (chi_sq_3D < edge->chi2() || !reproj_edge_wrap.depth_is_positive()) {
                outlier_observations.emplace_back(std::make_pair(reproj_edge_wrap.shot_, reproj_edge_wrap.lm_));
            }
        }
    }*/

    // 8. 情報を更新

    {
        std::lock_guard<std::mutex> lock(data::map_database::mtx_database_);

        if (!outlier_observations.empty()) {
            for (auto& outlier_obs : outlier_observations) {
                auto keyfrm = outlier_obs.first;
                auto lm = outlier_obs.second;
                keyfrm->erase_landmark(lm);
                lm->erase_observation(keyfrm);
            }
        }
        for(std::list<data::keyframe*>::const_iterator lit=local_keyfrms.begin(), lend=local_keyfrms.end(); lit!=lend; lit++)
        {
            data::keyframe* pKFi = *lit;
            ::g2o::VertexNavStatePVR* vNSPVR = static_cast<::g2o::VertexNavStatePVR*>(optimizer.vertex(2*pKFi->id_));
            ::g2o::VertexNavStateBias* vNSBias = static_cast<::g2o::VertexNavStateBias*>(optimizer.vertex(2*pKFi->id_+1));
            // In optimized navstate, bias not changed, delta_bias not zero, should be added to bias
            const NavState& optPVRns = vNSPVR->estimate();
            const NavState& optBiasns = vNSBias->estimate();
            NavState primaryns = pKFi->GetNavState();
            // Update NavState
            pKFi->SetNavStatePos(optPVRns.Get_P());
            pKFi->SetNavStateVel(optPVRns.Get_V());
            pKFi->SetNavStateRot(optPVRns.Get_R());
            //if(optBiasns.Get_dBias_Acc().norm()<1e-2 && optBiasns.Get_BiasGyr().norm()<1e-4)
            //{
            pKFi->SetNavStateDeltaBg(optBiasns.Get_dBias_Gyr());
            pKFi->SetNavStateDeltaBa(optBiasns.Get_dBias_Acc());

            // Update pose Tcw
            pKFi->UpdatePoseFromNS(ConfigParam::GetMatTbc());

            // Test log
            if( (primaryns.Get_BiasGyr() - optPVRns.Get_BiasGyr()).norm() > 1e-6 || (primaryns.Get_BiasGyr() - optBiasns.Get_BiasGyr()).norm() > 1e-6 )
                std::cerr<<"gyr bias change in optimization?"<<std::endl;
            if( (primaryns.Get_BiasAcc() - optPVRns.Get_BiasAcc()).norm() > 1e-6 || (primaryns.Get_BiasAcc() - optBiasns.Get_BiasAcc()).norm() > 1e-6 )
                std::cerr<<"acc bias change in optimization?"<<std::endl;
        }
        for(std::list<data::landmark*>::const_iterator lit=local_lms.begin(), lend=local_lms.end(); lit!=lend; lit++)
        {
            data::landmark* pMP = *lit;
            ::g2o::VertexSBAPointXYZ* vPoint = static_cast<::g2o::VertexSBAPointXYZ*>(optimizer.vertex(pMP->id_+maxKFid+1));
            pMP->set_pos_in_world(vPoint->estimate());
            pMP->update_normal_and_depth();
        }

        /*
        for (auto id_local_keyfrm_pair : local_keyfrms) {
            auto local_keyfrm = id_local_keyfrm_pair.second;

            auto keyfrm_vtx = keyfrm_vtx_container.get_vertex(local_keyfrm);
            local_keyfrm->set_cam_pose(keyfrm_vtx->estimate());
        }

        for (auto id_local_lm_pair : local_lms) {
            auto local_lm = id_local_lm_pair.second;

            auto lm_vtx = lm_vtx_container.get_vertex(local_lm);
            local_lm->set_pos_in_world(lm_vtx->estimate());
            local_lm->update_normal_and_depth();
        }*/
    }
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    if(mapper_)
    {
        mapper_->SetMapUpdateFlagInTracking(true);
    }
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
}

void local_bundle_adjuster::LocalBundleAdjustmentNavState(data::keyframe *pCurKF, std::unordered_map<unsigned int, data::keyframe*> &local_keyfrms, bool* const force_stop_flag, data::map_database* map_db, cv::Mat& gw, mapping_module* mapper_) const
{
    #ifndef NOT_UPDATE_GYRO_BIAS
    static bool dbgLBAfopen=false;
    static ofstream dbgLBAPVRErr,dbgLBABiasErr,dbgLBAPVRErr2,dbgLBABiasErr2;
    if(!dbgLBAfopen)
    {
        dbgLBAPVRErr.open("/home/jp/opensourcecode/ORB_SLAM2/tmp/dbgLBAPVRErr.txt");
        dbgLBABiasErr.open("/home/jp/opensourcecode/ORB_SLAM2/tmp/dbgLBABiasErr.txt");
        dbgLBAPVRErr2.open("/home/jp/opensourcecode/ORB_SLAM2/tmp/dbgLBAPVRErr2.txt");
        dbgLBABiasErr2.open("/home/jp/opensourcecode/ORB_SLAM2/tmp/dbgLBABiasErr2.txt");
        if(dbgLBAPVRErr.is_open() && dbgLBABiasErr.is_open() && dbgLBAPVRErr2.is_open() && dbgLBABiasErr2.is_open())
        {
            std::cerr<<"file opened."<<std::endl;
            dbgLBAfopen = true;
        }
        else
        {
            std::cerr<<"file open error in dbgLBAfopen"<<std::endl;
            dbgLBAfopen = false;
        }
        dbgLBAPVRErr<<std::fixed<<std::setprecision(6);
        dbgLBABiasErr<<std::fixed<<std::setprecision(10);
        dbgLBAPVRErr2<<std::fixed<<std::setprecision(6);
        dbgLBABiasErr2<<std::fixed<<std::setprecision(10);
    }
    #endif

    // Check current KeyFrame in local window
    /*if(pCurKF != lLocalKeyFrames.back())
        std::cerr<<"pCurKF != lLocalKeyFrames.back. check"<<std::endl;*/

    // Extrinsics
    Matrix4d Tbc = ConfigParam::GetEigTbc();
    Matrix3d Rbc = Tbc.topLeftCorner(3,3);
    Vector3d Pbc = Tbc.topRightCorner(3,1);
    // Gravity vector in world frame
    Vector3d GravityVec = util::converter::toVector3d(gw);

    // 1. local/fixed keyframes, local landmarksを集計する

    // correct local keyframes of the current keyframe
    //std::unordered_map<unsigned int, data::keyframe*> local_keyfrms;

    //local_keyfrms[curr_keyfrm->id_] = curr_keyfrm;
    //const auto curr_covisibilities = curr_keyfrm->graph_node_->get_covisibilities();
    /*for (auto local_keyfrm : local_keyfrms) {
        auto pKFi = local_keyfrm.second;
        pKFi-> = pCurKF->id_;
    }*/

    // correct local landmarks seen in local keyframes
    std::unordered_map<unsigned int, data::landmark*> local_lms;

    for (auto local_keyfrm : local_keyfrms) {
        const auto landmarks = local_keyfrm.second->get_landmarks();
        for (auto local_lm : landmarks) {
            if (!local_lm) {
                continue;
            }
            if (local_lm->will_be_erased()) {
                continue;
            }

            // 重複を避ける
            if (local_lms.count(local_lm->id_)) {
                continue;
            }

            local_lms[local_lm->id_] = local_lm;
        }
    }

    // fixed keyframes: keyframes which observe local landmarks but which are NOT in local keyframes
    std::unordered_map<unsigned int, data::keyframe*> fixed_keyfrms;
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    data::keyframe* pKFPrevLocal = fixed_keyfrms.begin()->second->GetPrevKeyFrame();
    //data::keyframe* pKFPrevLocal;// = lLocalKeyFrames.front()->GetPrevKeyFrame();
    if(pKFPrevLocal)
    {
        // Test log
        if(pKFPrevLocal->will_be_erased()) std::cerr<<"KeyFrame before local window is bad?"<<std::endl;
        /*if(pKFPrevLocal->mnBAFixedForKF == pCurKF->id_) std::cerr<<"KeyFrame before local, has been added to lFixedKF?"<<std::endl;
        if(pKFPrevLocal->mnBALocalForKF == pCurKF->id_) std::cerr<<"KeyFrame before local, has been added to lLocalKF?"<<std::endl;*/

        //pKFPrevLocal->mnBAFixedForKF = pCurKF->id_;
        if(!pKFPrevLocal->will_be_erased())
        {
            fixed_keyfrms[pKFPrevLocal->id_] = pKFPrevLocal;
        }
        else
            std::cerr<<"pKFPrevLocal is Bad?"<<std::endl;
    }
    // Test log
    else {std::cerr<<"pKFPrevLocal is NULL?"<<std::endl;}
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    for (auto local_lm : local_lms) {
        const auto observations = local_lm.second->get_observations();
        for (auto& obs : observations) {
            auto fixed_keyfrm = obs.first;
            if (!fixed_keyfrm) {
                continue;
            }
            if (fixed_keyfrm->will_be_erased()) {
                continue;
            }

            // local keyframesに属しているときは追加しない
            if (local_keyfrms.count(fixed_keyfrm->id_)) {
                continue;
            }

            // 重複を避ける
            if (fixed_keyfrms.count(fixed_keyfrm->id_)) {
                continue;
            }

            fixed_keyfrms[fixed_keyfrm->id_] = fixed_keyfrm;
        }
    }

    // 2. optimizerを構築

    auto linear_solver = ::g2o::make_unique<::g2o::LinearSolverCSparse<::g2o::BlockSolverX::PoseMatrixType>>();
    auto block_solver = ::g2o::make_unique<::g2o::BlockSolverX>(std::move(linear_solver));
    auto algorithm = new ::g2o::OptimizationAlgorithmLevenberg(std::move(block_solver));

    ::g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(algorithm);

    if (force_stop_flag) {
        optimizer.setForceStopFlag(force_stop_flag);
    }

    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    unsigned long maxKFid = 0;
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------

    // 3. keyframeをg2oのvertexに変換してoptimizerにセットする

    // shot vertexのcontainer
    g2o::se3::shot_vertex_container keyfrm_vtx_container(0, local_keyfrms.size() + fixed_keyfrms.size());
    // vertexに変換されたkeyframesを保存しておく
    std::unordered_map<unsigned int, data::keyframe*> all_keyfrms;

    // local keyframesをoptimizerにセット
    for (auto& id_local_keyfrm_pair : local_keyfrms) {
        auto local_keyfrm = id_local_keyfrm_pair.second;
        //-------------------------------------------------------------------------------------------
        //-------------------------------------------------------------------------------------------
        //-------------------------------------------------------------------------------------------
        int idKF = local_keyfrm->id_*2;
        /*VertexNavStatePVR(local_keyfrm, idKF, optimizer);
        VertexNavStateBias(local_keyfrm, idKF, optimizer);*/
        // Vertex of PVR
        {
            ::g2o::VertexNavStatePVR * vNSPVR = new ::g2o::VertexNavStatePVR();
            vNSPVR->setEstimate(local_keyfrm->GetNavState());
            vNSPVR->setId(idKF);
            vNSPVR->setFixed(false);
            optimizer.addVertex(vNSPVR);
        }
        // Vertex of Bias
        {
            ::g2o::VertexNavStateBias * vNSBias = new ::g2o::VertexNavStateBias();
            vNSBias->setEstimate(local_keyfrm->GetNavState());
            vNSBias->setId(idKF+1);
            vNSBias->setFixed(false);
            optimizer.addVertex(vNSBias);
        }
        if(idKF + 1 > maxKFid)
            maxKFid = idKF + 1;
        // Test log
        if(local_keyfrm->id_ == 0) std::cerr<<"local_keyfrm->id_ == 0, shouldn't in LocalBA of NavState"<<std::endl;
        //-------------------------------------------------------------------------------------------
        //-------------------------------------------------------------------------------------------
        //-------------------------------------------------------------------------------------------

        /*all_keyfrms.emplace(id_local_keyfrm_pair);
        auto keyfrm_vtx = keyfrm_vtx_container.create_vertex(local_keyfrm, local_keyfrm->id_ == 0);
        optimizer.addVertex(keyfrm_vtx);*/
    }

    // fixed keyframesをoptimizerにセット
    for (auto& id_fixed_keyfrm_pair : fixed_keyfrms) {
        auto fixed_keyfrm = id_fixed_keyfrm_pair.second;
        //-------------------------------------------------------------------------------------------
        //-------------------------------------------------------------------------------------------
        //-------------------------------------------------------------------------------------------
        int idKF = fixed_keyfrm->id_*2;
        // For common fixed KeyFrames, only add PVR vertex
        {
            ::g2o::VertexNavStatePVR * vNSPVR = new ::g2o::VertexNavStatePVR();
            vNSPVR->setEstimate(fixed_keyfrm->GetNavState());
            vNSPVR->setId(idKF);
            vNSPVR->setFixed(true);
            optimizer.addVertex(vNSPVR);
        }
        // For Local-Window-Previous KeyFrame, add Bias vertex
        if(fixed_keyfrm == pKFPrevLocal)
        {
            ::g2o::VertexNavStateBias * vNSBias = new ::g2o::VertexNavStateBias();
            vNSBias->setEstimate(fixed_keyfrm->GetNavState());
            vNSBias->setId(idKF+1);
            vNSBias->setFixed(true);
            optimizer.addVertex(vNSBias);
        }

        if(idKF + 1 > maxKFid)
            maxKFid = idKF + 1;
        //-------------------------------------------------------------------------------------------
        //-------------------------------------------------------------------------------------------
        //-------------------------------------------------------------------------------------------

        /*all_keyfrms.emplace(id_fixed_keyfrm_pair);
        auto keyfrm_vtx = keyfrm_vtx_container.create_vertex(fixed_keyfrm, true);
        optimizer.addVertex(keyfrm_vtx);*/
    }

    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    // Edges between KeyFrames in Local Window
    // and
    // Edges between 1st KeyFrame of Local Window and its previous (fixed)KeyFrame - pKFPrevLocal
    std::vector<::g2o::EdgeNavStatePVR*> vpEdgesNavStatePVR;
    std::vector<::g2o::EdgeNavStateBias*> vpEdgesNavStateBias;
    // Use chi2inv() in MATLAB to compute the value corresponding to 0.95/0.99 prob. w.r.t 15DOF: 24.9958/30.5779
    // 12.592/16.812 for 0.95/0.99 6DoF
    // 16.919/21.666 for 0.95/0.99 9DoF
    //const float thHuberNavState = sqrt(30.5779);
    const float thHuberNavStatePVR = sqrt(21.666);
    const float thHuberNavStateBias = sqrt(16.812);
    // Inverse covariance of bias random walk
#ifndef NOT_UPDATE_GYRO_BIAS
    Matrix<double,6,6> InvCovBgaRW = Matrix<double,6,6>::Identity();
    InvCovBgaRW.topLeftCorner(3,3) = Matrix3d::Identity()/IMUData::getGyrBiasRW2();       // Gyroscope bias random walk, covariance INVERSE
    InvCovBgaRW.bottomRightCorner(3,3) = Matrix3d::Identity()/IMUData::getAccBiasRW2();   // Accelerometer bias random walk, covariance INVERSE
#else
    Matrix<double,3,3> InvCovBgaRW = Matrix3d::Identity()/IMUData::getAccBiasRW2();   // Accelerometer bias random walk, covariance INVERSE
#endif

    for (auto& local_keyfrm : local_keyfrms)
    {
        auto pKF1 = local_keyfrm.second;                      // Current KF, store the IMU pre-integration between previous-current
        data::keyframe* pKF0 = pKF1->GetPrevKeyFrame();   // Previous KF

        // PVR edge
        {
            ::g2o::EdgeNavStatePVR * epvr = new ::g2o::EdgeNavStatePVR();
            epvr->setVertex(0, dynamic_cast<::g2o::OptimizableGraph::Vertex*>(optimizer.vertex(2*pKF0->id_)));
            epvr->setVertex(1, dynamic_cast<::g2o::OptimizableGraph::Vertex*>(optimizer.vertex(2*pKF1->id_)));
            epvr->setVertex(2, dynamic_cast<::g2o::OptimizableGraph::Vertex*>(optimizer.vertex(2*pKF0->id_+1)));
            epvr->setMeasurement(pKF1->GetIMUPreInt());

            Matrix9d InvCovPVR = pKF1->GetIMUPreInt().getCovPVPhi().inverse();
            epvr->setInformation(InvCovPVR);
            epvr->SetParams(GravityVec);

            ::g2o::RobustKernelHuber* rk = new ::g2o::RobustKernelHuber;
            epvr->setRobustKernel(rk);
            rk->setDelta(thHuberNavStatePVR);

            optimizer.addEdge(epvr);
            vpEdgesNavStatePVR.push_back(epvr);
        }
        // Bias edge
        {
            ::g2o::EdgeNavStateBias * ebias = new ::g2o::EdgeNavStateBias();
            ebias->setVertex(0, dynamic_cast<::g2o::OptimizableGraph::Vertex*>(optimizer.vertex(2*pKF0->id_+1)));
            ebias->setVertex(1, dynamic_cast<::g2o::OptimizableGraph::Vertex*>(optimizer.vertex(2*pKF1->id_+1)));
            ebias->setMeasurement(pKF1->GetIMUPreInt());

            ebias->setInformation(InvCovBgaRW/pKF1->GetIMUPreInt().getDeltaTime());

            ::g2o::RobustKernelHuber* rk = new ::g2o::RobustKernelHuber;
            ebias->setRobustKernel(rk);
            rk->setDelta(thHuberNavStateBias);

            optimizer.addEdge(ebias);
            vpEdgesNavStateBias.push_back(ebias);
        }

        // Test log
        if(pKF1->GetIMUPreInt().getDeltaTime() < 1e-3)
        {
            std::cerr<<"IMU pre-integrator delta time between 2 KFs too small: "<<pKF1->GetIMUPreInt().getDeltaTime()<<std::endl;
            std::cerr<<"No EdgeNavState added"<<std::endl;
            continue;
        }
        // Lo dejo comentado porque solo imprime por pantalla
        /*if(lit == lLocalKeyFrames.begin())
        {
            // First KF in Local Window, link (fixed) pKFPrevLocal
            if(pKF0 != pKFPrevLocal) cerr<<"pKF0 != pKFPrevLocal for 1st KF in Local Window, id: "<<pKF0->mnId<<","<<pKFPrevLocal->mnId<<endl;
        }
        else
        {
            // KFs in Local Window, link another local KF
        }*/
    }
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------

    // 4. keyframeとlandmarkのvertexをreprojection edgeで接続する

    // landmark vertexのcontainer
    g2o::landmark_vertex_container lm_vtx_container(keyfrm_vtx_container.get_max_vertex_id() + 1, local_lms.size());

    /*// reprojection edgeのcontainer
    using reproj_edge_wrapper = g2o::se3::reproj_edge_wrapper<data::keyframe>;
    std::vector<reproj_edge_wrapper> reproj_edge_wraps;
    reproj_edge_wraps.reserve(all_keyfrms.size() * local_lms.size());*/

    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    // Set MapPoint vertices
    const int nExpectedSize = (local_keyfrms.size() + fixed_keyfrms.size())*local_lms.size();

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

    std::vector<data::keyframe*> vpEdgeKFMono;
    vpEdgeKFMono.reserve(nExpectedSize);

    std::vector<data::landmark*> vpMapPointEdgeMono;
    vpMapPointEdgeMono.reserve(nExpectedSize);
    const float thHuberMono = sqrt(5.991);
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------

    // 有意水準5%のカイ2乗値
    // 自由度n=2
    constexpr float chi_sq_2D = 5.99146;
    const float sqrt_chi_sq_2D = std::sqrt(chi_sq_2D);
    // 自由度n=3
    constexpr float chi_sq_3D = 7.81473;
    const float sqrt_chi_sq_3D = std::sqrt(chi_sq_3D);

    for (auto& id_local_lm_pair : local_lms) {
        //auto local_lm = id_local_lm_pair.second;
        //-------------------------------------------------------------------------------------------
        //-------------------------------------------------------------------------------------------
        //-------------------------------------------------------------------------------------------
        auto pMP = id_local_lm_pair.second;
        ::g2o::VertexSBAPointXYZ* vPoint = new ::g2o::VertexSBAPointXYZ();
        vPoint->setEstimate(pMP->get_pos_in_world());
        int id = pMP->id_+maxKFid+1;
        vPoint->setId(id);
        vPoint->setMarginalized(true);
        optimizer.addVertex(vPoint);

        cv::Mat Pw;
        eigen2cv(pMP->get_pos_in_world(), Pw);
        //-------------------------------------------------------------------------------------------
        //-------------------------------------------------------------------------------------------
        //-------------------------------------------------------------------------------------------
        /*
        // landmarkをg2oのvertexに変換してoptimizerにセットする
        auto lm_vtx = lm_vtx_container.create_vertex(local_lm, false);
        optimizer.addVertex(lm_vtx);
        */
        //-------------------------------------------------------------------------------------------
        //-------------------------------------------------------------------------------------------
        //-------------------------------------------------------------------------------------------

        const auto observations = pMP->get_observations();
        for (const auto& obs : observations) {
            //auto keyfrm = obs.first;
            data::keyframe* pKFi = obs.first;
            auto idx = obs.second;
            if (!pKFi->will_be_erased()) {
                const cv::KeyPoint &kpUn = pKFi->undist_keypts_[idx];

                // Monocular observation
                if(pKFi->stereo_x_right_[idx]<0)
                {
                    Eigen::Matrix<double,2,1> obs;
                    obs << kpUn.pt.x, kpUn.pt.y;

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

                    e->setVertex(0, dynamic_cast<::g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
                    e->setVertex(1, dynamic_cast<::g2o::OptimizableGraph::Vertex*>(optimizer.vertex(2*pKFi->id_)));
                    e->setMeasurement(obs);
                    const float &invSigma2 = pKFi->inv_level_sigma_sq_[kpUn.octave];

                    e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);

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

                    cv::Mat cam_param = ConfigParam::GetCamMatrix();
                    float fx = cam_param.at<float>(0,0);
                    float fy = cam_param.at<float>(1,1);
                    float cx = cam_param.at<float>(0,2);
                    float cy = cam_param.at<float>(1,2);
                    e->SetParams(fx,fy,cx,cy,Rbc,Pbc);

                    optimizer.addEdge(e);
                    vpEdgesMono.push_back(e);
                    vpEdgeKFMono.push_back(pKFi);
                    vpMapPointEdgeMono.push_back(pMP);
                }
                else
                {
                    // Test log
                   std:: cerr<<"Stereo not supported yet, why here?? check."<<std::endl;
                }
            }
            /*
            if (!keyfrm) {
                continue;
            }
            if (keyfrm->will_be_erased()) {
                continue;
            }

            const auto keyfrm_vtx = keyfrm_vtx_container.get_vertex(keyfrm);
            const auto& undist_keypt = keyfrm->undist_keypts_.at(idx);
            const float x_right = keyfrm->stereo_x_right_.at(idx);
            const float inv_sigma_sq = keyfrm->inv_level_sigma_sq_.at(undist_keypt.octave);
            const auto sqrt_chi_sq = (keyfrm->camera_->setup_type_ == camera::setup_type_t::Monocular)
                                         ? sqrt_chi_sq_2D
                                         : sqrt_chi_sq_3D;
            auto reproj_edge_wrap = reproj_edge_wrapper(keyfrm, keyfrm_vtx, local_lm, lm_vtx,
                                                        idx, undist_keypt.pt.x, undist_keypt.pt.y, x_right,
                                                        inv_sigma_sq, sqrt_chi_sq);
            reproj_edge_wraps.push_back(reproj_edge_wrap);
            optimizer.addEdge(reproj_edge_wrap.edge_);*/
        }
    }

    // 5. 1回目の最適化を実行

    if (force_stop_flag) {
        if (*force_stop_flag) {
            return;
        }
    }

    optimizer.initializeOptimization();
    optimizer.optimize(num_first_iter_);

    // 6. アウトライア除去をして2回目の最適化を実行

    bool run_robust_BA = true;

    if (force_stop_flag) {
        if (*force_stop_flag) {
            run_robust_BA = false;
        }
    }

    if (run_robust_BA) {
        for(size_t i=0, iend=vpEdgesMono.size(); i<iend;i++)
        {
            ::g2o::EdgeNavStatePVRPointXYZ* e = vpEdgesMono[i];
            data::landmark* pMP = vpMapPointEdgeMono[i];

            if(pMP->will_be_erased())
                continue;

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

            e->setRobustKernel(0);
        }

        // Check inlier observations
        int tmpcnt=0;
        for(size_t i=0, iend=vpEdgesNavStatePVR.size(); i<iend; i++)
        {
            ::g2o::EdgeNavStatePVR* e = vpEdgesNavStatePVR[i];
            if(e->chi2()>21.666)
            {
                //e->setLevel(1);
                //cout<<"1 PVRedge "<<i<<", chi2 "<<e->chi2()<<". ";
                tmpcnt++;
            }
            //e->setRobustKernel(0);

        #ifndef NOT_UPDATE_GYRO_BIAS
                e->computeError();
                Vector9d err=e->error();
                for(int n=0;n<9;n++)
                    dbgLBAPVRErr<<err[n]<<" ";
                dbgLBAPVRErr<<endl;
        #endif
        }

        if(tmpcnt>0)
            std::cout<<std::endl;

        // Optimize again without the outliers
        optimizer.initializeOptimization();
        optimizer.optimize(num_second_iter_);
        /*for (auto& reproj_edge_wrap : reproj_edge_wraps) {
            auto edge = reproj_edge_wrap.edge_;

            auto local_lm = reproj_edge_wrap.lm_;
            if (local_lm->will_be_erased()) {
                continue;
            }

            if (reproj_edge_wrap.is_monocular_) {
                if (chi_sq_2D < edge->chi2() || !reproj_edge_wrap.depth_is_positive()) {
                    reproj_edge_wrap.set_as_outlier();
                }
            }
            else {
                if (chi_sq_3D < edge->chi2() || !reproj_edge_wrap.depth_is_positive()) {
                    reproj_edge_wrap.set_as_outlier();
                }
            }

            edge->setRobustKernel(nullptr);
        }

        optimizer.initializeOptimization();
        optimizer.optimize(num_second_iter_);*/
    }

    // 7. アウトライアを集計する
    std::vector<std::pair<data::keyframe*, data::landmark*>> outlier_observations;
    //outlier_observations.reserve(reproj_edge_wraps.size());
    outlier_observations.reserve(vpEdgesMono.size());
    double PosePointchi2=0, PosePosechi2=0, BiasBiaschi2=0;

    for(size_t i=0, iend=vpEdgesMono.size(); i<iend;i++)
    {
        ::g2o::EdgeNavStatePVRPointXYZ* e = vpEdgesMono[i];
        data::landmark* pMP = vpMapPointEdgeMono[i];

        if(pMP->will_be_erased())
            continue;

        if(e->chi2()>5.991 || !e->isDepthPositive())
        {
            data::keyframe* pKFi = vpEdgeKFMono[i];
            outlier_observations.push_back(std::make_pair(pKFi,pMP));
        }

        PosePointchi2 += e->chi2();
    }

     // Debug log
    // Check inlier observations
    int tmpcnt2=0;
    for(size_t i=0, iend=vpEdgesNavStatePVR.size(); i<iend; i++)
    {
        ::g2o::EdgeNavStatePVR* e = vpEdgesNavStatePVR[i];
        if(e->chi2()>21.666)
        {
            //cout<<"2 PVRedge "<<i<<", chi2 "<<e->chi2()<<". ";
            tmpcnt2++;
        }

    #ifndef NOT_UPDATE_GYRO_BIAS
        e->computeError();
        Vector9d err=e->error();
        for(int n=0;n<9;n++)
            dbgLBAPVRErr2<<err[n]<<" ";
        dbgLBAPVRErr2<<endl;
    #endif
    }

        //cout<<endl<<"edge bias ns bad: ";
    for(size_t i=0, iend=vpEdgesNavStateBias.size(); i<iend; i++)
    {
        ::g2o::EdgeNavStateBias* e = vpEdgesNavStateBias[i];
        if(e->chi2()>16.812)
        {
            //cout<<"2 Bias edge "<<i<<", chi2 "<<e->chi2()<<". ";
            tmpcnt2++;
        }

        #ifndef NOT_UPDATE_GYRO_BIAS
            e->computeError();
            Vector6d err=e->error();
            for(int n=0;n<6;n++)
                dbgLBABiasErr2<<err[n]<<" ";
            dbgLBABiasErr2<<endl;
        #endif
    }
    if(tmpcnt2>0)
        std::cout<<std::endl;
    /*
    for (auto& reproj_edge_wrap : reproj_edge_wraps) {
        auto edge = reproj_edge_wrap.edge_;

        auto local_lm = reproj_edge_wrap.lm_;
        if (local_lm->will_be_erased()) {
            continue;
        }

        if (reproj_edge_wrap.is_monocular_) {
            if (chi_sq_2D < edge->chi2() || !reproj_edge_wrap.depth_is_positive()) {
                outlier_observations.emplace_back(std::make_pair(reproj_edge_wrap.shot_, reproj_edge_wrap.lm_));
            }
        }
        else {
            if (chi_sq_3D < edge->chi2() || !reproj_edge_wrap.depth_is_positive()) {
                outlier_observations.emplace_back(std::make_pair(reproj_edge_wrap.shot_, reproj_edge_wrap.lm_));
            }
        }
    }*/

    // 8. 情報を更新

    {
        std::lock_guard<std::mutex> lock(data::map_database::mtx_database_);

        if (!outlier_observations.empty()) {
            for (auto& outlier_obs : outlier_observations) {
                auto keyfrm = outlier_obs.first;
                auto lm = outlier_obs.second;
                keyfrm->erase_landmark(lm);
                lm->erase_observation(keyfrm);
            }
        }

        for (auto id_local_keyfrm_pair : local_keyfrms) {
            auto pKFi = id_local_keyfrm_pair.second;
            ::g2o::VertexNavStatePVR* vNSPVR = static_cast<::g2o::VertexNavStatePVR*>(optimizer.vertex(2*pKFi->id_));
            ::g2o::VertexNavStateBias* vNSBias = static_cast<::g2o::VertexNavStateBias*>(optimizer.vertex(2*pKFi->id_+1));
            // In optimized navstate, bias not changed, delta_bias not zero, should be added to bias
            const NavState& optPVRns = vNSPVR->estimate();
            const NavState& optBiasns = vNSBias->estimate();
            NavState primaryns = pKFi->GetNavState();
            // Update NavState
            pKFi->SetNavStatePos(optPVRns.Get_P());
            pKFi->SetNavStateVel(optPVRns.Get_V());
            pKFi->SetNavStateRot(optPVRns.Get_R());
            //if(optBiasns.Get_dBias_Acc().norm()<1e-2 && optBiasns.Get_BiasGyr().norm()<1e-4)
            //{
            pKFi->SetNavStateDeltaBg(optBiasns.Get_dBias_Gyr());
            pKFi->SetNavStateDeltaBa(optBiasns.Get_dBias_Acc());

            // Update pose Tcw
            pKFi->UpdatePoseFromNS(ConfigParam::GetMatTbc());

            // Test log
            if( (primaryns.Get_BiasGyr() - optPVRns.Get_BiasGyr()).norm() > 1e-6 || (primaryns.Get_BiasGyr() - optBiasns.Get_BiasGyr()).norm() > 1e-6 )
                std::cerr<<"gyr bias change in optimization?"<<std::endl;
            if( (primaryns.Get_BiasAcc() - optPVRns.Get_BiasAcc()).norm() > 1e-6 || (primaryns.Get_BiasAcc() - optBiasns.Get_BiasAcc()).norm() > 1e-6 )
                std::cerr<<"acc bias change in optimization?"<<std::endl;
        }
        for (auto id_local_lm_pair : local_lms) {
            auto pMP = id_local_lm_pair.second;
            ::g2o::VertexSBAPointXYZ* vPoint = static_cast<::g2o::VertexSBAPointXYZ*>(optimizer.vertex(pMP->id_+maxKFid+1));
            pMP->set_pos_in_world(vPoint->estimate());
            pMP->update_normal_and_depth();
        }

        /*
        for (auto id_local_keyfrm_pair : local_keyfrms) {
            auto local_keyfrm = id_local_keyfrm_pair.second;

            auto keyfrm_vtx = keyfrm_vtx_container.get_vertex(local_keyfrm);
            local_keyfrm->set_cam_pose(keyfrm_vtx->estimate());
        }

        for (auto id_local_lm_pair : local_lms) {
            auto local_lm = id_local_lm_pair.second;

            auto lm_vtx = lm_vtx_container.get_vertex(local_lm);
            local_lm->set_pos_in_world(lm_vtx->estimate());
            local_lm->update_normal_and_depth();
        }*/
    }
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    if(mapper_)
    {
        mapper_->SetMapUpdateFlagInTracking(true);
    }
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
}

/*
void local_bundle_adjuster::optimize(data::keyframe *curr_keyfrm, std::unordered_map<unsigned int, data::keyframe*> &local_keyfrms, bool* const force_stop_flag, data::map_database* map_db, mapping_module* mapper_) const
{
    // 1. local/fixed keyframes, local landmarksを集計する

    // correct local keyframes of the current keyframe
    //std::unordered_map<unsigned int, data::keyframe*> local_keyfrms;

    local_keyfrms[curr_keyfrm->id_] = curr_keyfrm;
    const auto curr_covisibilities = curr_keyfrm->graph_node_->get_covisibilities();
    for (auto local_keyfrm : curr_covisibilities) {
        if (!local_keyfrm) {
            continue;
        }
        if (local_keyfrm->will_be_erased()) {
            continue;
        }

        local_keyfrms[local_keyfrm->id_] = local_keyfrm;
    }

    // correct local landmarks seen in local keyframes
    std::unordered_map<unsigned int, data::landmark*> local_lms;

    for (auto local_keyfrm : local_keyfrms) {
        const auto landmarks = local_keyfrm.second->get_landmarks();
        for (auto local_lm : landmarks) {
            if (!local_lm) {
                continue;
            }
            if (local_lm->will_be_erased()) {
                continue;
            }

            // 重複を避ける
            if (local_lms.count(local_lm->id_)) {
                continue;
            }

            local_lms[local_lm->id_] = local_lm;
        }
    }

    // fixed keyframes: keyframes which observe local landmarks but which are NOT in local keyframes
    std::unordered_map<unsigned int, data::keyframe*> fixed_keyfrms;

    for (auto local_lm : local_lms) {
        const auto observations = local_lm.second->get_observations();
        for (auto& obs : observations) {
            auto fixed_keyfrm = obs.first;
            if (!fixed_keyfrm) {
                continue;
            }
            if (fixed_keyfrm->will_be_erased()) {
                continue;
            }

            // local keyframesに属しているときは追加しない
            if (local_keyfrms.count(fixed_keyfrm->id_)) {
                continue;
            }

            // 重複を避ける
            if (fixed_keyfrms.count(fixed_keyfrm->id_)) {
                continue;
            }

            fixed_keyfrms[fixed_keyfrm->id_] = fixed_keyfrm;
        }
    }

    // 2. optimizerを構築

    auto linear_solver = ::g2o::make_unique<::g2o::LinearSolverCSparse<::g2o::BlockSolver_6_3::PoseMatrixType>>();
    auto block_solver = ::g2o::make_unique<::g2o::BlockSolver_6_3>(std::move(linear_solver));
    auto algorithm = new ::g2o::OptimizationAlgorithmLevenberg(std::move(block_solver));

    ::g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(algorithm);

    if (force_stop_flag) {
        optimizer.setForceStopFlag(force_stop_flag);
    }

    // 3. keyframeをg2oのvertexに変換してoptimizerにセットする

    // shot vertexのcontainer
    g2o::se3::shot_vertex_container keyfrm_vtx_container(0, local_keyfrms.size() + fixed_keyfrms.size());
    // vertexに変換されたkeyframesを保存しておく
    std::unordered_map<unsigned int, data::keyframe*> all_keyfrms;

    // local keyframesをoptimizerにセット
    for (auto& id_local_keyfrm_pair : local_keyfrms) {
        auto local_keyfrm = id_local_keyfrm_pair.second;

        all_keyfrms.emplace(id_local_keyfrm_pair);
        auto keyfrm_vtx = keyfrm_vtx_container.create_vertex(local_keyfrm, local_keyfrm->id_ == 0);
        optimizer.addVertex(keyfrm_vtx);
    }

    // fixed keyframesをoptimizerにセット
    for (auto& id_fixed_keyfrm_pair : fixed_keyfrms) {
        auto fixed_keyfrm = id_fixed_keyfrm_pair.second;

        all_keyfrms.emplace(id_fixed_keyfrm_pair);
        auto keyfrm_vtx = keyfrm_vtx_container.create_vertex(fixed_keyfrm, true);
        optimizer.addVertex(keyfrm_vtx);
    }

    // 4. keyframeとlandmarkのvertexをreprojection edgeで接続する

    // landmark vertexのcontainer
    g2o::landmark_vertex_container lm_vtx_container(keyfrm_vtx_container.get_max_vertex_id() + 1, local_lms.size());

    // reprojection edgeのcontainer
    using reproj_edge_wrapper = g2o::se3::reproj_edge_wrapper<data::keyframe>;
    std::vector<reproj_edge_wrapper> reproj_edge_wraps;
    reproj_edge_wraps.reserve(all_keyfrms.size() * local_lms.size());

    // 有意水準5%のカイ2乗値
    // 自由度n=2
    constexpr float chi_sq_2D = 5.99146;
    const float sqrt_chi_sq_2D = std::sqrt(chi_sq_2D);
    // 自由度n=3
    constexpr float chi_sq_3D = 7.81473;
    const float sqrt_chi_sq_3D = std::sqrt(chi_sq_3D);

    for (auto& id_local_lm_pair : local_lms) {
        auto local_lm = id_local_lm_pair.second;

        // landmarkをg2oのvertexに変換してoptimizerにセットする
        auto lm_vtx = lm_vtx_container.create_vertex(local_lm, false);
        optimizer.addVertex(lm_vtx);

        const auto observations = local_lm->get_observations();
        for (const auto& obs : observations) {
            auto keyfrm = obs.first;
            auto idx = obs.second;
            if (!keyfrm) {
                continue;
            }
            if (keyfrm->will_be_erased()) {
                continue;
            }

            const auto keyfrm_vtx = keyfrm_vtx_container.get_vertex(keyfrm);
            const auto& undist_keypt = keyfrm->undist_keypts_.at(idx);
            const float x_right = keyfrm->stereo_x_right_.at(idx);
            const float inv_sigma_sq = keyfrm->inv_level_sigma_sq_.at(undist_keypt.octave);
            const auto sqrt_chi_sq = (keyfrm->camera_->setup_type_ == camera::setup_type_t::Monocular)
                                         ? sqrt_chi_sq_2D
                                         : sqrt_chi_sq_3D;
            auto reproj_edge_wrap = reproj_edge_wrapper(keyfrm, keyfrm_vtx, local_lm, lm_vtx,
                                                        idx, undist_keypt.pt.x, undist_keypt.pt.y, x_right,
                                                        inv_sigma_sq, sqrt_chi_sq);
            reproj_edge_wraps.push_back(reproj_edge_wrap);
            optimizer.addEdge(reproj_edge_wrap.edge_);
        }
    }

    // 5. 1回目の最適化を実行

    if (force_stop_flag) {
        if (*force_stop_flag) {
            return;
        }
    }

    optimizer.initializeOptimization();
    optimizer.optimize(num_first_iter_);

    // 6. アウトライア除去をして2回目の最適化を実行

    bool run_robust_BA = true;

    if (force_stop_flag) {
        if (*force_stop_flag) {
            run_robust_BA = false;
        }
    }

    if (run_robust_BA) {
        for (auto& reproj_edge_wrap : reproj_edge_wraps) {
            auto edge = reproj_edge_wrap.edge_;

            auto local_lm = reproj_edge_wrap.lm_;
            if (local_lm->will_be_erased()) {
                continue;
            }

            if (reproj_edge_wrap.is_monocular_) {
                if (chi_sq_2D < edge->chi2() || !reproj_edge_wrap.depth_is_positive()) {
                    reproj_edge_wrap.set_as_outlier();
                }
            }
            else {
                if (chi_sq_3D < edge->chi2() || !reproj_edge_wrap.depth_is_positive()) {
                    reproj_edge_wrap.set_as_outlier();
                }
            }

            edge->setRobustKernel(nullptr);
        }

        optimizer.initializeOptimization();
        optimizer.optimize(num_second_iter_);
    }

    // 7. アウトライアを集計する

    std::vector<std::pair<data::keyframe*, data::landmark*>> outlier_observations;
    outlier_observations.reserve(reproj_edge_wraps.size());

    for (auto& reproj_edge_wrap : reproj_edge_wraps) {
        auto edge = reproj_edge_wrap.edge_;

        auto local_lm = reproj_edge_wrap.lm_;
        if (local_lm->will_be_erased()) {
            continue;
        }

        if (reproj_edge_wrap.is_monocular_) {
            if (chi_sq_2D < edge->chi2() || !reproj_edge_wrap.depth_is_positive()) {
                outlier_observations.emplace_back(std::make_pair(reproj_edge_wrap.shot_, reproj_edge_wrap.lm_));
            }
        }
        else {
            if (chi_sq_3D < edge->chi2() || !reproj_edge_wrap.depth_is_positive()) {
                outlier_observations.emplace_back(std::make_pair(reproj_edge_wrap.shot_, reproj_edge_wrap.lm_));
            }
        }
    }

    // 8. 情報を更新

    {
        std::lock_guard<std::mutex> lock(data::map_database::mtx_database_);

        if (!outlier_observations.empty()) {
            for (auto& outlier_obs : outlier_observations) {
                auto keyfrm = outlier_obs.first;
                auto lm = outlier_obs.second;
                keyfrm->erase_landmark(lm);
                lm->erase_observation(keyfrm);
            }
        }

        for (auto id_local_keyfrm_pair : local_keyfrms) {
            auto local_keyfrm = id_local_keyfrm_pair.second;

            auto keyfrm_vtx = keyfrm_vtx_container.get_vertex(local_keyfrm);
            local_keyfrm->set_cam_pose(keyfrm_vtx->estimate());
        }

        for (auto id_local_lm_pair : local_lms) {
            auto local_lm = id_local_lm_pair.second;

            auto lm_vtx = lm_vtx_container.get_vertex(local_lm);
            local_lm->set_pos_in_world(lm_vtx->estimate());
            local_lm->update_normal_and_depth();
        }
    }
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    if(mapper_)
    {
        mapper_->SetMapUpdateFlagInTracking(true);
    }
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
    //-------------------------------------------------------------------------------------------
}*/

} // namespace optimize
} // namespace openvslam


/* -*-C++-*-
   (c) Copyright 2002-2005, Hewlett-Packard Development Company, LP

   See the file named COPYING for license details
*/

/** @file
    Regression test for the MersenneTwisterRandom class
*/

#include <iostream>
#include <boost/format.hpp>

using namespace std;

#include <Lintel/MarsagliaRandom.hpp>
#include <Lintel/MersenneTwisterRandom.hpp>
#include <Lintel/Clock.hpp>

typedef Clock::Tfrac Tfrac;

using namespace lintel;

void shuffleShow() {
    vector<int> distr;
    vector<int> base;
    distr.resize(10);
    base.resize(distr.size());
    for(uint32_t i = 0; i < base.size(); ++i) {
	base[i] = i;
    }

    MersenneTwisterRandom rng;

    vector<int> work;
    work.resize(base.size());
    for(uint32_t i = 0; i < 100000; ++i) {
	work = base;
	MT_random_shuffle(work.begin(), work.end(), rng);
	distr[work[0]] += 1;
    }

    for(uint32_t i = 0; i < distr.size(); ++i) {
	cout << boost::format("%d -> %d\n") % i % distr[i];
    }
}

template <class R> void speedTest(R rng) {
        int64_t res = 0;
        int64_t reps = 1;
        Tfrac start, stop;
        Stats plain, modulus, dbl, boole, boolefast;

        do {
            reps *= 2;
            start = Clock::todTfrac();
            for(int64_t i = 0; i<reps; ++i) {
                res += rng.randInt();
            }
            stop = Clock::todTfrac();
        } while(Clock::TfracToDouble(stop-start) < 1.0);

        plain.add(Clock::TfracToDouble(stop-start) / reps * 1000000000.0);  // In nanos

        do {
            start = Clock::todTfrac();
            for(int64_t i = 0; i<reps; ++i) {
                res += rng.randInt();
            }
            stop = Clock::todTfrac();
            plain.add(Clock::TfracToDouble(stop-start) / reps * 1000000000.0);  // In nanos
        } while (plain.relconf95() > .05 || plain.count() < 10);
        cout << "For plain: " << plain.debugString() << endl;

        reps = 1;

        do {
            reps *= 2;
            start = Clock::todTfrac();
            for(int64_t i = 0; i<reps; ++i) {
                res += rng.randInt(1000);
            }
            stop = Clock::todTfrac();
        } while(Clock::TfracToDouble(stop-start) < 1.0);

        modulus.add(Clock::TfracToDouble(stop-start) / reps * 1000000000.0);  // In nanos

        do {
            start = Clock::todTfrac();
            for(int64_t i = 0; i<reps; ++i) {
                res += rng.randInt(1000);
            }
            stop = Clock::todTfrac();
            modulus.add(Clock::TfracToDouble(stop-start) / reps * 1000000000.0);  // In nanos
        } while (modulus.relconf95() > .05 || modulus.count() < 10);
        cout << "For modulus: " << modulus.debugString() << endl;

        reps = 1;
        double dres = res;
        do {
            reps *= 2;
            start = Clock::todTfrac();
            for(int64_t i = 0; i<reps; ++i) {
                dres += rng.randDouble();
            }
            stop = Clock::todTfrac();
        } while(Clock::TfracToDouble(stop-start) < 1.0);

        dbl.add(Clock::TfracToDouble(stop-start) / reps * 1000000000.0);  // In nanos

        do {
            start = Clock::todTfrac();
            for(int64_t i = 0; i<reps; ++i) {
                dres += rng.randDouble();
            }
            stop = Clock::todTfrac();
            dbl.add(Clock::TfracToDouble(stop-start) / reps * 1000000000.0);  // In nanos
        } while (dbl.relconf95() > .05 || dbl.count() < 10);
        cout << "For dbl: " << dbl.debugString() << endl;

        dres += res;

        reps = 1;

        do {
            reps *= 2;
            start = Clock::todTfrac();
            for(int64_t i = 0; i<reps; ++i) {
                res += rng.randBool();
            }
            stop = Clock::todTfrac();
        } while(Clock::TfracToDouble(stop-start) < 1.0);

        boole.add(Clock::TfracToDouble(stop-start) / reps * 1000000000.0);  // In nanos

        do {
            start = Clock::todTfrac();
            for(int64_t i = 0; i<reps; ++i) {
                res += rng.randBool();
            }
            stop = Clock::todTfrac();
            boole.add(Clock::TfracToDouble(stop-start) / reps * 1000000000.0);  // In nanos
        } while (boole.relconf95() > .05 || boole.count() < 10);
        cout << "For boole: " << boole.debugString() << endl;

        reps = 1;

        do {
            reps *= 2;
            start = Clock::todTfrac();
            for(int64_t i = 0; i<reps; ++i) {
                res += rng.randBoolFast();
            }
            stop = Clock::todTfrac();
        } while(Clock::TfracToDouble(stop-start) < 1.0);

        boolefast.add(Clock::TfracToDouble(stop-start) / reps * 1000000000.0);  // In nanos

        do {
            start = Clock::todTfrac();
            for(int64_t i = 0; i<reps; ++i) {
                res += rng.randBoolFast();
            }
            stop = Clock::todTfrac();
            boolefast.add(Clock::TfracToDouble(stop-start) / reps * 1000000000.0);  // In nanos
        } while (boolefast.relconf95() > .05 || boolefast.count() < 10);
        cout << "For boolefast: " << boolefast.debugString() << endl;

        cout << "Result was " << dres << endl;
}

int main() {
    MersenneTwisterRandom::selfTest();

    //    shuffleShow();

    if (0) { // Performnace test
        MersenneTwisterRandom mtr;
        MarsagliaRNGHuge mrhuge;
        MarsagliaRNGTiny mrtiny;
        MarsagliaRNGMicro mrmicro;

        speedTest<MarsagliaRNGMicro>(mrmicro);

        speedTest<MersenneTwisterRandom>(mtr);
        speedTest<MarsagliaRNGHuge>(mrhuge);
        speedTest<MarsagliaRNGTiny>(mrtiny);
    }

    return 0;
}


#include <stdio.h>
#include <fstream>
#include <iostream>
#include <limits>
#include <opencv2/opencv.hpp>
#include <sstream>
#include <string>

#include <v4r/common/noise_models.h>
#include <v4r/common/normals.h>
#include <v4r/common/pcl_opencv.h>
#include <v4r/io/eigen.h>
#include <v4r/io/filesystem.h>
#include <v4r/registration/noise_model_based_cloud_integration.h>

#include <pcl/common/time.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/passthrough.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>

#include <glog/logging.h>
#include <boost/algorithm/string.hpp>
#include <boost/filesystem.hpp>
#include <boost/format.hpp>
#include <boost/program_options.hpp>

namespace po = boost::program_options;
namespace bf = boost::filesystem;
using namespace v4r;

int main(int argc, const char *argv[]) {
  typedef pcl::PointXYZRGB PointT;
  bf::path test_dir, out_dir;
  std::string view_prefix = "cloud_", obj_indices_prefix = "object_indices_", pose_prefix = "pose_";
  float chop_z = std::numeric_limits<float>::max();
  bool visualize = false, use_object_mask = false, use_pose_file = false, debug = false;

  google::InitGoogleLogging(argv[0]);

  NMBasedCloudIntegrationParameter nm_int_param;
  nm_int_param.min_points_per_voxel_ = 1;
  nm_int_param.octree_resolution_ = 0.002f;

  NguyenNoiseModelParameter nm_param;

  typename v4r::NormalEstimator<PointT>::Ptr normal_estimator_;

  NormalEstimatorType normal_method = NormalEstimatorType::PCL_INTEGRAL_NORMAL;

  po::options_description desc(
      "Noise model based cloud integration\n======================================\n**Allowed options");
  desc.add_options()("help,h", "produce help message");
  desc.add_options()("input_dir,i", po::value<bf::path>(&test_dir)->required(), "directory containing point clouds");
  desc.add_options()(
      "out_dir,o", po::value<bf::path>(&out_dir),
      "output directory where the registered cloud will be stored. If not set, nothing will be written to disk");
  desc.add_options()("view_prefix", po::value<std::string>(&view_prefix)->default_value(view_prefix),
                     "view filename prefix for each point cloud (used when using object mask)");
  desc.add_options()("obj_indices_prefix",
                     po::value<std::string>(&obj_indices_prefix)->default_value(obj_indices_prefix),
                     "filename prefix for each object mask file(used when using object mask)");
  desc.add_options()("pose_prefix", po::value<std::string>(&pose_prefix)->default_value(pose_prefix),
                     "filename prefix for each camera pose (used when using use_pose_file)");
  desc.add_options()("resolution,r",
                     po::value<float>(&nm_int_param.octree_resolution_)->default_value(nm_int_param.octree_resolution_),
                     "");
  desc.add_options()(
      "min_points_per_voxel",
      po::value<size_t>(&nm_int_param.min_points_per_voxel_)->default_value(nm_int_param.min_points_per_voxel_), "")
      //            ("threshold_explained",
      //            po::value<float>(&nm_int_param.threshold_explained_)->default_value(nm_int_param.threshold_explained_),
      //            "")
      ;
  desc.add_options()("use_depth_edges",
                     po::value<bool>(&nm_param.use_depth_edges_)->default_value(nm_param.use_depth_edges_), "");
  desc.add_options()("focal_length,f", po::value<float>(&nm_param.focal_length_)->default_value(nm_param.focal_length_),
                     "");
  desc.add_options()("normal_method,n", po::value<NormalEstimatorType>(&normal_method)->default_value(normal_method),
                     "method used for normal computation");
  desc.add_options()("chop_z,z", po::value<float>(&chop_z)->default_value(chop_z), "cut of distance in m ");
  desc.add_options()("visualize,v", po::bool_switch(&visualize), "turn visualization on");
  desc.add_options()("use_object_mask,m", po::bool_switch(&use_object_mask),
                     "reads mask file and only extracts those indices (only if file exists)");
  desc.add_options()(
      "use_pose_file,p", po::bool_switch(&use_pose_file),
      "reads pose from separate pose file instead of extracting it directly from .pcd file (only if file exists)");
  desc.add_options()("debug,d", po::bool_switch(&debug),
                     "saves debug information (e.g. point properties) if output dir is set");
  po::variables_map vm;
  po::parsed_options parsed = po::command_line_parser(argc, argv).options(desc).allow_unregistered().run();
  std::vector<std::string> to_pass_further = po::collect_unrecognized(parsed.options, po::include_positional);
  po::store(parsed, vm);
  if (vm.count("help")) {
    std::cout << desc << std::endl;
    to_pass_further.push_back("-h");
  }
  try {
    po::notify(vm);
  } catch (std::exception &e) {
    std::cerr << "Error: " << e.what() << std::endl << std::endl << desc << std::endl;
  }

  normal_estimator_ = v4r::initNormalEstimator<PointT>(normal_method, to_pass_further);

  std::vector<std::string> folder_names = io::getFoldersInDirectory(test_dir);

  if (folder_names.empty())
    folder_names.push_back("");

  int vp1, vp2;
  pcl::visualization::PCLVisualizer::Ptr vis;

  for (const std::string &sub_folder : folder_names) {
    const bf::path test_seq = test_dir / sub_folder;
    std::vector<std::string> views = io::getFilesInDirectory(test_seq, ".*.pcd", false);

    pcl::PointCloud<PointT>::Ptr big_cloud_unfiltered(new pcl::PointCloud<PointT>);
    std::vector<pcl::PointCloud<PointT>::ConstPtr> clouds(views.size());
    std::vector<pcl::PointCloud<pcl::Normal>::ConstPtr> normals(views.size());
    std::vector<std::vector<std::vector<float>>> pt_properties(views.size());
    std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> camera_transforms(views.size());
    std::vector<std::vector<int>> obj_indices(views.size());

    for (size_t v_id = 0; v_id < views.size(); v_id++) {
      std::stringstream txt;
      txt << "processing view " << v_id;
      pcl::ScopeTime t(txt.str().c_str());
      pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
      pcl::PointCloud<pcl::Normal>::Ptr normal(new pcl::PointCloud<pcl::Normal>);

      pcl::io::loadPCDFile((test_seq / views[v_id]).string(), *cloud);

      std::string obj_fn(views[v_id]);
      boost::replace_first(obj_fn, view_prefix, obj_indices_prefix);
      boost::replace_last(obj_fn, ".pcd", ".txt");

      if (use_object_mask) {
        bf::path object_indices_fn = test_seq;
        object_indices_fn /= obj_fn;
        if (io::existsFile(object_indices_fn.string())) {
          ifstream f((test_seq / obj_fn).string().c_str());
          int idx;
          while (f >> idx)
            obj_indices[v_id].push_back(idx);
          f.close();
        } else
          std::cerr << "Parameter use_object_mask is set but object indices file " << object_indices_fn.string()
                    << " not found! " << std::endl;
      }

      std::string pose_fn(views[v_id]);
      boost::replace_first(pose_fn, view_prefix, pose_prefix);
      boost::replace_last(pose_fn, ".pcd", ".txt");

      if (io::existsFile(test_seq / pose_fn) && use_pose_file)
        camera_transforms[v_id] = io::readMatrixFromFile(test_seq / pose_fn);
      else
        camera_transforms[v_id] = RotTrans2Mat4f(cloud->sensor_orientation_, cloud->sensor_origin_);

      // reset view point otherwise pcl visualization is potentially messed up
      cloud->sensor_orientation_ = Eigen::Quaternionf::Identity();
      cloud->sensor_origin_ = Eigen::Vector4f::Zero();

      pcl::PassThrough<PointT> pass;
      pass.setInputCloud(cloud);
      pass.setFilterFieldName("z");
      pass.setFilterLimits(0.f, chop_z);
      pass.setKeepOrganized(true);
      pass.filter(*cloud);

      {
        pcl::ScopeTime tt("Computing normals");
        normal_estimator_->setInputCloud(cloud);
        normal = normal_estimator_->compute();
      }

      {
        pcl::ScopeTime tt("Computing noise model parameter for cloud");
        NguyenNoiseModel<PointT> nm(nm_param);
        nm.setInputCloud(cloud);
        nm.setInputNormals(normal);
        nm.compute();
        pt_properties[v_id] = nm.getPointProperties();
      }

      pcl::PointCloud<PointT> object_cloud, object_aligned;
      if (use_object_mask)
        pcl::copyPointCloud(*cloud, obj_indices[v_id], object_cloud);
      else
        pcl::copyPointCloud(*cloud, object_cloud);

      pcl::transformPointCloud(object_cloud, object_aligned, camera_transforms[v_id]);
      *big_cloud_unfiltered += object_aligned;

      clouds[v_id] = cloud;
      normals[v_id] = normal;
    }

    pcl::PointCloud<PointT>::Ptr octree_cloud(new pcl::PointCloud<PointT>);
    NMBasedCloudIntegration<PointT> nmIntegration(nm_int_param);
    nmIntegration.setInputClouds(clouds);
    nmIntegration.setPointProperties(pt_properties);
    nmIntegration.setTransformations(camera_transforms);
    nmIntegration.setInputNormals(normals);
    nmIntegration.setIndices(obj_indices);
    nmIntegration.compute(octree_cloud);
    std::vector<pcl::PointCloud<PointT>::Ptr> clouds_used;
    nmIntegration.getInputCloudsUsed(clouds_used);

    std::cout << "Size cloud unfiltered: " << big_cloud_unfiltered->points.size()
              << ", filtered: " << octree_cloud->points.size() << std::endl;

    if (vm.count("out_dir")) {
      const bf::path out_path = out_dir / sub_folder;
      io::createDirIfNotExist(out_path);
      pcl::io::savePCDFileBinaryCompressed((out_path / "/registered_cloud_filtered.pcd").string(), *octree_cloud);
      pcl::io::savePCDFileBinaryCompressed((out_path / "/registered_cloud_unfiltered.pcd").string(),
                                           *big_cloud_unfiltered);

      for (size_t v_id = 0; v_id < clouds_used.size(); v_id++) {
        if (debug) {
          std::stringstream fn;
          fn << out_path.string() << "/filtered_input_" << setfill('0') << setw(5) << v_id << ".pcd";
          pcl::io::savePCDFileBinaryCompressed(fn.str(), *clouds_used[v_id]);

          fn.str("");
          fn << out_path.string() << "/distance_to_edge_px_" << setfill('0') << setw(5) << v_id << ".txt";
          std::ofstream f(fn.str());
          for (size_t pt_id = 0; pt_id < clouds_used[v_id]->points.size(); pt_id++) {
            f << pt_properties[v_id][pt_id][2] << std::endl;

            PointT &pt_tmp = clouds_used[v_id]->points[pt_id];
            if (!pcl::isFinite(pt_tmp))  // set background color for nan points
              pt_tmp.r = pt_tmp.g = pt_tmp.b = 255.f;
          }
          f.close();

          fn.str("");
          fn << out_path.string() << "/filter_input_image" << setfill('0') << setw(5) << v_id << ".png";
          PCLOpenCVConverter<PointT> pcl_opencv_converter;
          pcl_opencv_converter.setInputCloud(clouds_used[v_id]);
          cv::imwrite(fn.str(), pcl_opencv_converter.getRGBImage());
        }
      }
    }

    if (visualize) {
      if (!vis) {
        vis.reset(new pcl::visualization::PCLVisualizer("registered cloud"));
        vis->createViewPort(0, 0, 0.5, 1, vp1);
        vis->createViewPort(0.5, 0, 1, 1, vp2);
      }
      vis->removeAllPointClouds(vp1);
      vis->removeAllPointClouds(vp2);
      vis->addPointCloud(big_cloud_unfiltered, "unfiltered_cloud", vp1);
      vis->addPointCloud(octree_cloud, "filtered_cloud", vp2);
      vis->spin();
    }
  }
}