diff --git a/CMakeLists.txt b/CMakeLists.txt index 0e7f254b..d6ee5b04 100644 --- 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 index 67b7e8d7..c943c755 100644 --- 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 index a3bd7090..f030ea69 100644 --- 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 index 59504cd6..f7a26d4e 100644 --- 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 new file mode 100644 index 00000000..a0410054 --- /dev/null +++ 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 new file mode 100644 index 00000000..759a5582 --- /dev/null +++ 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 new file mode 100644 index 00000000..5c2596e4 --- /dev/null +++ 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 new file mode 100644 index 00000000..1bbec055 --- /dev/null +++ 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 new file mode 100755 index 00000000..ca668553 --- /dev/null +++ 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 new file mode 100644 index 00000000..c9b387a1 --- /dev/null +++ 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 new file mode 100755 index 00000000..1cf87f3c --- /dev/null +++ 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] diff --git a/test/data/EurocParams/flags/Mesher.flags b/test/data/EurocParams/flags/Mesher.flags new file mode 100644 index 00000000..1fe3e78f --- /dev/null +++ b/test/data/EurocParams/flags/Mesher.flags @@ -0,0 +1,58 @@ +#!/bin/bash +# Mesher parameters. +# General functionality for the mesher. +--add_extra_lmks_from_stereo=true +--reduce_mesh_to_time_horizon=true +--compute_per_vertex_normals=false + +# Visualization. +--visualize_histogram_1D=false +--log_histogram_1D=false +--visualize_histogram_2D=false +--log_histogram_2D=false +--visualize_mesh_2d=true + +# Mesh filters. +--max_grad_in_triangle=-1 +--min_ratio_btw_largest_smallest_side=0.2 +--min_elongation_ratio=0.2 +--max_triangle_side=2.0 + +# Association. +--normal_tolerance_polygon_plane_association=0.021 +--distance_tolerance_polygon_plane_association=0.20 +--normal_tolerance_plane_plane_association=0.011 +--distance_tolerance_plane_plane_association=0.20 +--do_double_association=true +--only_associate_a_polygon_to_a_single_plane=true + +# Segmentation. +--normal_tolerance_horizontal_surface=0.035 +--normal_tolerance_walls=0.021 +--only_use_non_clustered_points=true + +# Histogram 2D. +--hist_2d_gaussian_kernel_size=3 +--hist_2d_nr_of_local_max=2 +--hist_2d_min_support=20 +--hist_2d_min_dist_btw_local_max=5 +--hist_2d_theta_bins=40 +--hist_2d_distance_bins=40 +--hist_2d_theta_range_min=0 +--hist_2d_theta_range_max=3.14159265 +--hist_2d_distance_range_min=-6.0 +--hist_2d_distance_range_max=6.0 + +# Z histogram. +--z_histogram_bins=512 +--z_histogram_min_range=-0.75 +--z_histogram_max_range=3.0 +--z_histogram_window_size=3 +--z_histogram_peak_per=0.5 +--z_histogram_min_support=50 +--z_histogram_min_separation=0.1 +--z_histogram_gaussian_kernel_size=5 +--z_histogram_max_number_of_peaks_to_select=3 + +# Extras +--return_mesh_2d=true diff --git a/test/data/EurocParams/flags/RegularVioBackEnd.flags b/test/data/EurocParams/flags/RegularVioBackEnd.flags new file mode 100644 index 00000000..7804e83a --- /dev/null +++ b/test/data/EurocParams/flags/RegularVioBackEnd.flags @@ -0,0 +1,13 @@ +#!/bin/bash +# RegularVioBackEnd parameters. +--min_num_of_observations=2 +--max_parallax=150 +--min_num_obs_for_proj_factor=4 +--convert_extra_smart_factors_to_proj_factors=false +--remove_old_reg_factors=true +--min_num_of_plane_constraints_to_add_factors=40 +--min_num_of_plane_constraints_to_remove_factors=25 +--use_unstable_plane_removal=false +--min_num_of_plane_constraints_to_avoid_seg_fault=10 +--prior_noise_sigma_normal=0.1 +--prior_noise_sigma_distance=0.1 diff --git a/test/data/EurocParams/flags/VioBackEnd.flags b/test/data/EurocParams/flags/VioBackEnd.flags new file mode 100644 index 00000000..8a29e6de --- /dev/null +++ b/test/data/EurocParams/flags/VioBackEnd.flags @@ -0,0 +1,4 @@ +#!/bin/bash +--debug_graph_before_opt=true +--process_cheirality=true +--max_number_of_cheirality_exceptions=5 diff --git a/test/data/EurocParams/flags/Visualizer3D.flags b/test/data/EurocParams/flags/Visualizer3D.flags new file mode 100644 index 00000000..cd84734e --- /dev/null +++ b/test/data/EurocParams/flags/Visualizer3D.flags @@ -0,0 +1,9 @@ +#!/bin/bash +# Visualizer 3D parameters. +--mesh_shading=0 +--mesh_representation=1 +--set_mesh_lighting=true +--set_mesh_ambient=false +--log_mesh=false +--log_accumulated_mesh=false +--displayed_trajectory_length=-1 diff --git a/test/data/EurocParams/flags/stereoVIOEuroc.flags b/test/data/EurocParams/flags/stereoVIOEuroc.flags new file mode 100644 index 00000000..ba2d91e6 --- /dev/null +++ b/test/data/EurocParams/flags/stereoVIOEuroc.flags @@ -0,0 +1,27 @@ +#!/bin/bash +--log_output=false +--backend_type=1 +--regular_vio_backend_modality=0 +--deterministic_random_number_generator=true +--visualize=true +--visualize_lmk_type=false +--visualize_mesh=true +--visualize_mesh_with_colored_polygon_clusters=false +--visualize_point_cloud=true +--visualize_convex_hull=false +--visualize_plane_constraints=false +--visualize_planes=false +--visualize_plane_label=false +--visualize_semantic_mesh=false +--visualize_mesh_in_frustum=true +#--visualize_load_mesh_filename=/home/tonirv/datasets/euroc/V1_01_easy/mav0/pointcloud0/data.ply +--viz_type=0 +--min_num_obs_for_mesher_points=3 +--extract_planes_from_the_scene=false + +# For displaying/saving frontend images: +--visualize_frontend_images=false +--save_frontend_images=false +--log_feature_tracks=true +--log_mono_tracking_images=false +--log_stereo_matching_images=false diff --git a/test/data/MicroEurocRosbag/MicroEuroc.bag b/test/data/MicroEurocRosbag/MicroEuroc.bag new file mode 100644 index 00000000..324838ba Binary files /dev/null and b/test/data/MicroEurocRosbag/MicroEuroc.bag differ diff --git a/test/testKimeraVioRos.cpp b/test/testKimeraVioRos.cpp index 2210788b..66eb3a03 100644 --- a/test/testKimeraVioRos.cpp +++ b/test/testKimeraVioRos.cpp @@ -1,19 +1,29 @@ +/** + * @file testKimeraVioRos.cpp + * @brief Unit test stub and entry point. + * @author Antoni Rosinol + * @author Andrew Violette + */ + #include #include +#include "kimera_vio_ros/KimeraVioRos.h" +// Executable is at ./devel/lib/kimera_vio_ros/testKimeraVioRos +// Executable is run from ./build/kimera_vio_ros +DEFINE_string(test_data_path, "../../src/Kimera-VIO-ROS/test/data", + "Path to data for unit tests."); namespace VIO { -TEST(KimeraVioRosTest, KimeraVioRosTest) { - // Works? - EXPECT_TRUE(false); -} - } // namespace VIO int main(int argc, char** argv) { ::testing::InitGoogleTest(&argc, argv); google::InitGoogleLogging(argv[0]); + // Initialize ROS node. + ros::init(argc, argv, "kimera_vio"); + FLAGS_logtostderr = true; FLAGS_alsologtostderr = true; FLAGS_colorlogtostderr = true; diff --git a/test/testRosBagDataProvider.cpp b/test/testRosBagDataProvider.cpp new file mode 100644 index 00000000..336da2d5 --- /dev/null +++ b/test/testRosBagDataProvider.cpp @@ -0,0 +1,215 @@ +/** + * @file testRosBagDataProvider.cpp + * @brief Test rosbag data provider in many input modes + * @author Antoni Rosinol + * @author Andrew Violette + */ + +#include +#include +#include + +#include +#include +#include + +#include + +#include "kimera_vio_ros/RosBagDataProvider.h" +#include "kimera-vio/pipeline/Pipeline.h" + +DECLARE_string(test_data_path); + +namespace VIO { + +class TestRosBagDataProvider : public ::testing::Test { + public: + TestRosBagDataProvider() + : dataset_parser_(nullptr), + vio_pipeline_(nullptr), + vio_params_(FLAGS_test_data_path + "/EurocParams") { + buildOnlinePipeline(vio_params_); + } + ~TestRosBagDataProvider() override { + destroyPipeline(); + } + + protected: + void SetUp() override {} + void TearDown() override {} + + void buildOnlinePipeline(const VioParams& vio_params) { + constexpr int initial_k = 10; + constexpr int final_k = 80; + // Needed in order to disconnect previous pipeline in case someone calls + // this function repeatedly within the same test. + destroyPipeline(); + LOG(INFO) << "Building pipeline."; + vio_pipeline_ = VIO::make_unique(vio_params); + dataset_parser_ = makeMicroEurocRosbagReader(vio_params); + connectVioPipeline(); + } + + void buildOfflinePipeline(const VioParams& vio_params) { + constexpr int initial_k = 10; + constexpr int final_k = 80; + // Needed in order to disconnect previous pipeline in case someone calls + // this function repeatedly within the same test. + destroyPipeline(); + LOG(INFO) << "Building pipeline."; + vio_pipeline_ = VIO::make_unique(vio_params); + dataset_parser_ = makeMicroEurocRosbagReader(vio_params); + connectVioPipelineWithBlockingIfFullQueues(); + } + + VIO::RosbagDataProvider::UniquePtr makeMicroEurocRosbagReader( + const VioParams& vio_params) { + ros::NodeHandle parameter_server("~"); + + parameter_server.setParam( + "rosbag_path", + FLAGS_test_data_path + "/MicroEurocRosbag/MicroEuroc.bag"); + // For dataprovider + parameter_server.setParam("left_cam_rosbag_topic", "/cam0/image_raw"); + parameter_server.setParam("right_cam_rosbag_topic", "/cam1/image_raw"); + parameter_server.setParam("imu_rosbag_topic", "/imu0"); + parameter_server.setParam("ground_truth_odometry_rosbag_topic", ""); + parameter_server.setParam("base_link_frame_id", "base_link"); + parameter_server.setParam("world_frame_id", "world"); + parameter_server.setParam("map_frame_id", "map"); + parameter_server.setParam("left_cam_frame_id", "cam0"); + parameter_server.setParam("right_cam_frame_id", "cam1"); + + // For others + parameter_server.setParam("online", "false"); + parameter_server.setParam("use_lcd", "false"); + + VIO::RosbagDataProvider::UniquePtr micro_euroc_provider = + VIO::make_unique(vio_params); + return micro_euroc_provider; + } + + void connectVioPipeline() { + LOG(INFO) << "Connecting pipeline."; + CHECK(dataset_parser_); + CHECK(vio_pipeline_); + + // Register callback to shutdown data provider in case VIO pipeline + // shutsdown. + vio_pipeline_->registerShutdownCallback(std::bind( + &VIO::DataProviderInterface::shutdown, dataset_parser_.get())); + + // Register callback to vio pipeline. + dataset_parser_->registerImuSingleCallback( + std::bind(&VIO::Pipeline::fillSingleImuQueue, + vio_pipeline_.get(), + std::placeholders::_1)); + // We use blocking variants to avoid overgrowing the input queues (use + // the non-blocking versions with real sensor streams) + dataset_parser_->registerLeftFrameCallback( + std::bind(&VIO::Pipeline::fillLeftFrameQueue, + vio_pipeline_.get(), + std::placeholders::_1)); + dataset_parser_->registerRightFrameCallback( + std::bind(&VIO::Pipeline::fillRightFrameQueue, + vio_pipeline_.get(), + std::placeholders::_1)); + } + + void connectVioPipelineWithBlockingIfFullQueues() { + LOG(INFO) << "Connecting pipeline."; + CHECK(dataset_parser_); + CHECK(vio_pipeline_); + + // Register callback to shutdown data provider in case VIO pipeline + // shutsdown. + vio_pipeline_->registerShutdownCallback(std::bind( + &VIO::DataProviderInterface::shutdown, dataset_parser_.get())); + + // Register callback to vio pipeline. + dataset_parser_->registerImuSingleCallback( + std::bind(&VIO::Pipeline::fillSingleImuQueue, + vio_pipeline_.get(), + std::placeholders::_1)); + // We use blocking variants to avoid overgrowing the input queues (use + // the non-blocking versions with real sensor streams) + dataset_parser_->registerLeftFrameCallback( + std::bind(&VIO::Pipeline::fillLeftFrameQueueBlockingIfFull, + vio_pipeline_.get(), + std::placeholders::_1)); + dataset_parser_->registerRightFrameCallback( + std::bind(&VIO::Pipeline::fillRightFrameQueueBlockingIfFull, + vio_pipeline_.get(), + std::placeholders::_1)); + } + + void destroyPipeline() { + LOG(INFO) << "Destroying pipeline."; + // First destroy the VIO pipeline (since this will call the shutdown of + // the dataset_parser) + vio_pipeline_.reset(); + // Then destroy the dataset parser. + dataset_parser_.reset(); + } + + protected: + DataProviderInterface::UniquePtr dataset_parser_; + Pipeline::UniquePtr vio_pipeline_; + VioParams vio_params_; +}; + +TEST_F(TestRosBagDataProvider, OnlineParallelStartManualShutdown) { + ASSERT_TRUE(vio_params_.parallel_run_); + ASSERT_TRUE(dataset_parser_); + ASSERT_TRUE(vio_pipeline_); + auto handle_pipeline = + std::async(std::launch::async, &VIO::Pipeline::spin, vio_pipeline_.get()); + vio_pipeline_->shutdown(); + // Expect false, since the pipeline has been shut down. + EXPECT_FALSE(handle_pipeline.get()); +} + +TEST_F(TestRosBagDataProvider, OnlineParallelSpinManualShutdown) { + ASSERT_TRUE(vio_params_.parallel_run_); + ASSERT_TRUE(dataset_parser_); + ASSERT_TRUE(vio_pipeline_); + auto handle = std::async(std::launch::async, + &VIO::DataProviderInterface::spin, + dataset_parser_.get()); + auto handle_pipeline = + std::async(std::launch::async, &VIO::Pipeline::spin, vio_pipeline_.get()); + vio_pipeline_->shutdown(); + // Expect false, since the pipeline has been shut down. + EXPECT_FALSE(handle.get()); + EXPECT_FALSE(handle_pipeline.get()); +} + +TEST_F(TestRosBagDataProvider, OfflineParallelStartManualShutdown) { + buildOfflinePipeline(vio_params_); + ASSERT_TRUE(vio_params_.parallel_run_); + ASSERT_TRUE(dataset_parser_); + ASSERT_TRUE(vio_pipeline_); + auto handle_pipeline = + std::async(std::launch::async, &VIO::Pipeline::spin, vio_pipeline_.get()); + vio_pipeline_->shutdown(); + // Expect false, since the pipeline has been shut down. + EXPECT_FALSE(handle_pipeline.get()); +} + +TEST_F(TestRosBagDataProvider, OfflineParallelSpinManualShutdown) { + buildOfflinePipeline(vio_params_); + ASSERT_TRUE(vio_params_.parallel_run_); + ASSERT_TRUE(dataset_parser_); + ASSERT_TRUE(vio_pipeline_); + auto handle = std::async(std::launch::async, + &VIO::DataProviderInterface::spin, + dataset_parser_.get()); + auto handle_pipeline = + std::async(std::launch::async, &VIO::Pipeline::spin, vio_pipeline_.get()); + vio_pipeline_->shutdown(); + // Expect false, since the pipeline has been shut down. + EXPECT_FALSE(handle.get()); + EXPECT_FALSE(handle_pipeline.get()); +} + +} // namespace VIO