initial commit

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

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;

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

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,

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;