debug gpu version done

GPU_version/svo/CMakeLists.txt

@@ -96,3 +96,4 @@ TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${LINK_LIBS} )
 set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 14)
 set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD_REQUIRED ON)
 target_compile_features(${PROJECT_NAME} PRIVATE cxx_range_for)
+ADD_DEFINITIONS(-DKERNEL_DIR=\"${PROJECT_SOURCE_DIR}/kernel\")

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 <gpu_svo/global.h>
-#include <svo/feature_detection.h>
+#include <gpu_svo/feature_detection.h>
-#include <svo/matcher.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 <gpu_svo/global.h>
-#include <svo/frame.h>
+#include <gpu_svo/frame.h>
-#include <svo/cl_class.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 <gpu_svo/global.h>
-#include <svo/map.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 <gpu_svo/frame_handler_base.h>
-#include <svo/reprojector.h>
+#include <gpu_svo/reprojector.h>
-#include <svo/initialization.h>
+#include <gpu_svo/initialization.h>
-#include <svo/imu_integration.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>& initFeatureTrackRefPx() const { return klt_homography_init_->px_ref_; }
-  const vector<cv::Point2f>& initFeatureTrackCurPx() const { return klt_homography_init_.px_cur_; }
+  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 <gpu_svo/global.h>
-#include <svo/matcher.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 {
 

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 <gpu_svo/bundle_adjustment.h>
-#include <svo/frame.h>
+#include <gpu_svo/frame.h>
-#include <svo/feature.h>
+#include <gpu_svo/feature.h>
-#include <svo/point.h>
+#include <gpu_svo/point.h>
-#include <svo/config.h>
+#include <gpu_svo/config.h>
-#include <svo/map.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 <gpu_svo/global.h>
-#include <svo/depth_filter.h>
+#include <gpu_svo/depth_filter.h>
-#include <svo/frame.h>
+#include <gpu_svo/frame.h>
-#include <svo/point.h>
+#include <gpu_svo/point.h>
-#include <svo/feature.h>
+#include <gpu_svo/feature.h>
-#include <svo/matcher.h>
+#include <gpu_svo/matcher.h>
-#include <svo/config.h>
+#include <gpu_svo/config.h>
-#include <svo/feature_detection.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 <gpu_svo/feature_detection.h>
-#include <svo/feature.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 <gpu_svo/frame.h>
-#include <svo/feature.h>
+#include <gpu_svo/feature.h>
-#include <svo/point.h>
+#include <gpu_svo/point.h>
-#include <svo/config.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 <gpu_svo/frame_handler_base.h>
-#include <svo/config.h>
+#include <gpu_svo/config.h>
-#include <svo/feature.h>
+#include <gpu_svo/feature.h>
-#include <svo/matcher.h>
+#include <gpu_svo/matcher.h>
-#include <svo/map.h>
+#include <gpu_svo/map.h>
-#include <svo/point.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 <gpu_svo/config.h>
-#include <svo/frame_handler_mono.h>
+#include <gpu_svo/frame_handler_mono.h>
-#include <svo/map.h>
+#include <gpu_svo/map.h>
-#include <svo/frame.h>
+#include <gpu_svo/frame.h>
-#include <svo/feature.h>
+#include <gpu_svo/feature.h>
-#include <svo/point.h>
+#include <gpu_svo/point.h>
-#include <svo/pose_optimizer.h>
+#include <gpu_svo/pose_optimizer.h>
-#include <svo/sparse_img_align.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 "gpu_svo/feature.h"
-#include "svo/point.h"
+#include "gpu_svo/point.h"
-#include "svo/svoVisionFactor.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 <gpu_svo/config.h>
-#include <svo/frame.h>
+#include <gpu_svo/frame.h>
-#include <svo/point.h>
+#include <gpu_svo/point.h>
-#include <svo/feature.h>
+#include <gpu_svo/feature.h>
-#include <svo/initialization.h>
+#include <gpu_svo/initialization.h>
-#include <svo/feature_detection.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 <gpu_svo/map.h>
-#include <svo/point.h>
+#include <gpu_svo/point.h>
-#include <svo/frame.h>
+#include <gpu_svo/frame.h>
-#include <svo/feature.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 <gpu_svo/matcher.h>
-#include <svo/frame.h>
+#include <gpu_svo/frame.h>
-#include <svo/feature.h>
+#include <gpu_svo/feature.h>
-#include <svo/point.h>
+#include <gpu_svo/point.h>
-#include <svo/config.h>
+#include <gpu_svo/config.h>
-#include <svo/feature_alignment.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 <gpu_svo/point.h>
-#include <svo/frame.h>
+#include <gpu_svo/frame.h>
-#include <svo/feature.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 <gpu_svo/pose_optimizer.h>
-#include <svo/frame.h>
+#include <gpu_svo/frame.h>
-#include <svo/feature.h>
+#include <gpu_svo/feature.h>
-#include <svo/point.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 <gpu_svo/reprojector.h>
-#include <svo/frame.h>
+#include <gpu_svo/frame.h>
-#include <svo/point.h>
+#include <gpu_svo/point.h>
-#include <svo/feature.h>
+#include <gpu_svo/feature.h>
-#include <svo/map.h>
+#include <gpu_svo/map.h>
-#include <svo/config.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 <gpu_svo/sparse_img_align.h>
-#include <svo/frame.h>
+#include <gpu_svo/frame.h>
-#include <svo/feature.h>
+#include <gpu_svo/feature.h>
-#include <svo/config.h>
+#include <gpu_svo/config.h>
-#include <svo/point.h>
+#include <gpu_svo/point.h>
 #include <vikit/abstract_camera.h>
 #include <vikit/vision.h>
 #include <vikit/math_utils.h>

GPU_version/vio/.idea/.gitignore

@@ -0,0 +1,8 @@
+# Default ignored files
+/shelf/
+/workspace.xml
+# Datasource local storage ignored files
+/dataSources/
+/dataSources.local.xml
+# Editor-based HTTP Client requests
+/httpRequests/

GPU_version/vio/.idea/misc.xml

@@ -0,0 +1,4 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="CMakeWorkspace" PROJECT_DIR="$PROJECT_DIR$" />
+</project>

GPU_version/vio/.idea/modules.xml

@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectModuleManager">
+    <modules>
+      <module fileurl="file://$PROJECT_DIR$/.idea/svo_ros.iml" filepath="$PROJECT_DIR$/.idea/svo_ros.iml" />
+    </modules>
+  </component>
+</project>

GPU_version/vio/.idea/vcs.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="VcsDirectoryMappings">
+    <mapping directory="$PROJECT_DIR$/.." vcs="Git" />
+  </component>
+</project>

GPU_version/vio/CMakeLists.txt

@@ -0,0 +1,92 @@
+SET(PROJECT_NAME vio_gpu)
+PROJECT(${PROJECT_NAME})
+CMAKE_MINIMUM_REQUIRED (VERSION 2.8.3)
+SET(CMAKE_BUILD_TYPE Release) # Release, RelWithDebInfo
+SET(CMAKE_VERBOSE_MAKEFILE OFF)
+SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PROJECT_SOURCE_DIR}/CMakeModules/")
+
+# Set build flags. Set IS_ARM on odroid board as environment variable
+SET(CMAKE_CXX_FLAGS "-Wall -D_LINUX -D_REENTRANT -march=native -Wno-unused-variable -Wno-unused-but-set-variable -Wno-unknown-pragmas")
+IF(DEFINED ENV{ARM_ARCHITECTURE})
+  SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
+ELSE()
+  SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mmmx -msse -msse -msse2 -msse3 -mssse3")
+ENDIF()
+IF(CMAKE_COMPILER_IS_GNUCC)
+  SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+ELSE()
+  SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+ENDIF()
+SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops")
+
+# Add catkin and required ROS packages
+FIND_PACKAGE(catkin REQUIRED COMPONENTS
+        geometry_msgs
+        roscpp
+  cmake_modules
+  nav_msgs
+  std_msgs
+  visualization_msgs
+  sensor_msgs
+  image_transport
+  cv_bridge
+  tf
+  gpu_svo
+  vikit_common
+  vikit_ros
+)
+
+# Add plain cmake packages 
+FIND_PACKAGE(OpenCV REQUIRED)
+FIND_PACKAGE(Eigen REQUIRED)
+FIND_PACKAGE(Sophus REQUIRED)
+FIND_PACKAGE(Boost REQUIRED COMPONENTS thread)
+FIND_PACKAGE(GTSAM REQUIRED)
+FIND_PACKAGE(OpenCL REQUIRED)
+
+# notify catkin to look at the previously found msg files
+add_message_files(
+        FILES
+        Info.msg
+)
+
+# build the header files from the msg files, and notify catkin about the dependencies
+generate_messages(
+        DEPENDENCIES
+        geometry_msgs
+        sensor_msgs
+)
+# Describe catkin Project
+catkin_package(
+  DEPENDS Eigen OpenCV Sophus Boost
+  CATKIN_DEPENDS roscpp nav_msgs std_msgs visualization_msgs sensor_msgs image_transport cv_bridge tf gpu_svo vikit_common vikit_ros geometry_msgs
+  INCLUDE_DIRS include
+  LIBRARIES ${PROJECT_NAME}_visualizer
+)
+
+# Include dirs
+INCLUDE_DIRECTORIES(
+        include
+  ${Eigen_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
+  ${Sophus_INCLUDE_DIRS}
+  ${Boost_INCLUDE_DIRS}
+  ${catkin_INCLUDE_DIRS}
+)
+
+# Set link libraries
+LIST(APPEND LINK_LIBS 
+  ${OpenCV_LIBS}
+  ${Sophus_LIBRARIES}
+  ${Boost_LIBRARIES}
+  ${catkin_LIBRARIES}
+  ${OpenCL_LIBRARIES}
+)
+
+# Create visualizer library
+ADD_LIBRARY(${PROJECT_NAME}_visualizer src/visualizer.cpp)
+TARGET_LINK_LIBRARIES(${PROJECT_NAME}_visualizer ${LINK_LIBS} gtsam)
+                                 
+# Create Executables                      
+ADD_EXECUTABLE(${PROJECT_NAME} src/vo_node.cpp)
+TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${PROJECT_NAME}_visualizer ${LINK_LIBS})

GPU_version/vio/include/vio_gpu/visualizer.h

@@ -0,0 +1,73 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef SVO_VISUALIZER_H_
+#define SVO_VISUALIZER_H_
+
+#include <queue>
+#include <ros/ros.h>
+#include <gpu_svo/global.h>
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
+#include <tf/transform_broadcaster.h>
+#include <image_transport/image_transport.h>
+#include <boost/thread.hpp>
+#include <boost/shared_ptr.hpp>
+
+namespace svo {
+
+class Frame;
+class Point;
+class Map;
+class FrameHandlerMono;
+
+typedef boost::shared_ptr<Frame> FramePtr;
+
+/// This class bundles all functions to publish visualisation messages.
+class Visualizer
+{
+public:
+  ros::NodeHandle pnh_;
+  size_t trace_id_;
+  ros::Publisher pub_frames_;
+  ros::Publisher pub_points_;
+  ros::Publisher pub_pose_;
+  ros::Publisher pub_info_;
+  tf::TransformBroadcaster br_;
+  SE3 T_world_from_vision_;
+
+  Visualizer();
+
+  ~Visualizer() {};
+
+  void publishMinimal(
+      const cv::Mat& img,
+      const FramePtr& frame,
+      const FrameHandlerMono& slam,
+      const double timestamp);
+
+  void visualizeMarkers(
+      const FramePtr& frame,
+      const set<FramePtr>& core_kfs,
+      const Map& map);
+
+  void removeDeletedPts(const Map& map);
+
+};
+
+} // end namespace svo
+
+#endif /* SVO_VISUALIZER_H_ */

GPU_version/vio/launch/Px30.launch

@@ -0,0 +1,22 @@
+<launch>
+
+  <node pkg="vio_gpu" type="vio_gpu" name="vio" clear_params="true" output="screen">
+
+    <!-- Camera topic to subscribe to -->
+    <param name="cam_topic" value="/CoreNode/grey_img" type="str" />
+    <param name="imu_topic" value="/SensorNode/imu" type="str" />
+    
+    <!-- Camera calibration file -->
+    <rosparam file="$(find vio_gpu)/param/px30.yaml" />
+
+    <!-- Initial camera orientation, make it point downwards -->
+    <param name="init_rx" value="3.14" />
+    <param name="init_ry" value="0.00" />
+    <param name="init_rz" value="0.00" />
+
+  </node>
+
+  <!-- Default parameter settings: choose between vo_fast and vo_accurate -->
+  <rosparam file="$(find vio_gpu)/param/vo_fast.yaml" />
+
+</launch>

GPU_version/vio/launch/live.launch

@@ -0,0 +1,16 @@
+<launch>
+  
+    <node pkg="svo_ros" type="vo" name="svo" clear_params="true" output="screen">
+    
+        <!-- Camera topic to subscribe to -->
+        <param name="cam_topic" value="/camera/image_raw" type="str" />
+        
+        <!-- Camera calibration file -->
+        <rosparam file="$(find svo_ros)/param/camera_atan.yaml" />
+        
+        <!-- Default parameter settings: choose between vo_fast and vo_accurate -->
+        <rosparam file="$(find svo_ros)/param/vo_fast.yaml" />
+
+    </node>
+        
+</launch>

GPU_version/vio/launch/test.launch

@@ -0,0 +1,22 @@
+<launch>
+
+  <node pkg="vio_svo" type="vo" name="vio" clear_params="true" output="screen">
+
+    <!-- Camera topic to subscribe to -->
+    <param name="cam_topic" value="/camera/image_raw" type="str" />
+    <param name="imu_topic" value="/SensorNode/imu" type="str" />
+
+    <!-- Camera calibration file -->
+    <rosparam file="$(find vio_svo)/param/camera_atan.yaml" />
+
+    <!-- Default parameter settings: choose between vo_fast and vo_accurate -->
+    <rosparam file="$(find vio_svo)/param/vo_fast.yaml" />
+
+    <!-- Initial camera orientation, make it point downwards -->
+    <param name="init_rx" value="3.14" />
+    <param name="init_ry" value="0.00" />
+    <param name="init_rz" value="0.00" />
+
+  </node>
+
+</launch>

GPU_version/vio/launch/unity.launch

@@ -0,0 +1,22 @@
+<launch>
+
+  <node pkg="vio_svo" type="vo" name="vio" clear_params="true" output="screen">
+
+    <!-- Camera topic to subscribe to -->
+    <param name="cam_topic" value="/image_converter/output_image " type="str" />
+    <param name="imu_topic" value="/imu_unity/imu" type="str" />
+
+    <!-- Camera calibration file -->
+    <rosparam file="$(find vio_svo)/param/unity.yaml" />
+
+    <!-- Default parameter settings: choose between vo_fast and vo_accurate -->
+    <rosparam file="$(find vio_svo)/param/vo_fast.yaml" />
+
+    <!-- Initial camera orientation, make it point downwards -->
+    <param name="init_rx" value="0.00" />
+    <param name="init_ry" value="0.00" />
+    <param name="init_rz" value="0.00" />
+
+  </node>
+
+</launch>

GPU_version/vio/msg/Info.msg

@@ -0,0 +1,6 @@
+Header      header
+float32     processing_time
+int32[]     keyframes
+int32       num_matches
+int32       tracking_quality
+int32       stage

GPU_version/vio/package.xml

@@ -0,0 +1,43 @@
+<?xml version="1.0"?>
+<package>
+  <name>vio_gpu</name>
+  <version>0.1.0</version>
+  <description>
+    Monocular Visual Odometry - ROS Nodes
+  </description>
+  <author>Majid Geravand</author>
+  <maintainer email="[email protected]">Majid Geravand</maintainer>
+  <license>GPLv3</license>
+
+  <!-- Dependencies which this package needs to build itself. -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <!-- Dependencies needed to compile this package. -->
+  <build_depend>roscpp</build_depend>
+  <build_depend>cmake_modules</build_depend> <!-- for FindEigen.cmake module -->
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>visualization_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>image_transport</build_depend>
+  <build_depend>cv_bridge</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>gpu_svo</build_depend>
+  <build_depend>vikit_common</build_depend>
+  <build_depend>vikit_ros</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  
+  <!-- Dependencies needed after this package is compiled. -->
+  <run_depend>roscpp</run_depend>
+  <run_depend>nav_msgs</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>visualization_msgs</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>image_transport</run_depend>
+  <run_depend>cv_bridge</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>vikit_common</run_depend>
+  <run_depend>vikit_ros</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>gpu_svo</run_depend>
+</package>

GPU_version/vio/param/camera_atan.yaml

@@ -0,0 +1,8 @@
+cam_model: ATAN
+cam_width: 752
+cam_height: 480
+cam_fx: 0.509326
+cam_fy: 0.796651
+cam_cx: 0.45905
+cam_cy: 0.510056
+cam_d0: 0.9320

GPU_version/vio/param/camera_pinhole.yaml

@@ -0,0 +1,11 @@
+cam_model: Pinhole
+cam_width: 640
+cam_height: 480
+cam_fx: 789.860721
+cam_fy: 790.097440
+cam_cx: 305.323524
+cam_cy: 260.464072
+cam_d0: 0.027470
+cam_d1: -0.051518
+cam_d2: 0.004375
+cam_d3: -0.004028

GPU_version/vio/param/px30.yaml

@@ -0,0 +1,8 @@
+cam_model: ATAN
+cam_width: 640
+cam_height: 480
+cam_fx: 0.463632
+cam_fy: 0.823661
+cam_cx: 0.497465
+cam_cy: 0.498443
+cam_d0: 1.01687

GPU_version/vio/param/unity.yaml

@@ -0,0 +1,8 @@
+cam_model: ATAN
+cam_width: 1024
+cam_height: 720
+cam_fx: 1.98248
+cam_fy: 2.68212
+cam_cx: 0.502524
+cam_cy: 0.497744
+cam_d0: -0.000499999

GPU_version/vio/param/vo_accurate.yaml

@@ -0,0 +1,4 @@
+grid_size: 20
+max_n_kfs: 50
+loba_num_iter: 10
+max_fts: 200

GPU_version/vio/param/vo_fast.yaml

@@ -0,0 +1,13 @@
+svo:
+  grid_size: 30            #Feature grid size of a cell in [px].
+  max_n_kfs: 10            #Limit the number of keyframes in the map. This makes nslam essentially. a Visual Odometry. Set to 0 if unlimited number of keyframes are allowed.  Minimum number of keyframes is 3.
+  loba_num_iter: 0         #Number of iterations in the local bundle adjustment.
+  quality_min_fts: 10      #If the number of tracked features drops below this threshold. Tracking quality is bad.
+  quality_max_drop_fts: 60 #If within one frame, this amount of features are dropped. Tracking quality is bad.
+  ACC_noise: 0.00001          #acc white noise in continuous
+  GYO_noise: 0.00001          #gyro white noise in continuous
+  IUC: 0.1                #integration uncertainty continuous
+  ABC: 0.1                #acc bias in continuous
+  GBC: 0.1                #gyro bias in continuous
+  EBP: 0.1                #error in the bias used for preintegration
+  NRP: 0.0000001                #noise used for projection in preintegration

GPU_version/vio/rqt.perspective

@@ -0,0 +1,293 @@
+{
+  "keys": {}, 
+  "groups": {
+    "pluginmanager": {
+      "keys": {
+        "running-plugins": {
+          "type": "repr", 
+          "repr": "{u'rqt_rviz/RViz': [1], u'rqt_reconfigure/Param': [1], u'rqt_image_view/ImageView': [1], u'svo_rqt/Svo': [1]}"
+        }
+      }, 
+      "groups": {
+        "plugin__rqt_rviz__RViz__1": {
+          "keys": {}, 
+          "groups": {
+            "dock_widget__": {
+              "keys": {
+                "dockable": {
+                  "type": "repr", 
+                  "repr": "True"
+                }, 
+                "parent": {
+                  "type": "repr", 
+                  "repr": "None"
+                }
+              }, 
+              "groups": {}
+            }
+          }
+        }, 
+        "plugin__rqt_robot_steering__RobotSteering__1": {
+          "keys": {}, 
+          "groups": {
+            "dock_widget__RobotSteeringUi": {
+              "keys": {
+                "dockable": {
+                  "type": "repr", 
+                  "repr": "u'true'"
+                }, 
+                "parent": {
+                  "type": "repr", 
+                  "repr": "None"
+                }
+              }, 
+              "groups": {}
+            }, 
+            "plugin": {
+              "keys": {
+                "topic": {
+                  "type": "repr", 
+                  "repr": "u'/cmd_vel'"
+                }, 
+                "vx_min": {
+                  "type": "repr", 
+                  "repr": "u'-1'"
+                }, 
+                "vw_min": {
+                  "type": "repr", 
+                  "repr": "u'-3'"
+                }, 
+                "vw_max": {
+                  "type": "repr", 
+                  "repr": "u'3'"
+                }, 
+                "vx_max": {
+                  "type": "repr", 
+                  "repr": "u'1'"
+                }
+              }, 
+              "groups": {}
+            }
+          }
+        }, 
+        "plugin__rqt_reconfigure__Param__1": {
+          "keys": {}, 
+          "groups": {
+            "dock_widget___plugincontainer_top_widget": {
+              "keys": {
+                "dockable": {
+                  "type": "repr", 
+                  "repr": "True"
+                }, 
+                "parent": {
+                  "type": "repr", 
+                  "repr": "None"
+                }
+              }, 
+              "groups": {}
+            }, 
+            "plugin": {
+              "keys": {
+                "splitter": {
+                  "type": "repr(QByteArray.hex)", 
+                  "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff0000000000000002000000c00000006401000000060100000001')", 
+                  "pretty-print": "         d     "
+                }, 
+                "_splitter": {
+                  "type": "repr(QByteArray.hex)", 
+                  "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000000000020000013e0000006401000000090100000002')", 
+                  "pretty-print": "       > d     "
+                }
+              }, 
+              "groups": {}
+            }
+          }
+        }, 
+        "plugin__rqt_image_view__ImageView__1": {
+          "keys": {}, 
+          "groups": {
+            "dock_widget__ImageViewWidget": {
+              "keys": {
+                "dockable": {
+                  "type": "repr", 
+                  "repr": "True"
+                }, 
+                "parent": {
+                  "type": "repr", 
+                  "repr": "None"
+                }
+              }, 
+              "groups": {}
+            }, 
+            "plugin": {
+              "keys": {
+                "dynamic_range": {
+                  "type": "repr", 
+                  "repr": "False"
+                }, 
+                "max_range": {
+                  "type": "repr", 
+                  "repr": "10.0"
+                }, 
+                "topic": {
+                  "type": "repr", 
+                  "repr": "u'/svo/image'"
+                }, 
+                "zoom1": {
+                  "type": "repr", 
+                  "repr": "False"
+                }
+              }, 
+              "groups": {}
+            }
+          }
+        }, 
+        "plugin__svo_rqt__Svo__1": {
+          "keys": {}, 
+          "groups": {
+            "dock_widget__SvoWidget": {
+              "keys": {
+                "dockable": {
+                  "type": "repr", 
+                  "repr": "True"
+                }, 
+                "parent": {
+                  "type": "repr", 
+                  "repr": "None"
+                }
+              }, 
+              "groups": {}
+            }, 
+            "dock_widget__SvoUi": {
+              "keys": {
+                "dockable": {
+                  "type": "repr", 
+                  "repr": "u'true'"
+                }, 
+                "parent": {
+                  "type": "repr", 
+                  "repr": "None"
+                }
+              }, 
+              "groups": {}
+            }
+          }
+        }, 
+        "plugin__rqt_shell__Shell__1": {
+          "keys": {}, 
+          "groups": {
+            "plugin": {
+              "keys": {
+                "shell_type": {
+                  "type": "repr", 
+                  "repr": "u'0'"
+                }
+              }, 
+              "groups": {}
+            }
+          }
+        }, 
+        "plugin__rqt_console__Console__1": {
+          "keys": {}, 
+          "groups": {
+            "plugin": {
+              "keys": {
+                "show_highlighted_only": {
+                  "type": "repr", 
+                  "repr": "u'false'"
+                }, 
+                "message_limit": {
+                  "type": "repr", 
+                  "repr": "u'20000'"
+                }, 
+                "exclude_filters": {
+                  "type": "repr", 
+                  "repr": "u'severity'"
+                }, 
+                "table_splitter": {
+                  "type": "repr(QByteArray.hex)", 
+                  "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000000000020000004e000000ed01000000090100000002')", 
+                  "pretty-print": "       N       "
+                }, 
+                "highlight_filters": {
+                  "type": "repr", 
+                  "repr": "u'message'"
+                }, 
+                "paused": {
+                  "type": "repr", 
+                  "repr": "u'false'"
+                }, 
+                "settings_exist": {
+                  "type": "repr", 
+                  "repr": "u'true'"
+                }, 
+                "filter_splitter": {
+                  "type": "repr(QByteArray.hex)", 
+                  "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff0000000000000002000000720000007201000000090100000002')", 
+                  "pretty-print": "       r r     "
+                }
+              }, 
+              "groups": {
+                "exclude_filter_0": {
+                  "keys": {
+                    "enabled": {
+                      "type": "repr", 
+                      "repr": "u'true'"
+                    }, 
+                    "itemlist": {
+                      "type": "repr", 
+                      "repr": "u''"
+                    }
+                  }, 
+                  "groups": {}
+                }, 
+                "highlight_filter_0": {
+                  "keys": {
+                    "regex": {
+                      "type": "repr", 
+                      "repr": "u'false'"
+                    }, 
+                    "text": {
+                      "type": "repr", 
+                      "repr": "u''"
+                    }, 
+                    "enabled": {
+                      "type": "repr", 
+                      "repr": "u'true'"
+                    }
+                  }, 
+                  "groups": {}
+                }
+              }
+            }
+          }
+        }
+      }
+    }, 
+    "mainwindow": {
+      "keys": {
+        "geometry": {
+          "type": "repr(QByteArray.hex)", 
+          "repr(QByteArray.hex)": "QtCore.QByteArray('01d9d0cb00010000000007a9fffffffc00001187000005a7000007b1000000180000117f0000059f000000000000')", 
+          "pretty-print": "                       "
+        }, 
+        "state": {
+          "type": "repr(QByteArray.hex)", 
+          "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000fd0000000100000003000009cf00000559fc0100000005fb0000002c00730076006f005f007200710074005f005f00530076006f005f005f0031005f005f00530076006f005500690100000000000003890000000000000000fb0000006a007200710074005f0072006f0062006f0074005f007300740065006500720069006e0067005f005f0052006f0062006f0074005300740065006500720069006e0067005f005f0031005f005f0052006f0062006f0074005300740065006500720069006e00670055006901000000000000020b0000000000000000fc00000000000003530000022e00fffffffc0200000004fb0000003400730076006f005f007200710074005f005f00530076006f005f005f0031005f005f00530076006f0057006900640067006500740100000019000000a30000009900fffffffb0000004c007200710074005f0063006f006e0073006f006c0065005f005f0043006f006e0073006f006c0065005f005f0031005f005f0043006f006e0073006f006c006500570069006400670065007401000000c3000003b50000000000000000fb0000005a007200710074005f0069006d006100670065005f0076006900650077005f005f0049006d0061006700650056006900650077005f005f0031005f005f0049006d006100670065005600690065007700570069006400670065007401000000c2000002700000005100fffffffb0000006c007200710074005f007200650063006f006e006600690067007500720065005f005f0050006100720061006d005f005f0031005f005f005f0070006c007500670069006e0063006f006e007400610069006e00650072005f0074006f0070005f00770069006400670065007401000003380000023a0000010f00fffffffb00000026007200710074005f007200760069007a005f005f005200560069007a005f005f0031005f005f010000035900000676000000a700fffffffb00000040007200710074005f007300680065006c006c005f005f005300680065006c006c005f005f0031005f005f0058005400650072006d005700690064006700650074010000091c000000b30000000000000000000009cf0000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')", 
+          "pretty-print": "                 ,svo_rqt__Svo__1__SvoUi          jrqt_robot_steering__RobotSteering__1__RobotSteeringUi            S .                                                                                                                                              p Q                                                            8 :                           Y v                                                                                                       "
+        }
+      }, 
+      "groups": {
+        "toolbar_areas": {
+          "keys": {
+            "MinimizedDockWidgetsToolbar": {
+              "type": "repr", 
+              "repr": "8"
+            }
+          }, 
+          "groups": {}
+        }
+      }
+    }
+  }
+}

GPU_version/vio/rviz_config.rviz

@@ -0,0 +1,153 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 108
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /TF1
+        - /Marker1
+        - /Marker2
+        - /Marker2/Namespaces1
+        - /Image1
+      Splitter Ratio: 0.5
+    Tree Height: 680
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: Image
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz/TF
+      Enabled: false
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        {}
+      Update Interval: 0
+      Value: false
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /svo/keyframes
+      Name: Marker
+      Namespaces:
+        cams: true
+        kfs: true
+      Queue Size: 100
+      Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /svo/points
+      Name: Marker
+      Namespaces:
+        pts: true
+        trajectory: true
+      Queue Size: 100
+      Value: true
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /svo/image
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 255; 255; 255
+    Fixed Frame: /world
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 5.48911
+      Focal Point:
+        X: 0.777511
+        Y: 0.689927
+        Z: -0.445833
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 0.125204
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 4.45215
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1416
+  Hide Left Dock: false
+  Hide Right Dock: true
+  Image:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000002cc00000542fc0200000009fb0000001200530065006c0065006300740069006f006e00000000280000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000355000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000383000001e70000001600ffffff000000010000010f000003dafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000003da000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003ce0000003efc0100000002fb0000000800540069006d00650000000000000003ce000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000006fd0000054200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 2511
+  X: 1962
+  Y: 24

GPU_version/vio/src/visualizer.cpp

@@ -0,0 +1,127 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// 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 <vio_gpu/visualizer.h>
+#include <gpu_svo/frame_handler_mono.h>
+#include <gpu_svo/point.h>
+#include <gpu_svo/map.h>
+#include <gpu_svo/feature.h>
+#include <vio_gpu/Info.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <vikit/timer.h>
+#include <vikit/output_helper.h>
+#include <deque>
+
+namespace svo {
+
+Visualizer::
+Visualizer() :
+    pnh_("~"),
+    trace_id_(0),
+    T_world_from_vision_(Matrix3d::Identity(), Vector3d::Zero())
+{
+  // Init ROS Marker Publishers
+  pub_frames_ = pnh_.advertise<visualization_msgs::Marker>("keyframes", 10);
+  pub_points_ = pnh_.advertise<visualization_msgs::Marker>("points", 1000);
+  pub_pose_ = pnh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose",10);
+  pub_info_ = pnh_.advertise<vio_gpu::Info>("info", 10);
+}
+
+void Visualizer::publishMinimal(
+    const cv::Mat& img,
+    const FramePtr& frame,
+    const FrameHandlerMono& slam,
+    const double timestamp)
+{
+  ++trace_id_;
+  std_msgs::Header header_msg;
+  header_msg.frame_id = "/cam";
+  header_msg.seq = trace_id_;
+  header_msg.stamp = ros::Time(timestamp);
+
+  // publish info msg.
+  if(pub_info_.getNumSubscribers() > 0)
+  {
+      vio_gpu::Info msg_info;
+    msg_info.header = header_msg;
+    msg_info.processing_time = slam.lastProcessingTime();
+    msg_info.keyframes.reserve(slam.map().keyframes_.size());
+    for(list<FramePtr>::const_iterator it=slam.map().keyframes_.begin(); it!=slam.map().keyframes_.end(); ++it)
+      msg_info.keyframes.push_back((*it)->id_);
+    msg_info.stage = static_cast<int>(slam.stage());
+    msg_info.tracking_quality = static_cast<int>(slam.trackingQuality());
+    if(frame != NULL)
+      msg_info.num_matches = slam.lastNumObservations();
+    else
+      msg_info.num_matches = 0;
+    pub_info_.publish(msg_info);
+  }
+  if(pub_pose_.getNumSubscribers() > 0 && slam.stage() == FrameHandlerBase::STAGE_DEFAULT_FRAME)
+  {
+    Quaterniond q;
+    Vector3d p;
+    Matrix<double,6,6> Cov;
+    // publish cam in world frame (Estimated odometry in the worls frame)
+    SE3 T_world_from_cam(T_world_from_vision_*frame->T_f_w_.inverse());// do not confused with inverse. the T_f_w_ is the from camera to initial position with reference of camera
+    q = Quaterniond(T_world_from_cam.rotation_matrix()*T_world_from_vision_.rotation_matrix().transpose());
+    p = T_world_from_cam.translation();
+    Cov = T_world_from_cam.Adj()*frame->Cov_*T_world_from_cam.inverse().Adj();
+    geometry_msgs::PoseWithCovarianceStampedPtr msg_pose(new geometry_msgs::PoseWithCovarianceStamped);
+    msg_pose->header = header_msg;
+    msg_pose->pose.pose.position.x = p[0];
+    msg_pose->pose.pose.position.y = p[1];
+    msg_pose->pose.pose.position.z = p[2];
+    msg_pose->pose.pose.orientation.x = q.x();
+    msg_pose->pose.pose.orientation.y = q.y();
+    msg_pose->pose.pose.orientation.z = q.z();
+    msg_pose->pose.pose.orientation.w = q.w();
+    for(size_t i=0; i<36; ++i)
+      msg_pose->pose.covariance[i] = Cov(i%6, i/6);
+    pub_pose_.publish(msg_pose);
+  }
+}
+
+void Visualizer::visualizeMarkers(
+    const FramePtr& frame,
+    const set<FramePtr>& core_kfs,
+    const Map& map)
+{
+  if(frame == NULL)
+    return;
+
+  vk::output_helper::publishTfTransform(
+      frame->T_f_w_*T_world_from_vision_.inverse(),
+      ros::Time(frame->timestamp_), "cam_pos", "world", br_);
+
+  if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0)
+  {
+    vk::output_helper::publishPointMarker(
+        pub_points_, T_world_from_vision_*frame->pos(), "trajectory",
+        ros::Time::now(), trace_id_, 0, 0.006, Vector3d(0.,0.,0.5));
+    removeDeletedPts(map);
+  }
+}
+
+void Visualizer::removeDeletedPts(const Map& map)
+{
+  if(pub_points_.getNumSubscribers() > 0)
+  {
+    for(list<Point*>::const_iterator it=map.trash_points_.begin(); it!=map.trash_points_.end(); ++it)
+      vk::output_helper::publishPointMarker(pub_points_, Vector3d(), "pts", ros::Time::now(), (*it)->id_, 2, 0.006, Vector3d());
+  }
+}
+
+} // end namespace gpu_svo

GPU_version/vio/src/vo_node.cpp

@@ -0,0 +1,148 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// 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 <ros/package.h>
+#include <string>
+#include <gpu_svo/frame_handler_mono.h>
+#include <vio_gpu/visualizer.h>
+#include <vikit/params_helper.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/Imu.h>
+#include <image_transport/image_transport.h>
+#include <boost/thread.hpp>
+#include <cv_bridge/cv_bridge.h>
+#include <Eigen/Core>
+#include <vikit/abstract_camera.h>
+#include <vikit/camera_loader.h>
+#include <gpu_svo/frame.h>
+
+
+namespace svo {
+
+/// SVO Interface
+class VoNode
+{
+public:
+      svo::FrameHandlerMono* vo_;
+      svo::Visualizer visualizer_;
+      bool publish_markers_;                 //!< publish only the minimal amount of info (choice for embedded devices)
+      vk::AbstractCamera* cam_;
+      FILE* logtime;
+      bool quit_;
+      boost::thread* imu_th;
+      VoNode();
+      ~VoNode();
+      void imgCb(const sensor_msgs::ImageConstPtr& msg);
+      void imuCb(const sensor_msgs::ImuPtr& imu);
+      void imu();
+private:
+    double* imu_;
+};
+
+VoNode::VoNode() :
+      vo_(NULL),
+      publish_markers_(vk::getParam<bool>("vio/publish_markers", true)),
+      cam_(NULL),
+      quit_(false)
+{
+      // Create Camera
+      imu_=(double *)calloc(static_cast<std::size_t>(6), sizeof(double));
+      if(!vk::camera_loader::loadFromRosNs("vio", cam_))
+        throw std::runtime_error("Camera model not correctly specified.");
+      // Get initial position and orientation
+      visualizer_.T_world_from_vision_ = Sophus::SE3(
+          vk::rpy2dcm(Vector3d(vk::getParam<double>("vio/init_rx", 0.0),
+                               vk::getParam<double>("vio/init_ry", 0.0),
+                               vk::getParam<double>("vio/init_rz", 0.0))),
+          Eigen::Vector3d(vk::getParam<double>("vio/init_tx", 0.0),
+                          vk::getParam<double>("vio/init_ty", 0.0),
+                          vk::getParam<double>("vio/init_tz", 0.0)));
+      // Init VO and start
+      vo_ = new svo::FrameHandlerMono(cam_,visualizer_.T_world_from_vision_);
+      vo_->start();
+    imu_th=new boost::thread(&VoNode::imu,this);
+    logtime = fopen((std::string(getenv("HOME"))+"/time.txt").c_str(), "w+");
+
+}
+
+VoNode::~VoNode()
+{
+    quit_=true;
+    fclose(logtime);
+    imu_th->join();
+    delete imu_th;
+    delete logtime;
+    delete vo_;
+    delete cam_;
+}
+
+void VoNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
+{
+      try {
+          auto start=std::chrono::steady_clock::now();
+          cv::Mat img=cv_bridge::toCvShare(msg, "mono8")->image;
+          vo_->addImage(img, msg->header.stamp.toSec());
+          visualizer_.publishMinimal(img, vo_->lastFrame(), *vo_, msg->header.stamp.toSec());
+          if(publish_markers_ && vo_->stage() != FrameHandlerBase::STAGE_PAUSED)
+              visualizer_.visualizeMarkers(vo_->lastFrame(), vo_->coreKeyframes(), vo_->map());
+          if(vo_->stage() == FrameHandlerMono::STAGE_PAUSED)
+              usleep(100000);
+          fprintf(logtime, "%s\n",std::to_string(std::chrono::duration<double>(std::chrono::steady_clock::now()-start).count()).c_str());
+
+      } catch (cv_bridge::Exception& e) {
+        ROS_ERROR("cv_bridge exception: %s", e.what());
+      }
+}
+void VoNode::imuCb(const sensor_msgs::ImuPtr &imu) {
+    double imu_in[6];
+    imu_in[0] = 0.1*imu_[0]+0.9*imu->linear_acceleration.x;
+    imu_in[1] = 0.1*imu_[1]+0.9*imu->linear_acceleration.y;
+    imu_in[2] = 0.1*imu_[2]+0.9*imu->linear_acceleration.z;
+    imu_in[3] = 0.1*imu_[3]+0.9*imu->angular_velocity.x;
+    imu_in[4] = 0.1*imu_[4]+0.9*imu->angular_velocity.y;
+    imu_in[5] = 0.1*imu_[5]+0.9*imu->angular_velocity.z;
+    memcpy(imu_, imu_in, static_cast<std::size_t>(6*sizeof(double)));
+    vo_->imu_integPtr_->update(imu_in);
+}
+void VoNode::imu(){
+    ros::NodeHandle nh;
+    // subscribe to cam msgs
+    image_transport::ImageTransport it(nh);
+    image_transport::Subscriber it_sub = it.subscribe(vk::getParam<std::string>("vio/cam_topic", "camera/image_raw"), 1, &svo::VoNode::imgCb, this);
+    // start processing callbacks
+    while(ros::ok() && !quit_)
+    {
+        ros::spinOnce();
+    }
+
+}
+} // namespace svo
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "vio");
+  svo::VoNode vo_node;
+  ros::NodeHandle nh;
+  ros::Subscriber imu_sub=nh.subscribe(vk::getParam<std::string>("vio/imu_topic", "imu/raw"),1,&svo::VoNode::imuCb, &vo_node);
+  // start processing callbacks
+  while(ros::ok() && !vo_node.quit_)
+  {
+      ros::spinOnce();
+      usleep(1000);
+      // TODO check when last image was processed. when too long ago. publish warning that no msgs are received!
+  }
+  printf("SVO terminated.\n");
+  return 0;
+}