text: ﻿/**
  * This file is part of ORB-SLAM2.
  *
  * Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
  * For more information see <https://github.com/raulmur/ORB_SLAM2>
  *
  * ORB-SLAM2 is free software: you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
  * the Free Software Foundation, either version 3 of the License, or
  * (at your option) any later version.
  *
  * ORB-SLAM2 is distributed in the hope that it will be useful,
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  * GNU General Public License for more details.
  *
  * You should have received a copy of the GNU General Public License
  * along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
  */
  
  #include "Map.h"
  #include "Optimizer.h"
  
  #include "g2o/core/block_solver.h"
  #include "g2o/core/optimization_algorithm_levenberg.h"
  #include "g2o/solvers/eigen/linear_solver_eigen.h"
  #include "g2o/solvers/cholmod/linear_solver_cholmod.h"
  #include "g2o/types/sba/types_six_dof_expmap.h"
  #include "g2o/core/robust_kernel_impl.h"
  #include "g2o/solvers/dense/linear_solver_dense.h"
  #include "g2o/types/sim3/types_seven_dof_expmap.h"
  
  #include <Eigen/StdVector>
  
  #include "Converter.h"
  
  #include <mutex>
  #include <chrono>
  
  namespace ORB_SLAM2
  {
  
  
  void Optimizer::InitialGlobalBundleAdjustemnt(Map* pMap, int nIterations)
  {
      vector<KeyFrame*> vpKFs = pMap->GetAllKeyFrames();
      vector<MapPoint*> vpMP = pMap->GetAllMapPoints();
  
      vector<bool> vbNotIncludedMP;
      vbNotIncludedMP.resize(vpMP.size());
  
      g2o::SparseOptimizer optimizer;
      g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
  
      linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
  
      g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
  
      g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
      optimizer.setAlgorithm(solver);
  
      long unsigned int maxKFid = 0;
  
      // Set KeyFrame vertices
      for(size_t i=0; i<vpKFs.size(); i++)
      {
          KeyFrame* pKF = vpKFs[i];
          if(pKF->isBad())
              continue;
          g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
          vSE3->setEstimate(Converter::toSE3Quat(pKF->GetPose()));
          vSE3->setId(pKF->mnId);
          vSE3->setFixed(true);
          optimizer.addVertex(vSE3);
          if(pKF->mnId>maxKFid)
              maxKFid=pKF->mnId;
      }
  
      const float thHuber2D = sqrt(5.99);
      const float thHuber3D = sqrt(7.815);
  
      // Set MapPoint vertices
      for(size_t i=0; i<vpMP.size(); i++)
      {
          MapPoint* pMP = vpMP[i];
          if(pMP->isBad())
              continue;
          g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
          vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
          const int id = pMP->mnId+maxKFid+1;
          vPoint->setId(id);
          vPoint->setMarginalized(true);
          optimizer.addVertex(vPoint);
  
         const map<KeyFrame*,size_t> observations = pMP->GetObservations();
  
          int nEdges = 0;
          //SET EDGES
          for(map<KeyFrame*,size_t>::const_iterator mit=observations.begin(); mit!=observations.end(); mit++)
          {
  
              KeyFrame* pKF = mit->first;
              if(pKF->isBad() || pKF->mnId>maxKFid)
                  continue;
  
              nEdges++;
  
              const cv::KeyPoint &kpUn = pKF->mvKeysUn[mit->second];
  
              Eigen::Matrix<double,2,1> obs;
              obs << kpUn.pt.x, kpUn.pt.y;
  
              g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ();
  
              e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
              e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKF->mnId)));
              e->setMeasurement(obs);
              const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];
              e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);
  
              if(true)
              {
                  g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
                  e->setRobustKernel(rk);
                  rk->setDelta(thHuber2D);
              }
  
              e->fx = pKF->fx;
              e->fy = pKF->fy;
              e->cx = pKF->cx;
              e->cy = pKF->cy;
  
              optimizer.addEdge(e);
          }
  
          if(nEdges==0)
          {
              optimizer.removeVertex(vPoint);
              vbNotIncludedMP[i]=true;
          }
          else
          {
              vbNotIncludedMP[i]=false;
          }
      }
  
      // Optimize!
      optimizer.initializeOptimization();
      optimizer.optimize(nIterations);
  
      // Recover optimized data
  
      //Keyframes
      for(size_t i=0; i<vpKFs.size(); i++)
      {
          KeyFrame* pKF = vpKFs[i];
          if(pKF->isBad())
              continue;
          g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));
          g2o::SE3Quat SE3quat = vSE3->estimate();
          pKF->SetPose(Converter::toCvMat(SE3quat));
      }
  
      //Points
      for(size_t i=0; i<vpMP.size(); i++)
      {
          if(vbNotIncludedMP[i])
              continue;
  
          MapPoint* pMP = vpMP[i];
  
          if(pMP->isBad())
              continue;
          g2o::VertexSBAPointXYZ* vPoint = static_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(pMP->mnId+maxKFid+1));
  
          pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate()));
          pMP->UpdateNormalAndDepth();
      }
  }
  
  void Optimizer::GlobalBundleAdjustemnt(Map* pMap, int nIterations, bool* pbStopFlag, const unsigned long nLoopKF, const bool bRobust)
  {
      vector<KeyFrame*> vpKFs = pMap->GetAllKeyFrames();
      vector<MapPoint*> vpMP = pMap->GetAllMapPoints();
      BundleAdjustment(vpKFs,vpMP,nIterations,pbStopFlag, nLoopKF, bRobust);
  }
  
  
  void Optimizer::BundleAdjustment(const vector<KeyFrame *> &vpKFs, const vector<MapPoint *> &vpMP,
                                   int nIterations, bool* pbStopFlag, const unsigned long nLoopKF, const bool bRobust)
  {
      vector<bool> vbNotIncludedMP;
      vbNotIncludedMP.resize(vpMP.size());
  
      g2o::SparseOptimizer optimizer;
      g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
  
      // std::unique_ptr<g2o::BlockSolver_6_3::LinearSolverType> linearSolver;
  
      // linearSolver = g2o::make_unique<g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>>();
  
      // g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(
      //                                                       g2o::make_unique<g2o::BlockSolver_6_3>(std::move(linearSolver)));
      linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
  
      g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
  
      g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
      optimizer.setAlgorithm(solver);
  
      if(pbStopFlag)
          optimizer.setForceStopFlag(pbStopFlag);
  
      long unsigned int maxKFid = 0;
  
      // Set KeyFrame vertices
      for(size_t i=0; i<vpKFs.size(); i++)
      {
          KeyFrame* pKF = vpKFs[i];
          if(pKF->isBad())
              continue;
          g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
          vSE3->setEstimate(Converter::toSE3Quat(pKF->GetPose()));
          vSE3->setId(pKF->mnId);
          vSE3->setFixed(pKF->mnId==0);// || pKF->mnId==1);
          optimizer.addVertex(vSE3);
          if(pKF->mnId>maxKFid)
              maxKFid=pKF->mnId;
      }
  
      const float thHuber2D = sqrt(5.99);
      const float thHuber3D = sqrt(7.815);
  
      // Set MapPoint vertices
      for(size_t i=0; i<vpMP.size(); i++)
      {
          MapPoint* pMP = vpMP[i];
          if(pMP->isBad())
              continue;
          g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
          vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
          const int id = pMP->mnId+maxKFid+1;
          vPoint->setId(id);
          vPoint->setMarginalized(true);
          optimizer.addVertex(vPoint);
  
         const map<KeyFrame*,size_t> observations = pMP->GetObservations();
  
          int nEdges = 0;
          //SET EDGES
          for(map<KeyFrame*,size_t>::const_iterator mit=observations.begin(); mit!=observations.end(); mit++)
          {
  
              KeyFrame* pKF = mit->first;
              if(pKF->isBad() || pKF->mnId>maxKFid)
                  continue;
  
              nEdges++;
  
              const cv::KeyPoint &kpUn = pKF->mvKeysUn[mit->second];
  
              Eigen::Matrix<double,2,1> obs;
              obs << kpUn.pt.x, kpUn.pt.y;
  
              g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ();
  
              e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
              e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKF->mnId)));
              e->setMeasurement(obs);
              const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];
              e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);
  
              if(bRobust)
              {
                  g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
                  e->setRobustKernel(rk);
                  rk->setDelta(thHuber2D);
              }
  
              e->fx = pKF->fx;
              e->fy = pKF->fy;
              e->cx = pKF->cx;
              e->cy = pKF->cy;
  
              optimizer.addEdge(e);
          }
  
          if(nEdges==0)
          {
              optimizer.removeVertex(vPoint);
              vbNotIncludedMP[i]=true;
          }
          else
          {
              vbNotIncludedMP[i]=false;
          }
      }
  
      // Optimize!
      optimizer.initializeOptimization();
      optimizer.optimize(nIterations);
  
      // Recover optimized data
  
      //Keyframes
      for(size_t i=0; i<vpKFs.size(); i++)
      {
          KeyFrame* pKF = vpKFs[i];
          if(pKF->isBad())
              continue;
          g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));
          g2o::SE3Quat SE3quat = vSE3->estimate();
          if(nLoopKF==0)
          {
              pKF->SetPose(Converter::toCvMat(SE3quat));
          }
          else
          {
              pKF->mTcwGBA.create(4,4,CV_32F);
              Converter::toCvMat(SE3quat).copyTo(pKF->mTcwGBA);
              pKF->mnBAGlobalForKF = nLoopKF;
          }
      }
  
      //Points
      for(size_t i=0; i<vpMP.size(); i++)
      {
          if(vbNotIncludedMP[i])
              continue;
  
          MapPoint* pMP = vpMP[i];
  
          if(pMP->isBad())
              continue;
          g2o::VertexSBAPointXYZ* vPoint = static_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(pMP->mnId+maxKFid+1));
          pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate()));
          pMP->UpdateNormalAndDepth();
      }
  }
  
  
  int Optimizer::PoseOptimization(Frame *pFrame)
  {
      g2o::SparseOptimizer optimizer;
  
      // std::unique_ptr<g2o::BlockSolver_6_3::LinearSolverType> linearSolver;
  
      // linearSolver = g2o::make_unique<g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>>();
  
      // g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(
      //                                                       g2o::make_unique<g2o::BlockSolver_6_3>(std::move(linearSolver)));
  
      g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
  
      linearSolver = new g2o::LinearSolverDense<g2o::BlockSolver_6_3::PoseMatrixType>();
  
      g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
  
      g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
      optimizer.setAlgorithm(solver);
  
      int nInitialCorrespondences=0;
  
      // Set Frame vertex
      g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
      vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));
      vSE3->setId(0);
      vSE3->setFixed(false);
      optimizer.addVertex(vSE3);
  
      // Set MapPoint vertices
      const int N = pFrame->N;
  
      vector<g2o::EdgeSE3ProjectXYZOnlyPose*> vpEdgesMono;
      vector<size_t> vnIndexEdgeMono;
      vpEdgesMono.reserve(N);
      vnIndexEdgeMono.reserve(N);
  
      const float deltaMono = sqrt(5.991);
  
      {
      unique_lock<mutex> lock(MapPoint::mGlobalMutex);
  
      for(int i=0; i<N; i++)
      {
          MapPoint* pMP = pFrame->mvpMapPoints[i];
          if(pMP)
          {
              // Monocular observation
              nInitialCorrespondences++;
              pFrame->mvbOutlier[i] = false;
  
              Eigen::Matrix<double,2,1> obs;
              const cv::KeyPoint &kpUn = pFrame->mvKeysUn[i];
              obs << kpUn.pt.x, kpUn.pt.y;
  
              g2o::EdgeSE3ProjectXYZOnlyPose* e = new g2o::EdgeSE3ProjectXYZOnlyPose();
  
              e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
              e->setMeasurement(obs);
              const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave];
              e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);
  
              g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
              e->setRobustKernel(rk);
              rk->setDelta(deltaMono);
  
              e->fx = pFrame->fx;
              e->fy = pFrame->fy;
              e->cx = pFrame->cx;
              e->cy = pFrame->cy;
              cv::Mat Xw = pMP->GetWorldPos();
              e->Xw[0] = Xw.at<float>(0);
              e->Xw[1] = Xw.at<float>(1);
              e->Xw[2] = Xw.at<float>(2);
  
              optimizer.addEdge(e);
  
              vpEdgesMono.push_back(e);
              vnIndexEdgeMono.push_back(i);
          }
  
      }
      }
  
      if(nInitialCorrespondences<3)
          return 0;
  
      // We perform 4 optimizations, after each optimization we classify observation as inlier/outlier
      // At the next optimization, outliers are not included, but at the end they can be classified as inliers again.
      const float chi2Mono[4]={5.991,5.991,5.991,5.991};
      const int its[4]={10,10,10,10};
  
      int nBad=0;
      for(size_t it=0; it<4; it++)
      {
  
          vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));
          optimizer.initializeOptimization(0);
          optimizer.optimize(its[it]);
  
          nBad=0;
          for(size_t i=0, iend=vpEdgesMono.size(); i<iend; ++i)
          {
              g2o::EdgeSE3ProjectXYZOnlyPose* e = vpEdgesMono[i];
  
              const size_t idx = vnIndexEdgeMono[i];
  
              if(pFrame->mvbOutlier[idx])
              {
                  e->computeError();
              }
  
              const float chi2 = e->chi2();
  
              if(chi2>chi2Mono[it])
              {
                  pFrame->mvbOutlier[idx]=true;
                  e->setLevel(1);
                  nBad++;
              }
              else
              {
                  pFrame->mvbOutlier[idx]=false;
                  e->setLevel(0);
              }
  
              if(it==2)
                  e->setRobustKernel(0);
          }
  
          if(optimizer.edges().size()<10)
              break;
      }
  
      // Recover optimized pose and return number of inliers
      g2o::VertexSE3Expmap* vSE3_recov = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(0));
      g2o::SE3Quat SE3quat_recov = vSE3_recov->estimate();
      cv::Mat pose = Converter::toCvMat(SE3quat_recov);
      pFrame->SetPose(pose);
  
      return nInitialCorrespondences-nBad;
  }
  
  
  void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap,
                                        pcl::IterativeClosestPoint7dof *icp_,
                                        double dist_coeff, bool use_icp, int use_icp2)
  {
      // Local KeyFrames: First Breath Search from Current Keyframe
      list<KeyFrame*> lLocalKeyFrames;
  
      lLocalKeyFrames.push_back(pKF);
      pKF->mnBALocalForKF = pKF->mnId;
  
      const vector<KeyFrame*> vNeighKFs = pKF->GetVectorCovisibleKeyFrames();
      for(int i=0, iend=vNeighKFs.size(); i<iend; ++i)
      {
          KeyFrame* pKFi = vNeighKFs[i];
          pKFi->mnBALocalForKF = pKF->mnId;
          if(!pKFi->isBad())
              lLocalKeyFrames.push_back(pKFi);
      }
  
      // Local MapPoints seen in Local KeyFrames
      list<MapPoint*> lLocalMapPoints;
      for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; ++lit)
      {
          vector<MapPoint*> vpMPs = (*lit)->GetMapPointMatches();
          for(vector<MapPoint*>::iterator vit=vpMPs.begin(), vend=vpMPs.end(); vit!=vend; ++vit)
          {
              MapPoint* pMP = *vit;
              if(pMP)
                  if(!pMP->isBad())
                      if(pMP->mnBALocalForKF!=pKF->mnId)
                      {
                          lLocalMapPoints.push_back(pMP);
                          pMP->mnBALocalForKF=pKF->mnId;
                      }
          }
      }
  
      // Fixed Keyframes. Keyframes that see Local MapPoints but that are not Local Keyframes
      list<KeyFrame*> lFixedCameras;
      for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; ++lit)
      {
          map<KeyFrame*,size_t> observations = (*lit)->GetObservations();
          for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; ++mit)
          {
              KeyFrame* pKFi = mit->first;
  
              if(pKFi->mnBALocalForKF!=pKF->mnId && pKFi->mnBAFixedForKF!=pKF->mnId)
              {
                  pKFi->mnBAFixedForKF=pKF->mnId;
                  if(!pKFi->isBad())
                      lFixedCameras.push_back(pKFi);
              }
          }
      }
  
      std::cout << "############## KeyFrame Bundle: " << lLocalKeyFrames.size() << " ############\n";
      std::cout << "############## Fixed KeyFrame: " << lFixedCameras.size() << " ############\n";
  
      list<MapPoint*> lFixedMapPoints;
      lFixedMapPoints.clear();
      // for (auto&&  kf: lFixedCameras)
      // {
      //     vector<MapPoint*> vpMPs = kf->GetMapPointMatches();
      //     for (auto&& p : vpMPs)
      //     {
      //         if (p)
      //         {
      //             if (!p->isBad())
      //             {
      //               if (p->mnBALocalForKF != kf->mnId)
      //               {
      //                   lFixedMapPoints.push_back(p);
      //                   p->mnBALocalForKF = p->mnId;
      //               }
      //             }
      //         }
      //     }
      // }
  
      // Setup optimizer
      g2o::SparseOptimizer optimizer;
  
      g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
  
      linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
  
      g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
  
      g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
      optimizer.setAlgorithm(solver);
  
      if(pbStopFlag)
          optimizer.setForceStopFlag(pbStopFlag);
  
      unsigned long maxKFid = 0;
  
      // Set Local KeyFrame vertices
      for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++)
      {
          KeyFrame* pKFi = *lit;
          g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
          vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));
          vSE3->setId(pKFi->mnId);
          vSE3->setFixed(pKFi->mnId==0 || pKFi->mnId==1);
          optimizer.addVertex(vSE3);
          if(pKFi->mnId>maxKFid)
              maxKFid=pKFi->mnId;
      }
  
      // Set Fixed KeyFrame vertices
      for(list<KeyFrame*>::iterator lit=lFixedCameras.begin(), lend=lFixedCameras.end(); lit!=lend; lit++)
      {
          KeyFrame* pKFi = *lit;
          g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
          vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));
          vSE3->setId(pKFi->mnId);
          vSE3->setFixed(true);
          optimizer.addVertex(vSE3);
          if(pKFi->mnId>maxKFid)
              maxKFid=pKFi->mnId;
      }
  
      // Set MapPoint vertices
      const int nExpectedSize = (lLocalKeyFrames.size()+lFixedCameras.size())*lLocalMapPoints.size();
  
      vector<g2o::EdgeSE3ProjectXYZ*> vpEdgesMono;
      vpEdgesMono.reserve(nExpectedSize);
  
      vector<KeyFrame*> vpEdgeKFMono;
      vpEdgeKFMono.reserve(nExpectedSize);
  
      vector<MapPoint*> vpMapPointEdgeMono;
      vpMapPointEdgeMono.reserve(nExpectedSize);
  
      const float thHuberMono = sqrt(5.991);
  
      for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; ++lit)
      {
          MapPoint* pMP = *lit;
          g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
          vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
          int id = pMP->mnId+maxKFid+1;
          vPoint->setId(id);
          vPoint->setMarginalized(true);
          optimizer.addVertex(vPoint);
  
          const map<KeyFrame*,size_t> observations = pMP->GetObservations();
  
          //Set edges
          for(map<KeyFrame*,size_t>::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; ++mit)
          {
              KeyFrame* pKFi = mit->first;
  
              if(!pKFi->isBad())
              {
                  const cv::KeyPoint &kpUn = pKFi->mvKeysUn[mit->second];
  
                  // Monocular observation
                  Eigen::Matrix<double,2,1> obs;
                  obs << kpUn.pt.x, kpUn.pt.y;
  
                  g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ();
  
                  e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
                  e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKFi->mnId)));
                  e->setMeasurement(obs);
                  const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];
                  e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);
  
                  g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
                  e->setRobustKernel(rk);
                  rk->setDelta(thHuberMono);
  
                  e->fx = pKFi->fx;
                  e->fy = pKFi->fy;
                  e->cx = pKFi->cx;
                  e->cy = pKFi->cy;
  
                  optimizer.addEdge(e);
                  vpEdgesMono.push_back(e);
                  vpEdgeKFMono.push_back(pKFi);
                  vpMapPointEdgeMono.push_back(pMP);
              }
          }
      }
  
      if(pbStopFlag)
          if(*pbStopFlag)
              return;
  
      optimizer.initializeOptimization();
      optimizer.optimize(5);
  
      bool bDoMore = true;
  
      if(pbStopFlag)
          if(*pbStopFlag)
              bDoMore = false;
  
      if(bDoMore)
      {
        // Check inlier observations
        for(size_t i=0, iend=vpEdgesMono.size(); i<iend; i++)
        {
            g2o::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
            MapPoint* pMP = vpMapPointEdgeMono[i];
  
            if(pMP->isBad())
                continue;
  
            if(e->chi2()>5.991 || !e->isDepthPositive())
            {
                e->setLevel(1);
            }
  
            e->setRobustKernel(0);
        }
  
        // Optimize again without the outliers
        optimizer.initializeOptimization(0);
        optimizer.optimize(10);
      }
  
      vector<pair<KeyFrame*,MapPoint*> > vToErase;
      vToErase.reserve(vpEdgesMono.size());
  
      // Check inlier observations
      for(size_t i=0, iend=vpEdgesMono.size(); i<iend; i++)
      {
          g2o::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
          MapPoint* pMP = vpMapPointEdgeMono[i];
  
          if(pMP->isBad())
              continue;
  
          if(e->chi2()>5.991 || !e->isDepthPositive())
          {
              KeyFrame* pKFi = vpEdgeKFMono[i];
              vToErase.push_back(make_pair(pKFi,pMP));
          }
      }
  
      // icp matching using map point
      pcl::PointCloud<pcl::PointXYZ>::Ptr l_points (new pcl::PointCloud<pcl::PointXYZ>);
      for (auto&& p : lLocalMapPoints)
      {
          g2o::VertexSBAPointXYZ* vPoint = static_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(p->mnId+maxKFid+1));
          cv::Mat pos = Converter::toCvMat(vPoint->estimate());
          pcl::PointXYZ point;
          point.x = pos.at<float>(0);
          point.y = pos.at<float>(1);
          point.z = pos.at<float>(2);
          l_points->push_back(point);
      }
  
      for (auto&& p : lFixedMapPoints)
      {
          // cv::Mat pos = Converter::toCvMat(p->estimate());
          cv::Mat pos = p->GetWorldPos();
          pcl::PointXYZ point;
          point.x = pos.at<float>(0);
          point.y = pos.at<float>(1);
          point.z = pos.at<float>(2);
          l_points->push_back(point);
      }
  
      Eigen::Matrix4d toRef; // = Converter::toMatrix4d(mpCurrentKeyFrame->GetPose());
  
      for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; ++lit)
      {
          KeyFrame* pKF = *lit;
          g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));
          g2o::SE3Quat SE3quat = vSE3->estimate();
          toRef = Converter::toMatrix4d(Converter::toCvMat(SE3quat));
          break;
      }
  
      std::cout << "Bofore:\n" << toRef.inverse() << "\n";
  
      static int iteration_count = 0;
      iteration_count += 1;
  
      if (use_icp && use_icp2 && iteration_count % 1 == 0 & iteration_count > 10)
      {
          std::cout << "######## Use ICP or NDT method ########\n";
          pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_points (new pcl::PointCloud<pcl::PointXYZ>);
          *filtered_points = *l_points;
  
          icp_->transformCloudPublic(*filtered_points, *filtered_points, toRef);
  
          pcl::PassThrough<pcl::PointXYZ> pass;
          // pass.setInputCloud (filtered_points);
          // pass.setFilterFieldName ("x");
          // pass.setFilterLimits (-12.0, 12.0);
          // pass.filter (*filtered_points);
          // pass.setInputCloud (filtered_points);
          // pass.setFilterFieldName ("z");
          // pass.setFilterLimits (-80, 50.0);
          // pass.filter (*filtered_points);
          pass.setInputCloud (filtered_points);
          pass.setFilterFieldName ("y");
          pass.setFilterLimits (-3, 3);
          pass.filter (*filtered_points);
  
          // double filter_res = 0.1f;
          // pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_filter;
          // voxel_grid_filter.setLeafSize(filter_res, filter_res, filter_res);
          // voxel_grid_filter.setInputCloud(filtered_points);
          // voxel_grid_filter.filter(*filtered_points);
  
          icp_->transformCloudPublic(*filtered_points, *filtered_points, toRef.inverse());
  
          // pcl::console::setVerbosityLevel(pcl::console::L_DEBUG);
          icp_->setInputSource(filtered_points);
          icp_->setMaximumIterations(10);
          icp_->setDistThreshold(dist_coeff);
          pcl::PointCloud<pcl::PointXYZ> output;
          Eigen::Matrix4d transformation;
          Eigen::Matrix4d output_pos;
          double out_scale;
  
          cv::Mat cv_invToRef; // = cv_toRef.inv();
          cv::Mat cv_toRef = Converter::toCvMat(toRef);
  
          icp_->align(output, Eigen::Matrix4d::Identity(), toRef, output_pos, out_scale);
  
          transformation = icp_->getFinalTransformation();
  
          // double scale = pow(cv::determinant(cv_transformation), 1.0/3.0);
          double scale = out_scale;
  
          // cv::Mat cv_transformation = Converter::toCvMat(transformation);
  
          // Update map point and keyframes
  
          // Update points using PCL
          pcl::PointCloud<pcl::PointXYZ>::Ptr converted_points (new pcl::PointCloud<pcl::PointXYZ>);
          *converted_points = *l_points;
  
          icp_->transformCloudPublic(*converted_points, *converted_points, toRef);
          icp_->transformCloudPublic(*converted_points, *converted_points, transformation);
          icp_->transformCloudPublic(*converted_points, *converted_points, toRef.inverse()); //updateToGlobal);
          // Eigen::Matrix4d output_pos_inv = output_pos.inverse();
          // output_pos_inv.block(0, 0, 3, 3) *= scale;
          // icp_->transformCloudPublic(*converted_points, *converted_points, toRef);
          // icp_->transformCloudPublic(*converted_points, *converted_points, output_pos_inv);
  
          // icp_->transformCloudPublic(*filtered_points, *filtered_points, toRef);
          // icp_->transformCloudPublic(*filtered_points, *filtered_points, output_pos_inv);
  
          // Get Map Mutex
          unique_lock<mutex> lock(pMap->mMutexMapUpdate);
  
          // Remove outliers
          if(!vToErase.empty())
          {
              for(size_t i=0; i<vToErase.size(); ++i)
              {
                  KeyFrame* pKFi = vToErase[i].first;
                  MapPoint* pMPi = vToErase[i].second;
                  pKFi->EraseMapPointMatch(pMPi);
                  pMPi->EraseObservation(pKFi);
              }
          }
  
          std::cout << "#### Scale: #### : " << scale << "\n";
  
          for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; ++lit)
          {
              KeyFrame* pKF = *lit;
              pKF->SetPose(Converter::toCvMat(output_pos));
              cv_invToRef = pKF->GetPoseInverse();
              break;
          }
  
          // Update Keyframes
          for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; ++lit)
          {
              KeyFrame* pKF = *lit;
              g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));
              g2o::SE3Quat SE3quat = vSE3->estimate();
              pKF->SetPose(Converter::toCvMat(SE3quat));
              cv::Mat prev_pose = pKF->GetPoseInverse(); // GetPose();
              cv::Mat relative_pose = cv_toRef * prev_pose;
              relative_pose.rowRange(0,3).col(3) *= scale; // multiply scale to translation
              cv::Mat after_pose = cv_invToRef * relative_pose;
              pKF->SetPose(after_pose.inv());
          }
  
          // // Update Keyframes
          // for(list<KeyFrame*>::iterator lit=lFixedCameras.begin(), lend=lFixedCameras.end(); lit!=lend; ++lit)
          // {
          //     KeyFrame* pKF = *lit;
          //     // g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));
          //     // g2o::SE3Quat SE3quat = vSE3->estimate();
          //     // pKF->SetPose(Converter::toCvMat(SE3quat));
          //     cv::Mat prev_pose = pKF->GetPoseInverse(); // GetPose();
          //     cv::Mat relative_pose = cv_toRef * prev_pose;
          //     relative_pose.rowRange(0,3).col(3) *= scale; // multiply scale to translation
          //     cv::Mat after_pose = cv_invToRef * relative_pose;
          //     pKF->SetPose(after_pose.inv());
          // }
  
          vector<MapPoint*> hoge;
          hoge.clear();
  
          // Update points
          int i = 0;
          for (auto&& pMP: lLocalMapPoints)
          {
              if(pMP)
              {
                  if(!pMP->isBad())
                  {
                      pcl::PointXYZ p = converted_points->points[i];
                      cv::Mat pose = (cv::Mat_<float>(3,1) << p.x, p.y, p.z);
                      pMP->SetWorldPos(pose);
                      pMP->UpdateNormalAndDepth();
                      hoge.push_back(pMP);
                  }
              }
              i++;
          }
  
          for (auto&& pMP: lFixedMapPoints)
          {
              if(pMP)
              {
                  if(!pMP->isBad())
                  {
                      pcl::PointXYZ p = converted_points->points[i];
                      cv::Mat pose = (cv::Mat_<float>(3,1) << p.x, p.y, p.z);
                      pMP->SetWorldPos(pose);
                      pMP->UpdateNormalAndDepth();
                      hoge.push_back(pMP);
                  }
              }
              i++;
          }
  
          std::vector<cv::Mat> hoge2;
          for (auto&& p: filtered_points->points)
          {
              cv::Mat pose = (cv::Mat_<float>(3,1) << p.x, p.y, p.z);
              hoge2.push_back(pose);
          }
  
          pMap->SetLocalMappingPoint(hoge);
          pMap->SetUsedMatchingPoint(hoge2);
  
      } else
      {
          std::cout << "Not use ICP or NDT matching\n";
          if(!vToErase.empty())
          {
              for(size_t i=0;i<vToErase.size();i++)
              {
                  KeyFrame* pKFi = vToErase[i].first;
                  MapPoint* pMPi = vToErase[i].second;
                  pKFi->EraseMapPointMatch(pMPi);
                  pMPi->EraseObservation(pKFi);
              }
          }
  
          //Keyframes
          for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++)
          {
              KeyFrame* pKF = *lit;
              g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));
              g2o::SE3Quat SE3quat = vSE3->estimate();
              pKF->SetPose(Converter::toCvMat(SE3quat));
          }
  
          //Points
          vector<MapPoint*> hoge;
          hoge.clear();
  
          for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)
          {
              MapPoint* pMP = *lit;
              g2o::VertexSBAPointXYZ* vPoint = static_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(pMP->mnId+maxKFid+1));
              pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate()));
              pMP->UpdateNormalAndDepth();
              hoge.push_back(pMP);
          }
          pMap->SetLocalMappingPoint(hoge);
      }
  
  }
  
  
  int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &vpMatches1, g2o::Sim3 &g2oS12, const float th2, const bool bFixScale)
  {
      g2o::SparseOptimizer optimizer;
  
      // std::unique_ptr<g2o::BlockSolver_7_3::LinearSolverType> linearSolver;
  
      // linearSolver = g2o::make_unique<g2o::LinearSolverCholmod<g2o::BlockSolver_7_3::PoseMatrixType>>();
  
      // g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(
      //                                                       g2o::make_unique<g2o::BlockSolver_7_3>(std::move(linearSolver)));
      g2o::BlockSolverX::LinearSolverType * linearSolver;
  
      linearSolver = new g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>();
  
      g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver);
  
      g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
      optimizer.setAlgorithm(solver);
  
      // Calibration
      const cv::Mat &K1 = pKF1->mK;
      const cv::Mat &K2 = pKF2->mK;
  
      // Camera poses
      const cv::Mat R1w = pKF1->GetRotation();
      const cv::Mat t1w = pKF1->GetTranslation();
      const cv::Mat R2w = pKF2->GetRotation();
      const cv::Mat t2w = pKF2->GetTranslation();
  
      // Set Sim3 vertex
      g2o::VertexSim3Expmap * vSim3 = new g2o::VertexSim3Expmap();
      vSim3->_fix_scale=bFixScale;
      vSim3->setEstimate(g2oS12);
      vSim3->setId(0);
      vSim3->setFixed(false);
      vSim3->_principle_point1[0] = K1.at<float>(0,2);
      vSim3->_principle_point1[1] = K1.at<float>(1,2);
      vSim3->_focal_length1[0] = K1.at<float>(0,0);
      vSim3->_focal_length1[1] = K1.at<float>(1,1);
      vSim3->_principle_point2[0] = K2.at<float>(0,2);
      vSim3->_principle_point2[1] = K2.at<float>(1,2);
      vSim3->_focal_length2[0] = K2.at<float>(0,0);
      vSim3->_focal_length2[1] = K2.at<float>(1,1);
      optimizer.addVertex(vSim3);
  
      // Set MapPoint vertices
      const int N = vpMatches1.size();
      const vector<MapPoint*> vpMapPoints1 = pKF1->GetMapPointMatches();
      vector<g2o::EdgeSim3ProjectXYZ*> vpEdges12;
      vector<g2o::EdgeInverseSim3ProjectXYZ*> vpEdges21;
      vector<size_t> vnIndexEdge;
  
      vnIndexEdge.reserve(2*N);
      vpEdges12.reserve(2*N);
      vpEdges21.reserve(2*N);
  
      const float deltaHuber = sqrt(th2);
  
      int nCorrespondences = 0;
  
      for(int i=0; i<N; i++)
      {
          if(!vpMatches1[i])
              continue;
  
          MapPoint* pMP1 = vpMapPoints1[i];
          MapPoint* pMP2 = vpMatches1[i];
  
          const int id1 = 2*i+1;
          const int id2 = 2*(i+1);
  
          const int i2 = pMP2->GetIndexInKeyFrame(pKF2);
  
          if(pMP1 && pMP2)
          {
              if(!pMP1->isBad() && !pMP2->isBad() && i2>=0)
              {
                  g2o::VertexSBAPointXYZ* vPoint1 = new g2o::VertexSBAPointXYZ();
                  cv::Mat P3D1w = pMP1->GetWorldPos();
                  cv::Mat P3D1c = R1w*P3D1w + t1w;
                  vPoint1->setEstimate(Converter::toVector3d(P3D1c));
                  vPoint1->setId(id1);
                  vPoint1->setFixed(true);
                  optimizer.addVertex(vPoint1);
  
                  g2o::VertexSBAPointXYZ* vPoint2 = new g2o::VertexSBAPointXYZ();
                  cv::Mat P3D2w = pMP2->GetWorldPos();
                  cv::Mat P3D2c = R2w*P3D2w + t2w;
                  vPoint2->setEstimate(Converter::toVector3d(P3D2c));
                  vPoint2->setId(id2);
                  vPoint2->setFixed(true);
                  optimizer.addVertex(vPoint2);
              }
              else
                  continue;
          }
          else
              continue;
  
          nCorrespondences++;
  
          // Set edge x1 = S12*X2
          Eigen::Matrix<double,2,1> obs1;
          const cv::KeyPoint &kpUn1 = pKF1->mvKeysUn[i];
          obs1 << kpUn1.pt.x, kpUn1.pt.y;
  
          g2o::EdgeSim3ProjectXYZ* e12 = new g2o::EdgeSim3ProjectXYZ();
          e12->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id2)));
          e12->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
          e12->setMeasurement(obs1);
          const float &invSigmaSquare1 = pKF1->mvInvLevelSigma2[kpUn1.octave];
          e12->setInformation(Eigen::Matrix2d::Identity()*invSigmaSquare1);
  
          g2o::RobustKernelHuber* rk1 = new g2o::RobustKernelHuber;
          e12->setRobustKernel(rk1);
          rk1->setDelta(deltaHuber);
          optimizer.addEdge(e12);
  
          // Set edge x2 = S21*X1
          Eigen::Matrix<double,2,1> obs2;
          const cv::KeyPoint &kpUn2 = pKF2->mvKeysUn[i2];
          obs2 << kpUn2.pt.x, kpUn2.pt.y;
  
          g2o::EdgeInverseSim3ProjectXYZ* e21 = new g2o::EdgeInverseSim3ProjectXYZ();
  
          e21->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id1)));
          e21->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
          e21->setMeasurement(obs2);
          float invSigmaSquare2 = pKF2->mvInvLevelSigma2[kpUn2.octave];
          e21->setInformation(Eigen::Matrix2d::Identity()*invSigmaSquare2);
  
          g2o::RobustKernelHuber* rk2 = new g2o::RobustKernelHuber;
          e21->setRobustKernel(rk2);
          rk2->setDelta(deltaHuber);
          optimizer.addEdge(e21);
  
          vpEdges12.push_back(e12);
          vpEdges21.push_back(e21);
          vnIndexEdge.push_back(i);
      }
  
      // Optimize!
      optimizer.initializeOptimization();
      optimizer.optimize(5);
  
      // Check inliers
      int nBad=0;
      for(size_t i=0; i<vpEdges12.size();i++)
      {
          g2o::EdgeSim3ProjectXYZ* e12 = vpEdges12[i];
          g2o::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i];
          if(!e12 || !e21)
              continue;
  
          if(e12->chi2()>th2 || e21->chi2()>th2)
          {
              size_t idx = vnIndexEdge[i];
              vpMatches1[idx]=static_cast<MapPoint*>(NULL);
              optimizer.removeEdge(e12);
              optimizer.removeEdge(e21);
              vpEdges12[i]=static_cast<g2o::EdgeSim3ProjectXYZ*>(NULL);
              vpEdges21[i]=static_cast<g2o::EdgeInverseSim3ProjectXYZ*>(NULL);
              nBad++;
          }
      }
  
      int nMoreIterations;
      if(nBad>0)
          nMoreIterations=10;
      else
          nMoreIterations=5;
  
      if(nCorrespondences-nBad<10)
          return 0;
  
      // Optimize again only with inliers
  
      optimizer.initializeOptimization();
      optimizer.optimize(nMoreIterations);
  
      int nIn = 0;
      for(size_t i=0; i<vpEdges12.size();i++)
      {
          g2o::EdgeSim3ProjectXYZ* e12 = vpEdges12[i];
          g2o::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i];
          if(!e12 || !e21)
              continue;
  
          if(e12->chi2()>th2 || e21->chi2()>th2)
          {
              size_t idx = vnIndexEdge[i];
              vpMatches1[idx]=static_cast<MapPoint*>(NULL);
          }
          else
              nIn++;
      }
  
      // Recover optimized Sim3
      g2o::VertexSim3Expmap* vSim3_recov = static_cast<g2o::VertexSim3Expmap*>(optimizer.vertex(0));
      g2oS12= vSim3_recov->estimate();
  
      return nIn;
  }
  
  
  } //namespace ORB_SLAM
  