cuda updated

src/image_processor.cpp

@@ -223,7 +223,9 @@ void ImageProcessor::stereoCallback(
       sensor_msgs::image_encodings::MONO8);
 
   // Build the image pyramids once since they're used at multiple places
-  createImagePyramids();
+
+  // removed due to alternate cuda construct
+  //createImagePyramids();
 
   // Detect features in the first frame.
   if (is_first_img) {
@@ -632,7 +634,30 @@ 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);
+  cam1_points_gpu = cv::cuda::GpuMat(cam1_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, cam1_points);
+  utils::download(inlier_markers_gpu, inlier_markers);
+
   // Track features using LK optical flow method.
+  /*
   calcOpticalFlowPyrLK(curr_cam0_pyramid_, curr_cam1_pyramid_,
       cam0_points, cam1_points,
       inlier_markers, noArray(),
@@ -642,7 +667,7 @@ void ImageProcessor::stereoMatch(
                    processor_config.max_iteration,
                    processor_config.track_precision),
       cv::OPTFLOW_USE_INITIAL_FLOW);
-
+  */
   // Mark those tracked points out of the image region
   // as untracked.
   for (int i = 0; i < cam1_points.size(); ++i) {
@@ -1002,7 +1027,7 @@ void ImageProcessor::twoPointRansac(
 
   // Check the size of input point size.
   if (pts1.size() != pts2.size())
-    ROS_ERROR("Sets of different size (%lu and %lu) are used...",
+    ROS_ERROR("Sets of different size (%i and %i) are used...",
         pts1.size(), pts2.size());
 
   double norm_pixel_unit = 2.0 / (intrinsics[0]+intrinsics[1]);

src/utils.cpp

@@ -11,14 +11,14 @@
 namespace msckf_vio {
 namespace utils {
 
-static void download(const cv::cuda::GpuMat& d_mat, std::vector<cv::Point2f>& vec)
+void download(const cv::cuda::GpuMat& d_mat, std::vector<cv::Point2f>& vec)
 {
     vec.resize(d_mat.cols);
     cv::Mat mat(1, d_mat.cols, CV_32FC2, (void*)&vec[0]);
     d_mat.download(mat);
 }
 
-static void download(const cv::cuda::GpuMat& d_mat, std::vector<uchar>& vec)
+void download(const cv::cuda::GpuMat& d_mat, std::vector<uchar>& vec)
 {
     vec.resize(d_mat.cols);
     cv::Mat mat(1, d_mat.cols, CV_8UC1, (void*)&vec[0]);