calculate average of jacobis and residual to reduce to single point in caclualtion - does not produce desired result

.vscode/c_cpp_properties.json

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

.vscode/settings.json

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

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: pre-radtan
+  distortion_model: 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: pre-radtan
+  distortion_model: radtan
   intrinsics: [457.587, 456.134, 379.999, 255.238]
   resolution: [752, 480]
   timeshift_cam_imu: 0.0

config/camchain-imucam-tum-scaled.yaml

@@ -1,36 +0,0 @@
-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]

include/msckf_vio/cam_state.h

@@ -18,8 +18,6 @@ namespace msckf_vio {
 
 struct Frame{
   cv::Mat image;
-  cv::Mat dximage;
-  cv::Mat dyimage;
   double exposureTime_ms;
 };
 
@@ -41,7 +39,6 @@ struct CameraCalibration{
   cv::Vec4d distortion_coeffs;
   movingWindow moving_window;
   cv::Mat featureVisu;
-  int id;
 };
 
 

include/msckf_vio/image_handler.h

@@ -16,16 +16,6 @@ namespace 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,

include/msckf_vio/image_processor.h

@@ -320,8 +320,6 @@ private:
     return;
   }
 
-  bool STREAMPAUSE;
-
   // Indicate if this is the first image message.
   bool is_first_img;
 

include/msckf_vio/math_utils.hpp

@@ -13,6 +13,13 @@
 
 namespace msckf_vio {
 
+
+inline double absoluted(double a){
+  if(a>0)
+    return a;
+  else return -a;
+}
+
 /*
  *  @brief Create a skew-symmetric matrix from a 3-element vector.
  *  @note Performs the operation:

include/msckf_vio/msckf_vio.h

@@ -202,111 +202,53 @@ class MsckfVio {
         Eigen::Vector4d& r);
     // This function computes the Jacobian of all measurements viewed
     // in the given camera states of this feature.
-    bool featureJacobian(
+    void featureJacobian(const FeatureIDType& feature_id,
-        const FeatureIDType& feature_id,
         const std::vector<StateIDType>& cam_state_ids,
         Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
 
 
-    void twodotMeasurementJacobian(
+    void PhotometricMeasurementJacobian(
-        const StateIDType& cam_state_id,
+    const StateIDType& cam_state_id,
-        const FeatureIDType& feature_id,
+    const FeatureIDType& feature_id,
-        Eigen::MatrixXd& H_x, Eigen::MatrixXd& H_y, Eigen::VectorXd& r);
+    Eigen::MatrixXd& H_x,
+    Eigen::MatrixXd& H_y,
+    Eigen::VectorXd& r);
 
-    bool ConstructJacobians(
+    void PhotometricFeatureJacobian(
-        Eigen::MatrixXd& H_rho,
+    const FeatureIDType& feature_id,
-        Eigen::MatrixXd& H_pl,
+    const std::vector<StateIDType>& cam_state_ids,
-        Eigen::MatrixXd& H_pA,
+    Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
-        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, int filter=0);
+        const Eigen::VectorXd&r, const int& dof);
     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;
+    bool PHOTOMETRIC;
 
     // 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;
@@ -369,7 +311,6 @@ class MsckfVio {
     // Subscribers and publishers
     ros::Subscriber imu_sub;
     ros::Subscriber truth_sub;
-    ros::Publisher truth_odom_pub;
     ros::Publisher odom_pub;
     ros::Publisher marker_pub;
     ros::Publisher feature_pub;

launch/image_processor_tinytum.launch

@@ -1,38 +0,0 @@
-<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

@@ -11,9 +11,6 @@
       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"/>

launch/msckf_vio_debug_tum.launch

@@ -22,10 +22,10 @@
       <param name="PHOTOMETRIC" value="true"/>
 
       <!-- Debugging Flaggs -->
-      <param name="PrintImages" value="true"/>
+      <param name="PrintImages" value="false"/>
       <param name="GroundTruth" value="false"/>
 
-      <param name="patch_size_n" value="3"/>
+      <param name="patch_size_n" value="7"/>
 
       <!-- Calibration parameters -->
       <rosparam command="load" file="$(arg calibration_file)"/>

launch/msckf_vio_euroc.launch

@@ -17,18 +17,6 @@
       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)"/>
 

launch/msckf_vio_tinytum.launch

@@ -1,77 +0,0 @@
-<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

@@ -17,16 +17,14 @@
       args='standalone msckf_vio/MsckfVioNodelet'
       output="screen">
 
-
+      <!-- Photometry Flag-->
-      <!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two -->
+      <param name="PHOTOMETRIC" value="true"/>
-      <param name="FILTER" value="1"/>
 
       <!-- Debugging Flaggs -->
-      <param name="StreamPause" value="true"/>
+      <param name="PrintImages" value="true"/>
-      <param name="PrintImages" value="false"/>
       <param name="GroundTruth" value="false"/>
 
-      <param name="patch_size_n" value="3"/>
+      <param name="patch_size_n" value="7"/>
       <!-- Calibration parameters -->
       <rosparam command="load" file="$(arg calibration_file)"/>
 
@@ -73,6 +71,4 @@
     </node>
   </group>
 
-  <!--node name="player" pkg="bagcontrol" type="control.py" /-->
-
 </launch>

log

@@ -1,73 +0,0 @@
-# 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
-
-

src/bagcontrol.py

@@ -1,64 +0,0 @@
-#!/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

@@ -1,64 +0,0 @@
-#!/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

@@ -14,40 +14,6 @@ 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,
@@ -76,37 +42,10 @@ void undistortPoint(
   if (distortion_model == "radtan") {
     cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
                         rectification_matrix, K_new);
-  } 
+  } else if (distortion_model == "equidistant") {
-  // equidistant
-  else if (distortion_model == "equidistant") {
     cv::fisheye::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
                                  rectification_matrix, K_new);
-  }
+  } else {
-  // 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,
@@ -140,35 +79,10 @@ void undistortPoints(
   if (distortion_model == "radtan") {
     cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
                         rectification_matrix, K_new);
-  } 
+  } else if (distortion_model == "equidistant") {
-  else if (distortion_model == "equidistant") {
     cv::fisheye::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
                                  rectification_matrix, K_new);
-  } 
+  } else {
-  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,
@@ -196,31 +110,7 @@ std::vector<cv::Point2f> distortPoints(
                       distortion_coeffs, pts_out);
   } else if (distortion_model == "equidistant") {
     cv::fisheye::distortPoints(pts_in, pts_out, K, distortion_coeffs);
-  }
+  } else {
-  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;
@@ -253,31 +143,7 @@ cv::Point2f distortPoint(
                       distortion_coeffs, pts_out);
   } else if (distortion_model == "equidistant") {
     cv::fisheye::distortPoints(pts_in, pts_out, K, distortion_coeffs);
-  }
+  } else {
-  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;
@@ -289,5 +155,5 @@ cv::Point2f distortPoint(
   return pts_out[0];
 }
 
-  } // namespace image_handler
+	} // namespace image_handler
-} // namespace msckf_vio
+} // namespace msckf_vio

src/image_processor.cpp

@@ -42,9 +42,6 @@ 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"));
@@ -214,9 +211,7 @@ void ImageProcessor::stereoCallback(
     const sensor_msgs::ImageConstPtr& cam0_img,
     const sensor_msgs::ImageConstPtr& cam1_img) {
 
-    // stop playing bagfile if printing images
+  //cout << "==================================" << endl;
-  //if(STREAMPAUSE)
-   // nh.setParam("/play_bag_image", false);
 
   // Get the current image.
   cam0_curr_img_ptr = cv_bridge::toCvShare(cam0_img,
@@ -224,27 +219,12 @@ void ImageProcessor::stereoCallback(
   cam1_curr_img_ptr = cv_bridge::toCvShare(cam1_img,
       sensor_msgs::image_encodings::MONO8);
 
-  ros::Time start_time = ros::Time::now();
-
-
-  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) {
-    start_time = ros::Time::now();
+    ros::Time start_time = ros::Time::now();
     initializeFirstFrame();
     //ROS_INFO("Detection time: %f",
     //    (ros::Time::now()-start_time).toSec());
@@ -257,7 +237,7 @@ void ImageProcessor::stereoCallback(
     //    (ros::Time::now()-start_time).toSec());
   } else {
     // Track the feature in the previous image.
-    start_time = ros::Time::now();
+    ros::Time start_time = ros::Time::now();
     trackFeatures();
     //ROS_INFO("Tracking time: %f",
     //    (ros::Time::now()-start_time).toSec());
@@ -265,7 +245,6 @@ 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());
 
@@ -288,18 +267,16 @@ void ImageProcessor::stereoCallback(
   //    (ros::Time::now()-start_time).toSec());
 
   // Publish features in the current image.
-  start_time = ros::Time::now();
+  ros::Time 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 <
@@ -307,10 +284,6 @@ 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;
 }
 
@@ -607,7 +580,6 @@ void ImageProcessor::trackFeatures() {
     ++after_ransac;
   }
 
-
   // Compute the tracking rate.
   int prev_feature_num = 0;
   for (const auto& item : *prev_features_ptr)
@@ -687,8 +659,6 @@ 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);
   image_handler::undistortPoints(
@@ -698,7 +668,6 @@ void ImageProcessor::stereoMatch(
       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]);

src/shrinkImage.py

@@ -1,75 +0,0 @@
-#!/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)