added moving window structure, not yet done. added timestame sync for images and features detected

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,
-      &MsckfVio::featureCallback, this);
+  //feature_sub = nh.subscribe("features", 40,
+  //    &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;