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

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);