finalized README

.vscode/c_cpp_properties.json

@@ -0,0 +1,16 @@
+{
+    "configurations": [
+        {
+            "name": "Linux",
+            "includePath": [
+                "${workspaceFolder}/**"
+            ],
+            "defines": [],
+            "compilerPath": "/usr/bin/gcc",
+            "cStandard": "c11",
+            "cppStandard": "c++14",
+            "intelliSenseMode": "clang-x64"
+        }
+    ],
+    "version": 4
+}

.vscode/settings.json

@@ -0,0 +1,6 @@
+{
+    "files.associations": {
+        "core": "cpp",
+        "sparsecore": "cpp"
+    }
+}

CMakeLists.txt

@@ -24,6 +24,7 @@ find_package(catkin REQUIRED COMPONENTS
   pcl_conversions
   pcl_ros
   std_srvs
+  visualization_msgs
 )
 
 ## System dependencies are found with CMake's conventions
@@ -79,6 +80,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 +89,7 @@ add_dependencies(msckf_vio
 target_link_libraries(msckf_vio
   ${catkin_LIBRARIES}
   ${SUITESPARSE_LIBRARIES}
+  ${OpenCV_LIBRARIES}
 )
 
 # Msckf Vio nodelet
@@ -106,6 +109,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}

README.md

@@ -1,12 +1,22 @@
 # MSCKF\_VIO
 
 
-The `MSCKF_VIO` package is a stereo version of MSCKF. The software takes in synchronized stereo images and IMU messages and generates real-time 6DOF pose estimation of the IMU frame.
+The `MSCKF_VIO` package is a stereo-photometric version of MSCKF. The software takes in synchronized stereo images and IMU messages and generates real-time 6DOF pose estimation of the IMU frame.
 
-The software is tested on Ubuntu 16.04 with ROS Kinetic.
+This approach is based on the paper written by Ke Sun et al.
+[https://arxiv.org/abs/1712.00036](https://arxiv.org/abs/1712.00036) and their Stereo MSCKF implementation, which tightly fuse the matched feature information of a stereo image pair into a 6DOF Pose.
+The approach implemented in this repository follows the semi-dense msckf approach tightly fusing the photometric
+information around the matched featues into the covariance matrix, as described and derived in the master thesis [Pose Estimation using a Stereo-Photometric Multi-State Constraint Kalman Filter](http://raphael.maenle.net/resources/sp-msckf/maenle_master_thesis.pdf).
+
+It's positioning is comparable to the approach from Ke Sun et al. with the photometric approach, with a higher
+computational load, especially with larger image patches around the feature. A video explaining the approach can be
+found on [https://youtu.be/HrqQywAnenQ](https://youtu.be/HrqQywAnenQ): 
+<br/>
+[![Stereo-Photometric MSCKF](https://img.youtube.com/vi/HrqQywAnenQ/0.jpg)](https://www.youtube.com/watch?v=HrqQywAnenQ)
+
+<br/>
+This software should be deployed using ROS Kinetic on Ubuntu 16.04 or 18.04.
 
-Video: [https://www.youtube.com/watch?v=jxfJFgzmNSw&t](https://www.youtube.com/watch?v=jxfJFgzmNSw&t=3s)<br/>
-Paper Draft: [https://arxiv.org/abs/1712.00036](https://arxiv.org/abs/1712.00036)
 
 ## License
 
@@ -28,16 +38,6 @@ cd your_work_space
 catkin_make --pkg msckf_vio --cmake-args -DCMAKE_BUILD_TYPE=Release
 ```
 
-## Calibration
-
-An accurate calibration is crucial for successfully running the software. To get the best performance of the software, the stereo cameras and IMU should be hardware synchronized. Note that for the stereo calibration, which includes the camera intrinsics, distortion, and extrinsics between the two cameras, you have to use a calibration software. **Manually setting these parameters will not be accurate enough.** [Kalibr](https://github.com/ethz-asl/kalibr) can be used for the stereo calibration and also to get the transformation between the stereo cameras and IMU. The yaml file generated by Kalibr can be directly used in this software. See calibration files in the `config` folder for details. The two calibration files in the `config` folder should work directly with the EuRoC and [fast flight](https://github.com/KumarRobotics/msckf_vio/wiki) datasets. The convention of the calibration file is as follows:
-
-`camx/T_cam_imu`: takes a vector from the IMU frame to the camx frame.
-`cam1/T_cn_cnm1`: takes a vector from the cam0 frame to the cam1 frame.
-
-The filter uses the first 200 IMU messages to initialize the gyro bias, acc bias, and initial orientation. Therefore, the robot is required to start from a stationary state in order to initialize the VIO successfully.
-
-
 ## EuRoC and UPenn Fast flight dataset example usage
 
 First obtain either the [EuRoC](https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets) or the [UPenn fast flight](https://github.com/KumarRobotics/msckf_vio/wiki/Dataset) dataset.
@@ -75,6 +75,8 @@ To visualize the pose and feature estimates you can use the provided rviz config
 
 ## ROS Nodes
 
+The general structure is similar to the structure of the MSCKF implementation this repository is derived from.
+
 ### `image_processor` node
 
 **Subscribed Topics**

config/camchain-imucam-euroc.yaml

@@ -9,7 +9,7 @@ cam0:
                      0,                   0,                   0,   1.000000000000000]
   camera_model: pinhole
   distortion_coeffs: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05]
-  distortion_model: radtan
+  distortion_model: pre-radtan
   intrinsics: [458.654, 457.296, 367.215, 248.375]
   resolution: [752, 480]
   timeshift_cam_imu: 0.0
@@ -26,7 +26,7 @@ cam1:
                      0,                   0,                   0,   1.000000000000000]
   camera_model: pinhole
   distortion_coeffs: [-0.28368365,  0.07451284, -0.00010473, -3.55590700e-05]
-  distortion_model: radtan
+  distortion_model: pre-radtan
   intrinsics: [457.587, 456.134, 379.999, 255.238]
   resolution: [752, 480]
   timeshift_cam_imu: 0.0

config/camchain-imucam-tum-scaled.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.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202,
+    0.00020293673591811182]
+  distortion_model: pre-equidistant
+  intrinsics: [190.97847715128717, 190.9733070521226, 254.93170605935475, 256.8974428996504]
+  resolution: [512, 512]
+  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.0034003170790442797, 0.001766278153469831, -0.00266312569781606,
+    0.0003299517423931039]
+  distortion_model: pre-equidistant
+  intrinsics: [190.44236969414825, 190.4344384721956, 252.59949716835982, 254.91723064636983]
+  resolution: [512, 512]
+  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]

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]

include/msckf_vio/cam_state.h

@@ -15,6 +15,37 @@
 #include "imu_state.h"
 
 namespace msckf_vio {
+
+struct Frame{
+  cv::Mat image;
+  cv::Mat dximage;
+  cv::Mat dyimage;
+  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;
+  int id;
+};
+
+
+
 /*
  * @brief CAMState Stored camera state in order to
  *    form measurement model.
@@ -35,6 +66,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

include/msckf_vio/image_handler.h

@@ -0,0 +1,61 @@
+#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 {
+
+cv::Point2f pinholeDownProject(const cv::Point2f& p_in, const cv::Vec4d& intrinsics);
+cv::Point2f pinholeUpProject(const cv::Point2f& p_in, const cv::Vec4d& intrinsics);
+
+void undistortImage(
+  cv::InputArray src,
+  cv::OutputArray dst,
+  const std::string& distortion_model,
+  const cv::Vec4d& intrinsics,
+  const cv::Vec4d& distortion_coeffs);
+
+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);
+
+void undistortPoint(
+    const cv::Point2f& pt_in,
+    const cv::Vec4d& intrinsics,
+    const std::string& distortion_model,
+    const cv::Vec4d& distortion_coeffs,
+    cv::Point2f& pt_out,
+    const cv::Matx33d &rectification_matrix = cv::Matx33d::eye(),
+    const cv::Vec4d &new_intrinsics = cv::Vec4d(1,1,0,0));
+
+}
+}
+#endif

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) {
@@ -322,6 +320,8 @@ private:
     return;
   }
 
+  bool STREAMPAUSE;
+
   // Indicate if this is the first image message.
   bool is_first_img;
 
@@ -336,15 +336,8 @@ private:
   std::vector<sensor_msgs::Imu> imu_msg_buffer;
 
   // Camera calibration parameters
-  std::string cam0_distortion_model;
+  CameraCalibration cam0;
-  cv::Vec2i cam0_resolution;
+  CameraCalibration cam1;
-  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;
 
   // Take a vector from cam0 frame to the IMU frame.
   cv::Matx33d R_cam0_imu;
@@ -367,13 +360,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;

include/msckf_vio/math_utils.hpp

@@ -43,6 +43,50 @@ inline void quaternionNormalize(Eigen::Vector4d& q) {
   return;
 }
 
+/*
+ * @brief invert rotation of passed quaternion through conjugation
+ *        and return conjugation
+ */
+inline Eigen::Vector4d quaternionConjugate(Eigen::Vector4d& q)
+{
+  Eigen::Vector4d p;
+  p(0) = -q(0);
+  p(1) = -q(1);
+  p(2) = -q(2);
+  p(3) =  q(3);
+  quaternionNormalize(p);
+  return p;
+}
+
+/*
+ * @brief converts a vector4 to a vector3, dropping (3)
+ *        this is typically used to get the vector part of a quaternion
+          for eq small angle approximation
+*/
+inline Eigen::Vector3d v4tov3(const Eigen::Vector4d& q)
+{
+  Eigen::Vector3d p;
+  p(0) = q(0);
+  p(1) = q(1);
+  p(2) = q(2);
+  return p;
+}
+
+/*
+ * @brief Perform q1 * q2
+ */
+
+inline Eigen::Vector4d QtoV(const Eigen::Quaterniond& q)
+{
+  Eigen::Vector4d p;
+  p(0) = q.x();
+  p(1) = q.y();
+  p(2) = q.z();
+  p(3) = q.w();
+  return p;
+}
+
+
 /*
  * @brief Perform q1 * q2
  */

include/msckf_vio/msckf_vio.h

@@ -14,11 +14,17 @@
 #include <string>
 #include <Eigen/Dense>
 #include <Eigen/Geometry>
+#include <math.h>
 #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,13 @@
 #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>
+
+#define PI 3.14159265
+
 namespace msckf_vio {
 /*
  * @brief MsckfVio Implements the algorithm in
@@ -97,11 +110,27 @@ class MsckfVio {
     void imuCallback(const sensor_msgs::ImuConstPtr& msg);
 
     /*
-     * @brief featureCallback
+     * @brief truthCallback
-     *    Callback function for feature measurements.
+     *      Callback function for ground truth navigation information
-     * @param msg Stereo feature measurements.
+     * @param TransformStamped msg
      */    
-    void featureCallback(const CameraMeasurementConstPtr& msg);
+    void truthCallback(
+        const geometry_msgs::TransformStampedPtr& msg);
+
+
+    /*
+     * @brief imageCallback
+     *    Callback function for feature measurements
+     *    Triggers measurement update
+     * @param msg
+     *    Camera 0 Image
+     *    Camera 1 Image
+     *    Stereo feature measurements.
+     */
+    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 +155,26 @@ class MsckfVio {
     bool resetCallback(std_srvs::Trigger::Request& req,
         std_srvs::Trigger::Response& res);
 
+    // Saves the exposure time and the camera measurementes
+    void manageMovingWindow(
+        const sensor_msgs::ImageConstPtr& cam0_img,
+        const sensor_msgs::ImageConstPtr& cam1_img,
+        const CameraMeasurementConstPtr& feature_msg);
+
+
+    void calcErrorState();
+
+    // Debug related Functions
+    // Propagate the true state
+    void batchTruthProcessing(
+        const double& time_bound);
+
+
+    void processTruthtoIMU(const double& time,
+        const Eigen::Vector4d& m_rot,
+        const Eigen::Vector3d& m_trans);
+
+
     // Filter related functions
     // Propogate the state
     void batchImuProcessing(
@@ -137,8 +186,12 @@ class MsckfVio {
         const Eigen::Vector3d& gyro,
         const Eigen::Vector3d& acc);
 
+    // groundtruth state augmentation
+    void truePhotometricStateAugmentation(const double& time);
+
     // Measurement update
     void stateAugmentation(const double& time);
+    void PhotometricStateAugmentation(const double& time);
     void addFeatureObservations(const CameraMeasurementConstPtr& msg);
     // This function is used to compute the measurement Jacobian
     // for a single feature observed at a single camera frame.
@@ -149,25 +202,118 @@ class MsckfVio {
         Eigen::Vector4d& r);
     // This function computes the Jacobian of all measurements viewed
     // in the given camera states of this feature.
-    void featureJacobian(const FeatureIDType& feature_id,
+    bool featureJacobian(
+        const FeatureIDType& feature_id,
         const std::vector<StateIDType>& cam_state_ids,
         Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
+
+
+    void twodotMeasurementJacobian(
+        const StateIDType& cam_state_id,
+        const FeatureIDType& feature_id,
+        Eigen::MatrixXd& H_x, Eigen::MatrixXd& H_y, Eigen::VectorXd& r);
+
+    bool ConstructJacobians(
+        Eigen::MatrixXd& H_rho,
+        Eigen::MatrixXd& H_pl,
+        Eigen::MatrixXd& H_pA,
+        const Feature& feature,
+        const StateIDType&  cam_state_id,
+        Eigen::MatrixXd& H_xl,
+        Eigen::MatrixXd& H_yl);
+
+    bool PhotometricPatchPointResidual(
+        const StateIDType& cam_state_id,
+        const Feature& feature, 
+        Eigen::VectorXd& r);
+
+    bool PhotometricPatchPointJacobian(
+        const CAMState& cam_state,
+        const StateIDType& cam_state_id,
+        const Feature& feature,
+        Eigen::Vector3d point,
+        int count,
+        Eigen::Matrix<double, 2, 1>& H_rhoj,
+        Eigen::Matrix<double, 2, 6>& H_plj,
+        Eigen::Matrix<double, 2, 6>& H_pAj,
+        Eigen::Matrix<double, 2, 4>& dI_dhj);
+
+    bool PhotometricMeasurementJacobian(
+        const StateIDType& cam_state_id,
+        const FeatureIDType& feature_id,
+        Eigen::MatrixXd& H_x,
+        Eigen::MatrixXd& H_y,
+        Eigen::VectorXd& r);
+
+
+    bool twodotFeatureJacobian(
+        const FeatureIDType& feature_id,
+        const std::vector<StateIDType>& cam_state_ids,
+        Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
+
+    bool PhotometricFeatureJacobian(
+        const FeatureIDType& feature_id,
+        const std::vector<StateIDType>& cam_state_ids,
+        Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
+
+    void photometricMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r);
     void measurementUpdate(const Eigen::MatrixXd& H,
         const Eigen::VectorXd& r);
+    void twoMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r);
+
     bool gatingTest(const Eigen::MatrixXd& H,
-        const Eigen::VectorXd&r, const int& dof);
+        const Eigen::VectorXd&r, const int& dof, int filter=0);
     void removeLostFeatures();
     void findRedundantCamStates(
         std::vector<StateIDType>& rm_cam_state_ids);
+
+    void pruneLastCamStateBuffer();
     void pruneCamStateBuffer();
     // Reset the system online if the uncertainty is too large.
     void onlineReset();
 
+    // Photometry flag
+    int FILTER;
+
+    // debug flag
+    bool STREAMPAUSE;
+    bool PRINTIMAGES;
+    bool GROUNDTRUTH;
+
+    bool nan_flag;
+    bool play;
+    double last_time_bound;
+    double time_offset;
+
+    // Patch size for Photometry
+    int N;
+    // Image rescale
+    int SCALE; 
     // Chi squared test table.
     static std::map<int, double> chi_squared_test_table;
 
+    double eval_time;
+
+    IMUState timed_old_imu_state;
+    IMUState timed_old_true_state;
+
+    IMUState old_imu_state;
+    IMUState old_true_state;
+
+    // change in position
+    Eigen::Vector3d delta_position;
+    Eigen::Vector3d delta_orientation;
+
     // State vector
     StateServer state_server;
+    StateServer photometric_state_server;
+
+    // Ground truth state vector 
+    StateServer true_state_server;
+
+    // error state based on ground truth
+    StateServer err_state_server;
+
     // Maximum number of camera states
     int max_cam_state_size;
 
@@ -179,6 +325,22 @@ class MsckfVio {
     // transfer delay between IMU and Image messages.
     std::vector<sensor_msgs::Imu> imu_msg_buffer;
 
+    // Ground Truth message data
+    std::vector<geometry_msgs::TransformStamped> truth_msg_buffer;
+    // Moving Window buffer
+    movingWindow cam0_moving_window;
+    movingWindow cam1_moving_window;
+
+    // Camera calibration parameters
+    CameraCalibration cam0;
+    CameraCalibration cam1;
+
+    // covariance data
+    double irradiance_frame_bias;
+
+
+    ros::Time image_save_time;
+
     // Indicate if the gravity vector is set.
     bool is_gravity_set;
 
@@ -206,12 +368,21 @@ class MsckfVio {
 
     // Subscribers and publishers
     ros::Subscriber imu_sub;
-    ros::Subscriber feature_sub;
+    ros::Subscriber truth_sub;
+    ros::Publisher truth_odom_pub;
     ros::Publisher odom_pub;
+    ros::Publisher marker_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;
@@ -232,6 +403,9 @@ class MsckfVio {
     ros::Publisher mocap_odom_pub;
     geometry_msgs::TransformStamped raw_mocap_odom_msg;
     Eigen::Isometry3d mocap_initial_frame;
+
+    Eigen::Vector4d mocap_initial_orientation;
+    Eigen::Vector3d mocap_initial_position;
 };
 
 typedef MsckfVio::Ptr MsckfVioPtr;

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

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"/>

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>

launch/image_processor_tinytum.launch

@@ -0,0 +1,38 @@
+<launch>
+
+  <arg name="robot" default="firefly_sbx"/>
+  <arg name="calibration_file"
+    default="$(find msckf_vio)/config/camchain-imucam-tum-scaled.yaml"/>
+
+  <!-- Image Processor Nodelet  -->
+  <group ns="$(arg robot)">
+    <node pkg="nodelet" type="nodelet" name="image_processor"
+      args="standalone msckf_vio/ImageProcessorNodelet"
+      output="screen"
+       >
+
+      <!-- Debugging Flaggs -->
+      <param name="StreamPause" value="true"/>
+
+      <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="5"/>
+      <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>

launch/image_processor_tum.launch

@@ -0,0 +1,37 @@
+<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"
+       >
+
+      <!-- Debugging Flaggs -->
+      <param name="StreamPause" value="true"/>
+
+      <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>

launch/msckf_vio_debug_tum.launch

@@ -0,0 +1,75 @@
+<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"
+      launch-prefix="xterm -e gdb --args">
+
+      <!-- Photometry Flag-->
+      <param name="PHOTOMETRIC" value="true"/>
+
+      <!-- Debugging Flaggs -->
+      <param name="PrintImages" value="true"/>
+      <param name="GroundTruth" value="false"/>
+
+      <param name="patch_size_n" value="3"/>
+
+      <!-- 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"/>
+      <param name="initial_covariance/irradiance_frame_bias" value="0.1"/>
+
+      <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>

launch/msckf_vio_euroc.launch

@@ -17,6 +17,18 @@
       args='standalone msckf_vio/MsckfVioNodelet'
       output="screen">
 
+
+      <!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two -->
+      <param name="FILTER" value="1"/>
+
+      <!-- Debugging Flaggs -->
+      <param name="StreamPause" value="true"/>
+      <param name="PrintImages" value="true"/>
+      <param name="GroundTruth" value="false"/>
+
+      <param name="patch_size_n" value="5"/>
+      <param name="image_scale" value ="1"/>
+
       <!-- Calibration parameters -->
       <rosparam command="load" file="$(arg calibration_file)"/>
 
@@ -53,6 +65,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>

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>

launch/msckf_vio_tinytum.launch

@@ -0,0 +1,77 @@
+<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-scaled.yaml"/>
+
+  <!-- Image Processor Nodelet  -->
+  <include file="$(find msckf_vio)/launch/image_processor_tinytum.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">
+
+      <param name="FILTER" value="0"/>
+
+      <!-- Debugging Flaggs -->
+      <param name="StreamPause" value="true"/>
+      <param name="PrintImages" value="false"/>
+      <param name="GroundTruth" value="false"/>
+
+      <param name="patch_size_n" value="3"/>
+      <param name="image_scale" value ="1"/>
+
+      <!-- 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="12"/>
+      <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"/>
+      <param name="initial_covariance/irradiance_frame_bias" value="0.1"/>
+
+      <remap from="~imu" to="/imu0"/>
+      <remap from="~ground_truth" to="/vrpn_client/raw_transform"/>
+      <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>

launch/msckf_vio_tum.launch

@@ -0,0 +1,78 @@
+<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">
+
+
+      <!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two -->
+      <param name="FILTER" value="1"/>
+
+      <!-- Debugging Flaggs -->
+      <param name="StreamPause" value="true"/>
+      <param name="PrintImages" value="false"/>
+      <param name="GroundTruth" value="false"/>
+
+      <param name="patch_size_n" value="3"/>
+      <!-- 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"/>
+      <param name="initial_covariance/irradiance_frame_bias" value="0.1"/>
+
+      <remap from="~imu" to="/imu0"/>
+      <remap from="~ground_truth" to="/vrpn_client/raw_transform"/>
+      <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>
+
+  <!--node name="player" pkg="bagcontrol" type="control.py" /-->
+
+</launch>

log

@@ -0,0 +1,73 @@
+# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST <raphael@raphael-desktop>
+# name: Hx
+# type: matrix
+# rows: 18
+# columns: 49
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 313.795 -58.7912 139.778 -46.7616 144.055 86.9644 0 -314.123 55.6434 -140.648 46.7616 -144.055 -86.9644 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 441.06 -94.50069999999999 174.424 -53.7653 204.822 120.248 0 -441.685 90.1101 -175.657 53.7653 -204.822 -120.248 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 225.35 -54.5629 77.60599999999999 -21.1425 105.886 60.3706 0 -225.756 52.3373 -78.2406 21.1425 -105.886 -60.3706 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 175.128 20.6203 175.127 -79.63939999999999 73.245 62.1868 0 -174.573 -22.5235 -175.576 79.63939999999999 -73.245 -62.1868 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 296.962 43.5469 311.307 -143.667 123.399 108.355 0 -295.905 -46.7952 -312.063 143.667 -123.399 -108.355 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 126.283 117.889 311.864 -161.264 38.8521 71.8019 0 -124.464 -119.541 -312.118 161.264 -38.8521 -71.8019 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 49.2502 63.7166 155.071 -80.81950000000001 12.7732 32.1826 0 -48.2934 -64.4113 -155.157 80.81950000000001 -12.7732 -32.1826 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 69.59699999999999 154.579 335.384 -179.355 9.212580000000001 62.0364 0 -67.35599999999999 -155.735 -335.462 179.355 -9.212580000000001 -62.0364 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -66.6965 304.947 500.218 -285.589 -71.31010000000001 55.5058 0 70.8009 -305.077 -499.831 285.589 71.31010000000001 -55.5058 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 323.404 -62.2043 141.092 -46.6015 148.737 89.1108 0 0 0 0 0 0 0 0 -324.336 57.8552 -141.991 46.6015 -148.737 -89.1108 1 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 454.208 -99.3095 175.986 -53.4094 211.276 123.174 0 0 0 0 0 0 0 0 -455.779 93.0992 -177.158 53.4094 -211.276 -123.174 1 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 231.884 -57.0266 78.2559 -20.8926 109.118 61.8183 0 0 0 0 0 0 0 0 -232.824 53.8025 -78.80719999999999 20.8926 -109.118 -61.8183 1 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 181.715 18.8525 177.045 -80.08499999999999 76.3716 63.8254 0 0 0 0 0 0 0 0 -181.07 -20.839 -177.959 80.08499999999999 -76.3716 -63.8254 1 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 308.249 40.5812 314.711 -144.494 128.766 111.207 0 0 0 0 0 0 0 0 -306.972 -43.8825 -316.328 144.494 -128.766 -111.207 1 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 133.309 116.865 315.474 -162.598 42.0763 73.8353 0 0 0 0 0 0 0 0 -130.603 -117.454 -316.931 162.598 -42.0763 -73.8353 1 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 52.4426 63.3607 156.902 -81.5288 14.2139 33.1222 0 0 0 0 0 0 0 0 -50.9976 -63.4393 -157.607 81.5288 -14.2139 -33.1222 1 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 75.5508 154.234 339.369 -180.995 11.859 63.8956 0 0 0 0 0 0 0 0 -72.1041 -153.816 -340.865 180.995 -11.859 -63.8956 1 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -62.0551 306.357 506.351 -288.542 -69.50409999999999 57.4562 0 0 0 0 0 0 0 0 68.6262 -303.028 -508.423 288.542 69.50409999999999 -57.4562 1 0 0 0 0 0 0 0
+
+
+# name: Hy
+# type: matrix
+# rows: 18
+# columns: 14
+ 1.56267 0 0 0 0 0 0 0 0 0 0.252873 0 0 0.110387
+ 0 1.56267 0 0 0 0 0 0 0 0 0.453168 0 0 0.162961
+ 0 0 1.56267 0 0 0 0 0 0 0 0.638441 0 0 0.0873758
+ 0 0 0 1.56267 0 0 0 0 0 0 0.21031 0 0 0.0321517
+ 0 0 0 0 1.56267 0 0 0 0 0 0.375554 0 0 0.0505151
+ 0 0 0 0 0 1.56267 0 0 0 0 0.638441 0 0 -0.0336034
+ 0 0 0 0 0 0 1.56267 0 0 0 0.157733 0 0 -0.0232704
+ 0 0 0 0 0 0 0 1.56267 0 0 0.212814 0 0 -0.0688343
+ 0 0 0 0 0 0 0 0 1.56267 0 0.360532 0 0 -0.187745
+ 1.56267 0 0 0 0 0 0 0 0 0 0 0.252873 0 0.322811
+ 0 1.56267 0 0 0 0 0 0 0 0 0 0.453168 0 0.467319
+ 0 0 1.56267 0 0 0 0 0 0 0 0 0.638441 0 0.245907
+ 0 0 0 1.56267 0 0 0 0 0 0 0 0.21031 0 0.135725
+ 0 0 0 0 1.56267 0 0 0 0 0 0 0.375554 0 0.225277
+ 0 0 0 0 0 1.56267 0 0 0 0 0 0.638441 0 0.012926
+ 0 0 0 0 0 0 1.56267 0 0 0 0 0.157733 0 -0.0108962
+ 0 0 0 0 0 0 0 1.56267 0 0 0 0.212814 0 -0.06974909999999999
+ 0 0 0 0 0 0 0 0 1.56267 0 0 0.360532 0 -0.318846
+
+
+# name: r
+# type: matrix
+# rows: 18
+# columns: 1
+ 0.0354809
+ 0.0153183
+ 0.0570191
+ -0.0372801
+ 0.0878601
+ 0.06811780000000001
+ -0.00426164
+ 5.162985999041026e-321
+ 6.927779999999998e-310
+ 0
+ 2.121999999910509e-314
+ 0
+ 0
+ 0
+ 3.6073900000086e-313
+ 4.446590812571219e-323
+ 3.952525166729972e-323
+ 3.952525166729972e-323
+
+

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

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="raphael@maenle.net">Raphael Maenle</author>
-  <author email="kartikmohta@gmail.com">Kartik Mohta</author>
 
   <buildtool_depend>catkin</buildtool_depend>
 
@@ -19,6 +18,7 @@
   <depend>nav_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>geometry_msgs</depend>
+  <depend>visualization_msgs</depend>
   <depend>eigen_conversions</depend>
   <depend>tf_conversions</depend>
   <depend>random_numbers</depend>

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

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_

src/bagcontrol.py

@@ -0,0 +1,64 @@
+#!/usr/bin/env python
+import rosbag
+import rospy
+from sensor_msgs.msg import Imu, Image
+from geometry_msgs.msg import TransformStamped
+import time
+import signal
+import sys
+
+
+def signal_handler(sig, frame):
+        print('gracefully exiting the program.')
+        bag.close()
+        sys.exit(0)
+
+def main():
+    global bag
+
+    cam0_topic = '/cam0/image_raw'
+    cam1_topic = '/cam1/image_raw'
+    imu0_topic = '/imu0'
+    grnd_topic = '/vrpn_client/raw_transform'
+
+    rospy.init_node('controlbag')
+    rospy.set_param('play_bag', False)
+
+    cam0_pub = rospy.Publisher(cam0_topic, Image, queue_size=10)
+    cam1_pub = rospy.Publisher(cam1_topic, Image, queue_size=10)
+    imu0_pub = rospy.Publisher(imu0_topic, Imu, queue_size=10)
+    grnd_pub = rospy.Publisher(grnd_topic, TransformStamped, queue_size=10)
+
+    signal.signal(signal.SIGINT, signal_handler)
+
+    bag = rosbag.Bag('/home/raphael/dev/MSCKF_ws/bag/TUM/dataset-corridor1_1024_16.bag')
+    for topic, msg, t in bag.read_messages(topics=[cam0_topic, cam1_topic, imu0_topic, grnd_topic]):
+        
+        # pause if parameter set to false
+        flag = False
+        while not rospy.get_param('/play_bag'):
+            time.sleep(0.01)
+            if not flag:
+                print("stopped playback")
+                flag = not flag
+
+        if flag:
+            print("resume playback")
+
+        if topic == cam0_topic:
+            cam0_pub.publish(msg)
+
+        elif topic == cam1_topic:
+            cam1_pub.publish(msg)
+
+        elif topic == imu0_topic:
+            imu0_pub.publish(msg)
+
+        elif topic ==grnd_topic:
+            grnd_pub.publish(msg)
+
+        #print msg
+    bag.close()
+
+if __name__== "__main__":
+    main()

src/control.py

@@ -0,0 +1,64 @@
+#!/usr/bin/env python
+import rosbag
+import rospy
+from sensor_msgs.msg import Imu, Image
+from geometry_msgs.msg import TransformStamped
+import time
+import signal
+import sys
+
+
+def signal_handler(sig, frame):
+        print('gracefully exiting the program.')
+        bag.close()
+        sys.exit(0)
+
+def main():
+    global bag
+
+    cam0_topic = '/cam0/image_raw'
+    cam1_topic = '/cam1/image_raw'
+    imu0_topic = '/imu0'
+    grnd_topic = '/vrpn_client/raw_transform'
+
+    rospy.init_node('controlbag')
+    rospy.set_param('play_bag', False)
+
+    cam0_pub = rospy.Publisher(cam0_topic, Image, queue_size=10)
+    cam1_pub = rospy.Publisher(cam1_topic, Image, queue_size=10)
+    imu0_pub = rospy.Publisher(imu0_topic, Imu, queue_size=10)
+    grnd_pub = rospy.Publisher(grnd_topic, TransformStamped, queue_size=10)
+
+    signal.signal(signal.SIGINT, signal_handler)
+
+    bag = rosbag.Bag('/home/raphael/dev/MSCKF_ws/bag/TUM/dataset-corridor1_1024_16.bag')
+    for topic, msg, t in bag.read_messages(topics=[cam0_topic, cam1_topic, imu0_topic, grnd_topic]):
+        
+        # pause if parameter set to false
+        flag = False
+        while not rospy.get_param('/play_bag'):
+            time.sleep(0.01)
+            if not flag:
+                print("stopped playback")
+                flag = not flag
+
+        if flag:
+            print("resume playback")
+
+        if topic == cam0_topic:
+            cam0_pub.publish(msg)
+
+        elif topic == cam1_topic:
+            cam1_pub.publish(msg)
+
+        elif topic == imu0_topic:
+            imu0_pub.publish(msg)
+
+        elif topic ==grnd_topic:
+            grnd_pub.publish(msg)
+
+        #print msg
+    bag.close()
+
+if __name__== "__main__":
+    main()

src/image_handler.cpp

@@ -0,0 +1,293 @@
+#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 {
+
+
+cv::Point2f pinholeDownProject(const cv::Point2f& p_in, const cv::Vec4d& intrinsics)
+{
+  return cv::Point2f(p_in.x * intrinsics[0] + intrinsics[2], p_in.y * intrinsics[1] + intrinsics[3]);
+}
+
+cv::Point2f pinholeUpProject(const cv::Point2f& p_in, const cv::Vec4d& intrinsics)
+{
+  return cv::Point2f((p_in.x - intrinsics[2])/intrinsics[0], (p_in.y - intrinsics[3])/intrinsics[1]);
+}
+
+void undistortImage(
+  cv::InputArray src,
+  cv::OutputArray dst,
+  const std::string& distortion_model,
+  const cv::Vec4d& intrinsics,
+  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);
+
+  if (distortion_model == "pre-equidistant")
+      cv::fisheye::undistortImage(src, dst, K, distortion_coeffs, K);
+  else if (distortion_model == "equidistant")
+      src.copyTo(dst);
+
+  else if (distortion_model == "pre-radtan")
+      cv::undistort(src, dst, K, distortion_coeffs, K);
+  else if (distortion_model == "radtan")
+      src.copyTo(dst);
+
+}
+
+void undistortPoint(
+    const cv::Point2f& pt_in,
+    const cv::Vec4d& intrinsics,
+    const std::string& distortion_model,
+    const cv::Vec4d& distortion_coeffs,
+    cv::Point2f& pt_out,
+    const cv::Matx33d &rectification_matrix,
+    const cv::Vec4d &new_intrinsics) {
+
+
+  std::vector<cv::Point2f> pts_in;
+  std::vector<cv::Point2f> pts_out;
+  pts_in.push_back(pt_in);
+  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);
+  } 
+  // equidistant
+  else if (distortion_model == "equidistant") {
+    cv::fisheye::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
+                                 rectification_matrix, K_new);
+  }
+  // fov
+  else if (distortion_model == "fov") {
+    for(int i = 0; i < pts_in.size(); i++)
+    {
+      float omega = distortion_coeffs[0];
+      float rd = sqrt(pts_in[i].x * pts_in[i].x + pts_in[i].y * pts_in[i].y);
+      float ru = tan(rd * omega)/(2 * tan(omega / 2));
+
+      cv::Point2f newPoint(
+              ((pts_in[i].x - intrinsics[2]) / intrinsics[0]) * (ru / rd),
+              ((pts_in[i].y - intrinsics[3]) / intrinsics[1]) * (ru / rd));
+
+      pts_out.push_back(newPoint);
+    }
+   }
+
+  else if (distortion_model == "pre-equidistant" or distortion_model == "pre-radtan")
+  {
+    std::vector<cv::Point2f> temp_pts_out;
+    for(int i = 0; i < pts_in.size(); i++)
+      temp_pts_out.push_back(pinholeUpProject(pts_in[i], intrinsics));
+
+    pts_out = temp_pts_out;
+  } 
+   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);
+  }
+  pt_out = pts_out[0];
+  return;
+}
+
+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 if (distortion_model == "fov") {
+    for(int i = 0; i < pts_in.size(); i++)
+    {
+      float omega = distortion_coeffs[0];
+      float rd = sqrt(pts_in[i].x * pts_in[i].x + pts_in[i].y * pts_in[i].y);
+      float ru = tan(rd * omega)/(2 * tan(omega / 2));
+
+      cv::Point2f newPoint(
+              ((pts_in[i].x - intrinsics[2]) / intrinsics[0]) * (ru / rd),
+              ((pts_in[i].y - intrinsics[3]) / intrinsics[1]) * (ru / rd));
+
+      pts_out.push_back(newPoint);
+    }
+  }
+  else if (distortion_model == "pre-equidistant" or distortion_model == "pre-radtan")
+  {
+    std::vector<cv::Point2f> temp_pts_out;
+    for(int i = 0; i < pts_in.size(); i++)
+      temp_pts_out.push_back(pinholeUpProject(pts_in[i], intrinsics));
+
+    pts_out = temp_pts_out;
+
+  }  
+  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 if (distortion_model == "fov") {
+    for(int i = 0; i < pts_in.size(); i++)
+    {
+      // based on 'straight lines have to be straight'
+      float ru = sqrt(pts_in[i].x * pts_in[i].x + pts_in[i].y * pts_in[i].y);
+      float omega = distortion_coeffs[0];
+      float rd = 1 / (omega)*atan(2*ru*tan(omega / 2));
+      
+      cv::Point2f newPoint(
+              pts_in[i].x * (rd/ru) * intrinsics[0] + intrinsics[2],
+              pts_in[i].y * (rd/ru) * intrinsics[1] + intrinsics[3]);
+
+      pts_out.push_back(newPoint);
+    }
+  }
+  else if (distortion_model == "pre-equidistant" or distortion_model == "pre-radtan")
+  {
+    std::vector<cv::Point2f> temp_pts_out;
+    for(int i = 0; i < pts_in.size(); i++)
+      temp_pts_out.push_back(pinholeDownProject(pts_in[i], intrinsics));
+
+    pts_out = temp_pts_out;
+  }   
+   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 if (distortion_model == "fov") {
+    for(int i = 0; i < pts_in.size(); i++)
+    {
+      // based on 'straight lines have to be straight'
+      float ru = sqrt(pts_in[i].x * pts_in[i].x + pts_in[i].y * pts_in[i].y);
+      float omega = distortion_coeffs[0];
+      float rd = 1 / (omega)*atan(2*ru*tan(omega / 2));
+      
+      cv::Point2f newPoint(
+              pts_in[i].x * (rd/ru) * intrinsics[0] + intrinsics[2],
+              pts_in[i].y * (rd/ru) * intrinsics[1] + intrinsics[3]);
+
+      pts_out.push_back(newPoint);
+    }
+  }
+  else if (distortion_model == "pre-equidistant" or distortion_model == "pre-radtan")
+  {
+    std::vector<cv::Point2f> temp_pts_out;
+    for(int i = 0; i < pts_in.size(); i++)
+      pts_out.push_back(pinholeDownProject(pts_in[i], intrinsics));
+
+    pts_out = temp_pts_out;
+  }    
+   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

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;
@@ -41,51 +42,54 @@ ImageProcessor::~ImageProcessor() {
 }
 
 bool ImageProcessor::loadParameters() {
+
+  // debug parameters
+  nh.param<bool>("StreamPause", STREAMPAUSE, false);
   // 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[0] = cam0_resolution_temp[0];
-  cam0_resolution[1] = cam0_resolution_temp[1];
+  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[0] = cam1_resolution_temp[0];
-  cam1_resolution[1] = cam1_resolution_temp[1];
+  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[0] = cam0_intrinsics_temp[0];
-  cam0_intrinsics[1] = cam0_intrinsics_temp[1];
+  cam0.intrinsics[1] = cam0_intrinsics_temp[1];
-  cam0_intrinsics[2] = cam0_intrinsics_temp[2];
+  cam0.intrinsics[2] = cam0_intrinsics_temp[2];
-  cam0_intrinsics[3] = cam0_intrinsics_temp[3];
+  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[0] = cam1_intrinsics_temp[0];
-  cam1_intrinsics[1] = cam1_intrinsics_temp[1];
+  cam1.intrinsics[1] = cam1_intrinsics_temp[1];
-  cam1_intrinsics[2] = cam1_intrinsics_temp[2];
+  cam1.intrinsics[2] = cam1_intrinsics_temp[2];
-  cam1_intrinsics[3] = cam1_intrinsics_temp[3];
+  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[0] = cam0_distortion_coeffs_temp[0];
-  cam0_distortion_coeffs[1] = cam0_distortion_coeffs_temp[1];
+  cam0.distortion_coeffs[1] = cam0_distortion_coeffs_temp[1];
-  cam0_distortion_coeffs[2] = cam0_distortion_coeffs_temp[2];
+  cam0.distortion_coeffs[2] = cam0_distortion_coeffs_temp[2];
-  cam0_distortion_coeffs[3] = cam0_distortion_coeffs_temp[3];
+  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[0] = cam1_distortion_coeffs_temp[0];
-  cam1_distortion_coeffs[1] = cam1_distortion_coeffs_temp[1];
+  cam1.distortion_coeffs[1] = cam1_distortion_coeffs_temp[1];
-  cam1_distortion_coeffs[2] = cam1_distortion_coeffs_temp[2];
+  cam1.distortion_coeffs[2] = cam1_distortion_coeffs_temp[2];
-  cam1_distortion_coeffs[3] = cam1_distortion_coeffs_temp[3];
+  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 +127,27 @@ bool ImageProcessor::loadParameters() {
       processor_config.stereo_threshold, 3);
 
   ROS_INFO("===========================================");
-  ROS_INFO("cam0_resolution: %d, %d",
+  ROS_INFO("cam0.resolution: %d, %d",
-      cam0_resolution[0], cam0_resolution[1]);
+      cam0.resolution[0], cam0.resolution[1]);
   ROS_INFO("cam0_intrinscs: %f, %f, %f, %f",
-      cam0_intrinsics[0], cam0_intrinsics[1],
+      cam0.intrinsics[0], cam0.intrinsics[1],
-      cam0_intrinsics[2], cam0_intrinsics[3]);
+      cam0.intrinsics[2], cam0.intrinsics[3]);
-  ROS_INFO("cam0_distortion_model: %s",
+  ROS_INFO("cam0.distortion_model: %s",
-      cam0_distortion_model.c_str());
+      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[0], cam0.distortion_coeffs[1],
-      cam0_distortion_coeffs[2], cam0_distortion_coeffs[3]);
+      cam0.distortion_coeffs[2], cam0.distortion_coeffs[3]);
 
-  ROS_INFO("cam1_resolution: %d, %d",
+  ROS_INFO("cam1.resolution: %d, %d",
-      cam1_resolution[0], cam1_resolution[1]);
+      cam1.resolution[0], cam1.resolution[1]);
   ROS_INFO("cam1_intrinscs: %f, %f, %f, %f",
-      cam1_intrinsics[0], cam1_intrinsics[1],
+      cam1.intrinsics[0], cam1.intrinsics[1],
-      cam1_intrinsics[2], cam1_intrinsics[3]);
+      cam1.intrinsics[2], cam1.intrinsics[3]);
-  ROS_INFO("cam1_distortion_model: %s",
+  ROS_INFO("cam1.distortion_model: %s",
-      cam1_distortion_model.c_str());
+      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[0], cam1.distortion_coeffs[1],
-      cam1_distortion_coeffs[2], cam1_distortion_coeffs[3]);
+      cam1.distortion_coeffs[2], cam1.distortion_coeffs[3]);
 
   cout << R_imu_cam0 << endl;
   cout << t_imu_cam0.t() << endl;
@@ -170,10 +174,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 +204,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...");
 
@@ -221,7 +214,9 @@ void ImageProcessor::stereoCallback(
     const sensor_msgs::ImageConstPtr& cam0_img,
     const sensor_msgs::ImageConstPtr& cam1_img) {
 
-  //cout << "==================================" << endl;
+    // stop playing bagfile if printing images
+  //if(STREAMPAUSE)
+   // nh.setParam("/play_bag_image", false);
 
   // Get the current image.
   cam0_curr_img_ptr = cv_bridge::toCvShare(cam0_img,
@@ -229,14 +224,27 @@ void ImageProcessor::stereoCallback(
   cam1_curr_img_ptr = cv_bridge::toCvShare(cam1_img,
       sensor_msgs::image_encodings::MONO8);
 
-  // Build the image pyramids once since they're used at multiple places
+  ros::Time start_time = ros::Time::now();
 
-  // removed due to alternate cuda construct
+
-  //createImagePyramids();
+  cv::Mat new_cam0;
+  cv::Mat new_cam1;
+
+  image_handler::undistortImage(cam0_curr_img_ptr->image, new_cam0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs);
+  image_handler::undistortImage(cam1_curr_img_ptr->image, new_cam1, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs);
+  
+  new_cam0.copyTo(cam0_curr_img_ptr->image);
+  new_cam1.copyTo(cam1_curr_img_ptr->image);
+
+  //ROS_INFO("Publishing: %f",
+  //    (ros::Time::now()-start_time).toSec());
+
+  // Build the image pyramids once since they're used at multiple places
+  createImagePyramids();
 
   // Detect features in the first frame.
   if (is_first_img) {
-    ros::Time start_time = ros::Time::now();
+    start_time = ros::Time::now();
     initializeFirstFrame();
     //ROS_INFO("Detection time: %f",
     //    (ros::Time::now()-start_time).toSec());
@@ -249,7 +257,7 @@ void ImageProcessor::stereoCallback(
     //    (ros::Time::now()-start_time).toSec());
   } else {
     // Track the feature in the previous image.
-    ros::Time start_time = ros::Time::now();
+    start_time = ros::Time::now();
     trackFeatures();
     //ROS_INFO("Tracking time: %f",
     //    (ros::Time::now()-start_time).toSec());
@@ -257,6 +265,7 @@ void ImageProcessor::stereoCallback(
     // Add new features into the current image.
     start_time = ros::Time::now();
     addNewFeatures();
+    
     //ROS_INFO("Addition time: %f",
     //    (ros::Time::now()-start_time).toSec());
 
@@ -279,16 +288,18 @@ void ImageProcessor::stereoCallback(
   //    (ros::Time::now()-start_time).toSec());
 
   // Publish features in the current image.
-  ros::Time start_time = ros::Time::now();
+  start_time = ros::Time::now();
   publish();
   //ROS_INFO("Publishing: %f",
   //    (ros::Time::now()-start_time).toSec());
 
   // Update the previous image and previous features.
+  
   cam0_prev_img_ptr = cam0_curr_img_ptr;
   prev_features_ptr = curr_features_ptr;
   std::swap(prev_cam0_pyramid_, curr_cam0_pyramid_);
 
+
   // Initialize the current features to empty vectors.
   curr_features_ptr.reset(new GridFeatures());
   for (int code = 0; code <
@@ -296,6 +307,10 @@ void ImageProcessor::stereoCallback(
     (*curr_features_ptr)[code] = vector<FeatureMetaData>(0);
   }
 
+    // stop playing bagfile if printing images
+  //if(STREAMPAUSE)
+  //  nh.setParam("/play_bag_image", true);
+
   return;
 }
 
@@ -309,7 +324,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 +331,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 +417,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 +447,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 +480,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 +492,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 +575,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_R_p_c, cam0.intrinsics, cam0.distortion_model,
-      cam0_distortion_coeffs, processor_config.ransac_threshold,
+      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_R_p_c, cam1.intrinsics, cam1.distortion_model,
-      cam1_distortion_coeffs, processor_config.ransac_threshold,
+      cam1.distortion_coeffs, processor_config.ransac_threshold,
       0.99, cam1_ransac_inliers);
 
   // Number of features after ransac.
@@ -617,6 +607,7 @@ void ImageProcessor::trackFeatures() {
     ++after_ransac;
   }
 
+
   // Compute the tracking rate.
   int prev_feature_num = 0;
   for (const auto& item : *prev_features_ptr)
@@ -646,7 +637,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 +644,20 @@ 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,
+    image_handler::undistortPoints(cam0_points, cam0.intrinsics, cam0.distortion_model,
-                    cam0_distortion_coeffs, cam0_points_undistorted,
+                    cam0.distortion_coeffs, cam0_points_undistorted,
                     R_cam0_cam1);
+<<<<<<< HEAD
     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);
+>>>>>>> photometry-jakobi
   }
 
-  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 +667,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) {
@@ -713,18 +692,21 @@ void ImageProcessor::stereoMatch(
 
   // Further remove outliers based on the known
   // essential matrix.
+
+
   vector<cv::Point2f> cam0_points_undistorted(0);
   vector<cv::Point2f> cam1_points_undistorted(0);
-  undistortPoints(
+  image_handler::undistortPoints(
-      cam0_points, cam0_intrinsics, cam0_distortion_model,
+      cam0_points, cam0.intrinsics, cam0.distortion_model,
-      cam0_distortion_coeffs, cam0_points_undistorted);
+      cam0.distortion_coeffs, cam0_points_undistorted);
-  undistortPoints(
+  image_handler::undistortPoints(
-      cam1_points, cam1_intrinsics, cam1_distortion_model,
+      cam1_points, cam1.intrinsics, cam1.distortion_model,
-      cam1_distortion_coeffs, cam1_points_undistorted);
+      cam1.distortion_coeffs, cam1_points_undistorted);
+
 
   double norm_pixel_unit = 4.0 / (
-      cam0_intrinsics[0]+cam0_intrinsics[1]+
+      cam0.intrinsics[0]+cam0.intrinsics[1]+
-      cam1_intrinsics[0]+cam1_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 +733,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 +754,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 +768,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 +780,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 +807,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 +828,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 +877,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 +928,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 +957,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 +971,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 +1192,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 +1211,12 @@ void ImageProcessor::publish() {
   vector<Point2f> curr_cam0_points_undistorted(0);
   vector<Point2f> curr_cam1_points_undistorted(0);
 
-  undistortPoints(
+  image_handler::undistortPoints(
-      curr_cam0_points, cam0_intrinsics, cam0_distortion_model,
+      curr_cam0_points, cam0.intrinsics, cam0.distortion_model,
-      cam0_distortion_coeffs, curr_cam0_points_undistorted);
+      cam0.distortion_coeffs, curr_cam0_points_undistorted);
-  undistortPoints(
+  image_handler::undistortPoints(
-      curr_cam1_points, cam1_intrinsics, cam1_distortion_model,
+      curr_cam1_points, cam1.intrinsics, cam1.distortion_model,
-      cam1_distortion_coeffs, curr_cam1_points_undistorted);
+      cam1.distortion_coeffs, curr_cam1_points_undistorted);
 
   for (int i = 0; i < curr_ids.size(); ++i) {
     feature_msg_ptr->features.push_back(FeatureMeasurement());

src/shrinkImage.py

@@ -0,0 +1,75 @@
+#!/usr/bin/env python
+from __future__ import print_function
+
+import sys
+import rospy
+import cv2
+from std_msgs.msg import String
+from sensor_msgs.msg import Image
+from cv_bridge import CvBridge, CvBridgeError
+
+class image_converter:
+
+  def __init__(self):
+    self.image0_pub = rospy.Publisher("/cam0/new_image_raw",Image, queue_size=10)
+    self.image1_pub = rospy.Publisher("/cam1/new_image_raw",Image, queue_size=10)
+
+    self.bridge = CvBridge()
+    self.image0_sub = rospy.Subscriber("/cam0/image_raw",Image,self.callback_cam0)
+    self.image1_sub = rospy.Subscriber("/cam1/image_raw",Image,self.callback_cam1)
+
+  def callback_cam0(self,data):
+    try:
+      cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
+    except CvBridgeError as e:
+      print(e)
+
+    imgScale = 0.25
+    newX,newY = cv_image.shape[1]*imgScale, cv_image.shape[0]*imgScale
+    newimg = cv2.resize(cv_image,(int(newX),int(newY)))
+
+    newpub = self.bridge.cv2_to_imgmsg(newimg, "bgr8")
+    newdata = data
+    newdata.height = newpub.height
+    newdata.width = newpub.width
+    newdata.step = newpub.step
+    newdata.data = newpub.data
+    try:
+      self.image0_pub.publish(newdata)
+    except CvBridgeError as e:
+      print(e)
+
+  def callback_cam1(self,data):
+    try:
+      cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
+    except CvBridgeError as e:
+      print(e)
+
+    imgScale = 0.25
+    newX,newY = cv_image.shape[1]*imgScale, cv_image.shape[0]*imgScale
+    newimg = cv2.resize(cv_image,(int(newX),int(newY)))
+
+    newpub = self.bridge.cv2_to_imgmsg(newimg, "bgr8")
+    newdata = data
+    newdata.height = newpub.height
+    newdata.width = newpub.width
+    newdata.step = newpub.step
+    newdata.data = newpub.data
+
+    try:
+      self.image1_pub.publish(newdata)
+    except CvBridgeError as e:
+      print(e)
+
+
+def main(args):
+  ic = image_converter()
+  rospy.init_node('image_converter', anonymous=True)
+  try:
+    rospy.spin()
+  except KeyboardInterrupt:
+    print("Shutting down")
+  cv2.destroyAllWindows()
+
+if __name__ == '__main__':
+    main(sys.argv)

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;