MIT-SPARK · violetteavi · Mar 26, 2020 · Mar 30, 2020 · Mar 30, 2020 · Mar 31, 2020
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -41,6 +41,7 @@ target_link_libraries(${PROJECT_NAME}_node ${PROJECT_NAME})
 
 catkin_add_gtest(testKimeraVioRos
   test/testKimeraVioRos.cpp
+  test/testRosBagDataProvider.cpp
 )
 target_link_libraries(testKimeraVioRos ${PROJECT_NAME})
 

diff --git a/README.md b/README.md
@@ -1,3 +1,4 @@
+
 # Kimera-VIO-ROS
 
 ROS Wrapper for [Kimera](https://github.com/MIT-SPARK/Kimera).
@@ -134,11 +135,18 @@ Download a [Euroc](https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisuali
 
 ## Running Unit tests
 
-To run unit tests using catkin for this specific package, call (after building the package and sourcing the workspace):
-
+To run unit tests using catkin for this package alone, build the package, source the workspace, then invoke:
 ```bash
+cd ~/catkin_ws/src/Kimera-VIO-ROS/
 catkin run_tests --no-deps --this
 ```
+Note that the tests will hang if roscore is not running in another terminal.
+To run unit tests through gdb, compile the tests with debug symbols and run the test executable. Note that the test's relative filepaths assume you are on the same folder level as the Kimera-VIO-ROS. This can be changed by passing the flag `--test_data_path=PATH/TO/KIMERA-VIO-ROS/test/data`
+```bash
+cd ~/catkin_ws/src/Kimera-VIO-ROS/
+catkin run_tests --no-deps --this DCMAKE_BUILD_TYPE=Debug
+gdb ../../devel/lib/kimera_vio_ros/testKimeraVioRos
+```
 
 ## Other functionalities
 
@@ -181,3 +189,4 @@ See the [documentation on hardware setup](docs/hardware_setup.md) for instructio
 
 # BSD License
 KimeraVIO ROS wrapper is open source under the BSD license, see the [LICENSE.BSD](./LICENSE.BSD) file.
+
diff --git a/include/kimera_vio_ros/RosDataProviderInterface.h b/include/kimera_vio_ros/RosDataProviderInterface.h
@@ -59,6 +59,9 @@ class RosDataProviderInterface : public DataProviderInterface {
   // Pipeline params
   VioParams vio_params_;
 
+  // Constants
+  static constexpr size_t kTfLookupTimeout = 5u;
+
  private:
   void printParsedParams() const;
 };

diff --git a/rviz/kimera_vio_euroc.rviz b/rviz/kimera_vio_euroc.rviz
@@ -6,6 +6,7 @@ Panels:
       Expanded:
         - /Global Options1
         - /PointCloud Time Horizon1
+        - /Debug Img1
         - /PoseGraph1
       Splitter Ratio: 0.5
     Tree Height: 493
@@ -38,26 +39,13 @@ Visualization Manager:
       Frame Timeout: 15
       Frames:
         All Enabled: false
-        base_link:
-          Value: true
-        cam0:
-          Value: true
-        cam1:
-          Value: true
-        world:
-          Value: true
       Marker Scale: 1
       Name: TF
       Show Arrows: true
       Show Axes: true
       Show Names: true
       Tree:
-        world:
-          base_link:
-            cam0:
-              {}
-            cam1:
-              {}
+        {}
       Update Interval: 0
       Value: true
     - Alpha: 1

diff --git a/test/data/EurocParams/BackendParams.yaml b/test/data/EurocParams/BackendParams.yaml
@@ -0,0 +1,89 @@
+%YAML:1.0
+# Backend modality for regular VIO
+# 0: Structureless factors only, 4: SPR pipeline
+backend_modality: 0
+
+# IMU PARAMETERS #############################################################
+gyroNoiseDensity: 0.00016968
+accNoiseDensity: 0.002
+gyroBiasSigma: 1.9393e-05
+accBiasSigma: 0.003
+imuIntegrationSigma: 1.0e-8
+n_gravity: [0.0, 0.0, -9.81]
+nominalImuRate: 0.005
+#INITIALIZATION PARAMETERS
+autoInitialize: 1
+roundOnAutoInitialize: 0
+initialPositionSigma: 1e-05
+initialRollPitchSigma: 0.174533 # 10.0/180.0*M_PI
+initialYawSigma: 0.00174533 # 0.1/180.0*M_PI
+initialVelocitySigma: 0.001
+initialAccBiasSigma: 0.1
+initialGyroBiasSigma: 0.01
+
+# VISION PARAMETERS ###########################################################
+## Smart Factors ##
+#
+# How to linearize the factor:
+# 0: Hessian, 1: Implicit Schur, 2:Jacobian_Q, 3:Jacobian_SVD
+linearizationMode: 0
+# How to manage degeneracy
+# 0: Ignore degeneracy, 1: Zero on degeneracy, 2: handle infinity
+degeneracyMode: 1
+# rankTolerance: threshold to decide whether triangulation is result.degenerate
+# (the rank is the number of singular values of the triangulation matrix which
+# are larger than rankTolerance)
+rankTolerance: 1
+# /** landmarkDistanceThreshold
+#   * if the landmark is triangulated at distance larger than this,
+#   * result is flagged as degenerate.
+#   */
+landmarkDistanceThreshold: 10
+
+# DynamicOutlierRejection:
+#  /**
+#   * If this is nonnegative the we will check if the average reprojection error
+#   * is smaller than this threshold after triangulation, otherwise result is
+#   * flagged as degenerate.
+#   */
+outlierRejection: 3
+# ///< threshold to decide whether to re-triangulate
+retriangulationThreshold: 0.001
+
+## Noise models ##
+smartNoiseSigma: 3.0
+monoNoiseSigma: 1.8
+monoNormType: 2
+monoNormParam: 4.6851
+stereoNoiseSigma: 1.8
+stereoNormType: 2
+stereoNormParam: 4.6851
+regularityNoiseSigma: 0.03
+regularityNormType: 1
+regularityNormParam: 0.04
+
+## Between Stereo Factors ##
+addBetweenStereoFactors: 1
+betweenRotationPrecision: 0 # Inverse of variance.
+betweenTranslationPrecision: 100 # 1/(0.1*0.1)
+
+# OPTIMIZATION PARAMETERS #####################################################
+relinearizeThreshold: 0.01
+relinearizeSkip: 1
+zeroVelocitySigma: 0.001
+noMotionPositionSigma: 0.001
+noMotionRotationSigma: 0.0001
+constantVelSigma: 0.01
+numOptimize: 1
+horizon: 6 # In seconds.
+wildfire_threshold: 0.001 # In seconds.
+useDogLeg: 0
+
+## NON PARSED PARAMS ##########################################################
+#  bool enableEPI; ///< if set to true, will refine triangulation using LM
+#  /**
+#   * If this is nonnegative the we will check if the average reprojection error
+#   * is smaller than this threshold after triangulation, otherwise result is
+#   * flagged as degenerate.
+#   */
+#  double dynamicOutlierRejectionThreshold;
diff --git a/test/data/EurocParams/FrontendParams.yaml b/test/data/EurocParams/FrontendParams.yaml
@@ -0,0 +1,74 @@
+%YAML:1.0
+#TRACKER PARAMETERS
+klt_win_size: 24
+klt_max_iter: 30
+klt_max_level: 4
+klt_eps: 0.1
+maxFeatureAge: 25
+
+# Detector Params
+# 0: FAST
+# 1: ORB
+# 2: AGAST
+# 3: GFTT, aka Good Features To Track
+feature_detector_type: 3
+maxFeaturesPerFrame: 800
+
+# Good Features To Track specific parameters
+quality_level: 0.001
+min_distance: 20
+block_size: 3
+use_harris_detector: 0
+k: 0.04
+
+# FAST detector specific parameters
+fast_thresh: 5
+
+equalizeImage: 0
+nominalBaseline: 0.11
+toleranceTemplateMatching: 0.15
+templ_cols: 101 #must be odd
+templ_rows: 11
+stripe_extra_rows: 0
+minPointDist: 0.5
+maxPointDist: 10
+bidirectionalMatching: 0
+
+# Non-maximum suppression params
+enable_non_max_suppression: 1
+non_max_suppression_type: 4
+
+# Subpixel corner refinement for the monocular case
+enable_subpixel_corner_finder: 1
+max_iters: 40
+epsilon_error: 0.001
+window_size: 10
+zero_zone: -1
+
+subpixelRefinementStereo: 0
+featureSelectionCriterion: 0
+featureSelectionHorizon: 3
+featureSelectionNrCornersToSelect: 600
+featureSelectionImuRate: 0.005
+featureSelectionDefaultDepth: 2
+featureSelectionCosineNeighborhood: 0.984807753012208 # Rad
+featureSelectionUseLazyEvaluation: 1
+useSuccessProbabilities: 1
+useRANSAC: 1
+minNrMonoInliers: 10
+minNrStereoInliers: 5
+ransac_threshold_mono: 1e-06
+ransac_threshold_stereo: 1
+ransac_use_1point_stereo: 1
+ransac_use_2point_mono: 1
+ransac_max_iterations: 100
+ransac_probability: 0.995
+ransac_randomize: 0
+intra_keyframe_time: 0.2
+minNumberFeatures: 0
+useStereoTracking: 1
+disparityThreshold: 0.5
+# Type of optical flow predictor to aid feature tracking:
+# 0: Static - assumes no optical flow between images (aka static camera).
+# 1: Rotational - use IMU gyro to estimate optical flow.
+optical_flow_predictor_type: 1
diff --git a/test/data/EurocParams/ImuParams.yaml b/test/data/EurocParams/ImuParams.yaml
@@ -0,0 +1,27 @@
+%YAML:1.0
+# Type of IMU preintegration:
+# 0: CombinedImuFactor
+# 1: ImuFactor
+imu_preintegration_type: 1
+
+# These are Euroc Dataset parameters
+# Sensor extrinsics wrt. the body-frame.
+T_BS:
+  cols: 4
+  rows: 4
+  data: [1.0, 0.0, 0.0, 0.0,
+         0.0, 1.0, 0.0, 0.0,
+         0.0, 0.0, 1.0, 0.0,
+         0.0, 0.0, 0.0, 1.0]
+rate_hz: 200
+
+# inertial sensor noise model parameters (static)
+gyroscope_noise_density: 1.6968e-04     # [ rad / s / sqrt(Hz) ]   ( gyro "white noise" )
+gyroscope_random_walk: 1.9393e-05       # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion )
+accelerometer_noise_density: 2.0000e-3  # [ m / s^2 / sqrt(Hz) ]   ( accel "white noise" )
+accelerometer_random_walk: 3.0000e-3    # [ m / s^3 / sqrt(Hz) ].  ( accel bias diffusion )
+
+# Extra parameters
+imu_integration_sigma: 1.0e-8
+imu_time_shift: 0.0
+n_gravity: [0.0, 0.0, -9.81]
diff --git a/test/data/EurocParams/LcdParams.yaml b/test/data/EurocParams/LcdParams.yaml
@@ -0,0 +1,60 @@
+%YAML:1.0
+# LoopClosureDetector parameters:
+use_nss: 1
+alpha: 0.001
+min_temporal_matches: 1
+dist_local: 1
+max_db_results: 50
+min_nss_factor: 0.05
+min_matches_per_group: 1
+max_intragroup_gap: 3
+max_distance_between_groups: 3
+max_distance_between_queries: 2
+
+geom_check_id: 0
+min_correspondences: 12
+max_ransac_iterations_mono: 500
+ransac_probability_mono: 0.995
+ransac_threshold_mono: 1e-5
+ransac_randomize_mono: 1
+ransac_inlier_threshold_mono: 0.01
+
+pose_recovery_option_id: 0
+max_ransac_iterations_stereo: 500
+ransac_probability_stereo: 0.995
+ransac_threshold_stereo: 0.3
+ransac_randomize_stereo: 1
+ransac_inlier_threshold_stereo: 0.3
+use_mono_rot: 0
+
+lowe_ratio: 0.2  # TODO(marcus): get rid, not used
+matcher_type: 3
+
+nfeatures: 1000
+scale_factor: 1.2
+nlevels: 8
+edge_threshold: 31
+first_level: 0
+WTA_K: 2
+score_type_id: 0
+patch_sze: 31
+fast_threshold: 20
+
+pgo_rot_threshold: 0.005
+pgo_trans_threshold: 0.05
+
+# geom_check_id options:
+#   0: NISTER
+#   1: NONE
+
+# pose_recovery_option_id options:
+#   0: RANSAC_ARUN
+#   1: GIVEN_ROT
+
+# matcher_type options:
+#   0: FLANNBASED
+#   1: BRUTEFORCE
+#   2: BRUTEFORCE_L1
+#   3: BRUTEFORCE_HAMMING
+#   4: BRUTEFORCE_HAMMINGLUT
+#   5: BRUTEFORCE_SL2
diff --git a/test/data/EurocParams/LeftCameraParams.yaml b/test/data/EurocParams/LeftCameraParams.yaml
@@ -0,0 +1,21 @@
+%YAML:1.0
+# General sensor definitions.
+camera_id: left_cam
+
+# Sensor extrinsics wrt. the body-frame.
+T_BS:
+  cols: 4
+  rows: 4
+  data: [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975,
+         0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768,
+        -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949,
+         0.0, 0.0, 0.0, 1.0]
+
+# Camera specific definitions.
+rate_hz: 20
+resolution: [752, 480]
+camera_model: pinhole
+intrinsics: [458.654, 457.296, 367.215, 248.375] #fu, fv, cu, cv
+distortion_model: radial-tangential
+distortion_coefficients: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05]
+
diff --git a/test/data/EurocParams/PipelineParams.yaml b/test/data/EurocParams/PipelineParams.yaml
@@ -0,0 +1,13 @@
+%YAML:1.0
+# frontend_type
+# Type of VIO Frontend to use
+# 0: StereoImu
+frontend_type: 0
+
+# Type of vioBackEnd to use:
+# 0: VioBackEnd
+# 1: RegularVioBackEnd
+backend_type: 1
+
+# Run VIO parallel or sequential
+parallel_run: 1
diff --git a/test/data/EurocParams/RightCameraParams.yaml b/test/data/EurocParams/RightCameraParams.yaml
@@ -0,0 +1,20 @@
+%YAML:1.0
+# General sensor definitions.
+camera_id: right_cam
+
+# Sensor extrinsics wrt. the body-frame.
+T_BS:
+  cols: 4
+  rows: 4
+  data: [0.0125552670891, -0.999755099723, 0.0182237714554, -0.0198435579556,
+         0.999598781151, 0.0130119051815, 0.0251588363115, 0.0453689425024,
+        -0.0253898008918, 0.0179005838253, 0.999517347078, 0.00786212447038,
+         0.0, 0.0, 0.0, 1.0]
+
+# Camera specific definitions.
+rate_hz: 20
+resolution: [752, 480]
+camera_model: pinhole
+intrinsics: [457.587, 456.134, 379.999, 255.238] #fu, fv, cu, cv
+distortion_model: radial-tangential
+distortion_coefficients: [-0.28368365,  0.07451284, -0.00010473, -3.55590700e-05]