added branch

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;

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);