full revert of master to original msckf. parallelization is now in cuda branch, photometic expansion in photometry branch

include/msckf_vio/image_processor.h

@@ -14,10 +14,6 @@
 #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>
@@ -312,7 +308,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(%i) and markers(%i) does not match...",
+      ROS_WARN("The input size of raw_vec(%lu) and markers(%lu) does not match...",
           raw_vec.size(), markers.size());
     }
     for (int i = 0; i < markers.size(); ++i) {
@@ -367,13 +363,6 @@ 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,9 +11,6 @@
 #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 {
@@ -21,10 +18,6 @@ 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,10 +170,6 @@ 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;
 }
@@ -204,13 +200,6 @@ 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...");
 
@@ -230,9 +219,7 @@ 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) {
@@ -309,7 +296,6 @@ 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),
@@ -317,7 +303,6 @@ 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),
@@ -471,8 +456,6 @@ 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,
@@ -483,25 +466,6 @@ 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.
@@ -661,24 +625,7 @@ 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(),
@@ -688,7 +635,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) {
@@ -1048,7 +995,7 @@ void ImageProcessor::twoPointRansac(
 
   // Check the size of input point size.
   if (pts1.size() != pts2.size())
-    ROS_ERROR("Sets of different size (%i and %i) are used...",
+    ROS_ERROR("Sets of different size (%lu and %lu) are used...",
         pts1.size(), pts2.size());
 
   double norm_pixel_unit = 2.0 / (intrinsics[0]+intrinsics[1]);

src/utils.cpp

@@ -11,20 +11,6 @@
 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;