commented visu parts cleanly

removed visu as result works so to not clutter the output
image reprojection visualization in images
2019-04-19 13:32:16 +02:00 · 2019-04-19 13:30:30 +02:00 · 2019-04-19 13:11:19 +02:00 · 2019-04-18 16:22:41 +02:00 · 2019-04-18 11:06:45 +02:00 · 2019-04-17 17:06:44 +02:00
22 changed files with 1421 additions and 350 deletions
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -79,6 +79,7 @@ include_directories(
 add_library(msckf_vio
  src/msckf_vio.cpp
  src/utils.cpp
+  src/image_handler.cpp
 )
 add_dependencies(msckf_vio
  ${${PROJECT_NAME}_EXPORTED_TARGETS}
@@ -87,6 +88,7 @@ add_dependencies(msckf_vio
 target_link_libraries(msckf_vio
  ${catkin_LIBRARIES}
  ${SUITESPARSE_LIBRARIES}
+  ${OpenCV_LIBRARIES}
 )

 # Msckf Vio nodelet
@@ -106,6 +108,7 @@ target_link_libraries(msckf_vio_nodelet
 add_library(image_processor
  src/image_processor.cpp
  src/utils.cpp
+  src/image_handler.cpp
 )
 add_dependencies(image_processor
  ${${PROJECT_NAME}_EXPORTED_TARGETS}
--- a/config/camchain-imucam-tum.yaml
+++ b/config/camchain-imucam-tum.yaml
@@ -0,0 +1,36 @@
+cam0:
+  T_cam_imu:
+      [-0.9995378259923383,   0.02917807204183088,  -0.008530798463872679, 0.047094247958417004,
+      0.007526588843243184,   -0.03435493139706542, -0.9993813532126198,   -0.04788273017221637,
+      -0.029453096117288798,  -0.9989836729399656,  0.034119442089241274,  -0.0697294754693238,
+      0.0,                    0.0,                  0.0,                   1.0]
+  camera_model: pinhole
+  distortion_coeffs: [0.010171079892421483, -0.010816440029919381, 0.005942781769412756,
+    -0.001662284667857643]
+  distortion_model: equidistant
+  intrinsics: [380.81042871360756, 380.81194179427075, 510.29465304840727, 514.3304630538506]
+  resolution: [1024, 1024]
+  rostopic: /cam0/image_raw
+cam1:
+  T_cam_imu:
+    [-0.9995240747493029, 0.02986739485347808, -0.007717688852024281, -0.05374086123613335,
+    0.008095979457928231, 0.01256553460985914, -0.9998882749870535, -0.04648588412432889,
+    -0.02976708103202316, -0.9994748851595197, -0.0128013601698453, -0.07333210787623645,
+    0.0, 0.0, 0.0, 1.0]
+  T_cn_cnm1:
+    [0.9999994317488622, -0.0008361847221513937, -0.0006612844045898121, -0.10092123225528335,
+    0.0008042457277382264, 0.9988989443471681, -0.04690684567228134, -0.001964540595211977,
+    0.0006997790813734836, 0.04690628718225568, 0.9988990492196964, -0.0014663556043866572,
+    0.0, 0.0, 0.0, 1.0]
+  camera_model: pinhole
+  distortion_coeffs: [0.01371679169245271, -0.015567360615942622, 0.00905043103315326,
+    -0.002347858896562788]
+  distortion_model: equidistant
+  intrinsics: [379.2869884263036, 379.26583742214524, 505.5666703237407, 510.2840961765407]
+  resolution: [1024, 1024]
+  rostopic: /cam1/image_raw
+T_imu_body:
+  [1.0000, 0.0000, 0.0000, 0.0000,
+  0.0000, 1.0000, 0.0000, 0.0000,
+  0.0000, 0.0000, 1.0000, 0.0000,
+  0.0000, 0.0000, 0.0000, 1.0000]
--- a/include/msckf_vio/cam_state.h
+++ b/include/msckf_vio/cam_state.h
@@ -15,6 +15,34 @@
 #include "imu_state.h"

 namespace msckf_vio {
+
+struct Frame{
+  cv::Mat image;
+  double exposureTime_ms;
+};
+
+typedef std::map<StateIDType, Frame, std::less<StateIDType>,
+        Eigen::aligned_allocator<
+        std::pair<StateIDType, Frame> > > movingWindow;
+
+struct IlluminationParameter{
+  double frame_bias;
+  double frame_gain;
+  double feature_bias;
+  double feature_gain;
+};
+
+struct CameraCalibration{
+  std::string distortion_model;
+  cv::Vec2i resolution;
+  cv::Vec4d intrinsics;
+  cv::Vec4d distortion_coeffs;
+  movingWindow moving_window;
+  cv::Mat featureVisu;
+};
+
+
+
 /*
 * @brief CAMState Stored camera state in order to
 *    form measurement model.
@@ -35,6 +63,9 @@ struct CAMState {
  // Position of the camera frame in the world frame.
  Eigen::Vector3d position;

+  // Illumination Information of the frame
+  IlluminationParameter illumination; 
+
  // These two variables should have the same physical
  // interpretation with `orientation` and `position`.
  // There two variables are used to modify the measurement
--- a/include/msckf_vio/feature.hpp
+++ b/include/msckf_vio/feature.hpp
@@ -16,6 +16,8 @@
 #include <Eigen/Geometry>
 #include <Eigen/StdVector>

+#include "image_handler.h"
+
 #include "math_utils.hpp"
 #include "imu_state.h"
 #include "cam_state.h"
@@ -57,11 +59,11 @@ struct Feature {

  // Constructors for the struct.
  Feature(): id(0), position(Eigen::Vector3d::Zero()),
-    is_initialized(false) {}
+    is_initialized(false), is_anchored(false) {}

  Feature(const FeatureIDType& new_id): id(new_id),
    position(Eigen::Vector3d::Zero()),
-    is_initialized(false) {}
+    is_initialized(false), is_anchored(false) {}

  /*
   * @brief cost Compute the cost of the camera observations
@@ -114,6 +116,19 @@ struct Feature {
  inline bool checkMotion(
      const CamStateServer& cam_states) const;

+  /*
+   * @brief InitializeAnchor generates the NxN patch around the
+   *        feature in the Anchor image
+   * @param cam_states: A map containing all recorded images 
+   *        currently presented in the camera state vector
+   * @return the irradiance of the Anchor NxN Patch 
+   * @return True if the Anchor can be estimated
+   */
+
+  bool initializeAnchor(
+   const CameraCalibration& cam);
+
+
  /*
   * @brief InitializePosition Intialize the feature position
   *    based on all current available measurements.
@@ -128,6 +143,48 @@ struct Feature {
  inline bool initializePosition(
      const CamStateServer& cam_states);

+/*
+*  @brief project PositionToCamera Takes a 3d position in a world frame
+*         and projects it into the passed camera frame using pinhole projection
+*         then distorts it using camera information to get 
+*         the resulting distorted pixel position  
+*/
+  inline cv::Point2f projectPositionToCamera(
+                  const CAMState& cam_state,
+                  const StateIDType& cam_state_id,
+                  const CameraCalibration& cam,
+                  Eigen::Vector3d& in_p) const;
+
+  /*
+  * @brief IrradianceAnchorPatch_Camera returns irradiance values
+  *        of the Anchor Patch position in a camera frame
+  *
+  */
+
+  bool estimate_FrameIrradiance(
+                  const CAMState& cam_state,
+                  const StateIDType& cam_state_id,
+                  CameraCalibration& cam0,
+                  std::vector<float>& anchorPatch_estimate) const;
+
+  bool FrameIrradiance(
+                  const CAMState& cam_state,
+                  const StateIDType& cam_state_id,
+                  CameraCalibration& cam0,
+                  std::vector<float>& anchorPatch_measurement) const;
+
+  /*
+  * @brief projectPixelToPosition uses the calcualted pixels
+  *     of the anchor patch to generate 3D positions of all of em
+  */
+inline Eigen::Vector3d projectPixelToPosition(cv::Point2f in_p,
+          const CameraCalibration& cam);
+
+  /*
+  * @brief Irradiance returns irradiance value of a pixel
+  */
+
+  inline float PixelIrradiance(cv::Point2f pose, cv::Mat image) const;

  // An unique identifier for the feature.
  // In case of long time running, the variable
@@ -144,13 +201,28 @@ struct Feature {
    Eigen::aligned_allocator<
      std::pair<const StateIDType, Eigen::Vector4d> > > observations;

+  // NxN Patch of Anchor Image
+  std::vector<double> anchorPatch;
+
+  // Position of NxN Patch in 3D space
+  std::vector<Eigen::Vector3d> anchorPatch_3d;
+
+  // Anchor Isometry
+  Eigen::Isometry3d T_anchor_w;
+
  // 3d postion of the feature in the world frame.
  Eigen::Vector3d position;

+  // inverse depth representation
+  double anchor_rho;
+
  // A indicator to show if the 3d postion of the feature
  // has been initialized or not.
  bool is_initialized;
+  bool is_anchored;

+  cv::Point2f anchor_center_pos;
+  cv::Point2f undist_anchor_center_pos;
  // Noise for a normalized feature measurement.
  static double observation_noise;

@@ -290,8 +362,207 @@ bool Feature::checkMotion(
  else return false;
 }

+bool Feature::estimate_FrameIrradiance(
+                  const CAMState& cam_state,
+                  const StateIDType& cam_state_id,
+                  CameraCalibration& cam0,
+                  std::vector<float>& anchorPatch_estimate) const
+{
+  // get irradiance of patch in anchor frame
+  // subtract estimated b and divide by a of anchor frame
+  // muliply by a and add b of this frame
+
+  auto anchor = observations.begin();
+  if(cam0.moving_window.find(anchor->first) == cam0.moving_window.end())
+    return false;
+
+  double anchorExposureTime_ms = cam0.moving_window.find(anchor->first)->second.exposureTime_ms;
+  double frameExposureTime_ms = cam0.moving_window.find(cam_state_id)->second.exposureTime_ms;
+
+
+  double a_A = anchorExposureTime_ms;
+  double b_A = 0;
+  double a_l =frameExposureTime_ms;
+  double b_l = 0;
+  
+  //printf("frames: %lld, %lld\n", anchor->first, cam_state_id);
+  //printf("exposure: %f, %f\n", a_A, a_l);
+  
+  for (double anchorPixel : anchorPatch)
+  {
+    float irradiance = ((anchorPixel - b_A) / a_A ) * a_l - b_l;
+    anchorPatch_estimate.push_back(irradiance);
+  }
+
+}
+
+bool Feature::FrameIrradiance(
+                  const CAMState& cam_state,
+                  const StateIDType& cam_state_id,
+                  CameraCalibration& cam0,
+                  std::vector<float>& anchorPatch_measurement) const
+{
+
+  // visu - feature
+  /*cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image;
+  cv::Mat dottedFrame(current_image.size(), CV_8UC3);
+  cv::cvtColor(current_image, dottedFrame, CV_GRAY2RGB);
+  */
+
+  // project every point in anchorPatch_3d.
+  for (auto point : anchorPatch_3d)
+  {
+
+    cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point);
+
+    // visu - feature
+    /*cv::Point xs(p_in_c0.x, p_in_c0.y);
+    cv::Point ys(p_in_c0.x, p_in_c0.y);
+    cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,0));
+  */
+    float irradiance = PixelIrradiance(p_in_c0, cam0.moving_window.find(cam_state_id)->second.image);
+    anchorPatch_measurement.push_back(irradiance);
+    
+    // testing
+    //if(cam_state_id == observations.begin()->first)
+      //if(count++ == 4)
+        //printf("dist:\n \tpos: %f, %f\n\ttrue pos: %f, %f\n\n", p_in_c0.x, p_in_c0.y, anchor_center_pos.x, anchor_center_pos.y);
+    
+  }
+
+  // visu - feature
+  //cv::resize(dottedFrame, dottedFrame, cv::Size(dottedFrame.cols*0.2, dottedFrame.rows*0.2));
+  /*if(cam0.featureVisu.empty())
+    cam0.featureVisu = dottedFrame.clone();
+  else
+    cv::hconcat(cam0.featureVisu, dottedFrame, cam0.featureVisu);
+  */
+
+}
+
+float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const
+{
+  return (float)image.at<uint8_t>(pose.x, pose.y);
+}
+
+cv::Point2f Feature::projectPositionToCamera(
+                  const CAMState& cam_state,
+                  const StateIDType& cam_state_id,
+                  const CameraCalibration& cam,
+                  Eigen::Vector3d& in_p) const
+{
+  Eigen::Isometry3d T_c0_w;
+
+  cv::Point2f out_p;
+
+  // transfrom position to camera frame
+  Eigen::Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation);
+  const Eigen::Vector3d& t_c0_w = cam_state.position;
+  Eigen::Vector3d p_c0 = R_w_c0 * (in_p-t_c0_w);
+
+  out_p = cv::Point2f(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2));
+
+ // if(cam_state_id == observations.begin()->first)
+      //printf("undist:\n  \tproj pos: %f, %f\n\ttrue pos: %f, %f\n", out_p.x, out_p.y, undist_anchor_center_pos.x, undist_anchor_center_pos.y);
+ 
+  cv::Point2f my_p = image_handler::distortPoint(out_p, 
+                                                 cam.intrinsics, 
+                                                 cam.distortion_model, 
+                                                 cam.distortion_coeffs);
+
+  // printf("truPosition: %f, %f, %f\n", position.x(), position.y(), position.z());
+  // printf("camPosition: %f, %f, %f\n", p_c0(0), p_c0(1), p_c0(2));
+  // printf("Photo projection: %f, %f\n", my_p[0].x,  my_p[0].y);
+  
+  return my_p;
+}
+
+Eigen::Vector3d Feature::projectPixelToPosition(cv::Point2f in_p,
+          const CameraCalibration& cam)
+{
+  // use undistorted position of point of interest
+  // project it back into 3D space using pinhole model
+  // save resulting NxN positions for this feature
+
+  Eigen::Vector3d PositionInCamera(in_p.x/anchor_rho, in_p.y/anchor_rho, 1/anchor_rho);
+  Eigen::Vector3d PositionInWorld= T_anchor_w.linear()*PositionInCamera + T_anchor_w.translation();
+  return PositionInWorld;
+  //printf("%f, %f, %f\n",PositionInWorld[0], PositionInWorld[1], PositionInWorld[2]);
+}
+
+//@test center projection must always be initial feature projection
+bool Feature::initializeAnchor(
+   const CameraCalibration& cam)
+{
+
+  //initialize patch Size
+  //TODO make N size a ros parameter
+  int N = 3;
+  int n = (int)(N-1)/2;
+
+  auto anchor = observations.begin();
+  if(cam.moving_window.find(anchor->first) == cam.moving_window.end())
+    return false;
+
+  cv::Mat anchorImage = cam.moving_window.find(anchor->first)->second.image;
+
+  auto u = anchor->second(0);//*cam.intrinsics[0] + cam.intrinsics[2];
+  auto v = anchor->second(1);//*cam.intrinsics[1] + cam.intrinsics[3];
+  
+  //testing
+  undist_anchor_center_pos = cv::Point2f(u,v);
+
+  //for NxN patch pixels around feature
+  int count = 0;
+
+  // get feature in undistorted pixel space
+  cv::Point2f und_pix_p = image_handler::distortPoint(cv::Point2f(u, v), 
+                                                    cam.intrinsics, 
+                                                    cam.distortion_model, 
+                                                    0);
+  // create vector of patch in pixel plane
+  std::vector<cv::Point2f> und_pix_v;
+  for(double u_run = -n; u_run <= n; u_run++)
+    for(double v_run = -n; v_run <= n; v_run++)
+      und_pix_v.push_back(cv::Point2f(und_pix_p.x-u_run, und_pix_p.y-v_run));
+  
+
+
+  //create undistorted pure points    
+  std::vector<cv::Point2f> und_v;
+  image_handler::undistortPoints(und_pix_v,
+                                cam.intrinsics,
+                                cam.distortion_model,
+                                0,
+                                und_v);
+//create distorted pixel points
+  std::vector<cv::Point2f> vec = image_handler::distortPoints(und_v,
+                                                             cam.intrinsics,
+                                                             cam.distortion_model,
+                                                             cam.distortion_coeffs);
+
+
+  // save anchor position for later visualisaztion
+  anchor_center_pos = vec[4];
+  for(auto point : vec)
+  {
+    if(point.x - n < 0 || point.x + n >= cam.resolution(0) || point.y - n < 0 || point.y + n >= cam.resolution(1))
+      return false;
+  }
+  for(auto point : vec)
+    anchorPatch.push_back((double)anchorImage.at<uint8_t>((int)point.x,(int)point.y));
+
+  // project patch pixel to 3D space
+  for(auto point : und_v)
+    anchorPatch_3d.push_back(projectPixelToPosition(point, cam));
+
+  is_anchored = true;
+  return true;
+}
+
 bool Feature::initializePosition(
    const CamStateServer& cam_states) {
+
  // Organize camera poses and feature observations properly.
  std::vector<Eigen::Isometry3d,
    Eigen::aligned_allocator<Eigen::Isometry3d> > cam_poses(0);
@@ -327,6 +598,7 @@ bool Feature::initializePosition(
  // vector from the first camera frame in the buffer to this
  // camera frame.
  Eigen::Isometry3d T_c0_w = cam_poses[0];
+  T_anchor_w = T_c0_w;
  for (auto& pose : cam_poses)
    pose = pose.inverse() * T_c0_w;

@@ -427,6 +699,9 @@ bool Feature::initializePosition(
    }
  }

+  //save inverse depth distance from camera
+  anchor_rho = solution(2);
+
  // Convert the feature position to the world frame.
  position = T_c0_w.linear()*final_position + T_c0_w.translation();

--- a/include/msckf_vio/image_handler.h
+++ b/include/msckf_vio/image_handler.h
@@ -0,0 +1,41 @@
+#ifndef MSCKF_VIO_IMAGE_HANDLER_H
+#define MSCKF_VIO_IMAGE_HANDLER_H
+
+#include <vector>
+#include <boost/shared_ptr.hpp>
+#include <opencv2/opencv.hpp>
+#include <opencv2/video.hpp>
+
+#include <ros/ros.h>
+#include <cv_bridge/cv_bridge.h>
+
+
+namespace msckf_vio {
+/*
+ * @brief utilities for msckf_vio
+ */
+namespace image_handler {
+
+void undistortPoints(
+    const std::vector<cv::Point2f>& pts_in,
+    const cv::Vec4d& intrinsics,
+    const std::string& distortion_model,
+    const cv::Vec4d& distortion_coeffs,
+    std::vector<cv::Point2f>& pts_out,
+    const cv::Matx33d &rectification_matrix = cv::Matx33d::eye(),
+    const cv::Vec4d &new_intrinsics = cv::Vec4d(1,1,0,0));
+
+std::vector<cv::Point2f> distortPoints(
+    const std::vector<cv::Point2f>& pts_in,
+    const cv::Vec4d& intrinsics,
+    const std::string& distortion_model,
+    const cv::Vec4d& distortion_coeffs);
+
+cv::Point2f distortPoint(
+    const cv::Point2f& pt_in,
+    const cv::Vec4d& intrinsics,
+    const std::string& distortion_model,
+    const cv::Vec4d& distortion_coeffs);
+}
+}
+#endif
--- a/include/msckf_vio/image_processor.h
+++ b/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>
@@ -26,6 +22,8 @@
 #include <message_filters/subscriber.h>
 #include <message_filters/time_synchronizer.h>

+#include "cam_state.h"
+
 namespace msckf_vio {

 /*
@@ -312,7 +310,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) {
@@ -336,15 +334,8 @@ private:
  std::vector<sensor_msgs::Imu> imu_msg_buffer;

  // Camera calibration parameters
-  std::string cam0_distortion_model;
-  cv::Vec2i cam0_resolution;
-  cv::Vec4d cam0_intrinsics;
-  cv::Vec4d cam0_distortion_coeffs;
-
-  std::string cam1_distortion_model;
-  cv::Vec2i cam1_resolution;
-  cv::Vec4d cam1_intrinsics;
-  cv::Vec4d cam1_distortion_coeffs;
+  CameraCalibration cam0;
+  CameraCalibration cam1;

  // Take a vector from cam0 frame to the IMU frame.
  cv::Matx33d R_cam0_imu;
@@ -367,13 +358,6 @@ private:
  boost::shared_ptr<GridFeatures> prev_features_ptr;
  boost::shared_ptr<GridFeatures> curr_features_ptr;

-  cv::Ptr<cv::cuda::SparsePyrLKOpticalFlow> d_pyrLK_sparse;
-
-  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;
--- a/include/msckf_vio/msckf_vio.h
+++ b/include/msckf_vio/msckf_vio.h
@@ -14,11 +14,17 @@
 #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 <sensor_msgs/PointCloud.h>
 #include <nav_msgs/Odometry.h>
+#include <std_msgs/Float64.h>
 #include <tf/transform_broadcaster.h>
 #include <std_srvs/Trigger.h>

@@ -27,6 +33,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
@@ -97,11 +108,18 @@ class MsckfVio {
    void imuCallback(const sensor_msgs::ImuConstPtr& msg);

    /*
-     * @brief featureCallback
-     *    Callback function for feature measurements.
-     * @param msg Stereo feature measurements.
+     * @brief imageCallback
+     *    Callback function for feature measurements
+     *    Triggers measurement update
+     * @param msg
+     *    Camera 0 Image
+     *    Camera 1 Image
+     *    Stereo feature measurements.
     */
-    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.
@@ -126,6 +144,11 @@ class MsckfVio {
    bool resetCallback(std_srvs::Trigger::Request& req,
        std_srvs::Trigger::Response& res);

+    void manageMovingWindow(
+        const sensor_msgs::ImageConstPtr& cam0_img,
+        const sensor_msgs::ImageConstPtr& cam1_img,
+        const CameraMeasurementConstPtr& feature_msg);
+
    // Filter related functions
    // Propogate the state
    void batchImuProcessing(
@@ -152,6 +175,20 @@ class MsckfVio {
    void featureJacobian(const FeatureIDType& feature_id,
        const std::vector<StateIDType>& cam_state_ids,
        Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
+
+
+    void PhotometricMeasurementJacobian(
+    const StateIDType& cam_state_id,
+    const FeatureIDType& feature_id,
+    Eigen::Matrix<double, 4, 6>& H_x,
+    Eigen::Matrix<double, 4, 3>& H_f,
+    Eigen::Vector4d& r);
+
+    void PhotometricFeatureJacobian(
+    const FeatureIDType& feature_id,
+    const std::vector<StateIDType>& cam_state_ids,
+    Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
+
    void measurementUpdate(const Eigen::MatrixXd& H,
        const Eigen::VectorXd& r);
    bool gatingTest(const Eigen::MatrixXd& H,
@@ -179,6 +216,18 @@ class MsckfVio {
    // transfer delay between IMU and Image messages.
    std::vector<sensor_msgs::Imu> imu_msg_buffer;

+    // Moving Window buffer
+    movingWindow cam0_moving_window;
+    movingWindow cam1_moving_window;
+
+    // Camera calibration parameters
+    CameraCalibration cam0;
+    CameraCalibration cam1;
+
+
+
+    ros::Time image_save_time;
+
    // Indicate if the gravity vector is set.
    bool is_gravity_set;

@@ -206,12 +255,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;
--- a/include/msckf_vio/utils.h
+++ b/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);

--- a/launch/image_processor_euroc.launch
+++ b/launch/image_processor_euroc.launch
@@ -8,7 +8,8 @@
  <group ns="$(arg robot)">
    <node pkg="nodelet" type="nodelet" name="image_processor"
      args="standalone msckf_vio/ImageProcessorNodelet"
-      output="screen">
+      output="screen"
+       >

      <rosparam command="load" file="$(arg calibration_file)"/>
      <param name="grid_row" value="4"/>
--- a/launch/image_processor_mynt.launch
+++ b/launch/image_processor_mynt.launch
@@ -0,0 +1,33 @@
+<launch>
+
+  <arg name="robot" default="firefly_sbx"/>
+  <arg name="calibration_file"
+    default="$(find msckf_vio)/config/camchain-imucam-mynt.yaml"/>
+
+  <!-- Image Processor Nodelet  -->
+  <group ns="$(arg robot)">
+    <node pkg="nodelet" type="nodelet" name="image_processor"
+      args="standalone msckf_vio/ImageProcessorNodelet"
+      output="screen">
+
+      <rosparam command="load" file="$(arg calibration_file)"/>
+      <param name="grid_row" value="4"/>
+      <param name="grid_col" value="5"/>
+      <param name="grid_min_feature_num" value="3"/>
+      <param name="grid_max_feature_num" value="4"/>
+      <param name="pyramid_levels" value="3"/>
+      <param name="patch_size" value="15"/>
+      <param name="fast_threshold" value="10"/>
+      <param name="max_iteration" value="30"/>
+      <param name="track_precision" value="0.01"/>
+      <param name="ransac_threshold" value="3"/>
+      <param name="stereo_threshold" value="5"/>
+
+      <remap from="~imu" to="/imu0"/>
+      <remap from="~cam0_image" to="/mynteye/left/image_raw"/>
+      <remap from="~cam1_image" to="/mynteye/right/image_raw"/>
+
+    </node>
+  </group>
+
+</launch>
--- a/launch/image_processor_tum.launch
+++ b/launch/image_processor_tum.launch
@@ -0,0 +1,34 @@
+<launch>
+
+  <arg name="robot" default="firefly_sbx"/>
+  <arg name="calibration_file"
+    default="$(find msckf_vio)/config/camchain-imucam-tum.yaml"/>
+
+  <!-- Image Processor Nodelet  -->
+  <group ns="$(arg robot)">
+    <node pkg="nodelet" type="nodelet" name="image_processor"
+      args="standalone msckf_vio/ImageProcessorNodelet"
+      output="screen"
+       >
+
+      <rosparam command="load" file="$(arg calibration_file)"/>
+      <param name="grid_row" value="4"/>
+      <param name="grid_col" value="4"/>
+      <param name="grid_min_feature_num" value="3"/>
+      <param name="grid_max_feature_num" value="4"/>
+      <param name="pyramid_levels" value="3"/>
+      <param name="patch_size" value="15"/>
+      <param name="fast_threshold" value="10"/>
+      <param name="max_iteration" value="30"/>
+      <param name="track_precision" value="0.01"/>
+      <param name="ransac_threshold" value="3"/>
+      <param name="stereo_threshold" value="5"/>
+
+      <remap from="~imu" to="/imu0"/>
+      <remap from="~cam0_image" to="/cam0/image_raw"/>
+      <remap from="~cam1_image" to="/cam1/image_raw"/>
+
+    </node>
+  </group>
+
+</launch>
--- a/launch/msckf_vio_euroc.launch
+++ b/launch/msckf_vio_euroc.launch
@@ -53,6 +53,9 @@
      <param name="initial_covariance/extrinsic_translation_cov" value="2.5e-5"/>

      <remap from="~imu" to="/imu0"/>
+      <remap from="~cam0_image" to="/cam0/image_raw"/>
+      <remap from="~cam1_image" to="/cam1/image_raw"/>
+
      <remap from="~features" to="image_processor/features"/>

    </node>
--- a/launch/msckf_vio_mynt.launch
+++ b/launch/msckf_vio_mynt.launch
@@ -0,0 +1,61 @@
+<launch>
+
+  <arg name="robot" default="firefly_sbx"/>
+  <arg name="fixed_frame_id" default="world"/>
+  <arg name="calibration_file"
+    default="$(find msckf_vio)/config/camchain-imucam-mynt.yaml"/>
+
+  <!-- Image Processor Nodelet  -->
+  <include file="$(find msckf_vio)/launch/image_processor_mynt.launch">
+    <arg name="robot" value="$(arg robot)"/>
+    <arg name="calibration_file" value="$(arg calibration_file)"/>
+  </include>
+
+  <!-- Msckf Vio Nodelet  -->
+  <group ns="$(arg robot)">
+    <node pkg="nodelet" type="nodelet" name="vio"
+      args='standalone msckf_vio/MsckfVioNodelet'
+      output="screen">
+
+      <!-- Calibration parameters -->
+      <rosparam command="load" file="$(arg calibration_file)"/>
+
+      <param name="publish_tf" value="true"/>
+      <param name="frame_rate" value="20"/>
+      <param name="fixed_frame_id" value="$(arg fixed_frame_id)"/>
+      <param name="child_frame_id" value="odom"/>
+      <param name="max_cam_state_size" value="20"/>
+      <param name="position_std_threshold" value="8.0"/>
+
+      <param name="rotation_threshold" value="0.2618"/>
+      <param name="translation_threshold" value="0.4"/>
+      <param name="tracking_rate_threshold" value="0.5"/>
+
+      <!-- Feature optimization config -->
+      <param name="feature/config/translation_threshold" value="-1.0"/>
+
+      <!-- These values should be standard deviation -->
+      <param name="noise/gyro" value="0.005"/>
+      <param name="noise/acc" value="0.05"/>
+      <param name="noise/gyro_bias" value="0.001"/>
+      <param name="noise/acc_bias" value="0.01"/>
+      <param name="noise/feature" value="0.035"/>
+
+      <param name="initial_state/velocity/x" value="0.0"/>
+      <param name="initial_state/velocity/y" value="0.0"/>
+      <param name="initial_state/velocity/z" value="0.0"/>
+
+      <!-- These values should be covariance -->
+      <param name="initial_covariance/velocity" value="0.25"/>
+      <param name="initial_covariance/gyro_bias" value="0.01"/>
+      <param name="initial_covariance/acc_bias" value="0.01"/>
+      <param name="initial_covariance/extrinsic_rotation_cov" value="3.0462e-4"/>
+      <param name="initial_covariance/extrinsic_translation_cov" value="2.5e-5"/>
+
+      <remap from="~imu" to="/mynteye/imu/data_raw"/>
+      <remap from="~features" to="image_processor/features"/>
+
+    </node>
+  </group>
+
+</launch>
--- a/launch/msckf_vio_tum.launch
+++ b/launch/msckf_vio_tum.launch
@@ -0,0 +1,64 @@
+<launch>
+
+  <arg name="robot" default="firefly_sbx"/>
+  <arg name="fixed_frame_id" default="world"/>
+  <arg name="calibration_file"
+    default="$(find msckf_vio)/config/camchain-imucam-tum.yaml"/>
+
+  <!-- Image Processor Nodelet  -->
+  <include file="$(find msckf_vio)/launch/image_processor_tum.launch">
+    <arg name="robot" value="$(arg robot)"/>
+    <arg name="calibration_file" value="$(arg calibration_file)"/>
+  </include>
+
+  <!-- Msckf Vio Nodelet  -->
+  <group ns="$(arg robot)">
+    <node pkg="nodelet" type="nodelet" name="vio"
+      args='standalone msckf_vio/MsckfVioNodelet'
+      output="screen">
+
+      <!-- Calibration parameters -->
+      <rosparam command="load" file="$(arg calibration_file)"/>
+
+      <param name="publish_tf" value="true"/>
+      <param name="frame_rate" value="20"/>
+      <param name="fixed_frame_id" value="$(arg fixed_frame_id)"/>
+      <param name="child_frame_id" value="odom"/>
+      <param name="max_cam_state_size" value="20"/>
+      <param name="position_std_threshold" value="8.0"/>
+
+      <param name="rotation_threshold" value="0.2618"/>
+      <param name="translation_threshold" value="0.4"/>
+      <param name="tracking_rate_threshold" value="0.5"/>
+
+      <!-- Feature optimization config -->
+      <param name="feature/config/translation_threshold" value="-1.0"/>
+
+      <!-- These values should be standard deviation -->
+      <param name="noise/gyro" value="0.005"/>
+      <param name="noise/acc" value="0.05"/>
+      <param name="noise/gyro_bias" value="0.001"/>
+      <param name="noise/acc_bias" value="0.01"/>
+      <param name="noise/feature" value="0.035"/>
+
+      <param name="initial_state/velocity/x" value="0.0"/>
+      <param name="initial_state/velocity/y" value="0.0"/>
+      <param name="initial_state/velocity/z" value="0.0"/>
+
+      <!-- These values should be covariance -->
+      <param name="initial_covariance/velocity" value="0.25"/>
+      <param name="initial_covariance/gyro_bias" value="0.01"/>
+      <param name="initial_covariance/acc_bias" value="0.01"/>
+      <param name="initial_covariance/extrinsic_rotation_cov" value="3.0462e-4"/>
+      <param name="initial_covariance/extrinsic_translation_cov" value="2.5e-5"/>
+
+      <remap from="~imu" to="/imu0"/>
+      <remap from="~cam0_image" to="/cam0/image_raw"/>
+      <remap from="~cam1_image" to="/cam1/image_raw"/>
+
+      <remap from="~features" to="image_processor/features"/>
+
+    </node>
+  </group>
+
+</launch>
--- a/msg/CameraMeasurement.msg
+++ b/msg/CameraMeasurement.msg
@@ -1,4 +1,4 @@
-std_msgs/Header header
+Header header
 # All features on the current image,
 # including tracked ones and newly detected ones.
 FeatureMeasurement[] features
--- a/package.xml
+++ b/package.xml
@@ -3,13 +3,12 @@

  <name>msckf_vio</name>
  <version>0.0.1</version>
-  <description>Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation</description>
+  <description>Multi-State Constraint Kalman Filter - Photometric expansion</description>

-  <maintainer email="sunke.polyu@gmail.com">Ke Sun</maintainer>
+  <maintainer email="raphael@maenle.net">Raphael Maenle</maintainer>
  <license>Penn Software License</license>

-  <author email="sunke.polyu@gmail.com">Ke Sun</author>
-  <author email="kartikmohta@gmail.com">Kartik Mohta</author>
+  <author email="raphael@maenle.net">Raphael Maenle</author>

  <buildtool_depend>catkin</buildtool_depend>

--- a/pseudocode/pseudocode_image_processing
+++ b/pseudocode/pseudocode_image_processing
@@ -0,0 +1,97 @@
+
+
+stereo callback()
+
+	create image pyramids
+		_Constructs the image pyramid which can be passed to calcOpticalFlowPyrLK._
+.
+	if first Frame:
+		*initialize first Frame ()
+
+	else:
+		*track Features ()
+
+		*addnewFeatures ()
+
+		*pruneGridFeatures()
+			_removes worst features from any overflowing grid_
+
+	publish features (u1, v1, u2, v2)
+		_undistorts them beforehand_
+
+addnewFeatures()
+	*mask existing features
+	*detect new fast features
+	*collect in a grid, keep only best n per grid
+	*stereomatch()
+	*save inliers into a new feature with u,v on cam0 and cam1
+
+
+track Features()
+	*integrateIMUData ()
+		_uses existing IMU data between two frames to calc. rotation between the two frames_
+
+	*predictFeatureTracking()
+		_compensates the rotation between consecutive frames - rotates previous camera frame features to current camera rotation_
+
+	*calcOpticalFlowPyrLK()
+		_measures the change between the features in the previous frame and in the current frame (using the predicted features)_
+
+	*remove points outside of image region
+		_how does this even happen?_
+
+	*stereo match()
+		_find tracked features from optical flow in the camera 1 image_
+		_remove all features that could not be matched_
+
+	*twoPointRansac(cam0)
+	*twoPointRansac(cam1)
+		_remove any features outside best found ransac model_
+
+
+
+
+twoPointRansac()
+	*mark all points as inliers
+	*compensate rotation between frames
+	*normalize points
+	*calculate difference bewteen previous and current points
+	*mark large distances (over 50 pixels currently)
+	*calculate mean points distance
+	*return if inliers (non marked) < 3
+	*return if motion smaller than norm pixel unit
+	*ransac
+		*optimize with found inlier after random sample
+
+	*set inlier markers
+
+initialize first Frame()
+
+	features = FastFeatureDetector detect ()
+
+	*stereo match ()
+
+	group features into grid
+		- according to position in the image
+		- sorting them by response
+		- save the top responses
+		- save the top responses
+
+
+stereo match ()
+
+	*undistort cam0 Points
+	*project cam0 Points to cam1 to initialize points in cam1
+
+	*calculate lk optical flow
+		_used because camera calibrations not perfect enough_
+		_also, calculation more efficient, because LK calculated anyway_
+
+	*compute relative trans/rot between cam0 and cam1*
+
+	*remove outliers based on essential matrix
+		_essential matrix relates points in stereo image (pinhole model)_
+		for every point:
+			- calculate epipolar line of point in cam0
+			- calculate error of cam1 to epipolar line 
+			- remove if to big
--- a/pseudocode/pseudocode_msckf
+++ b/pseudocode/pseudocode_msckf
@@ -0,0 +1,82 @@
+featureCallback
+	propagate IMU state()
+	state Augmentation()
+	add Feature Observations()
+
+	#the following possibly trigger ekf update step:
+	remove Lost Features ()
+	prune Camera State Buffer ()
+
+
+remove Lost Features()
+	every feature that does not have a current observation:
+		*just delete if not enough features
+
+		check Motion of Feature ()
+			_calculation here makes no sense - he uses pixel position as direction vector for feature?_
+		*initialize Position ()
+
+		caculate feature Jakobian and Residual()
+			*for every observation in this feature
+				- calculate u and v in camera frames, based on estimated feature position
+				- input results into jakobi d(measurement)/d(camera 0/1)
+				- input results into jakobi d(camera 0/1)/d(state) and jakobi d(camera 0/1)/d(feature position)
+				- project both jakobis to nullspace of feature position jakobi
+				- calculate residual: measurement - u and v of camera frames
+				- project residual onto nullspace of feature position jakobi
+
+			- stack residual and jakobians
+
+		gating Test()
+		
+		*measurementUpdate()
+			_use calculated residuals and jakobians to calculate change in error_
+measurementUpdate():
+	- QR reduce the stacked Measurment Jakobis
+	- calcualte Kalman Gain based on Measurement Jakobian, Error-State Covariance and Noise
+		_does some fancy shit here_
+	- calculate estimated error after observation: delta_x = KalmanGain * residual 
+	- add estimated error to state (imu states and cam states)
+
+initialize Position ():
+	* create initial guess for global feature position ()
+		_uses first feature measurement on left camera and last feature measurement of right camera_
+		- transform first measurement to plane of last measurement
+		- calcualte least square point between rays
+	* get position approximation using measured feature positions
+		_using Levenberg Marqhart iterative search_
+
+
+
+add Feature Observations()
+	* if feature not in map, add feature to map
+		_and add u0, v0, u1, v1 as first observation
+	* if feature in map, add new observation u0,v0,u1,v1
+
+
+
+state Augmentation()
+	* Add estimated cam position to state
+	* Update P with Jakobian of cam Position 
+
+
+propagate IMU state ()
+	_uses IMU process model for every saved IMU state_
+	
+	for every buffered imu state:
+		
+		*remove bias
+		
+		*Compute F and G matrix (continuous transition and noise cov.)
+			_using current orientation, gyro and acc. reading_
+		* approximate phi: phi = 1 + Fdt + ...
+
+		* calculate new state propagating through runge kutta
+		* modify transition matrix to have a propper null space?
+
+		* calculate Q = Phi*G*Q_noise*GT*PhiT
+		* calculate P = Phi*P*PhiT + Q
+
+
+	stateAugmentation ()
+		_save current IMU state as camera position_
--- a/src/image_handler.cpp
+++ b/src/image_handler.cpp
@@ -0,0 +1,118 @@
+#include <iostream>
+#include <algorithm>
+#include <set>
+#include <Eigen/Dense>
+
+#include <sensor_msgs/image_encodings.h>
+#include <random_numbers/random_numbers.h>
+
+#include <msckf_vio/CameraMeasurement.h>
+#include <msckf_vio/TrackingInfo.h>
+#include <msckf_vio/image_processor.h>
+
+namespace msckf_vio {
+namespace image_handler {
+
+
+void undistortPoints(
+    const std::vector<cv::Point2f>& pts_in,
+    const cv::Vec4d& intrinsics,
+    const std::string& distortion_model,
+    const cv::Vec4d& distortion_coeffs,
+    std::vector<cv::Point2f>& pts_out,
+    const cv::Matx33d &rectification_matrix,
+    const cv::Vec4d &new_intrinsics) {
+
+  if (pts_in.size() == 0) return;
+
+  const cv::Matx33d K(
+      intrinsics[0], 0.0, intrinsics[2],
+      0.0, intrinsics[1], intrinsics[3],
+      0.0, 0.0, 1.0);
+
+  const cv::Matx33d K_new(
+      new_intrinsics[0], 0.0, new_intrinsics[2],
+      0.0, new_intrinsics[1], new_intrinsics[3],
+      0.0, 0.0, 1.0);
+
+  if (distortion_model == "radtan") {
+    cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
+                        rectification_matrix, K_new);
+  } else if (distortion_model == "equidistant") {
+    cv::fisheye::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
+                                 rectification_matrix, K_new);
+  } else {
+    ROS_WARN_ONCE("The model %s is unrecognized, use radtan instead...",
+                  distortion_model.c_str());
+    cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
+                        rectification_matrix, K_new);
+  }
+
+  return;
+}
+
+std::vector<cv::Point2f> distortPoints(
+    const std::vector<cv::Point2f>& pts_in,
+    const cv::Vec4d& intrinsics,
+    const std::string& distortion_model,
+    const cv::Vec4d& distortion_coeffs) {
+
+  const cv::Matx33d K(intrinsics[0], 0.0, intrinsics[2],
+                      0.0, intrinsics[1], intrinsics[3],
+                      0.0, 0.0, 1.0);
+
+  std::vector<cv::Point2f> pts_out;
+  if (distortion_model == "radtan") {
+    std::vector<cv::Point3f> homogenous_pts;
+    cv::convertPointsToHomogeneous(pts_in, homogenous_pts);
+    cv::projectPoints(homogenous_pts, cv::Vec3d::zeros(), cv::Vec3d::zeros(), K,
+                      distortion_coeffs, pts_out);
+  } else if (distortion_model == "equidistant") {
+    cv::fisheye::distortPoints(pts_in, pts_out, K, distortion_coeffs);
+  } else {
+    ROS_WARN_ONCE("The model %s is unrecognized, using radtan instead...",
+                  distortion_model.c_str());
+    std::vector<cv::Point3f> homogenous_pts;
+    cv::convertPointsToHomogeneous(pts_in, homogenous_pts);
+    cv::projectPoints(homogenous_pts, cv::Vec3d::zeros(), cv::Vec3d::zeros(), K,
+                      distortion_coeffs, pts_out);
+  }
+
+  return pts_out;
+}
+
+cv::Point2f distortPoint(
+    const cv::Point2f& pt_in,
+    const cv::Vec4d& intrinsics,
+    const std::string& distortion_model,
+    const cv::Vec4d& distortion_coeffs) {
+
+  std::vector<cv::Point2f> pts_in;
+  pts_in.push_back(pt_in);
+
+  const cv::Matx33d K(intrinsics[0], 0.0, intrinsics[2],
+                      0.0, intrinsics[1], intrinsics[3],
+                      0.0, 0.0, 1.0);
+
+  std::vector<cv::Point2f> pts_out;
+  if (distortion_model == "radtan") {
+    std::vector<cv::Point3f> homogenous_pts;
+    cv::convertPointsToHomogeneous(pts_in, homogenous_pts);
+    cv::projectPoints(homogenous_pts, cv::Vec3d::zeros(), cv::Vec3d::zeros(), K,
+                      distortion_coeffs, pts_out);
+  } else if (distortion_model == "equidistant") {
+    cv::fisheye::distortPoints(pts_in, pts_out, K, distortion_coeffs);
+  } else {
+    ROS_WARN_ONCE("The model %s is unrecognized, using radtan instead...",
+                  distortion_model.c_str());
+    std::vector<cv::Point3f> homogenous_pts;
+    cv::convertPointsToHomogeneous(pts_in, homogenous_pts);
+    cv::projectPoints(homogenous_pts, cv::Vec3d::zeros(), cv::Vec3d::zeros(), K,
+                      distortion_coeffs, pts_out);
+  }
+
+  return pts_out[0];
+}
+
+	} // namespace image_handler
+} // namespace msckf_vio
--- a/src/image_processor.cpp
+++ b/src/image_processor.cpp
@@ -17,6 +17,7 @@
 #include <msckf_vio/TrackingInfo.h>
 #include <msckf_vio/image_processor.h>
 #include <msckf_vio/utils.h>
+#include <msckf_vio/image_handler.h>

 using namespace std;
 using namespace cv;
@@ -43,49 +44,49 @@ ImageProcessor::~ImageProcessor() {
 bool ImageProcessor::loadParameters() {
  // Camera calibration parameters
  nh.param<string>("cam0/distortion_model",
-      cam0_distortion_model, string("radtan"));
+      cam0.distortion_model, string("radtan"));
  nh.param<string>("cam1/distortion_model",
-      cam1_distortion_model, string("radtan"));
+      cam1.distortion_model, string("radtan"));

  vector<int> cam0_resolution_temp(2);
  nh.getParam("cam0/resolution", cam0_resolution_temp);
-  cam0_resolution[0] = cam0_resolution_temp[0];
-  cam0_resolution[1] = cam0_resolution_temp[1];
+  cam0.resolution[0] = cam0_resolution_temp[0];
+  cam0.resolution[1] = cam0_resolution_temp[1];

  vector<int> cam1_resolution_temp(2);
  nh.getParam("cam1/resolution", cam1_resolution_temp);
-  cam1_resolution[0] = cam1_resolution_temp[0];
-  cam1_resolution[1] = cam1_resolution_temp[1];
+  cam1.resolution[0] = cam1_resolution_temp[0];
+  cam1.resolution[1] = cam1_resolution_temp[1];

  vector<double> cam0_intrinsics_temp(4);
  nh.getParam("cam0/intrinsics", cam0_intrinsics_temp);
-  cam0_intrinsics[0] = cam0_intrinsics_temp[0];
-  cam0_intrinsics[1] = cam0_intrinsics_temp[1];
-  cam0_intrinsics[2] = cam0_intrinsics_temp[2];
-  cam0_intrinsics[3] = cam0_intrinsics_temp[3];
+  cam0.intrinsics[0] = cam0_intrinsics_temp[0];
+  cam0.intrinsics[1] = cam0_intrinsics_temp[1];
+  cam0.intrinsics[2] = cam0_intrinsics_temp[2];
+  cam0.intrinsics[3] = cam0_intrinsics_temp[3];

  vector<double> cam1_intrinsics_temp(4);
  nh.getParam("cam1/intrinsics", cam1_intrinsics_temp);
-  cam1_intrinsics[0] = cam1_intrinsics_temp[0];
-  cam1_intrinsics[1] = cam1_intrinsics_temp[1];
-  cam1_intrinsics[2] = cam1_intrinsics_temp[2];
-  cam1_intrinsics[3] = cam1_intrinsics_temp[3];
+  cam1.intrinsics[0] = cam1_intrinsics_temp[0];
+  cam1.intrinsics[1] = cam1_intrinsics_temp[1];
+  cam1.intrinsics[2] = cam1_intrinsics_temp[2];
+  cam1.intrinsics[3] = cam1_intrinsics_temp[3];

  vector<double> cam0_distortion_coeffs_temp(4);
  nh.getParam("cam0/distortion_coeffs",
      cam0_distortion_coeffs_temp);
-  cam0_distortion_coeffs[0] = cam0_distortion_coeffs_temp[0];
-  cam0_distortion_coeffs[1] = cam0_distortion_coeffs_temp[1];
-  cam0_distortion_coeffs[2] = cam0_distortion_coeffs_temp[2];
-  cam0_distortion_coeffs[3] = cam0_distortion_coeffs_temp[3];
+  cam0.distortion_coeffs[0] = cam0_distortion_coeffs_temp[0];
+  cam0.distortion_coeffs[1] = cam0_distortion_coeffs_temp[1];
+  cam0.distortion_coeffs[2] = cam0_distortion_coeffs_temp[2];
+  cam0.distortion_coeffs[3] = cam0_distortion_coeffs_temp[3];

  vector<double> cam1_distortion_coeffs_temp(4);
  nh.getParam("cam1/distortion_coeffs",
      cam1_distortion_coeffs_temp);
-  cam1_distortion_coeffs[0] = cam1_distortion_coeffs_temp[0];
-  cam1_distortion_coeffs[1] = cam1_distortion_coeffs_temp[1];
-  cam1_distortion_coeffs[2] = cam1_distortion_coeffs_temp[2];
-  cam1_distortion_coeffs[3] = cam1_distortion_coeffs_temp[3];
+  cam1.distortion_coeffs[0] = cam1_distortion_coeffs_temp[0];
+  cam1.distortion_coeffs[1] = cam1_distortion_coeffs_temp[1];
+  cam1.distortion_coeffs[2] = cam1_distortion_coeffs_temp[2];
+  cam1.distortion_coeffs[3] = cam1_distortion_coeffs_temp[3];

  cv::Mat     T_imu_cam0 = utils::getTransformCV(nh, "cam0/T_cam_imu");
  cv::Matx33d R_imu_cam0(T_imu_cam0(cv::Rect(0,0,3,3)));
@@ -123,27 +124,27 @@ bool ImageProcessor::loadParameters() {
      processor_config.stereo_threshold, 3);

  ROS_INFO("===========================================");
-  ROS_INFO("cam0_resolution: %d, %d",
-      cam0_resolution[0], cam0_resolution[1]);
+  ROS_INFO("cam0.resolution: %d, %d",
+      cam0.resolution[0], cam0.resolution[1]);
  ROS_INFO("cam0_intrinscs: %f, %f, %f, %f",
-      cam0_intrinsics[0], cam0_intrinsics[1],
-      cam0_intrinsics[2], cam0_intrinsics[3]);
-  ROS_INFO("cam0_distortion_model: %s",
-      cam0_distortion_model.c_str());
+      cam0.intrinsics[0], cam0.intrinsics[1],
+      cam0.intrinsics[2], cam0.intrinsics[3]);
+  ROS_INFO("cam0.distortion_model: %s",
+      cam0.distortion_model.c_str());
  ROS_INFO("cam0_distortion_coefficients: %f, %f, %f, %f",
-      cam0_distortion_coeffs[0], cam0_distortion_coeffs[1],
-      cam0_distortion_coeffs[2], cam0_distortion_coeffs[3]);
+      cam0.distortion_coeffs[0], cam0.distortion_coeffs[1],
+      cam0.distortion_coeffs[2], cam0.distortion_coeffs[3]);

-  ROS_INFO("cam1_resolution: %d, %d",
-      cam1_resolution[0], cam1_resolution[1]);
+  ROS_INFO("cam1.resolution: %d, %d",
+      cam1.resolution[0], cam1.resolution[1]);
  ROS_INFO("cam1_intrinscs: %f, %f, %f, %f",
-      cam1_intrinsics[0], cam1_intrinsics[1],
-      cam1_intrinsics[2], cam1_intrinsics[3]);
-  ROS_INFO("cam1_distortion_model: %s",
-      cam1_distortion_model.c_str());
+      cam1.intrinsics[0], cam1.intrinsics[1],
+      cam1.intrinsics[2], cam1.intrinsics[3]);
+  ROS_INFO("cam1.distortion_model: %s",
+      cam1.distortion_model.c_str());
  ROS_INFO("cam1_distortion_coefficients: %f, %f, %f, %f",
-      cam1_distortion_coeffs[0], cam1_distortion_coeffs[1],
-      cam1_distortion_coeffs[2], cam1_distortion_coeffs[3]);
+      cam1.distortion_coeffs[0], cam1.distortion_coeffs[1],
+      cam1.distortion_coeffs[2], cam1.distortion_coeffs[3]);

  cout << R_imu_cam0 << endl;
  cout << t_imu_cam0.t() << endl;
@@ -170,10 +171,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;
 }
@@ -204,13 +201,6 @@ bool ImageProcessor::initialize() {
  detector_ptr = FastFeatureDetector::create(
      processor_config.fast_threshold);

-  //create gpu optical flow lk
-  d_pyrLK_sparse = cuda::SparsePyrLKOpticalFlow::create(
-          Size(processor_config.patch_size, processor_config.patch_size),
-          processor_config.pyramid_levels, 
-          processor_config.max_iteration,
-          true);
-
  if (!createRosIO()) return false;
  ROS_INFO("Finish creating ROS IO...");

@@ -230,9 +220,7 @@ void ImageProcessor::stereoCallback(
      sensor_msgs::image_encodings::MONO8);

  // Build the image pyramids once since they're used at multiple places
-
-  // removed due to alternate cuda construct
-  //createImagePyramids();
+  createImagePyramids();

  // Detect features in the first frame.
  if (is_first_img) {
@@ -309,7 +297,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),
@@ -317,7 +304,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),
@@ -404,7 +390,6 @@ void ImageProcessor::predictFeatureTracking(
    const cv::Matx33f& R_p_c,
    const cv::Vec4d& intrinsics,
    vector<cv::Point2f>& compensated_pts) {
-
  // Return directly if there are no input features.
  if (input_pts.size() == 0) {
    compensated_pts.clear();
@@ -435,7 +420,6 @@ void ImageProcessor::trackFeatures() {
    cam0_curr_img_ptr->image.rows / processor_config.grid_row;
  static int grid_width =
    cam0_curr_img_ptr->image.cols / processor_config.grid_col;
-
  // Compute a rough relative rotation which takes a vector
  // from the previous frame to the current frame.
  Matx33f cam0_R_p_c;
@@ -469,10 +453,8 @@ void ImageProcessor::trackFeatures() {
  vector<unsigned char> track_inliers(0);

  predictFeatureTracking(prev_cam0_points,
-      cam0_R_p_c, cam0_intrinsics, curr_cam0_points);
+      cam0_R_p_c, cam0.intrinsics, curr_cam0_points);

-  //TODO: test change to sparse
-  /*
  calcOpticalFlowPyrLK(
      prev_cam0_pyramid_, curr_cam0_pyramid_,
      prev_cam0_points, curr_cam0_points,
@@ -483,25 +465,6 @@ void ImageProcessor::trackFeatures() {
        processor_config.max_iteration,
        processor_config.track_precision),
      cv::OPTFLOW_USE_INITIAL_FLOW);
-  */
-
-
-
-  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(prev_cam0_points);
-  cam1_points_gpu = cv::cuda::GpuMat(curr_cam0_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, curr_cam0_points);
-  utils::download(inlier_markers_gpu, track_inliers);

  // Mark those tracked points out of the image region
  // as untracked.
@@ -585,14 +548,14 @@ void ImageProcessor::trackFeatures() {
  // Step 2 and 3: RANSAC on temporal image pairs of cam0 and cam1.
  vector<int> cam0_ransac_inliers(0);
  twoPointRansac(prev_matched_cam0_points, curr_matched_cam0_points,
-      cam0_R_p_c, cam0_intrinsics, cam0_distortion_model,
-      cam0_distortion_coeffs, processor_config.ransac_threshold,
+      cam0_R_p_c, cam0.intrinsics, cam0.distortion_model,
+      cam0.distortion_coeffs, processor_config.ransac_threshold,
      0.99, cam0_ransac_inliers);

  vector<int> cam1_ransac_inliers(0);
  twoPointRansac(prev_matched_cam1_points, curr_matched_cam1_points,
-      cam1_R_p_c, cam1_intrinsics, cam1_distortion_model,
-      cam1_distortion_coeffs, processor_config.ransac_threshold,
+      cam1_R_p_c, cam1.intrinsics, cam1.distortion_model,
+      cam1.distortion_coeffs, processor_config.ransac_threshold,
      0.99, cam1_ransac_inliers);

  // Number of features after ransac.
@@ -646,7 +609,6 @@ void ImageProcessor::stereoMatch(
    const vector<cv::Point2f>& cam0_points,
    vector<cv::Point2f>& cam1_points,
    vector<unsigned char>& inlier_markers) {
-
  if (cam0_points.size() == 0) return;

  if(cam1_points.size() == 0) {
@@ -654,31 +616,15 @@ void ImageProcessor::stereoMatch(
    // rotation from stereo extrinsics
    const cv::Matx33d R_cam0_cam1 = R_cam1_imu.t() * R_cam0_imu;
    vector<cv::Point2f> cam0_points_undistorted;
-    undistortPoints(cam0_points, cam0_intrinsics, cam0_distortion_model,
-                    cam0_distortion_coeffs, cam0_points_undistorted,
+    image_handler::undistortPoints(cam0_points, cam0.intrinsics, cam0.distortion_model,
+                    cam0.distortion_coeffs, cam0_points_undistorted,
                    R_cam0_cam1);
-    cam1_points = distortPoints(cam0_points_undistorted, cam1_intrinsics,
-                                cam1_distortion_model, cam1_distortion_coeffs);
+
+    cam1_points = image_handler::distortPoints(cam0_points_undistorted, cam1.intrinsics,
+                                cam1.distortion_model, cam1.distortion_coeffs);
  }

-  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(),
@@ -688,7 +634,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) {
@@ -715,16 +661,16 @@ void ImageProcessor::stereoMatch(
  // essential matrix.
  vector<cv::Point2f> cam0_points_undistorted(0);
  vector<cv::Point2f> cam1_points_undistorted(0);
-  undistortPoints(
-      cam0_points, cam0_intrinsics, cam0_distortion_model,
-      cam0_distortion_coeffs, cam0_points_undistorted);
-  undistortPoints(
-      cam1_points, cam1_intrinsics, cam1_distortion_model,
-      cam1_distortion_coeffs, cam1_points_undistorted);
+  image_handler::undistortPoints(
+      cam0_points, cam0.intrinsics, cam0.distortion_model,
+      cam0.distortion_coeffs, cam0_points_undistorted);
+  image_handler::undistortPoints(
+      cam1_points, cam1.intrinsics, cam1.distortion_model,
+      cam1.distortion_coeffs, cam1_points_undistorted);

  double norm_pixel_unit = 4.0 / (
-      cam0_intrinsics[0]+cam0_intrinsics[1]+
-      cam1_intrinsics[0]+cam1_intrinsics[1]);
+      cam0.intrinsics[0]+cam0.intrinsics[1]+
+      cam1.intrinsics[0]+cam1.intrinsics[1]);

  for (int i = 0; i < cam0_points_undistorted.size(); ++i) {
    if (inlier_markers[i] == 0) continue;
@@ -751,8 +697,8 @@ void ImageProcessor::addNewFeatures() {
    cam0_curr_img_ptr->image.rows / processor_config.grid_row;
  static int grid_width =
    cam0_curr_img_ptr->image.cols / processor_config.grid_col;
-
  // Create a mask to avoid redetecting existing features.
+
  Mat mask(curr_img.rows, curr_img.cols, CV_8U, Scalar(1));

  for (const auto& features : *curr_features_ptr) {
@@ -772,7 +718,6 @@ void ImageProcessor::addNewFeatures() {
      mask(row_range, col_range) = 0;
    }
  }
-
  // Detect new features.
  vector<KeyPoint> new_features(0);
  detector_ptr->detect(curr_img, new_features, mask);
@@ -787,7 +732,6 @@ void ImageProcessor::addNewFeatures() {
    new_feature_sieve[
      row*processor_config.grid_col+col].push_back(feature);
  }
-
  new_features.clear();
  for (auto& item : new_feature_sieve) {
    if (item.size() > processor_config.grid_max_feature_num) {
@@ -800,7 +744,6 @@ void ImageProcessor::addNewFeatures() {
  }

  int detected_new_features = new_features.size();
-
  // Find the stereo matched points for the newly
  // detected features.
  vector<cv::Point2f> cam0_points(new_features.size());
@@ -828,7 +771,6 @@ void ImageProcessor::addNewFeatures() {
      static_cast<double>(detected_new_features) < 0.1)
    ROS_WARN("Images at [%f] seems unsynced...",
        cam0_curr_img_ptr->header.stamp.toSec());
-
  // Group the features into grids
  GridFeatures grid_new_features;
  for (int code = 0; code <
@@ -850,7 +792,6 @@ void ImageProcessor::addNewFeatures() {
    new_feature.cam1_point = cam1_point;
    grid_new_features[code].push_back(new_feature);
  }
-
  // Sort the new features in each grid based on its response.
  for (auto& item : grid_new_features)
    std::sort(item.second.begin(), item.second.end(),
@@ -900,73 +841,6 @@ void ImageProcessor::pruneGridFeatures() {
  return;
 }

-void ImageProcessor::undistortPoints(
-    const vector<cv::Point2f>& pts_in,
-    const cv::Vec4d& intrinsics,
-    const string& distortion_model,
-    const cv::Vec4d& distortion_coeffs,
-    vector<cv::Point2f>& pts_out,
-    const cv::Matx33d &rectification_matrix,
-    const cv::Vec4d &new_intrinsics) {
-
-  if (pts_in.size() == 0) return;
-
-  const cv::Matx33d K(
-      intrinsics[0], 0.0, intrinsics[2],
-      0.0, intrinsics[1], intrinsics[3],
-      0.0, 0.0, 1.0);
-
-  const cv::Matx33d K_new(
-      new_intrinsics[0], 0.0, new_intrinsics[2],
-      0.0, new_intrinsics[1], new_intrinsics[3],
-      0.0, 0.0, 1.0);
-
-  if (distortion_model == "radtan") {
-    cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
-                        rectification_matrix, K_new);
-  } else if (distortion_model == "equidistant") {
-    cv::fisheye::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
-                                 rectification_matrix, K_new);
-  } else {
-    ROS_WARN_ONCE("The model %s is unrecognized, use radtan instead...",
-                  distortion_model.c_str());
-    cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
-                        rectification_matrix, K_new);
-  }
-
-  return;
-}
-
-vector<cv::Point2f> ImageProcessor::distortPoints(
-    const vector<cv::Point2f>& pts_in,
-    const cv::Vec4d& intrinsics,
-    const string& distortion_model,
-    const cv::Vec4d& distortion_coeffs) {
-
-  const cv::Matx33d K(intrinsics[0], 0.0, intrinsics[2],
-                      0.0, intrinsics[1], intrinsics[3],
-                      0.0, 0.0, 1.0);
-
-  vector<cv::Point2f> pts_out;
-  if (distortion_model == "radtan") {
-    vector<cv::Point3f> homogenous_pts;
-    cv::convertPointsToHomogeneous(pts_in, homogenous_pts);
-    cv::projectPoints(homogenous_pts, cv::Vec3d::zeros(), cv::Vec3d::zeros(), K,
-                      distortion_coeffs, pts_out);
-  } else if (distortion_model == "equidistant") {
-    cv::fisheye::distortPoints(pts_in, pts_out, K, distortion_coeffs);
-  } else {
-    ROS_WARN_ONCE("The model %s is unrecognized, using radtan instead...",
-                  distortion_model.c_str());
-    vector<cv::Point3f> homogenous_pts;
-    cv::convertPointsToHomogeneous(pts_in, homogenous_pts);
-    cv::projectPoints(homogenous_pts, cv::Vec3d::zeros(), cv::Vec3d::zeros(), K,
-                      distortion_coeffs, pts_out);
-  }
-
-  return pts_out;
-}
-
 void ImageProcessor::integrateImuData(
    Matx33f& cam0_R_p_c, Matx33f& cam1_R_p_c) {
  // Find the start and the end limit within the imu msg buffer.
@@ -1018,7 +892,6 @@ void ImageProcessor::integrateImuData(
 void ImageProcessor::rescalePoints(
    vector<Point2f>& pts1, vector<Point2f>& pts2,
    float& scaling_factor) {
-
  scaling_factor = 0.0f;

  for (int i = 0; i < pts1.size(); ++i) {
@@ -1048,7 +921,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]);
@@ -1062,10 +935,10 @@ void ImageProcessor::twoPointRansac(
  // Undistort all the points.
  vector<Point2f> pts1_undistorted(pts1.size());
  vector<Point2f> pts2_undistorted(pts2.size());
-  undistortPoints(
+  image_handler::undistortPoints(
      pts1, intrinsics, distortion_model,
      distortion_coeffs, pts1_undistorted);
-  undistortPoints(
+  image_handler::undistortPoints(
      pts2, intrinsics, distortion_model,
      distortion_coeffs, pts2_undistorted);

@@ -1283,7 +1156,6 @@ void ImageProcessor::twoPointRansac(
 }

 void ImageProcessor::publish() {
-
  // Publish features.
  CameraMeasurementPtr feature_msg_ptr(new CameraMeasurement);
  feature_msg_ptr->header.stamp = cam0_curr_img_ptr->header.stamp;
@@ -1303,12 +1175,12 @@ void ImageProcessor::publish() {
  vector<Point2f> curr_cam0_points_undistorted(0);
  vector<Point2f> curr_cam1_points_undistorted(0);

-  undistortPoints(
-      curr_cam0_points, cam0_intrinsics, cam0_distortion_model,
-      cam0_distortion_coeffs, curr_cam0_points_undistorted);
-  undistortPoints(
-      curr_cam1_points, cam1_intrinsics, cam1_distortion_model,
-      cam1_distortion_coeffs, curr_cam1_points_undistorted);
+  image_handler::undistortPoints(
+      curr_cam0_points, cam0.intrinsics, cam0.distortion_model,
+      cam0.distortion_coeffs, curr_cam0_points_undistorted);
+  image_handler::undistortPoints(
+      curr_cam1_points, cam1.intrinsics, cam1.distortion_model,
+      cam1.distortion_coeffs, curr_cam1_points_undistorted);

  for (int i = 0; i < curr_ids.size(); ++i) {
    feature_msg_ptr->features.push_back(FeatureMeasurement());
--- a/src/msckf_vio.cpp
+++ b/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;
 }
@@ -116,6 +117,54 @@ bool MsckfVio::loadParameters() {
  nh.param<double>("initial_covariance/extrinsic_translation_cov",
      extrinsic_translation_cov, 1e-4);

+  // get camera information (used for back projection)
+  nh.param<string>("cam0/distortion_model",
+      cam0.distortion_model, string("radtan"));
+  nh.param<string>("cam1/distortion_model",
+      cam1.distortion_model, string("radtan"));
+
+  vector<int> cam0_resolution_temp(2);
+  nh.getParam("cam0/resolution", cam0_resolution_temp);
+  cam0.resolution[0] = cam0_resolution_temp[0];
+  cam0.resolution[1] = cam0_resolution_temp[1];
+
+  vector<int> cam1_resolution_temp(2);
+  nh.getParam("cam1/resolution", cam1_resolution_temp);
+  cam1.resolution[0] = cam1_resolution_temp[0];
+  cam1.resolution[1] = cam1_resolution_temp[1];
+
+  vector<double> cam0_intrinsics_temp(4);
+  nh.getParam("cam0/intrinsics", cam0_intrinsics_temp);
+  cam0.intrinsics[0] = cam0_intrinsics_temp[0];
+  cam0.intrinsics[1] = cam0_intrinsics_temp[1];
+  cam0.intrinsics[2] = cam0_intrinsics_temp[2];
+  cam0.intrinsics[3] = cam0_intrinsics_temp[3];
+
+  vector<double> cam1_intrinsics_temp(4);
+  nh.getParam("cam1/intrinsics", cam1_intrinsics_temp);
+  cam1.intrinsics[0] = cam1_intrinsics_temp[0];
+  cam1.intrinsics[1] = cam1_intrinsics_temp[1];
+  cam1.intrinsics[2] = cam1_intrinsics_temp[2];
+  cam1.intrinsics[3] = cam1_intrinsics_temp[3];
+
+  vector<double> cam0_distortion_coeffs_temp(4);
+  nh.getParam("cam0/distortion_coeffs",
+      cam0_distortion_coeffs_temp);
+  cam0.distortion_coeffs[0] = cam0_distortion_coeffs_temp[0];
+  cam0.distortion_coeffs[1] = cam0_distortion_coeffs_temp[1];
+  cam0.distortion_coeffs[2] = cam0_distortion_coeffs_temp[2];
+  cam0.distortion_coeffs[3] = cam0_distortion_coeffs_temp[3];
+
+  vector<double> cam1_distortion_coeffs_temp(4);
+  nh.getParam("cam1/distortion_coeffs",
+      cam1_distortion_coeffs_temp);
+  cam1.distortion_coeffs[0] = cam1_distortion_coeffs_temp[0];
+  cam1.distortion_coeffs[1] = cam1_distortion_coeffs_temp[1];
+  cam1.distortion_coeffs[2] = cam1_distortion_coeffs_temp[2];
+  cam1.distortion_coeffs[3] = cam1_distortion_coeffs_temp[3];
+
+
+
  state_server.state_cov = MatrixXd::Zero(21, 21);
  for (int i = 3; i < 6; ++i)
    state_server.state_cov(i, i) = gyro_bias_cov;
@@ -171,6 +220,12 @@ bool MsckfVio::loadParameters() {
  cout << T_imu_cam0.linear() << endl;
  cout << T_imu_cam0.translation().transpose() << endl;

+  cout << "OpenCV version : " << CV_VERSION << endl;
+  cout << "Major version : " << CV_MAJOR_VERSION << endl;
+  cout << "Minor version : " << CV_MINOR_VERSION << endl;
+  cout << "Subminor version : " << CV_SUBMINOR_VERSION << endl;
+
+
  ROS_INFO("max camera state #: %d", max_cam_state_size);
  ROS_INFO("===========================================");
  return true;
@@ -186,8 +241,14 @@ bool MsckfVio::createRosIO() {

  imu_sub = nh.subscribe("imu", 100,
      &MsckfVio::imuCallback, 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);
@@ -227,7 +288,8 @@ bool MsckfVio::initialize() {
 }

 void MsckfVio::imuCallback(
-    const sensor_msgs::ImuConstPtr& msg) {
+    const sensor_msgs::ImuConstPtr& msg)
+{

  // IMU msgs are pushed backed into a buffer instead of
  // being processed immediately. The IMU msgs are processed
@@ -245,6 +307,131 @@ void MsckfVio::imuCallback(
  return;
 }

+void MsckfVio::imageCallback(
+    const sensor_msgs::ImageConstPtr& cam0_img,
+    const sensor_msgs::ImageConstPtr& cam1_img,
+    const CameraMeasurementConstPtr& feature_msg)
+{
+  // Return if the gravity vector has not been set.
+  if (!is_gravity_set) return;
+
+  // 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_img, cam1_img, 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 sensor_msgs::ImageConstPtr& cam0_img,
+  const sensor_msgs::ImageConstPtr& cam1_img,
+  const CameraMeasurementConstPtr& feature_msg) {
+
+  //save exposure Time into moving window
+  cam0.moving_window[state_server.imu_state.id].exposureTime_ms = strtod(cam0_img->header.frame_id.data(), NULL) / 1000000;
+  cam1.moving_window[state_server.imu_state.id].exposureTime_ms = strtod(cam1_img->header.frame_id.data(), NULL) / 1000000;
+  if(cam0.moving_window[state_server.imu_state.id].exposureTime_ms < 1)
+    cam0.moving_window[state_server.imu_state.id].exposureTime_ms = 1;
+  if(cam1.moving_window[state_server.imu_state.id].exposureTime_ms < 1)
+    cam1.moving_window[state_server.imu_state.id].exposureTime_ms = 1;
+  if(cam0.moving_window[state_server.imu_state.id].exposureTime_ms > 100)
+    cam0.moving_window[state_server.imu_state.id].exposureTime_ms = 100;
+  if(cam1.moving_window[state_server.imu_state.id].exposureTime_ms > 100)
+    cam1.moving_window[state_server.imu_state.id].exposureTime_ms = 100;
+
+  // Get the current image.
+  cv_bridge::CvImageConstPtr cam0_img_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);
+
+  // save image information into moving window
+  cam0.moving_window[state_server.imu_state.id].image = cam0_img_ptr->image.clone();
+  cam1.moving_window[state_server.imu_state.id].image = cam1_img_ptr->image.clone();
+
+  //TODO handle any massive overflow correctly (should be pruned, before this ever triggers)
+  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.
@@ -274,12 +461,14 @@ void MsckfVio::initializeGravityAndBias() {
  // is consistent with the inertial frame.
  double gravity_norm = gravity_imu.norm();
  IMUState::gravity = Vector3d(0.0, 0.0, -gravity_norm);
-
+  
  Quaterniond q0_i_w = Quaterniond::FromTwoVectors(
    gravity_imu, -IMUState::gravity);
  state_server.imu_state.orientation =
    rotationToQuaternion(q0_i_w.toRotationMatrix().transpose());
-
+    // printf("gravity Norm %f\n", gravity_norm);
+    // printf("linear_acc %f, %f, %f\n", gravity_imu(0), gravity_imu(1), gravity_imu(2));
+    // printf("quaterniond: %f, %f, %f, %f\n", q0_i_w.w(),  q0_i_w.x(), q0_i_w.y(), q0_i_w.z());
  return;
 }

@@ -290,7 +479,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,97 +536,16 @@ 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;
 }

-void MsckfVio::featureCallback(
-    const CameraMeasurementConstPtr& msg) {
-
-  // Return if the gravity vector has not been set.
-  if (!is_gravity_set) return;
-
-  // 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 = 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 msg.
-  ros::Time start_time = ros::Time::now();
-  batchImuProcessing(msg->header.stamp.toSec());
-  double imu_processing_time = (
-      ros::Time::now()-start_time).toSec();
-
-  // Augment the state vector.
-  start_time = ros::Time::now();
-  stateAugmentation(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(msg);
-  double add_observations_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(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::mocapOdomCallback(
    const nav_msgs::OdometryConstPtr& msg) {
  static bool first_mocap_odom_msg = true;
@@ -716,6 +823,7 @@ void MsckfVio::stateAugmentation(const double& time) {
  cam_state.orientation_null = cam_state.orientation;
  cam_state.position_null = cam_state.position;

+
  // Update the covariance matrix of the state.
  // To simplify computation, the matrix J below is the nontrivial block
  // in Equation (16) in "A Multi-State Constraint Kalman Filter for Vision
@@ -754,6 +862,7 @@ void MsckfVio::stateAugmentation(const double& time) {
  return;
 }

+
 void MsckfVio::addFeatureObservations(
    const CameraMeasurementConstPtr& msg) {

@@ -786,6 +895,184 @@ void MsckfVio::addFeatureObservations(
  return;
 }

+void MsckfVio::PhotometricMeasurementJacobian(
+    const StateIDType& cam_state_id,
+    const FeatureIDType& feature_id,
+    Matrix<double, 4, 6>& H_x, Matrix<double, 4, 3>& H_f, Vector4d& r) {
+
+  // Prepare all the required data.
+  const CAMState& cam_state = state_server.cam_states[cam_state_id];
+  const Feature& feature = map_server[feature_id];
+
+  // Cam0 pose.
+  Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation);
+  const Vector3d& t_c0_w = cam_state.position;
+
+  // Cam1 pose.
+  Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear();
+  Matrix3d R_w_c1 = CAMState::T_cam0_cam1.linear() * R_w_c0;
+  Vector3d t_c1_w = t_c0_w - R_w_c1.transpose()*CAMState::T_cam0_cam1.translation();
+
+  // 3d feature position in the world frame.
+  // And its observation with the stereo cameras.
+  const Vector3d& p_w = feature.position;
+
+  //observation
+  const Vector4d& z = feature.observations.find(cam_state_id)->second;
+
+  //photometric observation
+  std::vector<float> photo_z;
+  feature.FrameIrradiance(cam_state, cam_state_id, cam0, photo_z);
+
+  // Convert the feature position from the world frame to
+  // the cam0 and cam1 frame.
+  Vector3d p_c0 = R_w_c0 * (p_w-t_c0_w);
+  Vector3d p_c1 = R_w_c1 * (p_w-t_c1_w);
+
+
+  //compute resulting esimtated position in image
+  cv::Point2f out_p = cv::Point2f(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2));
+  std::vector<cv::Point2f> out_v;
+  out_v.push_back(out_p);
+
+  // Compute the Jacobians.
+  Matrix<double, 4, 3> dz_dpc0 = Matrix<double, 4, 3>::Zero();
+  dz_dpc0(0, 0) = 1 / p_c0(2);
+  dz_dpc0(1, 1) = 1 / p_c0(2);
+  dz_dpc0(0, 2) = -p_c0(0) / (p_c0(2)*p_c0(2));
+  dz_dpc0(1, 2) = -p_c0(1) / (p_c0(2)*p_c0(2));
+
+  Matrix<double, 4, 3> dz_dpc1 = Matrix<double, 4, 3>::Zero();
+  dz_dpc1(2, 0) = 1 / p_c1(2);
+  dz_dpc1(3, 1) = 1 / p_c1(2);
+  dz_dpc1(2, 2) = -p_c1(0) / (p_c1(2)*p_c1(2));
+  dz_dpc1(3, 2) = -p_c1(1) / (p_c1(2)*p_c1(2));
+
+  Matrix<double, 3, 6> dpc0_dxc = Matrix<double, 3, 6>::Zero();
+  dpc0_dxc.leftCols(3) = skewSymmetric(p_c0);
+  dpc0_dxc.rightCols(3) = -R_w_c0;
+
+  Matrix<double, 3, 6> dpc1_dxc = Matrix<double, 3, 6>::Zero();
+  dpc1_dxc.leftCols(3) = R_c0_c1 * skewSymmetric(p_c0);
+  dpc1_dxc.rightCols(3) = -R_w_c1;
+
+  Matrix3d dpc0_dpg = R_w_c0;
+  Matrix3d dpc1_dpg = R_w_c1;
+
+  H_x = dz_dpc0*dpc0_dxc + dz_dpc1*dpc1_dxc;
+  H_f = dz_dpc0*dpc0_dpg + dz_dpc1*dpc1_dpg;
+
+  // Modifty the measurement Jacobian to ensure
+  // observability constrain.
+  Matrix<double, 4, 6> A = H_x;
+  Matrix<double, 6, 1> u = Matrix<double, 6, 1>::Zero();
+  u.block<3, 1>(0, 0) = quaternionToRotation(
+      cam_state.orientation_null) * IMUState::gravity;
+  u.block<3, 1>(3, 0) = skewSymmetric(
+      p_w-cam_state.position_null) * IMUState::gravity;
+  H_x = A - A*u*(u.transpose()*u).inverse()*u.transpose();
+  H_f = -H_x.block<4, 3>(0, 3);
+
+  // Compute the residual.
+  r = z - Vector4d(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2),
+      p_c1(0)/p_c1(2), p_c1(1)/p_c1(2));
+
+  // visu -residual
+  //printf("-----\n");
+
+  //estimate photometric measurement
+  std::vector<float> estimate_photo_z;
+  feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_photo_z);
+  std::vector<float> photo_r;
+  
+  //calculate photom. residual
+  for(int i = 0; i < photo_z.size(); i++)
+    photo_r.push_back(photo_z[i] - estimate_photo_z[i]);
+
+  // visu- residual
+  //for(int i = 0; i < photo_z.size(); i++)
+  //  printf("%.4f = %.4f - %.4f\n",photo_r[i], photo_z[i], estimate_photo_z[i]);
+
+  photo_z.clear();
+  return;
+}
+
+void MsckfVio::PhotometricFeatureJacobian(
+    const FeatureIDType& feature_id,
+    const std::vector<StateIDType>& cam_state_ids,
+    MatrixXd& H_x, VectorXd& r) {
+
+  const auto& feature = map_server[feature_id];
+
+  // Check how many camera states in the provided camera
+  // id camera has actually seen this feature.
+  vector<StateIDType> valid_cam_state_ids(0);
+  for (const auto& cam_id : cam_state_ids) {
+    if (feature.observations.find(cam_id) ==
+        feature.observations.end()) continue;
+
+    valid_cam_state_ids.push_back(cam_id);
+  }
+
+  int jacobian_row_size = 0;
+  jacobian_row_size = 4 * valid_cam_state_ids.size();
+
+  MatrixXd H_xj = MatrixXd::Zero(jacobian_row_size,
+      21+state_server.cam_states.size()*6);
+  MatrixXd H_fj = MatrixXd::Zero(jacobian_row_size, 3);
+  VectorXd r_j = VectorXd::Zero(jacobian_row_size);
+  int stack_cntr = 0;
+
+
+  // visu - residual
+  //printf("_____FEATURE:_____\n");
+  // visu - feature
+  //cam0.featureVisu.release();
+
+  for (const auto& cam_id : valid_cam_state_ids) {
+
+    Matrix<double, 4, 6> H_xi = Matrix<double, 4, 6>::Zero();
+    Matrix<double, 4, 3> H_fi = Matrix<double, 4, 3>::Zero();
+    Vector4d r_i = Vector4d::Zero();
+    PhotometricMeasurementJacobian(cam_id, feature.id, H_xi, H_fi, r_i);
+
+    auto cam_state_iter = state_server.cam_states.find(cam_id);
+    int cam_state_cntr = std::distance(
+        state_server.cam_states.begin(), cam_state_iter);
+
+    // Stack the Jacobians.
+    H_xj.block<4, 6>(stack_cntr, 21+6*cam_state_cntr) = H_xi;
+    H_fj.block<4, 3>(stack_cntr, 0) = H_fi;
+    r_j.segment<4>(stack_cntr) = r_i;
+    stack_cntr += 4;
+  }
+  // visu - feature
+  /*
+  if(!cam0.featureVisu.empty() && cam0.featureVisu.size().width > 3000)
+    imshow("feature", cam0.featureVisu);
+  cvWaitKey(1);
+
+  if((ros::Time::now() - image_save_time).toSec() > 1)
+  {
+    std::stringstream ss;
+    ss << "/home/raphael/dev/MSCKF_ws/img/feature_" << std::to_string(ros::Time::now().toSec()) << ".jpg";
+    imwrite(ss.str(), cam0.featureVisu);
+    image_save_time = ros::Time::now();
+  }
+  */
+
+  // Project the residual and Jacobians onto the nullspace
+  // of H_fj.
+  JacobiSVD<MatrixXd> svd_helper(H_fj, ComputeFullU | ComputeThinV);
+  MatrixXd A = svd_helper.matrixU().rightCols(
+      jacobian_row_size - 3);
+
+  H_x = A.transpose() * H_xj;
+  r = A.transpose() * r_j;
+
+  return;
+}
+
 void MsckfVio::measurementJacobian(
    const StateIDType& cam_state_id,
    const FeatureIDType& feature_id,
@@ -1073,6 +1360,14 @@ void MsckfVio::removeLostFeatures() {
        }
      }
    }
+    if(!feature.is_anchored)
+    {
+      if(!feature.initializeAnchor(cam0))
+      {
+        invalid_feature_ids.push_back(feature.id);
+        continue;
+      }
+    }

    jacobian_row_size += 4*feature.observations.size() - 3;
    processed_feature_ids.push_back(feature.id);
@@ -1105,7 +1400,7 @@ void MsckfVio::removeLostFeatures() {

    MatrixXd H_xj;
    VectorXd r_j;
-    featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
+    PhotometricFeatureJacobian(feature.id, cam_state_ids, H_xj, r_j);

    if (gatingTest(H_xj, r_j, cam_state_ids.size()-1)) {
      H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
@@ -1203,7 +1498,6 @@ void MsckfVio::pruneCamStateBuffer() {
      feature.observations.erase(involved_cam_state_ids[0]);
      continue;
    }
-
    if (!feature.is_initialized) {
      // Check if the feature can be initialize.
      if (!feature.checkMotion(state_server.cam_states)) {
@@ -1221,7 +1515,15 @@ void MsckfVio::pruneCamStateBuffer() {
        }
      }
    }
-
+    if(!feature.is_anchored)
+    {
+      if(!feature.initializeAnchor(cam0))
+      {
+        for (const auto& cam_id : involved_cam_state_ids)
+              feature.observations.erase(cam_id);
+        continue;
+      }
+    }
    jacobian_row_size += 4*involved_cam_state_ids.size() - 3;
  }

@@ -1232,7 +1534,6 @@ void MsckfVio::pruneCamStateBuffer() {
      21+6*state_server.cam_states.size());
  VectorXd r = VectorXd::Zero(jacobian_row_size);
  int stack_cntr = 0;
-
  for (auto& item : map_server) {
    auto& feature = item.second;
    // Check how many camera states to be removed are associated
@@ -1248,8 +1549,8 @@ void MsckfVio::pruneCamStateBuffer() {

    MatrixXd H_xj;
    VectorXd r_j;
-    featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
-
+    
+    PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
    if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {
      H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
      r.segment(stack_cntr, r_j.rows()) = r_j;
@@ -1265,7 +1566,7 @@ void MsckfVio::pruneCamStateBuffer() {

  // Perform measurement update.
  measurementUpdate(H_x, r);
-
+ 
  for (const auto& cam_id : rm_cam_state_ids) {
    int cam_sequence = std::distance(state_server.cam_states.begin(),
        state_server.cam_states.find(cam_id));
@@ -1298,6 +1599,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;
--- a/src/utils.cpp
+++ b/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;
Author	SHA1	Message	Date
g-spacewhale	8defb20c8e	commented visu parts cleanly	2019-04-19 13:32:16 +02:00
g-spacewhale	1949e4c43d	removed visu as result works so to not clutter the output	2019-04-19 13:30:30 +02:00
g-spacewhale	6f16f1b566	image reprojection visualization in images	2019-04-19 13:11:19 +02:00
g-spacewhale	1fa2518215	fixed incorrect undistortion/distortion. residual should now be calculated correctly	2019-04-18 16:22:41 +02:00
g-spacewhale	d91ff7ca9d	added tum launch files, removed anchor procedure being called multiple times through a flag	2019-04-18 11:06:45 +02:00
g-spacewhale	cfecefe29f	reinstantiated photometry removed slow-down problem	2019-04-17 17:06:44 +02:00
g-spacewhale	f4a17f8512	deactivated most to find reason for slowdown	2019-04-17 16:16:45 +02:00
g-spacewhale	6ae83f0db7	added saving exposure time from the frame ID, where the TUM dataset saves it	2019-04-17 10:54:54 +02:00
g-spacewhale	819e43bb3b	fixed pixel position return value	2019-04-17 09:03:27 +02:00
g-spacewhale	7f2140ae88	moved camera calibration information into a struct to make data handling smoother	2019-04-16 19:05:11 +02:00
g-spacewhale	010d36d216	added projection into feature observations camera states	2019-04-16 17:40:33 +02:00
g-spacewhale	abd343f576	corrected position calculation for NxN points	2019-04-12 19:04:45 +02:00
g-spacewhale	8227a8e48d	added position calculation	2019-04-12 17:37:01 +02:00
g-spacewhale	a85a4745f2	added anchor information generation	2019-04-12 11:02:58 +02:00
g-spacewhale	a6af82a269	manage moving window saves copies of images	2019-04-10 19:10:31 +02:00
g-spacewhale	b0dca3b15c	added pseudocode of original msckf	2019-04-10 18:43:30 +02:00
g-spacewhale	79cce26dad	added moving window structure, not yet done. added timestame sync for images and features detected	2019-04-10 18:36:11 +02:00
g-spacewhale	e6620a4ed4	edited launch files to support euroc and mynt	2019-04-10 18:35:26 +02:00