debug gpu version done

GPU_version/svo/include/gpu_svo/bundle_adjustment.h

@@ -17,7 +17,7 @@
 #ifndef SVO_BUNDLE_ADJUSTMENT_H_
 #define SVO_BUNDLE_ADJUSTMENT_H_
 
-#include <svo/global.h>
+#include <gpu_svo/global.h>
 
 namespace g2o {
 class EdgeProjectXYZ2UV;

GPU_version/svo/include/gpu_svo/cl_class.h

@@ -67,13 +67,15 @@ public:
     };
     int32_t reload(size_t id,cv::Mat& buf,cl::Context* context){
         try{
-            for(auto i:_images)if(i.second==id)
+            for(auto i:_images)if(i.second==id){
                     i.first=cl::Image2D(*context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR,
                                         cl::ImageFormat(CL_R, CL_UNSIGNED_INT8),
                                         buf.size().width,
                                         buf.size().height,
                                         0,
                                         reinterpret_cast<uchar*>(buf.data));
+            }
+
             return CL_SUCCESS;
         }catch (std::exception& err){
             std::cerr<<"Reload buffer on GPU failed\n"<<err.what()<<'\n';
@@ -127,7 +129,7 @@ public:
             _kernels.at(id1).write(id2,buf,context);
             return CL_SUCCESS;
         }catch (std::exception& err){
-            std::cerr<<"Write buffer on GPU failed\n"<<err.what()<<'\n';
+            std::cerr<<"Write image on GPU failed\n"<<err.what()<<'\n';
             return CL_MAP_FAILURE;
         }
     }

GPU_version/svo/include/gpu_svo/depth_filter.h

@@ -21,9 +21,9 @@
 #include <boost/thread.hpp>
 #include <boost/function.hpp>
 #include <vikit/performance_monitor.h>
-#include <svo/global.h>
-#include <svo/feature_detection.h>
-#include <svo/matcher.h>
+#include <gpu_svo/global.h>
+#include <gpu_svo/feature_detection.h>
+#include <gpu_svo/matcher.h>
 
 namespace svo {
 

GPU_version/svo/include/gpu_svo/feature.h

@@ -17,7 +17,7 @@
 #ifndef SVO_FEATURE_H_
 #define SVO_FEATURE_H_
 
-#include <svo/frame.h>
+#include <gpu_svo/frame.h>
 
 namespace svo {
 

GPU_version/svo/include/gpu_svo/feature_alignment.h

@@ -17,7 +17,7 @@
 #ifndef SVO_FEATURE_ALIGNMENT_H_
 #define SVO_FEATURE_ALIGNMENT_H_
 
-#include <svo/global.h>
+#include <gpu_svo/global.h>
 
 namespace svo {
 

GPU_version/svo/include/gpu_svo/feature_detection.h

@@ -17,9 +17,9 @@
 #ifndef SVO_FEATURE_DETECTION_H_
 #define SVO_FEATURE_DETECTION_H_
 
-#include <svo/global.h>
-#include <svo/frame.h>
-#include <svo/cl_class.h>
+#include <gpu_svo/global.h>
+#include <gpu_svo/frame.h>
+#include <gpu_svo/cl_class.h>
 
 namespace svo {
 
@@ -51,8 +51,6 @@ public:
       const int n_pyr_levels);
 
   virtual ~AbstractDetector() {
-      gpu_fast_->clear_buf();
-      gpu_fast_->make_kernel("fast_gray");
   };
 
   virtual void detect(
@@ -66,7 +64,6 @@ public:
 
   /// Set grid cells of existing features as occupied
   void setExistingFeatures(const Features& fts);
-  opencl* gpu_fast_= nullptr;
 
 protected:
 
@@ -95,6 +92,7 @@ public:
       const int img_width,
       const int img_height,
       const int cell_size,
+      opencl* gpu_fast_,
       const int n_pyr_levels);
 
   virtual ~FastDetector() {}
@@ -104,6 +102,7 @@ public:
       const ImgPyr& img_pyr,
       const double detection_threshold,
       Features& fts);
+  opencl* gpu_fast_;
 };
 
 } // namespace feature_detection

GPU_version/svo/include/gpu_svo/frame.h

@@ -21,7 +21,7 @@
 #include <vikit/math_utils.h>
 #include <vikit/abstract_camera.h>
 #include <boost/noncopyable.hpp>
-#include <svo/global.h>
+#include <gpu_svo/global.h>
 
 namespace g2o {
 class VertexSE3Expmap;

GPU_version/svo/include/gpu_svo/frame_handler_base.h

@@ -23,8 +23,8 @@
 #include <boost/noncopyable.hpp>
 #include <boost/function.hpp>
 #include <boost/thread.hpp>
-#include <svo/global.h>
-#include <svo/map.h>
+#include <gpu_svo/global.h>
+#include <gpu_svo/map.h>
 
 namespace vk
 {

GPU_version/svo/include/gpu_svo/frame_handler_mono.h

@@ -19,10 +19,11 @@
 
 #include <set>
 #include <vikit/abstract_camera.h>
-#include <svo/frame_handler_base.h>
-#include <svo/reprojector.h>
-#include <svo/initialization.h>
-#include <svo/imu_integration.h>
+#include <gpu_svo/frame_handler_base.h>
+#include <gpu_svo/reprojector.h>
+#include <gpu_svo/initialization.h>
+#include <gpu_svo/imu_integration.h>
+#include <gpu_svo/cl_class.h>
 
 namespace svo {
 
@@ -48,8 +49,8 @@ public:
   const set<FramePtr>& coreKeyframes() { return core_kfs_; }
 
   /// Return the feature track to visualize the KLT tracking during initialization.
-  const vector<cv::Point2f>& initFeatureTrackRefPx() const { return klt_homography_init_.px_ref_; }
-  const vector<cv::Point2f>& initFeatureTrackCurPx() const { return klt_homography_init_.px_cur_; }
+  const vector<cv::Point2f>& initFeatureTrackRefPx() const { return klt_homography_init_->px_ref_; }
+  const vector<cv::Point2f>& initFeatureTrackCurPx() const { return klt_homography_init_->px_cur_; }
 
   /// Access the depth filter.
   DepthFilter* depthFilter() const { return depth_filter_; }
@@ -69,9 +70,9 @@ protected:
   FramePtr last_frame_;                         //!< Last frame, not necessarily a keyframe.
   set<FramePtr> core_kfs_;                      //!< Keyframes in the closer neighbourhood.
   vector< pair<FramePtr,size_t> > overlap_kfs_; //!< All keyframes with overlapping field of view. the paired number specifies how many common mappoints are observed TODO: why vector!?
-  initialization::KltHomographyInit klt_homography_init_; //!< Used to estimate pose of the first two keyframes by estimating a homography.
+  initialization::KltHomographyInit* klt_homography_init_; //!< Used to estimate pose of the first two keyframes by estimating a homography.
   DepthFilter* depth_filter_;                   //!< Depth estimation algorithm runs in a parallel thread and is used to initialize new 3D points.
-
+  opencl* gpu_fast_;
 
   /// Initialize the visual odometry algorithm.
   virtual void initialize();

GPU_version/svo/include/gpu_svo/imu_integration.h

@@ -13,7 +13,7 @@
 #include <gtsam/slam/ProjectionFactor.h>
 #include <gtsam/nonlinear/ISAM2.h>
 #include <sophus/se3.h>
-#include <svo/frame.h>
+#include <gpu_svo/frame.h>
 
 using gtsam::symbol_shorthand::B;  // Bias  (ax,ay,az,gx,gy,gz)
 using gtsam::symbol_shorthand::V;  // Vel   (xdot,ydot,zdot)

GPU_version/svo/include/gpu_svo/initialization.h

@@ -17,7 +17,8 @@
 #ifndef SVO_INITIALIZATION_H
 #define SVO_INITIALIZATION_H
 
-#include <svo/global.h>
+#include <gpu_svo/global.h>
+#include <gpu_svo/cl_class.h>
 
 namespace svo {
 
@@ -35,7 +36,7 @@ public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
   FramePtr frame_ref_;
-  KltHomographyInit() {};
+  KltHomographyInit(opencl* gpu_fast_):gpu_fast_(gpu_fast_) {};
   ~KltHomographyInit() {};
   InitResult addFirstFrame(FramePtr frame_ref);
   InitResult addSecondFrame(FramePtr frame_ref);
@@ -50,13 +51,15 @@ protected:
   vector<int> inliers_;             //!< inliers after the geometric check (e.g., Homography).
   vector<Vector3d> xyz_in_cur_;     //!< 3D points computed during the geometric check.
   SE3 T_cur_from_ref_;              //!< computed transformation between the first two frames.
+  opencl* gpu_fast_;
 };
 
 /// Detect Fast corners in the image.
 void detectFeatures(
     FramePtr frame,
     vector<cv::Point2f>& px_vec,
-    vector<Vector3d>& f_vec);
+    vector<Vector3d>& f_vec,
+    opencl* gpu_fast);
 
 /// Compute optical flow (Lucas Kanade) for selected keypoints.
 void trackKlt(

GPU_version/svo/include/gpu_svo/map.h

@@ -20,7 +20,7 @@
 #include <queue>
 #include <boost/noncopyable.hpp>
 #include <boost/thread.hpp>
-#include <svo/global.h>
+#include <gpu_svo/global.h>
 
 namespace svo {
 

GPU_version/svo/include/gpu_svo/matcher.h

@@ -17,7 +17,7 @@
 #ifndef SVO_MATCHER_H_
 #define SVO_MATCHER_H_
 
-#include <svo/global.h>
+#include <gpu_svo/global.h>
 
 namespace vk {
   class AbstractCamera;

GPU_version/svo/include/gpu_svo/point.h

@@ -18,7 +18,7 @@
 #define SVO_POINT_H_
 
 #include <boost/noncopyable.hpp>
-#include <svo/global.h>
+#include <gpu_svo/global.h>
 
 namespace g2o {
   class VertexSBAPointXYZ;

GPU_version/svo/include/gpu_svo/pose_optimizer.h

@@ -17,7 +17,7 @@
 #ifndef SVO_POSE_OPTIMIZER_H_
 #define SVO_POSE_OPTIMIZER_H_
 
-#include <svo/global.h>
+#include <gpu_svo/global.h>
 
 namespace svo {
 

GPU_version/svo/include/gpu_svo/reprojector.h

@@ -17,8 +17,8 @@
 #ifndef SVO_REPROJECTION_H_
 #define SVO_REPROJECTION_H_
 
-#include <svo/global.h>
-#include <svo/matcher.h>
+#include <gpu_svo/global.h>
+#include <gpu_svo/matcher.h>
 
 namespace vk {
 class AbstractCamera;

GPU_version/svo/include/gpu_svo/sparse_img_align.h

@@ -19,7 +19,7 @@
 
 #include <vikit/nlls_solver.h>
 #include <vikit/performance_monitor.h>
-#include <svo/global.h>
+#include <gpu_svo/global.h>
 
 namespace vk {
 class AbstractCamera;

GPU_version/svo/include/gpu_svo/svoVisionFactor.h

@@ -12,7 +12,7 @@
 #include <gtsam/geometry/Pose3.h>
 #include <vikit/robust_cost.h>
 #include <vikit/math_utils.h>
-#include <svo/frame.h>
+#include <gpu_svo/frame.h>
 
 namespace svo {