From f4eb9068962b63082f66faf2c5ed5301b2767645 Mon Sep 17 00:00:00 2001
From: g-spacewhale <r.maenle@gmail.com>
Date: Fri, 15 Mar 2019 09:42:59 +0000
Subject: [PATCH] cuda updated

---
 include/msckf_vio/image_processor.h |  6 +++---
 include/msckf_vio/utils.h           |  4 ++--
 src/image_processor.cpp             | 31 ++++++++++++++++++++++++++---
 src/utils.cpp                       |  4 ++--
 4 files changed, 35 insertions(+), 10 deletions(-)

diff --git a/include/msckf_vio/image_processor.h b/include/msckf_vio/image_processor.h
index 236be1a..1004086 100644
--- a/include/msckf_vio/image_processor.h
+++ b/include/msckf_vio/image_processor.h
@@ -312,7 +312,7 @@ private:
       const std::vector<unsigned char>& markers,
       std::vector<T>& refined_vec) {
     if (raw_vec.size() != markers.size()) {
-      ROS_WARN("The input size of raw_vec(%lu) and markers(%lu) does not match...",
+      ROS_WARN("The input size of raw_vec(%i) and markers(%i) does not match...",
           raw_vec.size(), markers.size());
     }
     for (int i = 0; i < markers.size(); ++i) {
@@ -367,8 +367,8 @@ private:
   boost::shared_ptr<GridFeatures> prev_features_ptr;
   boost::shared_ptr<GridFeatures> curr_features_ptr;
 
-  cv::cuda::GpuMat curr_cam0_pyramid_gpu;
-  cv::cuda::GpuMat curr_cam1_pyramid_gpu;
+  cv::cuda::GpuMat cam0_curr_img;
+  cv::cuda::GpuMat cam1_curr_img;
   cv::cuda::GpuMat cam0_points_gpu;
   cv::cuda::GpuMat cam1_points_gpu;
 
diff --git a/include/msckf_vio/utils.h b/include/msckf_vio/utils.h
index 0f2a0c6..cf0d39a 100644
--- a/include/msckf_vio/utils.h
+++ b/include/msckf_vio/utils.h
@@ -22,8 +22,8 @@ namespace msckf_vio {
  */
 namespace utils {
 
-static void download(const cv::cuda::GpuMat& d_mat, std::vector<uchar>& vec);
-static void download(const cv::cuda::GpuMat& d_mat, std::vector<cv::Point2f>& vec);
+void download(const cv::cuda::GpuMat& d_mat, std::vector<uchar>& vec);
+void download(const cv::cuda::GpuMat& d_mat, std::vector<cv::Point2f>& vec);
 
 Eigen::Isometry3d getTransformEigen(const ros::NodeHandle &nh,
                                     const std::string &field);
diff --git a/src/image_processor.cpp b/src/image_processor.cpp
index 28363c5..ba220b1 100644
--- a/src/image_processor.cpp
+++ b/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]);
diff --git a/src/utils.cpp b/src/utils.cpp
index 1545566..830d84f 100644
--- a/src/utils.cpp
+++ b/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]);