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

include/msckf_vio/feature.hpp

@@ -144,8 +144,6 @@ 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/msckf_vio.h

@@ -14,14 +14,10 @@
 #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>
@@ -31,11 +27,6 @@
 #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
@@ -112,12 +103,6 @@ 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.
@@ -141,11 +126,6 @@ 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(
@@ -199,15 +179,6 @@ 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;
 
@@ -235,18 +206,12 @@ 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;

src/msckf_vio.cpp

@@ -53,7 +53,6 @@ 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;
 }
@@ -187,16 +186,8 @@ 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);
@@ -254,119 +245,6 @@ 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.
@@ -412,6 +290,7 @@ 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.
@@ -469,9 +348,8 @@ bool MsckfVio::resetCallback(
   // Restart the subscribers.
   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);
 
   // TODO: When can the reset fail?
   res.success = true;
@@ -876,7 +754,6 @@ void MsckfVio::stateAugmentation(const double& time) {
   return;
 }
 
-
 void MsckfVio::addFeatureObservations(
     const CameraMeasurementConstPtr& msg) {
 
@@ -1421,8 +1298,6 @@ 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;