From c6b8a2c2fc538fb40fccbea6eff4521f55e50e21 Mon Sep 17 00:00:00 2001
From: g-spacewhale <raphael@maenle.net>
Date: Wed, 3 Apr 2019 06:51:18 +0000
Subject: [PATCH] added branch

---
 include/msckf_vio/image_processor.h |  2 ++
 src/image_processor.cpp             | 35 +++++++++++++++++++++++------
 2 files changed, 30 insertions(+), 7 deletions(-)

diff --git a/include/msckf_vio/image_processor.h b/include/msckf_vio/image_processor.h
index 1004086..c3b0e2d 100644
--- a/include/msckf_vio/image_processor.h
+++ b/include/msckf_vio/image_processor.h
@@ -367,6 +367,8 @@ private:
   boost::shared_ptr<GridFeatures> prev_features_ptr;
   boost::shared_ptr<GridFeatures> curr_features_ptr;
 
+  cv::Ptr<cv::cuda::SparsePyrLKOpticalFlow> d_pyrLK_sparse;
+
   cv::cuda::GpuMat cam0_curr_img;
   cv::cuda::GpuMat cam1_curr_img;
   cv::cuda::GpuMat cam0_points_gpu;
diff --git a/src/image_processor.cpp b/src/image_processor.cpp
index ba220b1..840d606 100644
--- a/src/image_processor.cpp
+++ b/src/image_processor.cpp
@@ -204,6 +204,13 @@ bool ImageProcessor::initialize() {
   detector_ptr = FastFeatureDetector::create(
       processor_config.fast_threshold);
 
+  //create gpu optical flow lk
+  d_pyrLK_sparse = cuda::SparsePyrLKOpticalFlow::create(
+          Size(processor_config.patch_size, processor_config.patch_size),
+          processor_config.pyramid_levels, 
+          processor_config.max_iteration,
+          true);
+
   if (!createRosIO()) return false;
   ROS_INFO("Finish creating ROS IO...");
 
@@ -464,7 +471,8 @@ void ImageProcessor::trackFeatures() {
   predictFeatureTracking(prev_cam0_points,
       cam0_R_p_c, cam0_intrinsics, curr_cam0_points);
 
-  //TODO: change to GPU
+  //TODO: test change to sparse
+  /*
   calcOpticalFlowPyrLK(
       prev_cam0_pyramid_, curr_cam0_pyramid_,
       prev_cam0_points, curr_cam0_points,
@@ -475,6 +483,25 @@ void ImageProcessor::trackFeatures() {
         processor_config.max_iteration,
         processor_config.track_precision),
       cv::OPTFLOW_USE_INITIAL_FLOW);
+  */
+
+
+
+  cam0_curr_img = cv::cuda::GpuMat(cam0_curr_img_ptr->image);
+  cam1_curr_img = cv::cuda::GpuMat(cam1_curr_img_ptr->image);
+  cam0_points_gpu = cv::cuda::GpuMat(prev_cam0_points);
+  cam1_points_gpu = cv::cuda::GpuMat(curr_cam0_points);
+
+  cv::cuda::GpuMat inlier_markers_gpu;
+  d_pyrLK_sparse->calc(cam0_curr_img, 
+                       cam1_curr_img, 
+                       cam0_points_gpu, 
+                       cam1_points_gpu, 
+                       inlier_markers_gpu, 
+                       noArray());
+
+  utils::download(cam1_points_gpu, curr_cam0_points);
+  utils::download(inlier_markers_gpu, track_inliers);
 
   // Mark those tracked points out of the image region
   // as untracked.
@@ -634,12 +661,6 @@ void ImageProcessor::stereoMatch(
                                 cam1_distortion_model, cam1_distortion_coeffs);
   }
 
-  auto d_pyrLK_sparse = cuda::SparsePyrLKOpticalFlow::create(
-          Size(processor_config.patch_size, processor_config.patch_size),
-          processor_config.pyramid_levels, 
-          processor_config.max_iteration,
-          true);
-
   cam0_curr_img = cv::cuda::GpuMat(cam0_curr_img_ptr->image);
   cam1_curr_img = cv::cuda::GpuMat(cam1_curr_img_ptr->image);
   cam0_points_gpu = cv::cuda::GpuMat(cam0_points);