solve segmentation falt error in bandel adjustment

GPU_version/vio/depth_filter_log.txt

@@ -1,18 +0,0 @@
-[19:47:24.922] New key frame 
-[19:47:24.922] BA loop 
-[19:47:25.028] New key frame 
-[19:47:25.028] BA loop 
-[19:47:25.138] New key frame 
-[19:47:25.138] BA loop 
-[19:47:25.234] New key frame 
-[19:47:25.234] BA loop 
-[19:47:30.451] New key frame 
-[19:47:30.451] BA loop 
-[19:47:32.031] New key frame 
-[19:47:32.031] BA loop 
-[19:47:44.267] New key frame 
-[19:47:44.268] BA loop 
-[19:48:08.881] New key frame 
-[19:48:08.881] BA loop 
-[19:49:01.049] New key frame 
-[19:49:01.049] BA loop 

GPU_version/vio/include/vio/feature_detection.h

@@ -72,10 +72,10 @@ public:
 protected:
 
   static const int border_ = 8; //!< no feature should be within 8px of border.
-  const int cell_size_;
-  const int n_pyr_levels_;
-  const int grid_n_cols_;
-  const int grid_n_rows_;
+  int cell_size_;
+  int n_pyr_levels_;
+  int grid_n_cols_;
+  int grid_n_rows_;
   vector<bool> grid_occupancy_;
 
   inline int getCellIndex(int x, int y, int level)

GPU_version/vio/launch/Px30.launch

@@ -1,6 +1,6 @@
 <launch>
 
-  <node pkg="vio" type="vio" name="vio" clear_params="true" output="screen" />
+  <!-- node pkg="vio" type="vio" name="vio" clear_params="true" output="screen" / -->
 
     <!-- Camera topic to subscribe to -->
     <param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" />

GPU_version/vio/src/feature_detection.cpp

@@ -77,7 +77,7 @@ void FastDetector::detect(
   for(int L=0; L<n_pyr_levels_; ++L)
   {
     if(L>img_pyr.size())return;
-    const int scale = (1<<L);
+    int scale = (1<<L);
     cv::Mat img=img_pyr.at(L);
     if(!gpu_fast_->reload_buf(0,0,img)){
         ROS_ERROR("Failed to write into GPU goodbye :)");
@@ -97,35 +97,26 @@ void FastDetector::detect(
           ROS_ERROR("Can not communicate with GPU");
           exit(0);
     }
-    for(uint i=0;i<count[0];++i)
+    int size=count[0];
+    for(uint i=0;i<size;++i)
     {
-      if(fast_corners==NULL)return;
       if(fast_corners[i].x<1 || fast_corners[i].x>img_pyr[L].cols || fast_corners[i].y>img_pyr[L].rows || fast_corners[i].y<1){
           continue;
       }
-      const int k = static_cast<int>((fast_corners[i].y*scale)/cell_size_)*grid_n_cols_
-                  + static_cast<int>((fast_corners[i].x*scale)/cell_size_);
-
-      if(grid_occupancy_[k] || k > corners.size()){
-          continue;
-      }
-      const float score = vk::shiTomasiScore(img_pyr[L], fast_corners[i].x, fast_corners[i].y);
-      if(score > corners.at(k).score){
-          corners.at(k) = Corner(fast_corners[i].x, fast_corners[i].y, score, L, 0.0f);
-          keypoints.push_back(cv::KeyPoint(fast_corners[i].x*scale, fast_corners[i].y*scale, 7.f,-1,score));
-      }
+      float score = vk::shiTomasiScore(img_pyr[L], fast_corners[i].x, fast_corners[i].y);
+      keypoints.push_back(cv::KeyPoint(fast_corners[i].x*scale, fast_corners[i].y*scale, 7.f,-1,score));
     }
   }
   if(keypoints.size()<1){
       ROS_ERROR("GPU Driver crash try again!");
-      assert(false);
+      assert(0);
   }
   cv::Ptr<cv::xfeatures2d::FREAK> extractor = cv::xfeatures2d::FREAK::create(true, true, 50.0f, 4);
   cv::Mat descriptor;
   extractor->compute(frame->img(), keypoints, descriptor);
   for(auto&& p:_for(keypoints))fts.push_back(make_shared<Feature>(frame, Vector2d(p.item.pt.x, p.item.pt.y), p.item.response ,0,descriptor.data+(p.index*64)));
   resetGrid();
-};
+}
 
 } // namespace feature_detection
 } // namespace vio

GPU_version/vio/src/global_optimizer.cpp

@@ -98,7 +98,6 @@ namespace vio {
             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)
             {
@@ -177,7 +176,6 @@ namespace vio {
                     map_.removePtFrameRef(it->frame, it->feature);
                 }
             }
-            point_mut_.unlock();
         }
     }
 

GPU_version/vio/src/initialization.cpp

@@ -54,6 +54,12 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
       ukf_->UpdateSvo(0.0,0.0,0.0);
       return NO_KEYFRAME;
   }
+  if(disparities_.size() < 20){
+#if VIO_DEBUG
+      fprintf(log_,"[%s] VIO can not be initialized goodbye :)\n",vio::time_in_HH_MM_SS_MMM().c_str());
+#endif
+      assert(0);
+  }
   double min,max;
   double disparity = vk::getMean(disparities_,min,max);
     if(disparity < Config::initMinDisparity()){
@@ -130,9 +136,9 @@ void detectFeatures(
     opencl* gpu_fast)
 {
 
-  feature_detection::FastDetector detector(
+  std::unique_ptr<feature_detection::FastDetector> detector=std::make_unique<feature_detection::FastDetector>(
       frame->img().cols, frame->img().rows, Config::gridSize(), gpu_fast,Config::nPyrLevels());
-  detector.detect(frame, frame->img_pyr_, Config::triangMinCornerScore(), new_features);
+  detector->detect(frame, frame->img_pyr_, Config::triangMinCornerScore(), new_features);
 
   // now for all maximum corners, initialize a new seed
   px_vec.clear();

GPU_version/vio/src/map.cpp

@@ -62,6 +62,7 @@ bool Map::safeDeleteFrame(FramePtr frame)
 
 void Map::removePtFrameRef(FramePtr frame, std::shared_ptr<Feature> ftr)
 {
+  boost::unique_lock<boost::mutex> lock(point_mut_);
   if(ftr->point == NULL)
     return; // mappoint may have been deleted in a previous ref. removal
   if(ftr->point->obs_.size() <= 2)
@@ -80,10 +81,10 @@ void Map::safeDeletePoint(std::shared_ptr<Point> pt)
   if(pt->obs_.size()>1){
       for(auto&& ftr:pt->obs_){
           if(ftr->frame)ftr->frame->removeKeyPoint(ftr);
-          ftr->point.reset();
+          if(ftr->point!=nullptr)ftr->point.reset();
       }
   }else{
-      pt->obs_.back()->point.reset();
+      if(!pt->obs_.empty())if(pt->obs_.back()->point!=nullptr)pt->obs_.back()->point.reset();
   }
   pt->obs_.clear();
   // Delete mappoint

GPU_version/vio/src/point.cpp

@@ -143,7 +143,6 @@ void Point::optimize(const size_t n_iter)
   double chi2 = 0.0;
   Matrix3d A;
   Vector3d b;
-
   for(size_t i=0; i<n_iter; i++)
   {
     A.setZero();
@@ -183,12 +182,12 @@ void Point::optimize(const size_t n_iter)
     if(vk::norm_max(dp) <= EPS)
       break;
   }
+  for(auto it=obs_.begin(); it!=obs_.end(); ++it) {
+            Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d((*it)->frame->se3() * pos_));
+            if (e.norm() > 0.3)
+                n_failed_reproj_++;
+  }
 
-    for(auto it=obs_.begin(); it!=obs_.end(); ++it) {
-            Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d((*it)->frame->se3() * (*it)->point->pos_));
-            if (e.norm() > 1.0)
-                (*it)->point->n_failed_reproj_++;
-    }
 
 }