From ea93237912fdccb86fe37827feec011e96863f5c Mon Sep 17 00:00:00 2001
From: g-spacewhale <r.maenle@gmail.com>
Date: Fri, 15 Mar 2019 07:47:29 +0000
Subject: [PATCH] initial commit

---
 CMakeLists.txt                      |  2 +-
 include/msckf_vio/image_processor.h |  9 +++++++++
 include/msckf_vio/utils.h           |  7 +++++++
 src/image_processor.cpp             |  7 +++++++
 src/utils.cpp                       | 14 ++++++++++++++
 5 files changed, 38 insertions(+), 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index bb741cb..d81aed6 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -29,7 +29,7 @@ find_package(catkin REQUIRED COMPONENTS
 ## System dependencies are found with CMake's conventions
 find_package(Boost REQUIRED)
 find_package(Eigen3 REQUIRED)
-find_package(OpenCV REQUIRED)
+find_package(OpenCV 3.1 EXACT REQUIRED)
 find_package(SuiteSparse REQUIRED)
 
 ##################
diff --git a/include/msckf_vio/image_processor.h b/include/msckf_vio/image_processor.h
index 086c5a9..236be1a 100644
--- a/include/msckf_vio/image_processor.h
+++ b/include/msckf_vio/image_processor.h
@@ -14,6 +14,10 @@
 #include <opencv2/opencv.hpp>
 #include <opencv2/video.hpp>
 
+#include <opencv2/cudaoptflow.hpp>
+#include <opencv2/cudaimgproc.hpp>
+#include <opencv2/cudaarithm.hpp>
+
 #include <ros/ros.h>
 #include <cv_bridge/cv_bridge.h>
 #include <image_transport/image_transport.h>
@@ -363,6 +367,11 @@ 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_points_gpu;
+  cv::cuda::GpuMat cam1_points_gpu;
+
   // Number of features after each outlier removal step.
   int before_tracking;
   int after_tracking;
diff --git a/include/msckf_vio/utils.h b/include/msckf_vio/utils.h
index 97f0dac..0f2a0c6 100644
--- a/include/msckf_vio/utils.h
+++ b/include/msckf_vio/utils.h
@@ -11,6 +11,9 @@
 #include <ros/ros.h>
 #include <string>
 #include <opencv2/core/core.hpp>
+#include <opencv2/cudaoptflow.hpp>
+#include <opencv2/cudaimgproc.hpp>
+#include <opencv2/cudaarithm.hpp>
 #include <Eigen/Geometry>
 
 namespace msckf_vio {
@@ -18,6 +21,10 @@ namespace msckf_vio {
  * @brief utilities for 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);
+
 Eigen::Isometry3d getTransformEigen(const ros::NodeHandle &nh,
                                     const std::string &field);
 
diff --git a/src/image_processor.cpp b/src/image_processor.cpp
index a5039f3..28363c5 100644
--- a/src/image_processor.cpp
+++ b/src/image_processor.cpp
@@ -170,6 +170,10 @@ bool ImageProcessor::loadParameters() {
       processor_config.ransac_threshold);
   ROS_INFO("stereo_threshold: %f",
       processor_config.stereo_threshold);
+  ROS_INFO("OpenCV Major Version: %d",
+    CV_MAJOR_VERSION);
+  ROS_INFO("OpenCV Minor Version: %d",
+    CV_MINOR_VERSION);
   ROS_INFO("===========================================");
   return true;
 }
@@ -296,6 +300,7 @@ void ImageProcessor::imuCallback(
 
 void ImageProcessor::createImagePyramids() {
   const Mat& curr_cam0_img = cam0_curr_img_ptr->image;
+  // TODO: build cuda optical flow
   buildOpticalFlowPyramid(
       curr_cam0_img, curr_cam0_pyramid_,
       Size(processor_config.patch_size, processor_config.patch_size),
@@ -303,6 +308,7 @@ void ImageProcessor::createImagePyramids() {
       BORDER_CONSTANT, false);
 
   const Mat& curr_cam1_img = cam1_curr_img_ptr->image;
+  // TODO: build cuda optical flow
   buildOpticalFlowPyramid(
       curr_cam1_img, curr_cam1_pyramid_,
       Size(processor_config.patch_size, processor_config.patch_size),
@@ -456,6 +462,7 @@ void ImageProcessor::trackFeatures() {
   predictFeatureTracking(prev_cam0_points,
       cam0_R_p_c, cam0_intrinsics, curr_cam0_points);
 
+  //TODO: change to GPU
   calcOpticalFlowPyrLK(
       prev_cam0_pyramid_, curr_cam0_pyramid_,
       prev_cam0_points, curr_cam0_points,
diff --git a/src/utils.cpp b/src/utils.cpp
index 2af7278..1545566 100644
--- a/src/utils.cpp
+++ b/src/utils.cpp
@@ -11,6 +11,20 @@
 namespace msckf_vio {
 namespace utils {
 
+static 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)
+{
+    vec.resize(d_mat.cols);
+    cv::Mat mat(1, d_mat.cols, CV_8UC1, (void*)&vec[0]);
+    d_mat.download(mat);
+}
+
 Eigen::Isometry3d getTransformEigen(const ros::NodeHandle &nh,
                                     const std::string &field) {
   Eigen::Isometry3d T;