debug gpu version done

GPU_version/svo/src/bundle_adjustment.cpp

@@ -26,12 +26,12 @@
 #include <g2o/solvers/dense/linear_solver_dense.h>
 #include <g2o/types/sba/types_six_dof_expmap.h>
 #include <g2o/solvers/structure_only/structure_only_solver.h>
-#include <svo/bundle_adjustment.h>
-#include <svo/frame.h>
-#include <svo/feature.h>
-#include <svo/point.h>
-#include <svo/config.h>
-#include <svo/map.h>
+#include <gpu_svo/bundle_adjustment.h>
+#include <gpu_svo/frame.h>
+#include <gpu_svo/feature.h>
+#include <gpu_svo/point.h>
+#include <gpu_svo/config.h>
+#include <gpu_svo/map.h>
 
 #define SCHUR_TRICK 1
 

GPU_version/svo/src/cl_class.cpp

@@ -1,7 +1,7 @@
 //
 // Created by root on 4/27/21.
 //
-#include "svo/cl_class.h"
+#include "gpu_svo/cl_class.h"
 opencl::opencl() {
     std::vector<cl::Platform> all_platforms;
     cl::Platform::get(&all_platforms);
@@ -12,7 +12,7 @@ opencl::opencl() {
     //get default device of the default platform
     std::vector<cl::Device> all_devices;
     for(auto i:all_platforms){
-        std::cout << "Find platform: " << i.getInfo<CL_PLATFORM_NAME>() << "\n";
+        std::cout << "Find platform number:"<< i.getInfo<CL_PLATFORM_NAME>() << "\n";
         i.getDevices(CL_DEVICE_TYPE_GPU, &all_devices);
     }
 
@@ -30,7 +30,7 @@ opencl::opencl() {
 
     context=new cl::Context({ device });
     cl::Program::Sources sources;
-    read_cl fast("../kernel/fast-gray.cl");
+    read_cl fast(std::string(KERNEL_DIR)+"/fast-gray.cl");
     sources.push_back({ fast.src_str, fast.size });
     program=new cl::Program(*context, sources);
     if(program->build({ device },"-DFAST_THRESH=40") !=0)

GPU_version/svo/src/config.cpp

@@ -17,7 +17,7 @@
 #ifdef SVO_USE_ROS
 #include <vikit/params_helper.h>
 #endif
-#include <svo/config.h>
+#include <gpu_svo/config.h>
 
 namespace svo {
 

GPU_version/svo/src/depth_filter.cpp

@@ -20,14 +20,14 @@
 #include <vikit/vision.h>
 #include <boost/bind.hpp>
 #include <boost/math/distributions/normal.hpp>
-#include <svo/global.h>
-#include <svo/depth_filter.h>
-#include <svo/frame.h>
-#include <svo/point.h>
-#include <svo/feature.h>
-#include <svo/matcher.h>
-#include <svo/config.h>
-#include <svo/feature_detection.h>
+#include <gpu_svo/global.h>
+#include <gpu_svo/depth_filter.h>
+#include <gpu_svo/frame.h>
+#include <gpu_svo/point.h>
+#include <gpu_svo/feature.h>
+#include <gpu_svo/matcher.h>
+#include <gpu_svo/config.h>
+#include <gpu_svo/feature_detection.h>
 
 namespace svo {
 

GPU_version/svo/src/feature_alignment.cpp

@@ -20,7 +20,7 @@
 #ifdef __ARM_NEON__
 #include <arm_neon.h>
 #endif
-#include <svo/feature_alignment.h>
+#include <gpu_svo/feature_alignment.h>
 
 namespace svo {
 namespace feature_alignment {

GPU_version/svo/src/feature_detection.cpp

@@ -14,8 +14,8 @@
 // You should have received a copy of the GNU General Public License
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
-#include <svo/feature_detection.h>
-#include <svo/feature.h>
+#include <gpu_svo/feature_detection.h>
+#include <gpu_svo/feature.h>
 #include <vikit/vision.h>
 
 namespace svo {
@@ -32,14 +32,6 @@ AbstractDetector::AbstractDetector(
         grid_n_rows_(ceil(static_cast<double>(img_height)/cell_size_)),
         grid_occupancy_(grid_n_cols_*grid_n_rows_, false)
 {
-    gpu_fast_= new opencl();
-    cv::Mat init=cv::Mat::zeros(cv::Size(img_width, img_height),CV_8UC1);
-    gpu_fast_->write_buf(0,0,init);
-    cl_int2 corners[img_width*img_height];
-    gpu_fast_->write_buf(0,1,img_width*img_height,corners);
-    int icorner[1]={0};
-    gpu_fast_->write_buf(0,2,1,icorner);
-    gpu_fast_->write_buf(0,3,(1 << 18));
 }
 
 void AbstractDetector::resetGrid()
@@ -67,9 +59,11 @@ FastDetector::FastDetector(
     const int img_width,
     const int img_height,
     const int cell_size,
+    opencl* gpu_fast_,
     const int n_pyr_levels) :
-        AbstractDetector(img_width, img_height, cell_size, n_pyr_levels)
-{}
+        AbstractDetector(img_width, img_height, cell_size, n_pyr_levels),gpu_fast_(gpu_fast_)
+{
+}
 
 void FastDetector::detect(
     Frame* frame,
@@ -81,26 +75,28 @@ void FastDetector::detect(
   for(int L=0; L<n_pyr_levels_; ++L)
   {
     const int scale = (1<<L);
-    gpu_fast_->reload_buf(0,0,img_pyr[L].data);
+    cv::Mat img=img_pyr.at(L);
+    gpu_fast_->reload_buf(0,0,img);
     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;
+    cl_int2 fast_corners[1000000];
     gpu_fast_->read(0,1,fast_corners);
     int count[1]={0};
     gpu_fast_->read(0,2,count);
-    //vector<int> scores, nm_corners;
-    //fast::fast_corner_score_10((fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols, fast_corners, 20, scores);
-    //fast::fast_nonmax_3x3(fast_corners, scores, nm_corners);
-
     for(uint i=0;i<count[0];++i)
     {
-      if(fast_corners[i].x<0 || fast_corners[i].x>img_pyr[L].cols || fast_corners[i].y>img_pyr[L].rows || fast_corners[i].y<0)continue;
+      if(fast_corners[i].x<0 || fast_corners[i].x>img_pyr[L].cols || fast_corners[i].y>img_pyr[L].rows || fast_corners[i].y<0)
+          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])
         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*scale, fast_corners[i].y*scale, score, L, 0.0f);
     }

GPU_version/svo/src/frame.cpp

@@ -15,10 +15,10 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
 #include <stdexcept>
-#include <svo/frame.h>
-#include <svo/feature.h>
-#include <svo/point.h>
-#include <svo/config.h>
+#include <gpu_svo/frame.h>
+#include <gpu_svo/feature.h>
+#include <gpu_svo/point.h>
+#include <gpu_svo/config.h>
 #include <vikit/math_utils.h>
 #include <vikit/vision.h>
 #include <vikit/performance_monitor.h>

GPU_version/svo/src/frame_handler_base.cpp

@@ -19,12 +19,12 @@
 #include <Eigen/StdVector>
 #include <boost/bind.hpp>
 #include <fstream>
-#include <svo/frame_handler_base.h>
-#include <svo/config.h>
-#include <svo/feature.h>
-#include <svo/matcher.h>
-#include <svo/map.h>
-#include <svo/point.h>
+#include <gpu_svo/frame_handler_base.h>
+#include <gpu_svo/config.h>
+#include <gpu_svo/feature.h>
+#include <gpu_svo/matcher.h>
+#include <gpu_svo/map.h>
+#include <gpu_svo/point.h>
 
 namespace svo
 {

GPU_version/svo/src/frame_handler_mono.cpp

@@ -14,18 +14,18 @@
 // You should have received a copy of the GNU General Public License
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
-#include <svo/config.h>
-#include <svo/frame_handler_mono.h>
-#include <svo/map.h>
-#include <svo/frame.h>
-#include <svo/feature.h>
-#include <svo/point.h>
-#include <svo/pose_optimizer.h>
-#include <svo/sparse_img_align.h>
+#include <gpu_svo/config.h>
+#include <gpu_svo/frame_handler_mono.h>
+#include <gpu_svo/map.h>
+#include <gpu_svo/frame.h>
+#include <gpu_svo/feature.h>
+#include <gpu_svo/point.h>
+#include <gpu_svo/pose_optimizer.h>
+#include <gpu_svo/sparse_img_align.h>
 #include <vikit/performance_monitor.h>
-#include <svo/depth_filter.h>
+#include <gpu_svo/depth_filter.h>
 #ifdef USE_BUNDLE_ADJUSTMENT
-#include <svo/bundle_adjustment.h>
+#include <gpu_svo/bundle_adjustment.h>
 #endif
 
 namespace svo {
@@ -36,6 +36,16 @@ FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Sophus::SE3& SE_init)
   reprojector_(cam_, map_),
   depth_filter_(NULL)
 {
+    gpu_fast_= new opencl();
+    gpu_fast_->make_kernel("fast_gray");
+    cv::Mat img=cv::Mat(cv::Size(1000, 1000),CV_8UC1);
+    gpu_fast_->write_buf(0,0,img);
+    cl_int2 corners_[1000000];
+    gpu_fast_->write_buf(0,1,1000000,corners_);
+    int icorner[1]={0};
+    gpu_fast_->write_buf(0,2,1,icorner);
+    gpu_fast_->write_buf(0,3,(1 << 18));
+    klt_homography_init_=new initialization::KltHomographyInit(gpu_fast_);
   initialize();
   imu_integPtr_=std::make_unique<Imu_Integration>(SE_init);
 }
@@ -44,7 +54,7 @@ void FrameHandlerMono::initialize()
 {
   feature_detection::DetectorPtr feature_detector(
       new feature_detection::FastDetector(
-          cam_->width(), cam_->height(), Config::gridSize(), Config::nPyrLevels()));
+          cam_->width(), cam_->height(), Config::gridSize(),gpu_fast_, Config::nPyrLevels()));
   DepthFilter::callback_t depth_filter_cb = boost::bind(
       &MapPointCandidates::newCandidatePoint, &map_.point_candidates_, _1, _2);
   depth_filter_ = new DepthFilter(feature_detector, depth_filter_cb);
@@ -90,7 +100,7 @@ void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp)
 FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame()
 {
   new_frame_->T_f_w_ = SE3(Matrix3d::Identity(), Vector3d::Zero());
-  if(klt_homography_init_.addFirstFrame(new_frame_) == initialization::FAILURE)
+  if(klt_homography_init_->addFirstFrame(new_frame_) == initialization::FAILURE)
     return RESULT_NO_KEYFRAME;
   new_frame_->setKeyframe();
   map_.addKeyframe(new_frame_);
@@ -101,7 +111,7 @@ FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame()
 
 FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
 {
-  initialization::InitResult res = klt_homography_init_.addSecondFrame(new_frame_);
+  initialization::InitResult res = klt_homography_init_->addSecondFrame(new_frame_);
   if(res == initialization::FAILURE)
     return RESULT_FAILURE;
   else if(res == initialization::NO_KEYFRAME)
@@ -120,7 +130,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
   // add frame to map
   map_.addKeyframe(new_frame_);
   stage_ = STAGE_DEFAULT_FRAME;
-  klt_homography_init_.reset();
+  klt_homography_init_->reset();
   SVO_INFO_STREAM("Init: Selected second frame, triangulated initial map.");
   return RESULT_IS_KEYFRAME;
 }

GPU_version/svo/src/imu_integration.cpp

@@ -2,16 +2,16 @@
 // Created by gfuse on 4/9/21.
 //
 //#define NDEBUG
-#include "svo/imu_integration.h"
+#include "gpu_svo/imu_integration.h"
 #include <memory>
 #include <utility>
-#include "svo/feature.h"
-#include "svo/point.h"
-#include "svo/svoVisionFactor.h"
+#include "gpu_svo/feature.h"
+#include "gpu_svo/point.h"
+#include "gpu_svo/svoVisionFactor.h"
 #include <gtsam/nonlinear/Marginals.h>
 #include <gtsam/geometry/Pose3.h>
 #include <vikit/params_helper.h>
-#include <svo/config.h>
+#include <gpu_svo/config.h>
 
 Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){
     graphPtr=std::make_shared<gtsam::NonlinearFactorGraph>();

GPU_version/svo/src/initialization.cpp

@@ -14,12 +14,12 @@
 // You should have received a copy of the GNU General Public License
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
-#include <svo/config.h>
-#include <svo/frame.h>
-#include <svo/point.h>
-#include <svo/feature.h>
-#include <svo/initialization.h>
-#include <svo/feature_detection.h>
+#include <gpu_svo/config.h>
+#include <gpu_svo/frame.h>
+#include <gpu_svo/point.h>
+#include <gpu_svo/feature.h>
+#include <gpu_svo/initialization.h>
+#include <gpu_svo/feature_detection.h>
 #include <vikit/math_utils.h>
 #include <vikit/homography.h>
 
@@ -29,7 +29,7 @@ namespace initialization {
 InitResult KltHomographyInit::addFirstFrame(FramePtr frame_ref)
 {
   reset();
-  detectFeatures(frame_ref, px_ref_, f_ref_);
+  detectFeatures(frame_ref, px_ref_, 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.");
@@ -107,11 +107,12 @@ void KltHomographyInit::reset()
 void detectFeatures(
     FramePtr frame,
     vector<cv::Point2f>& px_vec,
-    vector<Vector3d>& f_vec)
+    vector<Vector3d>& f_vec,
+    opencl* gpu_fast)
 {
   Features new_features;
   feature_detection::FastDetector detector(
-      frame->img().cols, frame->img().rows, Config::gridSize(), Config::nPyrLevels());
+      frame->img().cols, frame->img().rows, Config::gridSize(), gpu_fast,Config::nPyrLevels());
   detector.detect(frame.get(), frame->img_pyr_, Config::triangMinCornerScore(), new_features);
 
   // now for all maximum corners, initialize a new seed

GPU_version/svo/src/map.cpp

@@ -15,10 +15,10 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
 #include <set>
-#include <svo/map.h>
-#include <svo/point.h>
-#include <svo/frame.h>
-#include <svo/feature.h>
+#include <gpu_svo/map.h>
+#include <gpu_svo/point.h>
+#include <gpu_svo/frame.h>
+#include <gpu_svo/feature.h>
 #include <boost/bind.hpp>
 
 namespace svo {

GPU_version/svo/src/matcher.cpp

@@ -19,12 +19,12 @@
 #include <vikit/vision.h>
 #include <vikit/math_utils.h>
 #include <vikit/patch_score.h>
-#include <svo/matcher.h>
-#include <svo/frame.h>
-#include <svo/feature.h>
-#include <svo/point.h>
-#include <svo/config.h>
-#include <svo/feature_alignment.h>
+#include <gpu_svo/matcher.h>
+#include <gpu_svo/frame.h>
+#include <gpu_svo/feature.h>
+#include <gpu_svo/point.h>
+#include <gpu_svo/config.h>
+#include <gpu_svo/feature_alignment.h>
 
 namespace svo {
 

GPU_version/svo/src/point.cpp

@@ -16,9 +16,9 @@
 
 #include <stdexcept>
 #include <vikit/math_utils.h>
-#include <svo/point.h>
-#include <svo/frame.h>
-#include <svo/feature.h>
+#include <gpu_svo/point.h>
+#include <gpu_svo/frame.h>
+#include <gpu_svo/feature.h>
  
 namespace svo {
 

GPU_version/svo/src/pose_optimizer.cpp

@@ -15,10 +15,10 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
 #include <stdexcept>
-#include <svo/pose_optimizer.h>
-#include <svo/frame.h>
-#include <svo/feature.h>
-#include <svo/point.h>
+#include <gpu_svo/pose_optimizer.h>
+#include <gpu_svo/frame.h>
+#include <gpu_svo/feature.h>
+#include <gpu_svo/point.h>
 #include <vikit/robust_cost.h>
 #include <vikit/math_utils.h>
 

GPU_version/svo/src/reprojector.cpp

@@ -16,12 +16,12 @@
 
 #include <algorithm>
 #include <stdexcept>
-#include <svo/reprojector.h>
-#include <svo/frame.h>
-#include <svo/point.h>
-#include <svo/feature.h>
-#include <svo/map.h>
-#include <svo/config.h>
+#include <gpu_svo/reprojector.h>
+#include <gpu_svo/frame.h>
+#include <gpu_svo/point.h>
+#include <gpu_svo/feature.h>
+#include <gpu_svo/map.h>
+#include <gpu_svo/config.h>
 #include <boost/bind.hpp>
 #include <boost/thread.hpp>
 #include <vikit/abstract_camera.h>

GPU_version/svo/src/sparse_img_align.cpp

@@ -15,11 +15,11 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
 #include <algorithm>
-#include <svo/sparse_img_align.h>
-#include <svo/frame.h>
-#include <svo/feature.h>
-#include <svo/config.h>
-#include <svo/point.h>
+#include <gpu_svo/sparse_img_align.h>
+#include <gpu_svo/frame.h>
+#include <gpu_svo/feature.h>
+#include <gpu_svo/config.h>
+#include <gpu_svo/point.h>
 #include <vikit/abstract_camera.h>
 #include <vikit/vision.h>
 #include <vikit/math_utils.h>