solve segemtation crash

GPU_version/vio/include/vio/cl_class.h

@@ -25,6 +25,7 @@ public:
     };
     template<typename T>
     int32_t write(size_t id/*buffer ID*/,T* buf,cl::CommandQueue* queue,cl::Context* context,size_t buf_size){
+        assert(buf);
         cl_int error;
         _buffers.push_back(std::pair<std::pair<cl::Buffer,size_t>,size_t>(std::pair<cl::Buffer,size_t>(cl::Buffer(*context, CL_MEM_READ_WRITE | CL_MEM_USE_HOST_PTR
                 , sizeof(T) * buf_size,buf,&error),
@@ -36,6 +37,7 @@ public:
 
     };
     int32_t write(size_t id/*buffer ID*/,cv::Mat& buf,cl::Context* context){
+        assert(!buf.empty());
         cl_int error;
         _images.push_back(std::pair<cl::Image2D,size_t>(cl::Image2D(*context, CL_MEM_READ_ONLY | CL_MEM_USE_HOST_PTR,
                                                                     cl::ImageFormat(CL_R, CL_UNSIGNED_INT8),
@@ -51,6 +53,7 @@ public:
 
     template<typename T>
     int32_t reload(size_t id,T* buf,cl::CommandQueue* queue){
+        assert(buf);
         cl_int error;
         for(auto&& i:_buffers)if(i.second==id){
                 cl::Event event;
@@ -65,6 +68,7 @@ public:
         return error;
     };
     int32_t reload(size_t id,cv::Mat& buf,cl::Context* context){
+        assert(!buf.empty());
         cl_int error;
         try{
             for(auto&& i:_images)if(i.second==id){
@@ -161,6 +165,7 @@ public:
     }
     template<typename T>
     void read(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,size_t size/*size*/, T* out){
+        assert(out);
         cl_int error;
         cl::Event event;
         T* Map_buf=(T*)queue->enqueueMapBuffer(_kernels.at(id1).read(id2).first,CL_NON_BLOCKING,CL_MAP_READ,0,sizeof(T) * size,NULL,&event,&error);

GPU_version/vio/include/vio/global.h

@@ -36,11 +36,8 @@ EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector3d)
 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector2d)
 #endif
 #include <ros/console.h>
-#define SVO_DEBUG_STREAM(x) ROS_DEBUG_STREAM(x)
+#include <boost/thread.hpp>
-#define SVO_INFO_STREAM(x) ROS_INFO_STREAM(x)
+#include <boost/function.hpp>
-#define SVO_WARN_STREAM(x) ROS_WARN_STREAM(x)
-#define SVO_WARN_STREAM_THROTTLE(rate, x) ROS_WARN_STREAM_THROTTLE(rate, x)
-#define SVO_ERROR_STREAM(x) ROS_ERROR_STREAM(x)
 
 
 namespace vio
@@ -51,6 +48,8 @@ namespace vio
 
     const double EPS = 0.0000000001;
     const double PI = 3.14159265;
+
+    static boost::mutex feature_detection_mut_;
     static std::string time_in_HH_MM_SS_MMM()
     {
         using namespace std::chrono;

GPU_version/vio/src/depth_filter.cpp

@@ -148,9 +148,11 @@ namespace vio {
         fprintf(log_,"[%s] init seeds\n",vio::time_in_HH_MM_SS_MMM().c_str());
 #endif
         feature_detector_->setExistingFeatures(frame->fts_);
+        feature_detection_mut_.lock();
         feature_detector_->detect(frame, frame->img_pyr_,
                                   Config::triangMinCornerScore(), new_features);
 
+        feature_detection_mut_.unlock();
         // initialize a seed for every new feature
         seeds_updating_halt_ = true;
         seeds_mut_.lock();

GPU_version/vio/src/feature_detection.cpp

@@ -89,7 +89,7 @@ void FastDetector::detect(
     gpu_fast_->run(0,img.cols,img.rows);
     int count[1]={0};
     gpu_fast_->read(0,2,1,count);
-    cl_int2 fast_corners[350000];
+    cl_int2 fast_corners[350000]={0};
     gpu_fast_->read(0,1,count[0],fast_corners);
     for(uint i=0;i<count[0];++i)
     {

GPU_version/vio/src/frame.cpp

@@ -187,7 +187,7 @@ bool getSceneDepth(const Frame& frame, double& depth_mean, double& depth_min)
   }
   if(depth_vec.empty())
   {
-    SVO_WARN_STREAM("Cannot set scene depth. Frame has no point-observations!");
+    //SVO_WARN_STREAM("Cannot set scene depth. Frame has no point-observations!");
     return false;
   }
   depth_mean = vk::getMedian(depth_vec);

GPU_version/vio/src/frame_handler_base.cpp

@@ -38,13 +38,10 @@ FrameHandlerBase::FrameHandlerBase() :
   num_obs_last_(0),
   tracking_quality_(TRACKING_INSUFFICIENT)
 {
-
-  SVO_INFO_STREAM("SVO initialized");
 }
 
 FrameHandlerBase::~FrameHandlerBase()
 {
-  SVO_INFO_STREAM("SVO destructor invoked");
 }
 
 bool FrameHandlerBase::startFrameProcessingCommon(const double timestamp)
@@ -58,7 +55,6 @@ bool FrameHandlerBase::startFrameProcessingCommon(const double timestamp)
   if(stage_ == STAGE_PAUSED)
     return false;
 
-  SVO_DEBUG_STREAM("New Frame");
 
   // some cleanup from last iteration, can't do before because of visualization
   map_.emptyTrash();
@@ -94,7 +90,6 @@ void FrameHandlerBase::resetCommon()
   set_start_ = false;
   tracking_quality_ = TRACKING_INSUFFICIENT;
   num_obs_last_ = 0;
-  SVO_INFO_STREAM("RESET");
 }
 
 void FrameHandlerBase::setTrackingQuality(const size_t num_observations)
@@ -102,13 +97,13 @@ void FrameHandlerBase::setTrackingQuality(const size_t num_observations)
   tracking_quality_ = TRACKING_GOOD;
   if(num_observations < Config::qualityMinFts())
   {
-    SVO_WARN_STREAM_THROTTLE(0.5, "Tracking less than "<< Config::qualityMinFts() <<" features!");
+    //SVO_WARN_STREAM_THROTTLE(0.5, "Tracking less than "<< Config::qualityMinFts() <<" features!");
     tracking_quality_ = TRACKING_INSUFFICIENT;
   }
   const int feature_drop = static_cast<int>(std::min(num_obs_last_, Config::maxFts())) - num_observations;
   if(feature_drop > Config::qualityMaxFtsDrop())
   {
-    SVO_WARN_STREAM("Lost "<< feature_drop <<" features!");
+    //SVO_WARN_STREAM("Lost "<< feature_drop <<" features!");
     tracking_quality_ = TRACKING_INSUFFICIENT;
   }
 }

GPU_version/vio/src/frame_handler_mono.cpp

@@ -181,7 +181,9 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   new_frame_->Cov_ = init_f.first;
   // sparse image align
   SparseImgAlignGpu img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlignGpu::GaussNewton, false,gpu_fast_);
+  feature_detection_mut_.lock();
   size_t err=img_align.run(last_frame_, new_frame_, log_);
+  feature_detection_mut_.unlock();
   if(err==-1){
 #if VIO_DEBUG
       fprintf(log_,"[%s] Image Alignment failed -1\n",vio::time_in_HH_MM_SS_MMM().c_str());

GPU_version/vio/src/initialization.cpp

@@ -31,7 +31,7 @@ InitResult KltHomographyInit::addFirstFrame(FramePtr frame_ref)
   detectFeatures(frame_ref, px_ref_, px_level, f_ref_, gpu_fast_);
   if(px_ref_.size() < 100)
   {
-    SVO_WARN_STREAM_THROTTLE(2.0, "First image has less than 100 features. Retry in more textured environment.");
+//    SVO_WARN_STREAM_THROTTLE(2.0, "First image has less than 100 features. Retry in more textured environment.");
     return FAILURE;
   }
 #if VIO_DEBUG
@@ -126,9 +126,11 @@ void detectFeatures(
     opencl* gpu_fast)
 {
   Features new_features;
+  feature_detection_mut_.lock();
   feature_detection::FastDetector detector(
       frame->img().cols, frame->img().rows, Config::gridSize(), gpu_fast,Config::nPyrLevels());
   detector.detect(frame, frame->img_pyr_, Config::triangMinCornerScore(), new_features);
+  feature_detection_mut_.unlock();
   // now for all maximum corners, initialize a new seed
   px_vec.clear();
   f_vec.clear();

GPU_version/vio/src/reprojector.cpp

@@ -76,9 +76,11 @@ namespace vio {
         Features keypoints_cur;
         cv::Ptr<cv::xfeatures2d::FREAK> extractor = cv::xfeatures2d::FREAK::create(true, true, 22.0f, 4);
         cv::Ptr<cv::BFMatcher> matcher = cv::BFMatcher::create(cv::NORM_HAMMING, true);
+        feature_detection_mut_.lock();
         feature_detection::FastDetector detector(
                 frame->img().cols, frame->img().rows, Config::gridSize(), gpu_fast_,Config::nPyrLevels());
         detector.detect(frame, frame->img_pyr_, Config::triangMinCornerScore(), keypoints_cur,&descriptors_cur);
+        feature_detection_mut_.unlock();
         list<pair<FramePtr, double> > close_kfs;
         map_.getCloseKeyframes(frame, close_kfs);
         if (!last_frame->fts_.empty())