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

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;