reverted master to original

include/msckf_vio/feature.hpp

@@ -144,6 +144,8 @@ struct Feature {
     Eigen::aligned_allocator<
       std::pair<const StateIDType, Eigen::Vector4d> > > observations;
 
+  // NxN Patch of Anchor Image
+  std::vector<double> patch;
   // 3d postion of the feature in the world frame.
   Eigen::Vector3d position;
 

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,11 +363,6 @@ private:
   boost::shared_ptr<GridFeatures> prev_features_ptr;
   boost::shared_ptr<GridFeatures> curr_features_ptr;
 
-  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/msckf_vio.h

@@ -14,10 +14,14 @@
 #include <string>
 #include <Eigen/Dense>
 #include <Eigen/Geometry>
+
 #include <boost/shared_ptr.hpp>
+#include <opencv2/opencv.hpp>
+#include <opencv2/video.hpp>
 
 #include <ros/ros.h>
 #include <sensor_msgs/Imu.h>
+#include <sensor_msgs/Image.h>
 #include <nav_msgs/Odometry.h>
 #include <tf/transform_broadcaster.h>
 #include <std_srvs/Trigger.h>
@@ -27,6 +31,11 @@
 #include "feature.hpp"
 #include <msckf_vio/CameraMeasurement.h>
 
+#include <cv_bridge/cv_bridge.h>
+#include <image_transport/image_transport.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/time_synchronizer.h>
+
 namespace msckf_vio {
 /*
  * @brief MsckfVio Implements the algorithm in
@@ -103,6 +112,12 @@ class MsckfVio {
      */
     void featureCallback(const CameraMeasurementConstPtr& msg);
 
+
+    void imageCallback (
+    const sensor_msgs::ImageConstPtr& cam0_img,
+    const sensor_msgs::ImageConstPtr& cam1_img,
+    const CameraMeasurementConstPtr& feature_msg);
+
     /*
      * @brief publish Publish the results of VIO.
      * @param time The time stamp of output msgs.
@@ -126,6 +141,11 @@ class MsckfVio {
     bool resetCallback(std_srvs::Trigger::Request& req,
         std_srvs::Trigger::Response& res);
 
+    void manageMovingWindow(
+        const cv_bridge::CvImageConstPtr& cam0_image_ptr,
+        const cv_bridge::CvImageConstPtr& cam1_image_ptr,
+        const CameraMeasurementConstPtr& feature_msg);
+
     // Filter related functions
     // Propogate the state
     void batchImuProcessing(
@@ -179,6 +199,15 @@ class MsckfVio {
     // transfer delay between IMU and Image messages.
     std::vector<sensor_msgs::Imu> imu_msg_buffer;
 
+    // Moving Window buffer
+    std::map<StateIDType, cv::Mat, std::less<StateIDType>,
+        Eigen::aligned_allocator<
+        std::pair<StateIDType, cv::Mat> > > cam0_moving_window;
+
+    std::map<StateIDType, cv::Mat, std::less<StateIDType>,
+        Eigen::aligned_allocator<
+        std::pair<StateIDType, cv::Mat> > > cam1_moving_window;
+    
     // Indicate if the gravity vector is set.
     bool is_gravity_set;
 
@@ -206,12 +235,18 @@ class MsckfVio {
 
     // Subscribers and publishers
     ros::Subscriber imu_sub;
-    ros::Subscriber feature_sub;
     ros::Publisher odom_pub;
     ros::Publisher feature_pub;
     tf::TransformBroadcaster tf_pub;
     ros::ServiceServer reset_srv;
 
+
+    message_filters::Subscriber<sensor_msgs::Image> cam0_img_sub;
+    message_filters::Subscriber<sensor_msgs::Image> cam1_img_sub;
+    message_filters::Subscriber<CameraMeasurement> feature_sub;
+
+    message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::Image, CameraMeasurement> image_sub;
+    
     // Frame id
     std::string fixed_frame_id;
     std::string child_frame_id;

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;
 }
@@ -223,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) {
@@ -302,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),
@@ -310,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),
@@ -464,7 +456,6 @@ 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,
@@ -634,30 +625,7 @@ 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(),
@@ -667,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) {
@@ -1027,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/msckf_vio.cpp

@@ -53,6 +53,7 @@ map<int, double> MsckfVio::chi_squared_test_table;
 MsckfVio::MsckfVio(ros::NodeHandle& pnh):
   is_gravity_set(false),
   is_first_img(true),
+  image_sub(10),
   nh(pnh) {
   return;
 }
@@ -186,8 +187,16 @@ bool MsckfVio::createRosIO() {
 
   imu_sub = nh.subscribe("imu", 100,
       &MsckfVio::imuCallback, this);
-  feature_sub = nh.subscribe("features", 40,
+  //feature_sub = nh.subscribe("features", 40,
-      &MsckfVio::featureCallback, this);
+  //    &MsckfVio::featureCallback, this);
+
+  cam0_img_sub.subscribe(nh, "cam0_image", 10);
+  cam1_img_sub.subscribe(nh, "cam1_image", 10);
+  feature_sub.subscribe(nh, "features", 10);
+
+  image_sub.connectInput(cam0_img_sub, cam1_img_sub, feature_sub);
+  image_sub.registerCallback(&MsckfVio::imageCallback, this);
+
 
   mocap_odom_sub = nh.subscribe("mocap_odom", 10,
       &MsckfVio::mocapOdomCallback, this);
@@ -245,6 +254,119 @@ void MsckfVio::imuCallback(
   return;
 }
 
+void MsckfVio::imageCallback (
+    const sensor_msgs::ImageConstPtr& cam0_img,
+    const sensor_msgs::ImageConstPtr& cam1_img,
+    const CameraMeasurementConstPtr& feature_msg)
+{
+  ROS_INFO("In Callback");
+
+  // Return if the gravity vector has not been set.
+  if (!is_gravity_set) return;
+
+    // Get the current image.
+  cv_bridge::CvImageConstPtr cam0_image_ptr = cv_bridge::toCvShare(cam0_img,
+      sensor_msgs::image_encodings::MONO8);
+  cv_bridge::CvImageConstPtr cam1_img_ptr = cv_bridge::toCvShare(cam1_img,
+      sensor_msgs::image_encodings::MONO8);
+
+  // Start the system if the first image is received.
+  // The frame where the first image is received will be
+  // the origin.
+  if (is_first_img) {
+    is_first_img = false;
+    state_server.imu_state.time = feature_msg->header.stamp.toSec();
+  }
+
+  static double max_processing_time = 0.0;
+  static int critical_time_cntr = 0;
+  double processing_start_time = ros::Time::now().toSec();
+
+  // Propogate the IMU state.
+  // that are received before the image feature_msg.
+  ros::Time start_time = ros::Time::now();
+  batchImuProcessing(feature_msg->header.stamp.toSec());
+  double imu_processing_time = (
+      ros::Time::now()-start_time).toSec();
+
+  // Augment the state vector.
+  start_time = ros::Time::now();
+  stateAugmentation(feature_msg->header.stamp.toSec());
+  double state_augmentation_time = (
+      ros::Time::now()-start_time).toSec();
+
+  // Add new observations for existing features or new
+  // features in the map server.
+  start_time = ros::Time::now();
+  addFeatureObservations(feature_msg);
+  double add_observations_time = (
+      ros::Time::now()-start_time).toSec();
+
+  // Add new images to moving window
+  start_time = ros::Time::now();
+  manageMovingWindow(cam0_image_ptr, cam1_img_ptr, feature_msg);
+  double manage_moving_window_time = (
+      ros::Time::now()-start_time).toSec();
+
+  // Perform measurement update if necessary.
+  start_time = ros::Time::now();
+  removeLostFeatures();
+  double remove_lost_features_time = (
+      ros::Time::now()-start_time).toSec();
+
+  start_time = ros::Time::now();
+  pruneCamStateBuffer();
+  double prune_cam_states_time = (
+      ros::Time::now()-start_time).toSec();
+
+  // Publish the odometry.
+  start_time = ros::Time::now();
+  publish(feature_msg->header.stamp);
+  double publish_time = (
+      ros::Time::now()-start_time).toSec();
+
+  // Reset the system if necessary.
+  onlineReset();
+
+  double processing_end_time = ros::Time::now().toSec();
+  double processing_time =
+    processing_end_time - processing_start_time;
+  if (processing_time > 1.0/frame_rate) {
+    ++critical_time_cntr;
+    //ROS_INFO("\033[1;31mTotal processing time %f/%d...\033[0m",
+    //    processing_time, critical_time_cntr);
+    //printf("IMU processing time: %f/%f\n",
+    //    imu_processing_time, imu_processing_time/processing_time);
+    //printf("State augmentation time: %f/%f\n",
+    //    state_augmentation_time, state_augmentation_time/processing_time);
+    //printf("Add observations time: %f/%f\n",
+    //    add_observations_time, add_observations_time/processing_time);
+    //printf("Remove lost features time: %f/%f\n",
+    //    remove_lost_features_time, remove_lost_features_time/processing_time);
+    //printf("Remove camera states time: %f/%f\n",
+    //    prune_cam_states_time, prune_cam_states_time/processing_time);
+    //printf("Publish time: %f/%f\n",
+    //    publish_time, publish_time/processing_time);
+  }
+
+  return;
+}
+
+void MsckfVio::manageMovingWindow(
+    const cv_bridge::CvImageConstPtr& cam0_image_ptr,
+    const cv_bridge::CvImageConstPtr& cam1_image_ptr,
+    const CameraMeasurementConstPtr& feature_msg) {
+
+  cam0_moving_window[state_server.imu_state.id] = cam0_image_ptr->image;
+  cam1_moving_window[state_server.imu_state.id] = cam1_image_ptr->image;
+
+  while(cam0_moving_window.size() > 100)
+  {
+    cam1_moving_window.erase(cam1_moving_window.begin());
+    cam0_moving_window.erase(cam0_moving_window.begin());
+  }
+}
+
 void MsckfVio::initializeGravityAndBias() {
 
   // Initialize gravity and gyro bias.
@@ -290,7 +412,6 @@ bool MsckfVio::resetCallback(
   ROS_WARN("Start resetting msckf vio...");
   // Temporarily shutdown the subscribers to prevent the
   // state from updating.
-  feature_sub.shutdown();
   imu_sub.shutdown();
 
   // Reset the IMU state.
@@ -348,10 +469,11 @@ bool MsckfVio::resetCallback(
   // Restart the subscribers.
   imu_sub = nh.subscribe("imu", 100,
       &MsckfVio::imuCallback, this);
-  feature_sub = nh.subscribe("features", 40,
-      &MsckfVio::featureCallback, this);
 
-  // TODO: When can the reset fail?
+//  feature_sub = nh.subscribe("features", 40,
+//      &MsckfVio::featureCallback, this);
+
+// TODO: When can the reset fail?
   res.success = true;
   ROS_WARN("Resetting msckf vio completed...");
   return true;
@@ -754,6 +876,7 @@ void MsckfVio::stateAugmentation(const double& time) {
   return;
 }
 
+
 void MsckfVio::addFeatureObservations(
     const CameraMeasurementConstPtr& msg) {
 
@@ -1298,6 +1421,8 @@ void MsckfVio::pruneCamStateBuffer() {
 
     // Remove this camera state in the state vector.
     state_server.cam_states.erase(cam_id);
+    cam0_moving_window.erase(cam_id);
+    cam1_moving_window.erase(cam_id);
   }
 
   return;

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;