bandle adjustment

GPU_version/vio/CMakeLists.txt

@@ -28,7 +28,8 @@ FIND_PACKAGE(OpenCV 4 REQUIRED)
 FIND_PACKAGE(Eigen REQUIRED)
 FIND_PACKAGE(Boost REQUIRED COMPONENTS thread system)
 FIND_PACKAGE(OpenCL REQUIRED)
-#FIND_PACKAGE(G2O REQUIRED)
+FIND_PACKAGE(G2O REQUIRED)
+#FIND_PACKAGE(SuiteSparse)
 
 
 FIND_PACKAGE(catkin REQUIRED COMPONENTS roscpp roslib cmake_modules image_transport cv_bridge tf geometry_msgs message_generation)
@@ -53,18 +54,22 @@ INCLUDE_DIRECTORIES(
         ${Boost_INCLUDE_DIRS}
         ${OpenCL_INCLUDE_DIRS}
         ${catkin_INCLUDE_DIRS}
-        #${G2O_INCLUDE_DIR}
+        ${G2O_INCLUDE_DIR}
-        #/usr/include/suitesparse # for cholmod
+        /usr/include/suitesparse # for cholmod
 )
 
 # Set link libraries
 LIST(APPEND LINK_LIBS
   ${OpenCV_LIBS}
   ${Boost_LIBRARIES}
+  ${Boost_Fibers_LIBRARIES}
   ${OpenCL_LIBRARIES}
   ${catkin_LIBRARIES}
- # ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY} ${G2O_SOLVER_CHOLMOD} ${G2O_SOLVER_CSPARSE} ${G2O_SOLVER_DENSE}
+  ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY} ${G2O_SOLVER_CHOLMOD}
- # ${G2O_SOLVER_PCG} ${G2O_TYPES_SBA} cholmod cxsparse
+  ${G2O_SOLVER_PCG}
+  ${G2O_TYPES_SBA}
+  ${G2O_TYPES_SLAM3D}
+  cholmod cxsparse
 )
 
 file(GLOB SRC  CONFIGURE_DEPENDS "src/*.cpp" "include/sophus/*.cpp")
@@ -80,5 +85,6 @@ target_compile_features(${PROJECT_NAME} PRIVATE cxx_range_for)
 ADD_DEFINITIONS(-DKERNEL_DIR=\"${PROJECT_SOURCE_DIR}/kernel\")
 ADD_DEFINITIONS(-DPROJECT_DIR=\"${PROJECT_SOURCE_DIR}\")
 ADD_DEFINITIONS(-DVIO_DEBUG=true)
+ADD_DEFINITIONS(-DG2O_USE_VENDORED_CERES=ON)
 
 

GPU_version/vio/CMakeModules/FindCholmod.cmake

@@ -0,0 +1,89 @@
+# Cholmod lib usually requires linking to a blas and lapack library.
+# It is up to the user of this module to find a BLAS and link to it.
+
+if (CHOLMOD_INCLUDES AND CHOLMOD_LIBRARIES)
+  set(CHOLMOD_FIND_QUIETLY TRUE)
+endif ()
+
+find_path(CHOLMOD_INCLUDES
+  NAMES
+  cholmod.h
+  PATHS
+  $ENV{CHOLMODDIR}
+  ${INCLUDE_INSTALL_DIR}
+  PATH_SUFFIXES
+  suitesparse
+  ufsparse
+)
+
+find_library(CHOLMOD_LIBRARIES cholmod PATHS $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
+
+if(CHOLMOD_LIBRARIES)
+
+  get_filename_component(CHOLMOD_LIBDIR ${CHOLMOD_LIBRARIES} PATH)
+
+  find_library(AMD_LIBRARY amd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
+  if (AMD_LIBRARY)
+    set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${AMD_LIBRARY})
+  else ()
+    set(CHOLMOD_LIBRARIES FALSE)
+  endif ()
+
+endif()
+
+if(CHOLMOD_LIBRARIES)
+
+  find_library(COLAMD_LIBRARY colamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
+  if (COLAMD_LIBRARY)
+    set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${COLAMD_LIBRARY})
+  else ()
+    set(CHOLMOD_LIBRARIES FALSE)
+  endif ()
+
+endif()
+
+if(CHOLMOD_LIBRARIES)
+
+  find_library(CAMD_LIBRARY camd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
+  if (CAMD_LIBRARY)
+    set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CAMD_LIBRARY})
+  else ()
+    set(CHOLMOD_LIBRARIES FALSE)
+  endif ()
+
+endif()
+
+if(CHOLMOD_LIBRARIES)
+
+  find_library(CCOLAMD_LIBRARY ccolamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
+  if (CCOLAMD_LIBRARY)
+    set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CCOLAMD_LIBRARY})
+  else ()
+    set(CHOLMOD_LIBRARIES FALSE)
+  endif ()
+
+endif()
+
+if(CHOLMOD_LIBRARIES)
+
+  find_library(CHOLMOD_METIS_LIBRARY metis PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
+  if (CHOLMOD_METIS_LIBRARY)
+    set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CHOLMOD_METIS_LIBRARY})
+  endif ()
+
+endif()
+
+if(CHOLMOD_LIBRARIES)
+
+  find_library(SUITESPARSE_LIBRARY SuiteSparse PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
+  if (SUITESPARSE_LIBRARY)
+    set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${SUITESPARSE_LIBRARY})
+  endif ()
+  
+endif()
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(Cholmod DEFAULT_MSG
+                                  CHOLMOD_INCLUDES CHOLMOD_LIBRARIES)
+
+mark_as_advanced(CHOLMOD_INCLUDES CHOLMOD_LIBRARIES AMD_LIBRARY COLAMD_LIBRARY SUITESPARSE_LIBRARY CAMD_LIBRARY CCOLAMD_LIBRARY CHOLMOD_METIS_LIBRARY)

GPU_version/vio/depth_filter_log.txt

@@ -0,0 +1,2 @@
+[13:20:52.582] New key frame 
+[13:20:52.582] BA loop 

GPU_version/vio/include/vio/frame.h

@@ -22,11 +22,8 @@
 #include <vio/abstract_camera.h>
 #include <boost/noncopyable.hpp>
 #include <vio/global.h>
+#include <g2o/types/sba/types_six_dof_expmap.h>
 
-namespace g2o {
-    class VertexSE3Expmap;
-}
-typedef g2o::VertexSE3Expmap g2oFrameSE3;
 
 namespace vio {
 
@@ -53,7 +50,7 @@ namespace vio {
         Features                      fts_;                   //!< List of features in the image.
         vector<std::shared_ptr<Feature>>  key_pts_;               //!< Five features and associated 3D points which are used to detect if two frames have overlapping field of view.
         bool                          is_keyframe_;           //!< Was this frames selected as keyframe?
-        g2oFrameSE3*                  v_kf_;                  //!< Temporary pointer to the g2o node object of the keyframe.
+        std::shared_ptr<g2o::VertexSE3Expmap>    v_kf_;                  //!< Temporary pointer to the g2o node object of the keyframe.
         int                           last_published_ts_;     //!< Timestamp of last publishing.
 
         Frame(vk::AbstractCamera* cam, const cv::Mat& img, double timestamp);

GPU_version/vio/include/vio/global_optimizer.h

@@ -24,15 +24,23 @@
 #include <vio/feature_detection.h>
 #include <vio/matcher.h>
 #include <vio/map.h>
+#include <g2o/types/sba/types_six_dof_expmap.h>
+#include <g2o/core/sparse_optimizer.h>
+#include <g2o/core/block_solver.h>
+#include <g2o/core/solver.h>
+#include <g2o/core/robust_kernel_impl.h>
+#include <g2o/core/optimization_algorithm_levenberg.h>
+#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
+#include <g2o/solvers/csparse/linear_solver_csparse.h>
+#include <g2o/solvers/dense/linear_solver_dense.h>
+#include <g2o/solvers/structure_only/structure_only_solver.h>
+
 
 namespace vio {
 
 class BA_Glob
 {
 public:
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-  typedef boost::unique_lock<boost::mutex> lock_;
 
   BA_Glob(Map& map);
 
@@ -44,18 +52,62 @@ public:
   /// Stop the parallel thread that is running.
   void stopThread();
 
-
+  void new_key_frame(){
+      boost::unique_lock< boost::mutex > lk( mtx_);
+      new_keyframe_=true;
+      cond_.notify_one();
+#if VIO_DEBUG
+      fprintf(log_,"[%s] New key frame \n",
+              vio::time_in_HH_MM_SS_MMM().c_str());
+#endif
+  }
 protected:
-  bool seeds_updating_halt_;            //!< Set this value to true when seeds updating should be interrupted.
+  bool new_keyframe_=false;
+  std::shared_ptr<g2o::OptimizationAlgorithmLevenberg> Levenberg_;
+  std::shared_ptr<g2o::SparseOptimizer> optimizer_;
+  boost::condition_variable cond_;
+  boost::mutex mtx_;
   boost::thread* thread_;
   Map& map_;
 
 #if VIO_DEBUG
   FILE* log_=nullptr;
 #endif
+        /// Temporary container to hold the g2o edge with reference to frame and point.
+        struct EdgeContainerSE3{
+            std::shared_ptr<g2o::EdgeProjectXYZ2UV>     edge;
+            std::shared_ptr<Frame>          frame;
+            std::shared_ptr<Feature>        feature;
+        };
 
+/// Initialize g2o with solver type, optimization strategy and camera model.
+        void setupG2o();
+        /// Run the optimization on the provided graph.
+        void runSparseBAOptimizer(
+                unsigned int num_iter,
+                double& init_error,
+                double& final_error);
 
+/// Create a g2o vertice from a keyframe object.
+        std::shared_ptr<g2o::VertexSE3Expmap> createG2oFrameSE3(
+                FramePtr kf,
+                size_t id,
+                bool fixed);
+    /// Creates a g2o vertice from a mappoint object.
+        std::shared_ptr<g2o::VertexPointXYZ> createG2oPoint(
+                Vector3d pos,
+                size_t id,
+                bool fixed);
+  /// Creates a g2o edge between a g2o keyframe and mappoint vertice with the provided measurement.
+        std::shared_ptr<g2o::EdgeProjectXYZ2UV> createG2oEdgeSE3(
+                std::shared_ptr<g2o::VertexSE3Expmap> v_kf,
+                std::shared_ptr<g2o::VertexPointXYZ> v_mp,
+                const Vector2d& f_up,
+                bool robust_kernel,
+                double huber_width,
+                double weight = 1);
   /// A thread that is continuously optimizing the map.
+  /// Global bundle adjustment.
   void updateLoop();
 };
 

GPU_version/vio/include/vio/initialization.h

@@ -84,12 +84,12 @@ protected:
         vector<int> outliers_hom, inlier_hom;
         vector<int> outliers_ekf,inlier_ekf;
         assert(ukf_!= nullptr);
-#if VIO_DEBUG
+/*#if VIO_DEBUG
         for(int i=0;i<f_cur.size();++i)fprintf(log_,"[%s] cur: x=%f, y=%f, z=%f ref: x=%f y=%f z=%f\n",
               vio::time_in_HH_MM_SS_MMM().c_str(),
               f_cur.at(i).x(),f_cur.at(i).y(),f_cur.at(i).z(),
               f_ref.at(i).x(),f_ref.at(i).y(),f_ref.at(i).z());
-#endif
+#endif*/
         vk::computeInliers(f_cur, f_ref,
                            Homography.T_c2_from_c1.rotation_matrix(), Homography.T_c2_from_c1.translation(),
                            reprojection_threshold, focal_length,

GPU_version/vio/include/vio/point.h

@@ -21,12 +21,8 @@
 #include <vio/global.h>
 #include <boost/thread.hpp>
 #include <boost/function.hpp>
-
+#include <g2o/types/sba/types_six_dof_expmap.h>
 static boost::mutex point_mut_;
-namespace g2o {
-  class VertexSBAPointXYZ;
-}
-typedef g2o::VertexSBAPointXYZ g2oPoint;
 
 namespace vio {
 
@@ -55,7 +51,7 @@ public:
   bool                        normal_set_;              //!< Flag whether the surface normal was estimated or not.
   list<std::shared_ptr<Feature>>              obs_;                     //!< References to keyframes which observe the point.
   size_t                      n_obs_;                   //!< Number of obervations: Keyframes AND successful reprojections in intermediate frames.
-  g2oPoint*                   v_pt_;                    //!< Temporary pointer to the point-vertex in g2o during bundle adjustment.
+  std::shared_ptr<g2o::VertexPointXYZ>     v_pt_;                    //!< Temporary pointer to the point-vertex in g2o during bundle adjustment.
   int                         last_published_ts_;       //!< Timestamp of last publishing.
   int                         last_frame_overlap_id_;    //!< Flag for the reprojection: don't reproject a pt twice.
   PointType                   type_;                    //!< Quality of the point.

GPU_version/vio/src/frame_handler_mono.cpp

@@ -168,6 +168,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
 #endif
   ROS_INFO("VIO initialized :)");
   ROS_INFO("Running ...");
+    ba_glob_->new_key_frame();
   return RESULT_IS_KEYFRAME;
 }
 
@@ -209,7 +210,6 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   pose_optimizer::optimizeGaussNewton(
             Config::poseOptimThresh(), 70, false,
             new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final,log_);
-  optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter());
 #if VIO_DEBUG
     fprintf(log_,"[%s] After pose optimization, distance between ekf and vo x:%f ,z=%f,angle between two frames:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
             new_frame_->T_f_w_.se2().translation().x()-init_f.second.se2().translation().x(),
@@ -229,6 +229,8 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
     fprintf(log_,"[%s] Update EKF and 3D points the number of feature in the new frame: %d and number of obs: %d\n",vio::time_in_HH_MM_SS_MMM().c_str(),
             new_frame_->fts_.size(),new_frame_->nObs());
 #endif
+
+  optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter());
   // select keyframe
   if(!needNewKf())//edited
   {
@@ -256,6 +258,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   }
   // add keyframe to map
   map_.addKeyframe(new_frame_);
+  ba_glob_->new_key_frame();
   return RESULT_IS_KEYFRAME;
 }
 

GPU_version/vio/src/global_optimizer.cpp

@@ -31,7 +31,7 @@
 namespace vio {
 
 
-    BA_Glob::BA_Glob(Map& map) :map_(map),seeds_updating_halt_(false),thread_(NULL)
+    BA_Glob::BA_Glob(Map& map) :map_(map),thread_(NULL)
     {
 #if VIO_DEBUG
         log_ =fopen((std::string(PROJECT_DIR)+"/depth_filter_log.txt").c_str(),"w+");
@@ -55,7 +55,6 @@ namespace vio {
 
         if(thread_ != NULL)
         {
-            seeds_updating_halt_ = true;
             thread_->interrupt();
             usleep(5000);
             thread_->join();
@@ -65,13 +64,176 @@ namespace vio {
         fclose(log_);
 #endif
     }
-
     void BA_Glob::updateLoop()
     {
         while(!boost::this_thread::interruption_requested())
         {
-            usleep(20000);
+            boost::unique_lock< boost::mutex > lk( mtx_);
+            while(map_.keyframes_.empty() || new_keyframe_ == false)
+                cond_.wait(lk);
+#if VIO_DEBUG
+            fprintf(log_,"[%s] BA loop \n",
+                    vio::time_in_HH_MM_SS_MMM().c_str());
+#endif
+            new_keyframe_=false;
+            // init g2o
+            setupG2o();
+            list<EdgeContainerSE3> edges;
+            list< pair<FramePtr,std::shared_ptr<Feature>> > incorrect_edges;
+            // Go through all Keyframes
+            size_t v_id = 0;
+            point_mut_.lock();
+            for(list<FramePtr>::iterator it_kf = map_.keyframes_.begin();
+                it_kf != map_.keyframes_.end(); ++it_kf)
+            {
+                // New Keyframe Vertex
+                std::shared_ptr<g2o::VertexSE3Expmap> v_kf = createG2oFrameSE3(*it_kf, v_id++, false);
+                (*it_kf)->v_kf_ = v_kf;
+                optimizer_->addVertex(v_kf.get());
+                for(auto&& it_ftr:(*it_kf)->fts_)
+                {
+                    // for each keyframe add edges to all observed mapoints
+                    std::shared_ptr<Point> mp = it_ftr->point;
+                    if(mp == NULL)
+                        continue;
+                    std::shared_ptr<g2o::VertexPointXYZ> v_mp = mp->v_pt_;
+                    if(v_mp == NULL)
+                    {
+                        // mappoint-vertex doesn't exist yet. create a new one:
+                        v_mp = createG2oPoint(mp->pos_, v_id++, false);
+                        mp->v_pt_ = v_mp;
+                        optimizer_->addVertex(v_mp.get());
+                    }
+
+                    // Due to merging of mappoints it is possible that references in kfs suddenly
+                    // have a very large reprojection error which may result in distorted results.
+                    Vector2d error = vk::project2d(it_ftr->f) - vk::project2d((*it_kf)->w2f(mp->pos_));
+                    if(error.squaredNorm() > pow(2,Config::poseOptimThresh()/(*it_kf)->cam_->errorMultiplier2()))
+                        incorrect_edges.push_back(pair<FramePtr,std::shared_ptr<Feature>>(*it_kf, it_ftr));
+                    else
+                    {
+                        std::shared_ptr<g2o::EdgeProjectXYZ2UV> e = createG2oEdgeSE3(v_kf, v_mp, vk::project2d(it_ftr->f),
+                                                                         true,
+                                                                         Config::poseOptimThresh()/(*it_kf)->cam_->errorMultiplier2()*Config::lobaRobustHuberWidth());
+                        EdgeContainerSE3 edge;
+                        edge.frame=(*it_kf);
+                        edge.edge=e;
+                        edge.feature=it_ftr;
+                        edges.push_back(edge);
+                        optimizer_->addEdge(e.get());
+                    }
+                }
+            }
+
+            // Optimization
+            double init_error=0.0, final_error=0.0;
+            if(Config::lobaNumIter() > 0)
+                runSparseBAOptimizer( Config::lobaNumIter(), init_error, final_error);
+
+            // Update Keyframe and MapPoint Positions
+            for(list<FramePtr>::iterator it_kf = map_.keyframes_.begin();
+                it_kf != map_.keyframes_.end(); ++it_kf)
+            {
+                (*it_kf)->T_f_w_ = SE3( (*it_kf)->v_kf_->estimate().rotation(),
+                                        (*it_kf)->v_kf_->estimate().translation());
+                (*it_kf)->v_kf_ = NULL;
+                for(Features::iterator it_ftr=(*it_kf)->fts_.begin(); it_ftr!=(*it_kf)->fts_.end(); ++it_ftr)
+                {
+                    std::shared_ptr<Point> mp = (*it_ftr)->point;
+                    if(mp == NULL)
+                        continue;
+                    if(mp->v_pt_ == NULL)
+                        continue;       // mp was updated before
+                    mp->pos_ = mp->v_pt_->estimate();
+                    mp->v_pt_ = NULL;
+                }
+            }
+
+            // Remove Measurements with too large reprojection error
+            for(list< pair<FramePtr,std::shared_ptr<Feature>> >::iterator it=incorrect_edges.begin();
+                it!=incorrect_edges.end(); ++it)
+                map_.removePtFrameRef(it->first, it->second);
+
+            for(list<EdgeContainerSE3>::iterator it = edges.begin(); it != edges.end(); ++it)
+            {
+                if(it->edge->chi2() > 0.001)
+                {
+                    map_.removePtFrameRef(it->frame, it->feature);
+                }
+            }
+            Levenberg_.reset();
+            point_mut_.unlock();
         }
     }
 
+   void BA_Glob::setupG2o()
+   {
+       // TODO: What's happening with all this HEAP stuff? Memory Leak?
+       optimizer_=std::make_shared<g2o::SparseOptimizer>();
+       optimizer_->setVerbose(false);
+       using LinearSolver = g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>;
+       Levenberg_ = std::make_shared<g2o::OptimizationAlgorithmLevenberg>(
+               g2o::make_unique<g2o::BlockSolver_6_3>(g2o::make_unique<LinearSolver>()));
+
+       Levenberg_->setMaxTrialsAfterFailure(5);
+       optimizer_->setAlgorithm(Levenberg_.get());
+
+       // setup camera
+       std::shared_ptr<g2o::CameraParameters> cam_params = std::make_shared<g2o::CameraParameters>(1.0, Vector2d(0.,0.), 0.);
+       cam_params->setId(0);
+       if (!optimizer_->addParameter(cam_params.get())) {
+           assert(false);
+       }
+   }
+
+   void
+   BA_Glob::runSparseBAOptimizer(unsigned int num_iter,
+                        double& init_error, double& final_error)
+   {
+       optimizer_->initializeOptimization();
+       optimizer_->computeActiveErrors();
+       init_error = optimizer_->activeChi2();
+       optimizer_->optimize(num_iter);
+       final_error = optimizer_->activeChi2();
+   }
+
+   std::shared_ptr<g2o::VertexSE3Expmap>
+   BA_Glob::createG2oFrameSE3(FramePtr frame, size_t id, bool fixed)
+   {
+       std::shared_ptr<g2o::VertexSE3Expmap> v=std::make_shared<g2o::VertexSE3Expmap>();
+       v->setId(id);
+       v->setFixed(fixed);
+       v->setEstimate(g2o::SE3Quat(frame->T_f_w_.se3().unit_quaternion(), frame->T_f_w_.se3().translation()));
+       return v;
+   }
+
+   std::shared_ptr<g2o::VertexPointXYZ>
+   BA_Glob::createG2oPoint(Vector3d pos,
+                  size_t id,
+                  bool fixed)
+   {
+       std::shared_ptr<g2o::VertexPointXYZ> v =std::make_shared<g2o::VertexPointXYZ>();
+       v->setId(id);
+       v->setFixed(fixed);
+       v->setEstimate(pos);
+       return v;
+   }
+
+   std::shared_ptr<g2o::EdgeProjectXYZ2UV> BA_Glob::createG2oEdgeSE3( std::shared_ptr<g2o::VertexSE3Expmap> v_frame,
+                     std::shared_ptr<g2o::VertexPointXYZ> v_point,
+                     const Vector2d& f_up,
+                     bool robust_kernel,
+                     double huber_width,
+                     double weight)
+   {
+       std::shared_ptr<g2o::EdgeProjectXYZ2UV> e=std::make_shared<g2o::EdgeProjectXYZ2UV>();
+       e->setVertex(0, (g2o::OptimizableGraph::Vertex*)(v_point).get());
+       e->setVertex(1, (g2o::OptimizableGraph::Vertex*)(v_frame).get());
+       e->setMeasurement(f_up);
+       e->information() = weight * Eigen::Matrix2d::Identity(2,2);
+       e->setRobustKernel(new g2o::RobustKernelHuber());
+       e->setParameterId(0, 0); //old: e->setId(v_point->id());
+       return e;
+   }
+
 } // namespace vio

GPU_version/vio/src/vo_node.cpp

@@ -3,13 +3,13 @@
 #include <ros/ros.h>
 #include <string>
 #include <chrono>
+#include <boost/thread.hpp>
 #include <vio/frame_handler_mono.h>
 #include <vio/params_helper.h>
 #include <sensor_msgs/Image.h>
 #include <sensor_msgs/Imu.h>
 #include <geometry_msgs/Twist.h>
 #include <image_transport/image_transport.h>
-#include <boost/thread.hpp>
 #include <cv_bridge/cv_bridge.h>
 #include <Eigen/Core>
 #include <vio/abstract_camera.h>