deactivate g2o in cmake

GPU_version/vio/CMakeLists.txt

@@ -28,7 +28,7 @@ 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(catkin REQUIRED COMPONENTS roscpp roslib cmake_modules image_transport cv_bridge tf geometry_msgs message_generation)
@@ -53,8 +53,8 @@ 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
@@ -63,8 +63,8 @@ LIST(APPEND LINK_LIBS
   ${Boost_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_CSPARSE} ${G2O_SOLVER_DENSE}
-  ${G2O_SOLVER_PCG} ${G2O_TYPES_SBA} cholmod cxsparse
+ # ${G2O_SOLVER_PCG} ${G2O_TYPES_SBA} cholmod cxsparse
 )
 
 file(GLOB SRC  CONFIGURE_DEPENDS "src/*.cpp" "include/sophus/*.cpp")

GPU_version/vio/include/vio/cl_class.h

@@ -16,7 +16,11 @@
 
 class kernel{
 public:
-    kernel(cl::Program* program,std::string name){_kernel = new cl::Kernel(*program,name.c_str());};
+    kernel(cl::Program* program,std::string name){
+        cl_int error;
+        _kernel = new cl::Kernel(*program,name.c_str(),&error);
+        assert(error== CL_SUCCESS);
+    };
     ~kernel(){
         for(auto&& i:_images)i.first.setDestructorCallback((void (*)(_cl_mem *, void *))notify, NULL);
         for(auto&& i:_buffers)i.first.first.setDestructorCallback((void (*)(_cl_mem *, void *))notify, NULL);
@@ -150,7 +154,10 @@ public:
     cl_int run(size_t id1/*kernal ID*/,std::size_t  x=1,std::size_t y=1,std::size_t z=1) {
         cl_int err=0;
         cl::Event event;
-        queue->flush();
+        if(queue->flush()==CL_OUT_OF_HOST_MEMORY){
+            std::cerr<<"GPU out of memory goodbye:)\n";
+            exit(0);
+        }
         if(z>1 && y>1){
             err=queue->enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x,y,z)/*Global*/, cl::NullRange/*local*/,NULL,&event);
         }else if(z<2 && y>1){
@@ -160,7 +167,11 @@ public:
         };
         if(err!=CL_SUCCESS)return -1;
         event.wait();
-        return queue->finish();
+        if(queue->finish()==CL_OUT_OF_HOST_MEMORY){
+            std::cerr<<"GPU out of memory goodbye:)\n";
+            exit(0);
+        }
+        return 1;
 
     }
     template<typename T>

GPU_version/vio/include/vio/nlls_solver.h

@@ -70,7 +70,7 @@ protected:
   /// Solve the linear system H*x = Jres. This function must set the update
   /// step in the member variable x_. Must return true if the system could be
   /// solved and false if it was singular.
-  virtual int
+  virtual bool
   solve                 () = 0;
 
   virtual void

GPU_version/vio/include/vio/nlls_solver_impl.hpp

@@ -33,13 +33,15 @@ void vk::NLLSSolver<D, T>::optimizeGaussNewton()
     rho_ = 0;
     n_meas_ = 0;
     double new_chi2 = computeResiduals(true, false);
+      std::cerr<<"36\t";
     // solve the linear system
     if(!solve())
     {
       // matrix was singular and could not be computed
-      if(verbose_)std::cout << "Matrix is close to singular! Stop Optimizing." << std::endl;
+      std::cout << "Matrix is close to singular! Stop Optimizing." << std::endl;
       stop_ = true;
     }
+    std::cerr<<"43\t";
     // check if error increased since last optimization
     if((iter_ > 0 && new_chi2 > chi2_) || stop_)
     {
@@ -52,8 +54,10 @@ void vk::NLLSSolver<D, T>::optimizeGaussNewton()
       }
       break;
     }
+      std::cerr<<"57\t";
     // update the model
     update();
+      std::cerr<<"60\n";
     chi2_ = new_chi2;
     if(verbose_)
     {

GPU_version/vio/include/vio/sparse_img_align.h

@@ -73,7 +73,7 @@ protected:
   void precomputeReferencePatches();
   virtual double computeResiduals(const SE2& model, bool linearize_system, bool compute_weight_scale = false);
   virtual double computeResiduals(bool linearize_system, bool compute_weight_scale = false){};
-  virtual int solve();
+  virtual bool solve();
   virtual void update (const ModelType& old_model, ModelType& new_model);
   virtual void update(){};
   virtual void startIteration();

GPU_version/vio/include/vio/sparse_img_align_gpu.h

@@ -73,7 +73,7 @@ protected:
         return 0.0;
     };
   virtual double computeResiduals(bool linearize_system, bool compute_weight_scale = false);
-  virtual int solve();
+  virtual bool solve();
   virtual void update();
   virtual void update(const ModelType& old_model, ModelType& new_model) { };
   virtual void startIteration();

GPU_version/vio/param/vo_fast.yaml

@@ -9,7 +9,7 @@ svo:
   structureoptim_max_pts: 50
   kfselect_mindist: 0.01
   poseoptim_thresh: 8.0
-  init_min_inliers: 30
+  init_min_inliers: 20
   ACC_noise: 0.5          #acc white noise in continuous in EKF
   GYO_noise: 0.5         #gyro white noise in continuous in EKF
   svo_ekf: 0.05              #Svo noise that will be devided by number of Reprojected points after opmization in EKF

GPU_version/vio/src/frame_handler_mono.cpp

@@ -162,8 +162,9 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
   new_frame_->setKeyframe();
   double depth_mean, depth_min;
   frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min);
-
   depth_filter_->addKeyframe(new_frame_, depth_mean, 0.5*depth_min);
+  frame_utils::getSceneDepth(*klt_homography_init_->frame_ref_, depth_mean, depth_min);
+  depth_filter_->addKeyframe(klt_homography_init_->frame_ref_, depth_mean, 0.5*depth_min);
   // add frame to map
   map_.addKeyframe(new_frame_);
   stage_ = STAGE_DEFAULT_FRAME;

GPU_version/vio/src/map.cpp

@@ -222,7 +222,7 @@ MapPointCandidates::~MapPointCandidates()
 void MapPointCandidates::newCandidatePoint(std::shared_ptr<Point> point, double depth_sigma2)
 {
   point->type_ = Point::TYPE_CANDIDATE;
-  boost::unique_lock<boost::mutex> lock(mut_);
+  //boost::unique_lock<boost::mutex> lock(mut_);
   candidates_.push_back(PointCandidate(point, point->obs_.front()));
 }
 

GPU_version/vio/src/sparse_img_align.cpp

@@ -235,7 +235,7 @@ double SparseImgAlign::computeResiduals(
   return chi2/n_meas_;
 }
 
-int SparseImgAlign::solve()
+bool SparseImgAlign::solve()
 {
   x_ = H_.ldlt().solve(Jres_);
   if((bool) std::isnan((double) x_[0]))

GPU_version/vio/src/sparse_img_align_gpu.cpp

@@ -106,40 +106,33 @@ double SparseImgAlignGpu::computeResiduals(
     return chi[0]/(feature_counter_*8);
 }
 
-int SparseImgAlignGpu::solve()
+bool SparseImgAlignGpu::solve()
 {
     cl_float H[300*9]={0.0};
     cl_float3 J[300]={0.0};
     residual_->read(1,8,feature_counter_*9,H);
     residual_->read(1,9,feature_counter_,J);
     for(int i = 1; i < feature_counter_; ++i){
-        H[0] += H[i*9];
+        H[0] += std::isnan(H[i*9]) ? 0.0 : H[i*9] ;
-        H[1] += H[i*9+1];
+        H[1] += std::isnan(H[i*9+1]) ? 0.0 : H[i*9+1];
-        H[2] += H[i*9+2];
+        H[2] += std::isnan(H[i*9+2]) ? 0.0 : H[i*9+2];
-        H[3] += H[i*9+3];
+        H[3] += std::isnan(H[i*9+3]) ? 0.0 : H[i*9+3];
-        H[4] += H[i*9+4];
+        H[4] += std::isnan(H[i*9+4]) ? 0.0 : H[i*9+4];
-        H[5] += H[i*9+5];
+        H[5] += std::isnan(H[i*9+5]) ? 0.0 : H[i*9+5];
-        H[6] += H[i*9+6];
+        H[6] += std::isnan(H[i*9+6]) ? 0.0 : H[i*9+6];
-        H[7] += H[i*9+7];
+        H[7] += std::isnan(H[i*9+7]) ? 0.0 : H[i*9+7];
-        H[8] += H[i*9+8];
+        H[8] += std::isnan(H[i*9+8]) ? 0.0 : H[i*9+8];
-        J[0].x -= J[i].x;
+        J[0].x -= std::isnan(J[i].x) ? 0.0 : J[i].x;
-        J[0].y -= J[i].y;
+        J[0].y -= std::isnan(J[i].y) ? 0.0 : J[i].y;
-        J[0].z -= J[i].z;
+        J[0].z -= std::isnan(J[i].z) ? 0.0 : J[i].z;
     }
     double Hd[9]={(double)H[0],(double)H[1],(double)H[2],
                   (double)H[3],(double)H[4],(double)H[5],
                   (double)H[6],(double)H[7],(double)H[8]};
-    double Jd[3]={(double)J[0].x,(double)J[0].y,(double)J[0].z};
+    x_ = Eigen::Matrix<double,3,3>(Hd).ldlt().solve(Eigen::Vector3d((double)J[0].x,(double)J[0].y,(double)J[0].z));
-    for(int i=0;i<9;++i){
-        if(i==0)std::cerr<<'\n'<<(double)J[0].x<<","<<(double)J[0].y<<","<<(double)J[0].z<<'\t';
-        std::cerr<<Hd[i]<<",";
-    }
-    x_ = Eigen::Matrix<double,3,3>(Hd).ldlt().solve(Eigen::Vector3d(Jd[0],Jd[1],Jd[2]));
-    if((bool) std::isnan((double) x_[0]) || (bool) std::isnan((double) x_[1]) ||(bool) std::isnan((double) x_[2]))
-        return 0;
     double norm=x_.norm();
     if(norm<=0 ||norm > 1.0)x_=Eigen::Vector3d(0.1,0.1,0.1);
-    return 1;
+    return true;
 }
 void SparseImgAlignGpu::update()
 {

GPU_version/vio/src/vo_node.cpp

@@ -41,9 +41,10 @@ public:
     bool start(vio::start::Request& req, vio::start::Response& res){
         if(req.on==1 && !start_){
             start_=true;
-            if(vo_->stage()!=vio::FrameHandlerBase::STAGE_DEFAULT_FRAME)
+            if(vo_->stage()!=vio::FrameHandlerBase::STAGE_DEFAULT_FRAME) {
+                vo_->depthFilter()->startThread();
                 vo_->start();
-            else{
+            }else{
                 vo_->depthFilter()->startThread();
                 vo_->reset();
             }