added branch

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>
@@ -308,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) {
@@ -363,6 +367,13 @@ 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;
+  cv::cuda::GpuMat cam1_points_gpu;
+
   // Number of features after each outlier removal step.
   int before_tracking;
   int after_tracking;

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

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;
 }
@@ -200,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...");
 
@@ -219,7 +230,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) {
@@ -296,6 +309,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 +317,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 +471,8 @@ void ImageProcessor::trackFeatures() {
   predictFeatureTracking(prev_cam0_points,
       cam0_R_p_c, cam0_intrinsics, curr_cam0_points);
 
+  //TODO: test change to sparse
+  /*
   calcOpticalFlowPyrLK(
       prev_cam0_pyramid_, curr_cam0_pyramid_,
       prev_cam0_points, curr_cam0_points,
@@ -466,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.
@@ -625,7 +661,24 @@ void ImageProcessor::stereoMatch(
                                 cam1_distortion_model, cam1_distortion_coeffs);
   }
 
+  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(),
@@ -635,7 +688,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) {
@@ -995,7 +1048,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,6 +11,20 @@
 namespace msckf_vio {
 namespace utils {
 
+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);
+}
+
+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;