optimized gpu

GPU_version/svo/src/feature_detection.cpp

@@ -80,7 +80,7 @@ void FastDetector::detect(
     int icorner[1]={0};
     gpu_fast_->reload_buf(0,2,icorner);
     gpu_fast_->run(0,img_pyr[L].cols,img_pyr[L].rows);
-    cl_int2 fast_corners[1000000];
+    cl_int2 fast_corners[350000];
     gpu_fast_->read(0,1,fast_corners);
     int count[1]={0};
     gpu_fast_->read(0,2,count);
@@ -97,18 +97,15 @@ void FastDetector::detect(
 
       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*scale, fast_corners[i].y*scale, score, L, 0.0f);
+      if(score > corners.at(k).score){
+          corners.at(k) = Corner(fast_corners[i].x*scale, fast_corners[i].y*scale, score, L, 0.0f);
+          // Create feature for every corner that has high enough corner score
+          if(corners.at(k).score > detection_threshold)
+              fts.push_back(new Feature(frame, Vector2d(corners.at(k).x, corners.at(k).y), corners.at(k).level));
+      }
     }
 
   }
-
-  // Create feature for every corner that has high enough corner score
-  std::for_each(corners.begin(), corners.end(), [&](Corner& c) {
-    if(c.score > detection_threshold)
-      fts.push_back(new Feature(frame, Vector2d(c.x, c.y), c.level));
-  });
-
   resetGrid();
 }
 

GPU_version/svo/src/frame_handler_mono.cpp

@@ -38,10 +38,10 @@ FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Sophus::SE3& SE_init)
 {
     gpu_fast_= new opencl();
     gpu_fast_->make_kernel("fast_gray");
-    cv::Mat img=cv::Mat(cv::Size(1000, 1000),CV_8UC1);
+    cv::Mat img=cv::Mat(cv::Size(700, 500),CV_8UC1);
     gpu_fast_->write_buf(0,0,img);
-    cl_int2 corners_[1000000];
-    gpu_fast_->write_buf(0,1,1000000,corners_);
+    cl_int2 corners_[350000];
+    gpu_fast_->write_buf(0,1,350000,corners_);
     int icorner[1]={0};
     gpu_fast_->write_buf(0,2,1,icorner);
     gpu_fast_->write_buf(0,3,(1 << 18));

GPU_version/svo/src/imu_integration.cpp

@@ -111,6 +111,8 @@ bool Imu_Integration::predict(boost::shared_ptr<svo::Frame>& new_frame,std::size
     }else{
         if(imu_n<1)return false;
         auto preint_imu_combined =dynamic_cast<const gtsam::PreintegratedCombinedMeasurements&>(*preintegratedPtr);
+        //preint_imu_combined.predict(*statePtr,*imu_biasPtr).print();
+        //std::cerr<<new_frame->T_f_w_.matrix();
         valuesPtr->insert(P(imu_factor_id), gtsam::Pose3(new_frame->T_f_w_.matrix()));
         valuesPtr->insert(V(imu_factor_id), statePtr->v());
         valuesPtr->insert(B(imu_factor_id), *imu_biasPtr);