removed scaling

.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}

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;
-  cv::Vec2i cam0_resolution;
-  cv::Vec4d cam0_intrinsics;
-  cv::Vec4d cam0_distortion_coeffs;
-
-  std::string cam1_distortion_model;
-  cv::Vec2i cam1_resolution;
-  cv::Vec4d cam1_intrinsics;
-  cv::Vec4d cam1_distortion_coeffs;
+  CameraCalibration cam0;
+  CameraCalibration cam1;
 
   // Take a vector from cam0 frame to the IMU frame.
   cv::Matx33d R_cam0_imu;
@@ -367,11 +360,6 @@ private:
   boost::shared_ptr<GridFeatures> prev_features_ptr;
   boost::shared_ptr<GridFeatures> curr_features_ptr;
 
-  cv::cuda::GpuMat cam0_curr_img;
-  cv::cuda::GpuMat cam1_curr_img;
-  cv::cuda::GpuMat cam0_points_gpu;
-  cv::cuda::GpuMat cam1_points_gpu;
-
   // Number of features after each outlier removal step.
   int before_tracking;
   int after_tracking;

include/msckf_vio/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
-     *    Callback function for feature measurements.
-     * @param msg Stereo feature measurements.
+     * @brief truthCallback
+     *      Callback function for ground truth navigation information
+     * @param TransformStamped 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 featureCallback(const CameraMeasurementConstPtr& msg);
+    void imageCallback (
+    const sensor_msgs::ImageConstPtr& cam0_img,
+    const sensor_msgs::ImageConstPtr& cam1_img,
+    const CameraMeasurementConstPtr& feature_msg);
 
     /*
      * @brief publish Publish the results of VIO.
@@ -126,6 +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="kartikmohta@gmail.com">Kartik Mohta</author>
+  <author email="raphael@maenle.net">Raphael Maenle</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[1] = cam0_resolution_temp[1];
+  cam0.resolution[0] = cam0_resolution_temp[0];
+  cam0.resolution[1] = cam0_resolution_temp[1];
 
   vector<int> cam1_resolution_temp(2);
   nh.getParam("cam1/resolution", cam1_resolution_temp);
-  cam1_resolution[0] = cam1_resolution_temp[0];
-  cam1_resolution[1] = cam1_resolution_temp[1];
+  cam1.resolution[0] = cam1_resolution_temp[0];
+  cam1.resolution[1] = cam1_resolution_temp[1];
 
   vector<double> cam0_intrinsics_temp(4);
   nh.getParam("cam0/intrinsics", cam0_intrinsics_temp);
-  cam0_intrinsics[0] = cam0_intrinsics_temp[0];
-  cam0_intrinsics[1] = cam0_intrinsics_temp[1];
-  cam0_intrinsics[2] = cam0_intrinsics_temp[2];
-  cam0_intrinsics[3] = cam0_intrinsics_temp[3];
+  cam0.intrinsics[0] = cam0_intrinsics_temp[0];
+  cam0.intrinsics[1] = cam0_intrinsics_temp[1];
+  cam0.intrinsics[2] = cam0_intrinsics_temp[2];
+  cam0.intrinsics[3] = cam0_intrinsics_temp[3];
 
   vector<double> cam1_intrinsics_temp(4);
   nh.getParam("cam1/intrinsics", cam1_intrinsics_temp);
-  cam1_intrinsics[0] = cam1_intrinsics_temp[0];
-  cam1_intrinsics[1] = cam1_intrinsics_temp[1];
-  cam1_intrinsics[2] = cam1_intrinsics_temp[2];
-  cam1_intrinsics[3] = cam1_intrinsics_temp[3];
+  cam1.intrinsics[0] = cam1_intrinsics_temp[0];
+  cam1.intrinsics[1] = cam1_intrinsics_temp[1];
+  cam1.intrinsics[2] = cam1_intrinsics_temp[2];
+  cam1.intrinsics[3] = cam1_intrinsics_temp[3];
 
   vector<double> cam0_distortion_coeffs_temp(4);
   nh.getParam("cam0/distortion_coeffs",
       cam0_distortion_coeffs_temp);
-  cam0_distortion_coeffs[0] = cam0_distortion_coeffs_temp[0];
-  cam0_distortion_coeffs[1] = cam0_distortion_coeffs_temp[1];
-  cam0_distortion_coeffs[2] = cam0_distortion_coeffs_temp[2];
-  cam0_distortion_coeffs[3] = cam0_distortion_coeffs_temp[3];
+  cam0.distortion_coeffs[0] = cam0_distortion_coeffs_temp[0];
+  cam0.distortion_coeffs[1] = cam0_distortion_coeffs_temp[1];
+  cam0.distortion_coeffs[2] = cam0_distortion_coeffs_temp[2];
+  cam0.distortion_coeffs[3] = cam0_distortion_coeffs_temp[3];
 
   vector<double> cam1_distortion_coeffs_temp(4);
   nh.getParam("cam1/distortion_coeffs",
       cam1_distortion_coeffs_temp);
-  cam1_distortion_coeffs[0] = cam1_distortion_coeffs_temp[0];
-  cam1_distortion_coeffs[1] = cam1_distortion_coeffs_temp[1];
-  cam1_distortion_coeffs[2] = cam1_distortion_coeffs_temp[2];
-  cam1_distortion_coeffs[3] = cam1_distortion_coeffs_temp[3];
+  cam1.distortion_coeffs[0] = cam1_distortion_coeffs_temp[0];
+  cam1.distortion_coeffs[1] = cam1_distortion_coeffs_temp[1];
+  cam1.distortion_coeffs[2] = cam1_distortion_coeffs_temp[2];
+  cam1.distortion_coeffs[3] = cam1_distortion_coeffs_temp[3];
 
   cv::Mat     T_imu_cam0 = utils::getTransformCV(nh, "cam0/T_cam_imu");
   cv::Matx33d R_imu_cam0(T_imu_cam0(cv::Rect(0,0,3,3)));
@@ -123,27 +127,27 @@ bool ImageProcessor::loadParameters() {
       processor_config.stereo_threshold, 3);
 
   ROS_INFO("===========================================");
-  ROS_INFO("cam0_resolution: %d, %d",
-      cam0_resolution[0], cam0_resolution[1]);
+  ROS_INFO("cam0.resolution: %d, %d",
+      cam0.resolution[0], cam0.resolution[1]);
   ROS_INFO("cam0_intrinscs: %f, %f, %f, %f",
-      cam0_intrinsics[0], cam0_intrinsics[1],
-      cam0_intrinsics[2], cam0_intrinsics[3]);
-  ROS_INFO("cam0_distortion_model: %s",
-      cam0_distortion_model.c_str());
+      cam0.intrinsics[0], cam0.intrinsics[1],
+      cam0.intrinsics[2], cam0.intrinsics[3]);
+  ROS_INFO("cam0.distortion_model: %s",
+      cam0.distortion_model.c_str());
   ROS_INFO("cam0_distortion_coefficients: %f, %f, %f, %f",
-      cam0_distortion_coeffs[0], cam0_distortion_coeffs[1],
-      cam0_distortion_coeffs[2], cam0_distortion_coeffs[3]);
+      cam0.distortion_coeffs[0], cam0.distortion_coeffs[1],
+      cam0.distortion_coeffs[2], cam0.distortion_coeffs[3]);
 
-  ROS_INFO("cam1_resolution: %d, %d",
-      cam1_resolution[0], cam1_resolution[1]);
+  ROS_INFO("cam1.resolution: %d, %d",
+      cam1.resolution[0], cam1.resolution[1]);
   ROS_INFO("cam1_intrinscs: %f, %f, %f, %f",
-      cam1_intrinsics[0], cam1_intrinsics[1],
-      cam1_intrinsics[2], cam1_intrinsics[3]);
-  ROS_INFO("cam1_distortion_model: %s",
-      cam1_distortion_model.c_str());
+      cam1.intrinsics[0], cam1.intrinsics[1],
+      cam1.intrinsics[2], cam1.intrinsics[3]);
+  ROS_INFO("cam1.distortion_model: %s",
+      cam1.distortion_model.c_str());
   ROS_INFO("cam1_distortion_coefficients: %f, %f, %f, %f",
-      cam1_distortion_coeffs[0], cam1_distortion_coeffs[1],
-      cam1_distortion_coeffs[2], cam1_distortion_coeffs[3]);
+      cam1.distortion_coeffs[0], cam1.distortion_coeffs[1],
+      cam1.distortion_coeffs[2], cam1.distortion_coeffs[3]);
 
   cout << R_imu_cam0 << endl;
   cout << t_imu_cam0.t() << endl;
@@ -170,10 +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;
 }
@@ -214,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,
@@ -222,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());
@@ -242,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());
@@ -250,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());
 
@@ -272,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 <
@@ -289,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;
 }
 
@@ -302,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),
@@ -310,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),
@@ -397,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();
@@ -428,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;
@@ -462,9 +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: change to GPU
   calcOpticalFlowPyrLK(
       prev_cam0_pyramid_, curr_cam0_pyramid_,
       prev_cam0_points, curr_cam0_points,
@@ -558,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_distortion_coeffs, processor_config.ransac_threshold,
+      cam0_R_p_c, cam0.intrinsics, cam0.distortion_model,
+      cam0.distortion_coeffs, processor_config.ransac_threshold,
       0.99, cam0_ransac_inliers);
 
   vector<int> cam1_ransac_inliers(0);
   twoPointRansac(prev_matched_cam1_points, curr_matched_cam1_points,
-      cam1_R_p_c, cam1_intrinsics, cam1_distortion_model,
-      cam1_distortion_coeffs, processor_config.ransac_threshold,
+      cam1_R_p_c, cam1.intrinsics, cam1.distortion_model,
+      cam1.distortion_coeffs, processor_config.ransac_threshold,
       0.99, cam1_ransac_inliers);
 
   // Number of features after ransac.
@@ -590,6 +607,7 @@ void ImageProcessor::trackFeatures() {
     ++after_ransac;
   }
 
+
   // Compute the tracking rate.
   int prev_feature_num = 0;
   for (const auto& item : *prev_features_ptr)
@@ -619,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) {
@@ -627,37 +644,15 @@ void ImageProcessor::stereoMatch(
     // rotation from stereo extrinsics
     const cv::Matx33d R_cam0_cam1 = R_cam1_imu.t() * R_cam0_imu;
     vector<cv::Point2f> cam0_points_undistorted;
-    undistortPoints(cam0_points, cam0_intrinsics, cam0_distortion_model,
-                    cam0_distortion_coeffs, cam0_points_undistorted,
+    image_handler::undistortPoints(cam0_points, cam0.intrinsics, cam0.distortion_model,
+                    cam0.distortion_coeffs, cam0_points_undistorted,
                     R_cam0_cam1);
-    cam1_points = distortPoints(cam0_points_undistorted, cam1_intrinsics,
-                                cam1_distortion_model, cam1_distortion_coeffs);
+
+    cam1_points = image_handler::distortPoints(cam0_points_undistorted, cam1.intrinsics,
+                                cam1.distortion_model, cam1.distortion_coeffs);
   }
 
-  auto d_pyrLK_sparse = cuda::SparsePyrLKOpticalFlow::create(
-          Size(processor_config.patch_size, processor_config.patch_size),
-          processor_config.pyramid_levels, 
-          processor_config.max_iteration,
-          true);
-
-  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(),
@@ -667,7 +662,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) {
@@ -692,18 +687,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(
-      cam0_points, cam0_intrinsics, cam0_distortion_model,
-      cam0_distortion_coeffs, cam0_points_undistorted);
-  undistortPoints(
-      cam1_points, cam1_intrinsics, cam1_distortion_model,
-      cam1_distortion_coeffs, cam1_points_undistorted);
+  image_handler::undistortPoints(
+      cam0_points, cam0.intrinsics, cam0.distortion_model,
+      cam0.distortion_coeffs, cam0_points_undistorted);
+  image_handler::undistortPoints(
+      cam1_points, cam1.intrinsics, cam1.distortion_model,
+      cam1.distortion_coeffs, cam1_points_undistorted);
+
 
   double norm_pixel_unit = 4.0 / (
-      cam0_intrinsics[0]+cam0_intrinsics[1]+
-      cam1_intrinsics[0]+cam1_intrinsics[1]);
+      cam0.intrinsics[0]+cam0.intrinsics[1]+
+      cam1.intrinsics[0]+cam1.intrinsics[1]);
 
   for (int i = 0; i < cam0_points_undistorted.size(); ++i) {
     if (inlier_markers[i] == 0) continue;
@@ -730,8 +728,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) {
@@ -751,7 +749,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);
@@ -766,7 +763,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) {
@@ -779,7 +775,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());
@@ -807,7 +802,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 <
@@ -829,7 +823,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(),
@@ -879,73 +872,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.
@@ -997,7 +923,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) {
@@ -1027,7 +952,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]);
@@ -1041,10 +966,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);
 
@@ -1262,7 +1187,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;
@@ -1282,12 +1206,12 @@ void ImageProcessor::publish() {
   vector<Point2f> curr_cam0_points_undistorted(0);
   vector<Point2f> curr_cam1_points_undistorted(0);
 
-  undistortPoints(
-      curr_cam0_points, cam0_intrinsics, cam0_distortion_model,
-      cam0_distortion_coeffs, curr_cam0_points_undistorted);
-  undistortPoints(
-      curr_cam1_points, cam1_intrinsics, cam1_distortion_model,
-      cam1_distortion_coeffs, curr_cam1_points_undistorted);
+  image_handler::undistortPoints(
+      curr_cam0_points, cam0.intrinsics, cam0.distortion_model,
+      cam0.distortion_coeffs, curr_cam0_points_undistorted);
+  image_handler::undistortPoints(
+      curr_cam1_points, cam1.intrinsics, cam1.distortion_model,
+      cam1.distortion_coeffs, curr_cam1_points_undistorted);
 
   for (int i = 0; i < curr_ids.size(); ++i) {
     feature_msg_ptr->features.push_back(FeatureMeasurement());

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;