diff --git a/.gitignore b/.gitignore index b5e0a7a67..fb1ed5e75 100644 --- a/.gitignore +++ b/.gitignore @@ -7,3 +7,4 @@ doxgen_generated *.swp *.swo doc +.vscode diff --git a/ReadMe.md b/ReadMe.md index 63a0bd4f7..472f356c5 100644 --- a/ReadMe.md +++ b/ReadMe.md @@ -17,10 +17,11 @@ details on what the system supports. * Github project page - https://github.com/rpng/open_vins * Documentation - https://docs.openvins.com/ * Getting started guide - https://docs.openvins.com/getting-started.html -* Publication reference - http://udel.edu/~pgeneva/downloads/papers/c10.pdf +* Publication reference - https://pgeneva.com/downloads/papers/Geneva2020ICRA.pdf ## News / Events +* **July 14, 2022** - Improved feature extraction logic for >100hz tracking, some bug fixes and updated scripts. See v2.6.1 [PR#259](https://github.com/rpng/open_vins/pull/259). * **March 14, 2022** - Initial dynamic initialization open sourcing, asynchronous subscription to inertial readings and publishing of odometry, support for lower frequency feature tracking. See v2.6 [PR#232](https://github.com/rpng/open_vins/pull/232) for details. * **December 13, 2021** - New YAML configuration system, ROS2 support, Docker images, robust static initialization based on disparity, internal logging system to reduce verbosity, image transport publishers, dynamic number of features support, and other small fixes. See v2.5 [PR#209](https://github.com/rpng/open_vins/pull/209) for details. diff --git a/config/euroc_mav/estimator_config.yaml b/config/euroc_mav/estimator_config.yaml index 17d3a1029..98e422462 100644 --- a/config/euroc_mav/estimator_config.yaml +++ b/config/euroc_mav/estimator_config.yaml @@ -27,7 +27,7 @@ feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH" # zero velocity update parameters we can use # we support either IMU-based or disparity detection. -try_zupt: true +try_zupt: false zupt_chi2_multipler: 0 # set to 0 for only disp-based zupt_max_velocity: 0.1 zupt_noise_multiplier: 50 @@ -39,24 +39,25 @@ zupt_only_at_beginning: true init_window_time: 2.0 # how many seconds to collect initialization information init_imu_thresh: 1.5 # threshold for variance of the accelerometer to detect a "jerk" in motion -init_max_disparity: 1.5 # max disparity to consider the platform stationary (dependent on resolution) -init_max_features: 25 # how many features to track during initialization (saves on computation) - -init_dyn_mle_opt_calib: false -init_dyn_mle_max_iter: 50 -init_dyn_mle_max_time: 0.05 -init_dyn_mle_max_threads: 6 -init_dyn_num_pose: 6 -init_dyn_min_deg: 10.0 - -init_dyn_inflation_ori: 10 -init_dyn_inflation_vel: 100 -init_dyn_inflation_bg: 10 -init_dyn_inflation_ba: 100 -init_dyn_min_rec_cond: 1e-12 - -init_dyn_bias_g: [0.0, 0.0, 0.0] -init_dyn_bias_a: [0.0, 0.0, 0.0] +init_max_disparity: 10.0 # max disparity to consider the platform stationary (dependent on resolution) +init_max_features: 75 # how many features to track during initialization (saves on computation) + +init_dyn_use: false # if dynamic initialization should be used +init_dyn_mle_opt_calib: false # if we should optimize calibration during intialization (not recommended) +init_dyn_mle_max_iter: 50 # how many iterations the MLE refinement should use (zero to skip the MLE) +init_dyn_mle_max_time: 0.05 # how many seconds the MLE should be completed in +init_dyn_mle_max_threads: 6 # how many threads the MLE should use +init_dyn_num_pose: 6 # number of poses to use within our window time (evenly spaced) +init_dyn_min_deg: 10.0 # orientation change needed to try to init + +init_dyn_inflation_ori: 10 # what to inflate the recovered q_GtoI covariance by +init_dyn_inflation_vel: 100 # what to inflate the recovered v_IinG covariance by +init_dyn_inflation_bg: 10 # what to inflate the recovered bias_g covariance by +init_dyn_inflation_ba: 100 # what to inflate the recovered bias_a covariance by +init_dyn_min_rec_cond: 1e-12 # reciprocal condition number thresh for info inversion + +init_dyn_bias_g: [ 0.0, 0.0, 0.0 ] # initial gyroscope bias guess +init_dyn_bias_a: [ 0.0, 0.0, 0.0 ] # initial accelerometer bias guess # ================================================================== # ================================================================== @@ -76,14 +77,14 @@ filepath_gt: "/tmp/ov_groundtruth.txt" # our front-end feature tracking parameters # we have a KLT and descriptor based (KLT is better implemented...) -use_klt: true -num_pts: 200 -fast_threshold: 20 -grid_x: 20 -grid_y: 20 -min_px_dist: 15 -knn_ratio: 0.70 -track_frequency: 21.0 +use_klt: true # if true we will use KLT, otherwise use a ORB descriptor + robust matching +num_pts: 200 # number of points (per camera) we will extract and try to track +fast_threshold: 20 # threshold for fast extraction (warning: lower threshs can be expensive) +grid_x: 5 # extraction sub-grid count for horizontal direction (uniform tracking) +grid_y: 5 # extraction sub-grid count for vertical direction (uniform tracking) +min_px_dist: 10 # distance between features (features near each other provide less information) +knn_ratio: 0.70 # descriptor knn threshold for the top two descriptor matches +track_frequency: 21.0 # frequency we will perform feature tracking at (in frames per second / hertz) downsample_cameras: false # will downsample image in half if true multi_threading: true # if should enable opencv multi threading histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE diff --git a/config/kaist/estimator_config.yaml b/config/kaist/estimator_config.yaml index 82dfff5f2..90bc96735 100644 --- a/config/kaist/estimator_config.yaml +++ b/config/kaist/estimator_config.yaml @@ -9,17 +9,16 @@ use_rk4int: true # if rk4 integration should be used (overrides imu averaging) use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints max_cameras: 2 -calib_cam_extrinsics: false +calib_cam_extrinsics: false # degenerate motion calib_cam_intrinsics: true -calib_cam_timeoffset: true +calib_cam_timeoffset: true # degenerate motion -max_clones: 12 +max_clones: 8 max_slam: 50 max_slam_in_update: 25 max_msckf_in_update: 50 dt_slam_delay: 1 -#gravity_mag: 9.79858 gravity_mag: 9.81 feat_rep_msckf: "ANCHORED_MSCKF_INVERSE_DEPTH" @@ -29,7 +28,7 @@ feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH" # zero velocity update parameters we can use # we support either IMU-based or disparity detection. try_zupt: true -zupt_chi2_multipler: 1 # set to 0 for only disp-based +zupt_chi2_multipler: 0.25 # set to 0 for only disp-based zupt_max_velocity: 0.1 zupt_noise_multiplier: 5 zupt_max_disparity: 0.4 # set to 0 for only imu-based @@ -39,16 +38,17 @@ zupt_only_at_beginning: false # ================================================================== init_window_time: 2.0 -init_imu_thresh: 0.5 #0.5 +init_imu_thresh: 0.5 init_max_disparity: 1.5 -init_max_features: 25 +init_max_features: 75 +init_dyn_use: true init_dyn_mle_opt_calib: false init_dyn_mle_max_iter: 50 init_dyn_mle_max_time: 0.05 init_dyn_mle_max_threads: 6 init_dyn_num_pose: 6 -init_dyn_min_deg: 0.0 # traj is mostly straight line motion +init_dyn_min_deg: 5.0 # traj is mostly straight line motion init_dyn_inflation_ori: 10 init_dyn_inflation_vel: 100 @@ -78,18 +78,19 @@ filepath_gt: "/tmp/ov_groundtruth.txt" # we have a KLT and descriptor based (KLT is better implemented...) use_klt: true num_pts: 200 -fast_threshold: 50 -grid_x: 20 -grid_y: 15 -min_px_dist: 30 +fast_threshold: 30 +grid_x: 5 +grid_y: 5 +min_px_dist: 20 knn_ratio: 0.65 -track_frequency: 21.0 +track_frequency: 31.0 downsample_cameras: false multi_threading: true histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE -fi_max_dist: 500 -fi_max_baseline: 800 +fi_min_dist: 0.25 +fi_max_dist: 150.0 +fi_max_baseline: 200 fi_max_cond_number: 20000 fi_triangulate_1d: false @@ -103,9 +104,9 @@ downsize_aruco: true # ================================================================== # camera noises and chi-squared threshold multipliers -up_msckf_sigma_px: 1 +up_msckf_sigma_px: 1.5 up_msckf_chi2_multipler: 1 -up_slam_sigma_px: 1 +up_slam_sigma_px: 1.5 up_slam_chi2_multipler: 1 up_aruco_sigma_px: 1 up_aruco_chi2_multipler: 1 diff --git a/config/kaist/kalibr_imu_chain.yaml b/config/kaist/kalibr_imu_chain.yaml index b3eea522e..df2714f67 100644 --- a/config/kaist/kalibr_imu_chain.yaml +++ b/config/kaist/kalibr_imu_chain.yaml @@ -1,22 +1,23 @@ %YAML:1.0 # need to specify the file type at the top! -# MTI-100 series converted from data sheet, guess on bias random walk +# MTI-100 series converted from datasheet, guess on bias random walk # https://www.xsens.com/hubfs/Downloads/usermanual/MTi_usermanual.pdf + imu0: T_i_b: - [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] - accelerometer_noise_density: 2.0000e-03 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) - accelerometer_random_walk: 3.0000e-03 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) - 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-03 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) +# accelerometer_random_walk: 3.0000e-03 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) +# 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: 5.8860e-03 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) + accelerometer_random_walk: 1.0000e-04 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) + gyroscope_noise_density: 1.7453e-04 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) + gyroscope_random_walk: 1.0000e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) model: calibrated rostopic: /imu/data_raw time_offset: 0.0 diff --git a/config/kaist/kalibr_imucam_chain.yaml b/config/kaist/kalibr_imucam_chain.yaml index 5f5e715a3..172591ee0 100644 --- a/config/kaist/kalibr_imucam_chain.yaml +++ b/config/kaist/kalibr_imucam_chain.yaml @@ -13,6 +13,7 @@ cam0: intrinsics: [8.1690378992770002e+02,8.1156803828490001e+02,6.0850726281690004e+02,2.6347599764440002e+02] resolution: [1280, 560] rostopic: /stereo/left/image_raw + cam1: T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI - [-0.00768,-0.01509,0.99986,1.73376] @@ -25,4 +26,4 @@ cam1: distortion_model: radtan intrinsics: [8.1378205539589999e+02,8.0852165574269998e+02,6.1386419539320002e+02,2.4941049348650000e+02] resolution: [1280, 560] - rostopic: /stereo/right/image_raw \ No newline at end of file + rostopic: /stereo/right/image_raw diff --git a/config/kaist_vio/estimator_config.yaml b/config/kaist_vio/estimator_config.yaml index b26db2d65..a23950973 100644 --- a/config/kaist_vio/estimator_config.yaml +++ b/config/kaist_vio/estimator_config.yaml @@ -16,18 +16,18 @@ calib_cam_timeoffset: false # disable since this is a degenerate motion max_clones: 11 # how many clones in the sliding window max_slam: 50 # number of features in our state vector max_slam_in_update: 25 # update can be split into sequential updates of batches, how many in a batch -max_msckf_in_update: 40 # how many MSCKF features to use in the update +max_msckf_in_update: 50 # how many MSCKF features to use in the update dt_slam_delay: 1 # delay before initializing (helps with stability from bad initialization...) gravity_mag: 9.81 # magnitude of gravity in this location -feat_rep_msckf: "GLOBAL_3D" +feat_rep_msckf: "ANCHORED_MSCKF_INVERSE_DEPTH" feat_rep_slam: "ANCHORED_MSCKF_INVERSE_DEPTH" feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH" # zero velocity update parameters we can use # we support either IMU-based or disparity detection. -try_zupt: true +try_zupt: false zupt_chi2_multipler: 0 # set to 0 for only disp-based zupt_max_velocity: 0.02 zupt_noise_multiplier: 10 @@ -38,10 +38,11 @@ zupt_only_at_beginning: false # ================================================================== init_window_time: 2.0 # how many seconds to collect initialization information -init_imu_thresh: 1.5 # threshold for variance of the accelerometer to detect a "jerk" in motion -init_max_disparity: 0.20 # max disparity to consider the platform stationary (dependent on resolution) -init_max_features: 25 # how many features to track during initialization (saves on computation) +init_imu_thresh: 0.60 # threshold for variance of the accelerometer to detect a "jerk" in motion +init_max_disparity: 5.0 # max disparity to consider the platform stationary (dependent on resolution) +init_max_features: 75 # how many features to track during initialization (saves on computation) +init_dyn_use: false init_dyn_mle_opt_calib: false init_dyn_mle_max_iter: 50 init_dyn_mle_max_time: 0.05 @@ -79,11 +80,11 @@ filepath_gt: "/tmp/ov_groundtruth.txt" use_klt: true num_pts: 200 fast_threshold: 30 -grid_x: 20 -grid_y: 20 +grid_x: 5 +grid_y: 5 min_px_dist: 15 knn_ratio: 0.70 -track_frequency: 10.0 +track_frequency: 31.0 downsample_cameras: false # will downsample image in half if true multi_threading: true # if should enable opencv multi threading histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE diff --git a/config/kaist_vio/kalibr_imu_chain.yaml b/config/kaist_vio/kalibr_imu_chain.yaml index ee06ed6df..b9a3fe5d1 100644 --- a/config/kaist_vio/kalibr_imu_chain.yaml +++ b/config/kaist_vio/kalibr_imu_chain.yaml @@ -6,14 +6,14 @@ imu0: - [0.0, 1.0, 0.0, 0.0] - [0.0, 0.0, 1.0, 0.0] - [0.0, 0.0, 0.0, 1.0] - accelerometer_noise_density: 0.00333388 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) - accelerometer_random_walk: 0.00047402 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) - gyroscope_noise_density: 0.00005770 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) - gyroscope_random_walk: 0.00001565 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) -# accelerometer_noise_density: 0.07 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) -# accelerometer_random_walk: 0.009 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) -# gyroscope_noise_density: 0.001 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) -# gyroscope_random_walk: 0.0003 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) +# accelerometer_noise_density: 0.00333388 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) +# accelerometer_random_walk: 0.00047402 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) +# gyroscope_noise_density: 0.00005770 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) +# gyroscope_random_walk: 0.00001565 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) + accelerometer_noise_density: 0.07 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) + accelerometer_random_walk: 0.009 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) + gyroscope_noise_density: 0.001 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) + gyroscope_random_walk: 0.0003 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) model: calibrated rostopic: /mavros/imu/data time_offset: 0.0 diff --git a/config/rpng_aruco/estimator_config.yaml b/config/rpng_aruco/estimator_config.yaml index 1bb04aba1..24a4186f6 100644 --- a/config/rpng_aruco/estimator_config.yaml +++ b/config/rpng_aruco/estimator_config.yaml @@ -27,7 +27,7 @@ feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH" # zero velocity update parameters we can use # we support either IMU-based or disparity detection. -try_zupt: true +try_zupt: false zupt_chi2_multipler: 0 # set to 0 for only disp-based zupt_max_velocity: 0.1 zupt_noise_multiplier: 50 @@ -39,9 +39,10 @@ zupt_only_at_beginning: true init_window_time: 2.0 init_imu_thresh: 1.2 -init_max_disparity: 1.5 -init_max_features: 25 +init_max_disparity: 2.0 +init_max_features: 75 +init_dyn_use: false init_dyn_mle_opt_calib: false init_dyn_mle_max_iter: 50 init_dyn_mle_max_time: 0.05 @@ -77,9 +78,9 @@ filepath_gt: "/tmp/ov_groundtruth.txt" use_klt: true num_pts: 150 fast_threshold: 30 -grid_x: 20 -grid_y: 20 -min_px_dist: 30 +grid_x: 5 +grid_y: 5 +min_px_dist: 20 knn_ratio: 0.85 track_frequency: 21.0 downsample_cameras: false @@ -96,12 +97,12 @@ downsize_aruco: true # ================================================================== # camera noises and chi-squared threshold multipliers -up_msckf_sigma_px: 1 +up_msckf_sigma_px: 1.5 up_msckf_chi2_multipler: 1 -up_slam_sigma_px: 1 +up_slam_sigma_px: 1.5 up_slam_chi2_multipler: 1 -up_aruco_sigma_px: 1 -up_aruco_chi2_multipler: 8 +up_aruco_sigma_px: 2.0 +up_aruco_chi2_multipler: 10 # masks for our images use_mask: false diff --git a/config/rpng_ironsides/estimator_config.yaml b/config/rpng_ironsides/estimator_config.yaml index dc04c6177..a0c4ee8d3 100644 --- a/config/rpng_ironsides/estimator_config.yaml +++ b/config/rpng_ironsides/estimator_config.yaml @@ -9,11 +9,11 @@ use_rk4int: true # if rk4 integration should be used (overrides imu averaging) use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints max_cameras: 2 -calib_cam_extrinsics: false +calib_cam_extrinsics: false # degenerate motion calib_cam_intrinsics: true -calib_cam_timeoffset: true +calib_cam_timeoffset: false # degenerate motion -max_clones: 12 +max_clones: 11 max_slam: 50 max_slam_in_update: 25 max_msckf_in_update: 50 @@ -37,12 +37,12 @@ zupt_only_at_beginning: false # ================================================================== # ================================================================== -#init_window_time: 1.0 init_window_time: 2.0 init_imu_thresh: 0.5 init_max_disparity: 1.5 -init_max_features: 25 +init_max_features: 75 +init_dyn_use: false init_dyn_mle_opt_calib: false init_dyn_mle_max_iter: 50 init_dyn_mle_max_time: 0.05 @@ -76,21 +76,23 @@ filepath_gt: "/tmp/ov_groundtruth.txt" # our front-end feature tracking parameters # we have a KLT and descriptor based (KLT is better implemented...) use_klt: true -num_pts: 200 -fast_threshold: 20 -grid_x: 20 -grid_y: 20 -min_px_dist: 20 +num_pts: 100 +fast_threshold: 30 +grid_x: 5 +grid_y: 5 +min_px_dist: 15 knn_ratio: 0.65 -track_frequency: 21.0 +track_frequency: 31.0 downsample_cameras: false multi_threading: true -histogram_method: "CLAHE" # NONE, HISTOGRAM, CLAHE - -fi_max_dist: 150 -fi_max_baseline: 200 -fi_max_cond_number: 20000 -fi_triangulate_1d: false +histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE + +fi_min_dist: 1.0 +fi_max_dist: 500.0 +#fi_max_baseline: 200 +#fi_max_cond_number: 20000 +#fi_max_runs: 10 +#fi_triangulate_1d: false # aruco tag tracker for the system # DICT_6X6_1000 from https://chev.me/arucogen/ diff --git a/config/rpng_sim/estimator_config.yaml b/config/rpng_sim/estimator_config.yaml index 138cf36d7..2ce974f1b 100644 --- a/config/rpng_sim/estimator_config.yaml +++ b/config/rpng_sim/estimator_config.yaml @@ -42,6 +42,7 @@ init_imu_thresh: 1.0 init_max_disparity: 1.5 init_max_features: 15 +init_dyn_use: true # if dynamic initialization should be used init_dyn_mle_opt_calib: false # if we should optimize calibration during intialization (not recommended) init_dyn_mle_max_iter: 50 # how many iterations the MLE refinement should use (zero to skip the MLE) init_dyn_mle_max_time: 0.5 # how many seconds the MLE should be completed in diff --git a/config/tum_vi/estimator_config.yaml b/config/tum_vi/estimator_config.yaml index def1cb94c..ce5097280 100644 --- a/config/tum_vi/estimator_config.yaml +++ b/config/tum_vi/estimator_config.yaml @@ -37,17 +37,18 @@ zupt_only_at_beginning: true # ================================================================== # ================================================================== -init_window_time: 2.0 +init_window_time: 1.5 # make 2sec if using dynamic... init_imu_thresh: 0.45 # room1-5:0.45, room6:0.25 -init_max_disparity: 5.0 -init_max_features: 25 +init_max_disparity: 15.0 +init_max_features: 75 +init_dyn_use: false init_dyn_mle_opt_calib: false init_dyn_mle_max_iter: 50 init_dyn_mle_max_time: 0.05 init_dyn_mle_max_threads: 6 init_dyn_num_pose: 6 -init_dyn_min_deg: 20.0 +init_dyn_min_deg: 10.0 init_dyn_inflation_ori: 10 init_dyn_inflation_vel: 100 @@ -75,10 +76,10 @@ filepath_gt: "/tmp/ov_groundtruth.txt" # our front-end feature tracking parameters # we have a KLT and descriptor based (KLT is better implemented...) use_klt: true -num_pts: 150 +num_pts: 200 fast_threshold: 20 -grid_x: 20 -grid_y: 20 +grid_x: 5 +grid_y: 5 min_px_dist: 15 knn_ratio: 0.65 track_frequency: 21.0 diff --git a/config/uzhfpv_indoor/estimator_config.yaml b/config/uzhfpv_indoor/estimator_config.yaml index e423ffb07..9f9d24d1b 100644 --- a/config/uzhfpv_indoor/estimator_config.yaml +++ b/config/uzhfpv_indoor/estimator_config.yaml @@ -16,10 +16,10 @@ calib_cam_timeoffset: true max_clones: 11 max_slam: 50 max_slam_in_update: 25 -max_msckf_in_update: 30 +max_msckf_in_update: 40 dt_slam_delay: 2 -gravity_mag: 9.81 +gravity_mag: 9.8065 # kalibr calibration feat_rep_msckf: "GLOBAL_3D" feat_rep_slam: "ANCHORED_MSCKF_INVERSE_DEPTH" @@ -27,7 +27,7 @@ feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH" # zero velocity update parameters we can use # we support either IMU-based or disparity detection. -try_zupt: true +try_zupt: false zupt_chi2_multipler: 0 # set to 0 for only disp-based zupt_max_velocity: 0.5 zupt_noise_multiplier: 20 @@ -38,10 +38,11 @@ zupt_only_at_beginning: false # ================================================================== init_window_time: 2.0 -init_imu_thresh: 0.0 -init_max_disparity: 1.5 -init_max_features: 25 +init_imu_thresh: 0.30 +init_max_disparity: 2.0 +init_max_features: 75 +init_dyn_use: false init_dyn_mle_opt_calib: false init_dyn_mle_max_iter: 50 init_dyn_mle_max_time: 0.05 @@ -75,10 +76,10 @@ filepath_gt: "/tmp/ov_groundtruth.txt" # our front-end feature tracking parameters # we have a KLT and descriptor based (KLT is better implemented...) use_klt: true -num_pts: 100 -fast_threshold: 65 -grid_x: 20 -grid_y: 20 +num_pts: 200 +fast_threshold: 50 +grid_x: 5 +grid_y: 5 min_px_dist: 15 knn_ratio: 0.70 track_frequency: 31.0 @@ -96,9 +97,9 @@ downsize_aruco: true # ================================================================== # camera noises and chi-squared threshold multipliers -up_msckf_sigma_px: 2 +up_msckf_sigma_px: 1.5 up_msckf_chi2_multipler: 1 -up_slam_sigma_px: 2 +up_slam_sigma_px: 1.5 up_slam_chi2_multipler: 1 up_aruco_sigma_px: 1 up_aruco_chi2_multipler: 1 diff --git a/config/uzhfpv_indoor_45/estimator_config.yaml b/config/uzhfpv_indoor_45/estimator_config.yaml index e423ffb07..9f9d24d1b 100644 --- a/config/uzhfpv_indoor_45/estimator_config.yaml +++ b/config/uzhfpv_indoor_45/estimator_config.yaml @@ -16,10 +16,10 @@ calib_cam_timeoffset: true max_clones: 11 max_slam: 50 max_slam_in_update: 25 -max_msckf_in_update: 30 +max_msckf_in_update: 40 dt_slam_delay: 2 -gravity_mag: 9.81 +gravity_mag: 9.8065 # kalibr calibration feat_rep_msckf: "GLOBAL_3D" feat_rep_slam: "ANCHORED_MSCKF_INVERSE_DEPTH" @@ -27,7 +27,7 @@ feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH" # zero velocity update parameters we can use # we support either IMU-based or disparity detection. -try_zupt: true +try_zupt: false zupt_chi2_multipler: 0 # set to 0 for only disp-based zupt_max_velocity: 0.5 zupt_noise_multiplier: 20 @@ -38,10 +38,11 @@ zupt_only_at_beginning: false # ================================================================== init_window_time: 2.0 -init_imu_thresh: 0.0 -init_max_disparity: 1.5 -init_max_features: 25 +init_imu_thresh: 0.30 +init_max_disparity: 2.0 +init_max_features: 75 +init_dyn_use: false init_dyn_mle_opt_calib: false init_dyn_mle_max_iter: 50 init_dyn_mle_max_time: 0.05 @@ -75,10 +76,10 @@ filepath_gt: "/tmp/ov_groundtruth.txt" # our front-end feature tracking parameters # we have a KLT and descriptor based (KLT is better implemented...) use_klt: true -num_pts: 100 -fast_threshold: 65 -grid_x: 20 -grid_y: 20 +num_pts: 200 +fast_threshold: 50 +grid_x: 5 +grid_y: 5 min_px_dist: 15 knn_ratio: 0.70 track_frequency: 31.0 @@ -96,9 +97,9 @@ downsize_aruco: true # ================================================================== # camera noises and chi-squared threshold multipliers -up_msckf_sigma_px: 2 +up_msckf_sigma_px: 1.5 up_msckf_chi2_multipler: 1 -up_slam_sigma_px: 2 +up_slam_sigma_px: 1.5 up_slam_chi2_multipler: 1 up_aruco_sigma_px: 1 up_aruco_chi2_multipler: 1 diff --git a/config/uzhfpv_outdoor/estimator_config.yaml b/config/uzhfpv_outdoor/estimator_config.yaml index fe985638a..e34ceaf8a 100644 --- a/config/uzhfpv_outdoor/estimator_config.yaml +++ b/config/uzhfpv_outdoor/estimator_config.yaml @@ -16,10 +16,10 @@ calib_cam_timeoffset: true max_clones: 11 max_slam: 50 max_slam_in_update: 25 -max_msckf_in_update: 30 +max_msckf_in_update: 40 dt_slam_delay: 2 -gravity_mag: 9.81 +gravity_mag: 9.8065 # kalibr calibration feat_rep_msckf: "GLOBAL_3D" feat_rep_slam: "ANCHORED_MSCKF_INVERSE_DEPTH" @@ -27,7 +27,7 @@ feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH" # zero velocity update parameters we can use # we support either IMU-based or disparity detection. -try_zupt: true +try_zupt: false zupt_chi2_multipler: 0 # set to 0 for only disp-based zupt_max_velocity: 0.5 zupt_noise_multiplier: 20 @@ -38,10 +38,11 @@ zupt_only_at_beginning: false # ================================================================== init_window_time: 2.0 -init_imu_thresh: 0.0 -init_max_disparity: 1.5 -init_max_features: 25 +init_imu_thresh: 0.30 +init_max_disparity: 2.0 +init_max_features: 75 +init_dyn_use: false init_dyn_mle_opt_calib: false init_dyn_mle_max_iter: 50 init_dyn_mle_max_time: 0.05 @@ -75,10 +76,10 @@ filepath_gt: "/tmp/ov_groundtruth.txt" # our front-end feature tracking parameters # we have a KLT and descriptor based (KLT is better implemented...) use_klt: true -num_pts: 100 -fast_threshold: 65 -grid_x: 20 -grid_y: 20 +num_pts: 200 +fast_threshold: 25 +grid_x: 5 +grid_y: 5 min_px_dist: 15 knn_ratio: 0.70 track_frequency: 31.0 @@ -96,9 +97,9 @@ downsize_aruco: true # ================================================================== # camera noises and chi-squared threshold multipliers -up_msckf_sigma_px: 2 +up_msckf_sigma_px: 1.5 up_msckf_chi2_multipler: 1 -up_slam_sigma_px: 2 +up_slam_sigma_px: 1.5 up_slam_chi2_multipler: 1 up_aruco_sigma_px: 1 up_aruco_chi2_multipler: 1 diff --git a/config/uzhfpv_outdoor_45/estimator_config.yaml b/config/uzhfpv_outdoor_45/estimator_config.yaml index e423ffb07..9f9d24d1b 100644 --- a/config/uzhfpv_outdoor_45/estimator_config.yaml +++ b/config/uzhfpv_outdoor_45/estimator_config.yaml @@ -16,10 +16,10 @@ calib_cam_timeoffset: true max_clones: 11 max_slam: 50 max_slam_in_update: 25 -max_msckf_in_update: 30 +max_msckf_in_update: 40 dt_slam_delay: 2 -gravity_mag: 9.81 +gravity_mag: 9.8065 # kalibr calibration feat_rep_msckf: "GLOBAL_3D" feat_rep_slam: "ANCHORED_MSCKF_INVERSE_DEPTH" @@ -27,7 +27,7 @@ feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH" # zero velocity update parameters we can use # we support either IMU-based or disparity detection. -try_zupt: true +try_zupt: false zupt_chi2_multipler: 0 # set to 0 for only disp-based zupt_max_velocity: 0.5 zupt_noise_multiplier: 20 @@ -38,10 +38,11 @@ zupt_only_at_beginning: false # ================================================================== init_window_time: 2.0 -init_imu_thresh: 0.0 -init_max_disparity: 1.5 -init_max_features: 25 +init_imu_thresh: 0.30 +init_max_disparity: 2.0 +init_max_features: 75 +init_dyn_use: false init_dyn_mle_opt_calib: false init_dyn_mle_max_iter: 50 init_dyn_mle_max_time: 0.05 @@ -75,10 +76,10 @@ filepath_gt: "/tmp/ov_groundtruth.txt" # our front-end feature tracking parameters # we have a KLT and descriptor based (KLT is better implemented...) use_klt: true -num_pts: 100 -fast_threshold: 65 -grid_x: 20 -grid_y: 20 +num_pts: 200 +fast_threshold: 50 +grid_x: 5 +grid_y: 5 min_px_dist: 15 knn_ratio: 0.70 track_frequency: 31.0 @@ -96,9 +97,9 @@ downsize_aruco: true # ================================================================== # camera noises and chi-squared threshold multipliers -up_msckf_sigma_px: 2 +up_msckf_sigma_px: 1.5 up_msckf_chi2_multipler: 1 -up_slam_sigma_px: 2 +up_slam_sigma_px: 1.5 up_slam_chi2_multipler: 1 up_aruco_sigma_px: 1 up_aruco_chi2_multipler: 1 diff --git a/docs/gs-datasets.dox b/docs/gs-datasets.dox index 3284ec7de..05bc0634d 100644 --- a/docs/gs-datasets.dox +++ b/docs/gs-datasets.dox @@ -204,6 +204,7 @@ Typically we process the datasets at 1.5x rate so we get a ~20 Hz image feed and | Dataset Name | Length (km) | Dataset Link | Groundtruth Traj. | Example Launch | |-------------:|--------|--------------|------------------|------------------| | Urban 28 | 11.47 | [download](https://sites.google.com/view/complex-urban-dataset/download-lidar-stereo?authuser=0) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist) | [config](https://github.com/rpng/open_vins/blob/master/config/kaist) | +| Urban 32 | 7.30 | [download](https://sites.google.com/view/complex-urban-dataset/download-lidar-stereo?authuser=0) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist) | [config](https://github.com/rpng/open_vins/blob/master/config/kaist) | | Urban 38 | 11.42 | [download](https://sites.google.com/view/complex-urban-dataset/download-lidar-stereo?authuser=0) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist) | [config](https://github.com/rpng/open_vins/blob/master/config/kaist) | | Urban 39 | 11.06 | [download](https://sites.google.com/view/complex-urban-dataset/download-lidar-stereo?authuser=0) | [link](https://github.com/rpng/open_vins/tree/master/ov_data/kaist) | [config](https://github.com/rpng/open_vins/blob/master/config/kaist) | @m_enddiv @@ -211,7 +212,7 @@ Typically we process the datasets at 1.5x rate so we get a ~20 Hz image feed and @section gs-data-kaist-vio KAIST VIO Dataset -The [KAIST VIO dataset](https://github.com/zinuok/kaistviodataset) @cite Jeon2021RAL is a dataset of a MAV in an indoor 3.15 x 3.60 x 2.50 meter environment which undergoes various trajectory motions. +The [KAIST VIO dataset](https://github.com/url-kaist/kaistviodataset) @cite Jeon2021RAL is a dataset of a MAV in an indoor 3.15 x 3.60 x 2.50 meter environment which undergoes various trajectory motions. The camera is intel realsense D435i 25 Hz, while the IMU is 100 Hz sensing rate from the pixelhawk 4 unit. A groundtruth "baseline" trajectory is also provided from a OptiTrack Mocap system at 50 Hz, the bag files have the marker body frame to IMU frame already applied. This topic has been provided in ov_data for convinces sake. diff --git a/docs/gs-installing.dox b/docs/gs-installing.dox index e6bcd1031..bc7bfe427 100644 --- a/docs/gs-installing.dox +++ b/docs/gs-installing.dox @@ -145,6 +145,16 @@ cmake .. make -j4 @endcode +If you wish to debug and run with assert statements, you can configure your workspace as follows: + + +@code{.shell-session} +catkin config --cmake-args -DCMAKE_BUILD_TYPE=Debug +catkin build --cmake-args -DCMAKE_BUILD_TYPE=Debug +colcon build --cmake-args -DCMAKE_BUILD_TYPE=Debug +@endcode + + @section gs-install-oveval Additional Evaluation Requirements @@ -192,7 +202,7 @@ You can disable this with `catkin build -DENABLE_ARUCO_TAGS=OFF` or `cmake -DENA -@subsection gs-install-ceres Ceres Solver +@section gs-install-ceres Ceres Solver Ceres solver @cite ceres-solver is required for dynamic initialization and backend optimization. Please refer to their [documentation](http://ceres-solver.org/installation.html#linux) for specifics to your platform. @@ -205,6 +215,8 @@ To install we can perform the following: @par Ceres Source Installation Try to first build with your system with `sudo apt-get install libceres-dev`. Only fall back onto this if it does not allow you to compile, or want a newer version! + You will need to build from source if there is an Eigen miss-match: + "Failed to find Ceres - Found Eigen dependency, but the version of Eigen found (3.3.7) does not exactly match the version of Eigen Ceres was compiled with (3.3.4)." @code{.shell-session} sudo apt-get install -y cmake libgoogle-glog-dev libgflags-dev libatlas-base-dev libeigen3-dev libsuitesparse-dev diff --git a/ov_core/CMakeLists.txt b/ov_core/CMakeLists.txt index a4a1bc23d..2308d6d01 100644 --- a/ov_core/CMakeLists.txt +++ b/ov_core/CMakeLists.txt @@ -40,6 +40,11 @@ set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) +# Default build as release +if (NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE "Release" CACHE STRING "Choose the type of build, options are: Debug Release RelWithDebInfo MinSizeRel." FORCE) +endif () + # Enable compile optimizations set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops") diff --git a/ov_core/cmake/ROS1.cmake b/ov_core/cmake/ROS1.cmake index ab2d319e4..181dc3e7d 100644 --- a/ov_core/cmake/ROS1.cmake +++ b/ov_core/cmake/ROS1.cmake @@ -34,7 +34,7 @@ list(APPEND thirdparty_libraries ${Boost_LIBRARIES} ${OpenCV_LIBRARIES} ${catkin_LIBRARIES} -) + ) ################################################## # Make the core library @@ -42,6 +42,8 @@ list(APPEND thirdparty_libraries list(APPEND LIBRARY_SOURCES src/dummy.cpp + src/cpi/CpiV1.cpp + src/cpi/CpiV2.cpp src/sim/BsplineSE3.cpp src/track/TrackBase.cpp src/track/TrackAruco.cpp @@ -50,9 +52,10 @@ list(APPEND LIBRARY_SOURCES src/track/TrackSIM.cpp src/types/Landmark.cpp src/feat/Feature.cpp + src/feat/FeatureDatabase.cpp src/feat/FeatureInitializer.cpp src/utils/print.cpp -) + ) file(GLOB_RECURSE LIBRARY_HEADERS "src/*.h") add_library(ov_core_lib SHARED ${LIBRARY_SOURCES} ${LIBRARY_HEADERS}) target_link_libraries(ov_core_lib ${thirdparty_libraries}) @@ -61,11 +64,11 @@ install(TARGETS ov_core_lib ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) + ) install(DIRECTORY src/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" -) + ) ################################################## # Make binary files! @@ -79,7 +82,7 @@ if (catkin_FOUND AND ENABLE_ROS) ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ) + ) endif () @@ -89,7 +92,7 @@ install(TARGETS test_webcam ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) + ) diff --git a/ov_core/cmake/ROS2.cmake b/ov_core/cmake/ROS2.cmake index 3c8dc116b..43d7f82bd 100644 --- a/ov_core/cmake/ROS2.cmake +++ b/ov_core/cmake/ROS2.cmake @@ -23,7 +23,7 @@ include_directories( list(APPEND thirdparty_libraries ${Boost_LIBRARIES} ${OpenCV_LIBRARIES} -) + ) ################################################## # Make the core library @@ -31,6 +31,8 @@ list(APPEND thirdparty_libraries list(APPEND LIBRARY_SOURCES src/dummy.cpp + src/cpi/CpiV1.cpp + src/cpi/CpiV2.cpp src/sim/BsplineSE3.cpp src/track/TrackBase.cpp src/track/TrackAruco.cpp @@ -39,9 +41,10 @@ list(APPEND LIBRARY_SOURCES src/track/TrackSIM.cpp src/types/Landmark.cpp src/feat/Feature.cpp + src/feat/FeatureDatabase.cpp src/feat/FeatureInitializer.cpp src/utils/print.cpp -) + ) file(GLOB_RECURSE LIBRARY_HEADERS "src/*.h") add_library(ov_core_lib SHARED ${LIBRARY_SOURCES} ${LIBRARY_HEADERS}) ament_target_dependencies(ov_core_lib rclcpp cv_bridge) @@ -51,11 +54,11 @@ install(TARGETS ov_core_lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin PUBLIC_HEADER DESTINATION include -) + ) install(DIRECTORY src/ DESTINATION include FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" -) + ) ament_export_include_directories(include) ament_export_libraries(ov_core_lib) diff --git a/ov_core/src/cam/CamBase.h b/ov_core/src/cam/CamBase.h index 6bdcd3244..c8de21798 100644 --- a/ov_core/src/cam/CamBase.h +++ b/ov_core/src/cam/CamBase.h @@ -49,6 +49,8 @@ class CamBase { */ CamBase(int width, int height) : _width(width), _height(height) {} + virtual ~CamBase() {} + /** * @brief This will set and update the camera calibration values. * This should be called on startup for each camera and after update! diff --git a/ov_core/src/cam/CamEqui.h b/ov_core/src/cam/CamEqui.h index 1e4eee1af..0709d0f73 100644 --- a/ov_core/src/cam/CamEqui.h +++ b/ov_core/src/cam/CamEqui.h @@ -98,6 +98,8 @@ class CamEqui : public CamBase { */ CamEqui(int width, int height) : CamBase(width, height) {} + ~CamEqui() {} + /** * @brief Given a raw uv point, this will undistort it based on the camera matrices into normalized camera coords. * @param uv_dist Raw uv coordinate we wish to undistort diff --git a/ov_core/src/cam/CamRadtan.h b/ov_core/src/cam/CamRadtan.h index 021a90728..4f8be1564 100644 --- a/ov_core/src/cam/CamRadtan.h +++ b/ov_core/src/cam/CamRadtan.h @@ -89,6 +89,8 @@ class CamRadtan : public CamBase { */ CamRadtan(int width, int height) : CamBase(width, height) {} + ~CamRadtan() {} + /** * @brief Given a raw uv point, this will undistort it based on the camera matrices into normalized camera coords. * @param uv_dist Raw uv coordinate we wish to undistort diff --git a/ov_core/src/cpi/CpiBase.h b/ov_core/src/cpi/CpiBase.h index a18ffab5a..a05be84f5 100644 --- a/ov_core/src/cpi/CpiBase.h +++ b/ov_core/src/cpi/CpiBase.h @@ -73,6 +73,8 @@ class CpiBase { e_3x = skew_x(e_3); } + virtual ~CpiBase() {} + /** * @brief Set linearization points of the integration. * @param[in] b_w_lin_ gyroscope bias linearization point diff --git a/ov_core/src/cpi/CpiV1.cpp b/ov_core/src/cpi/CpiV1.cpp new file mode 100644 index 000000000..7b9224d0b --- /dev/null +++ b/ov_core/src/cpi/CpiV1.cpp @@ -0,0 +1,315 @@ + +/* + * MIT License + * Copyright (c) 2018 Kevin Eckenhoff + * Copyright (c) 2018 Patrick Geneva + * Copyright (c) 2018 Guoquan Huang + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include "CpiV1.h" + +#include "utils/quat_ops.h" + +using namespace ov_core; + +void CpiV1::feed_IMU(double t_0, double t_1, Eigen::Matrix w_m_0, Eigen::Matrix a_m_0, + Eigen::Matrix w_m_1, Eigen::Matrix a_m_1) { + + // Get time difference + double delta_t = t_1 - t_0; + DT += delta_t; + + // If no time has passed do nothing + if (delta_t == 0) { + return; + } + + // Get estimated imu readings + Eigen::Matrix w_hat = w_m_0 - b_w_lin; + Eigen::Matrix a_hat = a_m_0 - b_a_lin; + + // If averaging, average + if (imu_avg) { + w_hat += w_m_1 - b_w_lin; + w_hat = 0.5 * w_hat; + a_hat += a_m_1 - b_a_lin; + a_hat = .5 * a_hat; + } + + // Get angle change w*dt + Eigen::Matrix w_hatdt = w_hat * delta_t; + + // Get entries of w_hat + double w_1 = w_hat(0, 0); + double w_2 = w_hat(1, 0); + double w_3 = w_hat(2, 0); + + // Get magnitude of w and wdt + double mag_w = w_hat.norm(); + double w_dt = mag_w * delta_t; + + // Threshold to determine if equations will be unstable + bool small_w = (mag_w < 0.008726646); + + // Get some of the variables used in the preintegration equations + double dt_2 = pow(delta_t, 2); + double cos_wt = cos(w_dt); + double sin_wt = sin(w_dt); + + Eigen::Matrix w_x = skew_x(w_hat); + Eigen::Matrix a_x = skew_x(a_hat); + Eigen::Matrix w_tx = skew_x(w_hatdt); + Eigen::Matrix w_x_2 = w_x * w_x; + + //========================================================================== + // MEASUREMENT MEANS + //========================================================================== + + // Get relative rotation + Eigen::Matrix R_tau2tau1 = small_w ? eye3 - delta_t * w_x + (pow(delta_t, 2) / 2) * w_x_2 + : eye3 - (sin_wt / mag_w) * w_x + ((1.0 - cos_wt) / (pow(mag_w, 2.0))) * w_x_2; + + // Updated rotation and its transpose + Eigen::Matrix R_k2tau1 = R_tau2tau1 * R_k2tau; + Eigen::Matrix R_tau12k = R_k2tau1.transpose(); + + // Intermediate variables for evaluating the measurement/bias Jacobian update + double f_1; + double f_2; + double f_3; + double f_4; + + if (small_w) { + f_1 = -(pow(delta_t, 3) / 3); + f_2 = (pow(delta_t, 4) / 8); + f_3 = -(pow(delta_t, 2) / 2); + f_4 = (pow(delta_t, 3) / 6); + } else { + f_1 = (w_dt * cos_wt - sin_wt) / (pow(mag_w, 3)); + f_2 = (pow(w_dt, 2) - 2 * cos_wt - 2 * w_dt * sin_wt + 2) / (2 * pow(mag_w, 4)); + f_3 = -(1 - cos_wt) / pow(mag_w, 2); + f_4 = (w_dt - sin_wt) / pow(mag_w, 3); + } + + // Compute the main part of our analytical means + Eigen::Matrix alpha_arg = ((dt_2 / 2.0) * eye3 + f_1 * w_x + f_2 * w_x_2); + Eigen::Matrix Beta_arg = (delta_t * eye3 + f_3 * w_x + f_4 * w_x_2); + + // Matrices that will multiply the a_hat in the update expressions + Eigen::MatrixXd H_al = R_tau12k * alpha_arg; + Eigen::MatrixXd H_be = R_tau12k * Beta_arg; + + // Update the measurement means + alpha_tau += beta_tau * delta_t + H_al * a_hat; + beta_tau += H_be * a_hat; + + //========================================================================== + // BIAS JACOBIANS (ANALYTICAL) + //========================================================================== + + // Get right Jacobian + Eigen::Matrix J_r_tau1 = + small_w ? eye3 - .5 * w_tx + (1.0 / 6.0) * w_tx * w_tx + : eye3 - ((1 - cos_wt) / (pow((w_dt), 2.0))) * w_tx + ((w_dt - sin_wt) / (pow(w_dt, 3.0))) * w_tx * w_tx; + + // Update orientation in respect to gyro bias Jacobians + J_q = R_tau2tau1 * J_q + J_r_tau1 * delta_t; + + // Update alpha and beta in respect to accel bias Jacobians + H_a -= H_al; + H_a += delta_t * H_b; + H_b -= H_be; + + // Derivatives of R_tau12k wrt bias_w entries + Eigen::MatrixXd d_R_bw_1 = -R_tau12k * skew_x(J_q * e_1); + Eigen::MatrixXd d_R_bw_2 = -R_tau12k * skew_x(J_q * e_2); + Eigen::MatrixXd d_R_bw_3 = -R_tau12k * skew_x(J_q * e_3); + + // Now compute the gyro bias Jacobian terms + double df_1_dbw_1; + double df_1_dbw_2; + double df_1_dbw_3; + + double df_2_dbw_1; + double df_2_dbw_2; + double df_2_dbw_3; + + double df_3_dbw_1; + double df_3_dbw_2; + double df_3_dbw_3; + + double df_4_dbw_1; + double df_4_dbw_2; + double df_4_dbw_3; + + if (small_w) { + double df_1_dw_mag = -(pow(delta_t, 5) / 15); + df_1_dbw_1 = w_1 * df_1_dw_mag; + df_1_dbw_2 = w_2 * df_1_dw_mag; + df_1_dbw_3 = w_3 * df_1_dw_mag; + + double df_2_dw_mag = (pow(delta_t, 6) / 72); + df_2_dbw_1 = w_1 * df_2_dw_mag; + df_2_dbw_2 = w_2 * df_2_dw_mag; + df_2_dbw_3 = w_3 * df_2_dw_mag; + + double df_3_dw_mag = -(pow(delta_t, 4) / 12); + df_3_dbw_1 = w_1 * df_3_dw_mag; + df_3_dbw_2 = w_2 * df_3_dw_mag; + df_3_dbw_3 = w_3 * df_3_dw_mag; + + double df_4_dw_mag = (pow(delta_t, 5) / 60); + df_4_dbw_1 = w_1 * df_4_dw_mag; + df_4_dbw_2 = w_2 * df_4_dw_mag; + df_4_dbw_3 = w_3 * df_4_dw_mag; + } else { + double df_1_dw_mag = (pow(w_dt, 2) * sin_wt - 3 * sin_wt + 3 * w_dt * cos_wt) / pow(mag_w, 5); + df_1_dbw_1 = w_1 * df_1_dw_mag; + df_1_dbw_2 = w_2 * df_1_dw_mag; + df_1_dbw_3 = w_3 * df_1_dw_mag; + + double df_2_dw_mag = (pow(w_dt, 2) - 4 * cos_wt - 4 * w_dt * sin_wt + pow(w_dt, 2) * cos_wt + 4) / (pow(mag_w, 6)); + df_2_dbw_1 = w_1 * df_2_dw_mag; + df_2_dbw_2 = w_2 * df_2_dw_mag; + df_2_dbw_3 = w_3 * df_2_dw_mag; + + double df_3_dw_mag = (2 * (cos_wt - 1) + w_dt * sin_wt) / (pow(mag_w, 4)); + df_3_dbw_1 = w_1 * df_3_dw_mag; + df_3_dbw_2 = w_2 * df_3_dw_mag; + df_3_dbw_3 = w_3 * df_3_dw_mag; + + double df_4_dw_mag = (2 * w_dt + w_dt * cos_wt - 3 * sin_wt) / (pow(mag_w, 5)); + df_4_dbw_1 = w_1 * df_4_dw_mag; + df_4_dbw_2 = w_2 * df_4_dw_mag; + df_4_dbw_3 = w_3 * df_4_dw_mag; + } + + // Update alpha and beta gyro bias Jacobians + J_a += J_b * delta_t; + J_a.block(0, 0, 3, 1) += + (d_R_bw_1 * alpha_arg + R_tau12k * (df_1_dbw_1 * w_x - f_1 * e_1x + df_2_dbw_1 * w_x_2 - f_2 * (e_1x * w_x + w_x * e_1x))) * a_hat; + J_a.block(0, 1, 3, 1) += + (d_R_bw_2 * alpha_arg + R_tau12k * (df_1_dbw_2 * w_x - f_1 * e_2x + df_2_dbw_2 * w_x_2 - f_2 * (e_2x * w_x + w_x * e_2x))) * a_hat; + J_a.block(0, 2, 3, 1) += + (d_R_bw_3 * alpha_arg + R_tau12k * (df_1_dbw_3 * w_x - f_1 * e_3x + df_2_dbw_3 * w_x_2 - f_2 * (e_3x * w_x + w_x * e_3x))) * a_hat; + J_b.block(0, 0, 3, 1) += + (d_R_bw_1 * Beta_arg + R_tau12k * (df_3_dbw_1 * w_x - f_3 * e_1x + df_4_dbw_1 * w_x_2 - f_4 * (e_1x * w_x + w_x * e_1x))) * a_hat; + J_b.block(0, 1, 3, 1) += + (d_R_bw_2 * Beta_arg + R_tau12k * (df_3_dbw_2 * w_x - f_3 * e_2x + df_4_dbw_2 * w_x_2 - f_4 * (e_2x * w_x + w_x * e_2x))) * a_hat; + J_b.block(0, 2, 3, 1) += + (d_R_bw_3 * Beta_arg + R_tau12k * (df_3_dbw_3 * w_x - f_3 * e_3x + df_4_dbw_3 * w_x_2 - f_4 * (e_3x * w_x + w_x * e_3x))) * a_hat; + + //========================================================================== + // MEASUREMENT COVARIANCE + //========================================================================== + + // Going to need orientation at intermediate time i.e. at .5*dt; + Eigen::Matrix R_mid = + small_w ? eye3 - .5 * delta_t * w_x + (pow(.5 * delta_t, 2) / 2) * w_x_2 + : eye3 - (sin(mag_w * .5 * delta_t) / mag_w) * w_x + ((1.0 - cos(mag_w * .5 * delta_t)) / (pow(mag_w, 2.0))) * w_x_2; + R_mid = R_mid * R_k2tau; + + // Compute covariance (in this implementation, we use RK4) + // k1------------------------------------------------------------------------------------------------- + + // Build state Jacobian + Eigen::Matrix F_k1 = Eigen::Matrix::Zero(); + F_k1.block(0, 0, 3, 3) = -w_x; + F_k1.block(0, 3, 3, 3) = -eye3; + F_k1.block(6, 0, 3, 3) = -R_k2tau.transpose() * a_x; + F_k1.block(6, 9, 3, 3) = -R_k2tau.transpose(); + F_k1.block(12, 6, 3, 3) = eye3; + + // Build noise Jacobian + Eigen::Matrix G_k1 = Eigen::Matrix::Zero(); + G_k1.block(0, 0, 3, 3) = -eye3; + G_k1.block(3, 3, 3, 3) = eye3; + G_k1.block(6, 6, 3, 3) = -R_k2tau.transpose(); + G_k1.block(9, 9, 3, 3) = eye3; + + // Get covariance derivative + Eigen::Matrix P_dot_k1 = F_k1 * P_meas + P_meas * F_k1.transpose() + G_k1 * Q_c * G_k1.transpose(); + + // k2------------------------------------------------------------------------------------------------- + + // Build state Jacobian + Eigen::Matrix F_k2 = Eigen::Matrix::Zero(); + F_k2.block(0, 0, 3, 3) = -w_x; + F_k2.block(0, 3, 3, 3) = -eye3; + F_k2.block(6, 0, 3, 3) = -R_mid.transpose() * a_x; + F_k2.block(6, 9, 3, 3) = -R_mid.transpose(); + F_k2.block(12, 6, 3, 3) = eye3; + + // Build noise Jacobian + Eigen::Matrix G_k2 = Eigen::Matrix::Zero(); + G_k2.block(0, 0, 3, 3) = -eye3; + G_k2.block(3, 3, 3, 3) = eye3; + G_k2.block(6, 6, 3, 3) = -R_mid.transpose(); + G_k2.block(9, 9, 3, 3) = eye3; + + // Get covariance derivative + Eigen::Matrix P_k2 = P_meas + P_dot_k1 * delta_t / 2.0; + Eigen::Matrix P_dot_k2 = F_k2 * P_k2 + P_k2 * F_k2.transpose() + G_k2 * Q_c * G_k2.transpose(); + + // k3------------------------------------------------------------------------------------------------- + + // Our state and noise Jacobians are the same as k2 + // Since k2 and k3 correspond to the same estimates for the midpoint + Eigen::Matrix F_k3 = F_k2; + Eigen::Matrix G_k3 = G_k2; + + // Get covariance derivative + Eigen::Matrix P_k3 = P_meas + P_dot_k2 * delta_t / 2.0; + Eigen::Matrix P_dot_k3 = F_k3 * P_k3 + P_k3 * F_k3.transpose() + G_k3 * Q_c * G_k3.transpose(); + + // k4------------------------------------------------------------------------------------------------- + + // Build state Jacobian + Eigen::Matrix F_k4 = Eigen::Matrix::Zero(); + F_k4.block(0, 0, 3, 3) = -w_x; + F_k4.block(0, 3, 3, 3) = -eye3; + F_k4.block(6, 0, 3, 3) = -R_k2tau1.transpose() * a_x; + F_k4.block(6, 9, 3, 3) = -R_k2tau1.transpose(); + F_k4.block(12, 6, 3, 3) = eye3; + + // Build noise Jacobian + Eigen::Matrix G_k4 = Eigen::Matrix::Zero(); + G_k4.block(0, 0, 3, 3) = -eye3; + G_k4.block(3, 3, 3, 3) = eye3; + G_k4.block(6, 6, 3, 3) = -R_k2tau1.transpose(); + G_k4.block(9, 9, 3, 3) = eye3; + + // Get covariance derivative + Eigen::Matrix P_k4 = P_meas + P_dot_k3 * delta_t; + Eigen::Matrix P_dot_k4 = F_k4 * P_k4 + P_k4 * F_k4.transpose() + G_k4 * Q_c * G_k4.transpose(); + + // done------------------------------------------------------------------------------------------------- + + // Collect covariance solution + // Ensure it is positive definite + P_meas += (delta_t / 6.0) * (P_dot_k1 + 2.0 * P_dot_k2 + 2.0 * P_dot_k3 + P_dot_k4); + P_meas = 0.5 * (P_meas + P_meas.transpose()); + + // Update rotation mean + // Note we had to wait to do this, since we use the old orientation in our covariance calculation + R_k2tau = R_k2tau1; + q_k2tau = rot_2_quat(R_k2tau); +} diff --git a/ov_core/src/cpi/CpiV1.h b/ov_core/src/cpi/CpiV1.h index fdf64b311..bb5dbf0bf 100644 --- a/ov_core/src/cpi/CpiV1.h +++ b/ov_core/src/cpi/CpiV1.h @@ -27,7 +27,7 @@ */ #include "CpiBase.h" -#include "utils/quat_ops.h" + #include namespace ov_core { @@ -63,6 +63,8 @@ class CpiV1 : public CpiBase { CpiV1(double sigma_w, double sigma_wb, double sigma_a, double sigma_ab, bool imu_avg_ = false) : CpiBase(sigma_w, sigma_wb, sigma_a, sigma_ab, imu_avg_) {} + virtual ~CpiV1() {} + /** * @brief Our precompound function for Model 1 * @param[in] t_0 first IMU timestamp @@ -77,288 +79,7 @@ class CpiV1 : public CpiBase { */ void feed_IMU(double t_0, double t_1, Eigen::Matrix w_m_0, Eigen::Matrix a_m_0, Eigen::Matrix w_m_1 = Eigen::Matrix::Zero(), - Eigen::Matrix a_m_1 = Eigen::Matrix::Zero()) { - - // Get time difference - double delta_t = t_1 - t_0; - DT += delta_t; - - // If no time has passed do nothing - if (delta_t == 0) { - return; - } - - // Get estimated imu readings - Eigen::Matrix w_hat = w_m_0 - b_w_lin; - Eigen::Matrix a_hat = a_m_0 - b_a_lin; - - // If averaging, average - if (imu_avg) { - w_hat += w_m_1 - b_w_lin; - w_hat = 0.5 * w_hat; - a_hat += a_m_1 - b_a_lin; - a_hat = .5 * a_hat; - } - - // Get angle change w*dt - Eigen::Matrix w_hatdt = w_hat * delta_t; - - // Get entries of w_hat - double w_1 = w_hat(0, 0); - double w_2 = w_hat(1, 0); - double w_3 = w_hat(2, 0); - - // Get magnitude of w and wdt - double mag_w = w_hat.norm(); - double w_dt = mag_w * delta_t; - - // Threshold to determine if equations will be unstable - bool small_w = (mag_w < 0.008726646); - - // Get some of the variables used in the preintegration equations - double dt_2 = pow(delta_t, 2); - double cos_wt = cos(w_dt); - double sin_wt = sin(w_dt); - - Eigen::Matrix w_x = skew_x(w_hat); - Eigen::Matrix a_x = skew_x(a_hat); - Eigen::Matrix w_tx = skew_x(w_hatdt); - Eigen::Matrix w_x_2 = w_x * w_x; - - //========================================================================== - // MEASUREMENT MEANS - //========================================================================== - - // Get relative rotation - Eigen::Matrix R_tau2tau1 = small_w ? eye3 - delta_t * w_x + (pow(delta_t, 2) / 2) * w_x_2 - : eye3 - (sin_wt / mag_w) * w_x + ((1.0 - cos_wt) / (pow(mag_w, 2.0))) * w_x_2; - - // Updated rotation and its transpose - Eigen::Matrix R_k2tau1 = R_tau2tau1 * R_k2tau; - Eigen::Matrix R_tau12k = R_k2tau1.transpose(); - - // Intermediate variables for evaluating the measurement/bias Jacobian update - double f_1; - double f_2; - double f_3; - double f_4; - - if (small_w) { - f_1 = -(pow(delta_t, 3) / 3); - f_2 = (pow(delta_t, 4) / 8); - f_3 = -(pow(delta_t, 2) / 2); - f_4 = (pow(delta_t, 3) / 6); - } else { - f_1 = (w_dt * cos_wt - sin_wt) / (pow(mag_w, 3)); - f_2 = (pow(w_dt, 2) - 2 * cos_wt - 2 * w_dt * sin_wt + 2) / (2 * pow(mag_w, 4)); - f_3 = -(1 - cos_wt) / pow(mag_w, 2); - f_4 = (w_dt - sin_wt) / pow(mag_w, 3); - } - - // Compute the main part of our analytical means - Eigen::Matrix alpha_arg = ((dt_2 / 2.0) * eye3 + f_1 * w_x + f_2 * w_x_2); - Eigen::Matrix Beta_arg = (delta_t * eye3 + f_3 * w_x + f_4 * w_x_2); - - // Matrices that will multiply the a_hat in the update expressions - Eigen::MatrixXd H_al = R_tau12k * alpha_arg; - Eigen::MatrixXd H_be = R_tau12k * Beta_arg; - - // Update the measurement means - alpha_tau += beta_tau * delta_t + H_al * a_hat; - beta_tau += H_be * a_hat; - - //========================================================================== - // BIAS JACOBIANS (ANALYTICAL) - //========================================================================== - - // Get right Jacobian - Eigen::Matrix J_r_tau1 = - small_w ? eye3 - .5 * w_tx + (1.0 / 6.0) * w_tx * w_tx - : eye3 - ((1 - cos_wt) / (pow((w_dt), 2.0))) * w_tx + ((w_dt - sin_wt) / (pow(w_dt, 3.0))) * w_tx * w_tx; - - // Update orientation in respect to gyro bias Jacobians - J_q = R_tau2tau1 * J_q + J_r_tau1 * delta_t; - - // Update alpha and beta in respect to accel bias Jacobians - H_a -= H_al; - H_a += delta_t * H_b; - H_b -= H_be; - - // Derivatives of R_tau12k wrt bias_w entries - Eigen::MatrixXd d_R_bw_1 = -R_tau12k * skew_x(J_q * e_1); - Eigen::MatrixXd d_R_bw_2 = -R_tau12k * skew_x(J_q * e_2); - Eigen::MatrixXd d_R_bw_3 = -R_tau12k * skew_x(J_q * e_3); - - // Now compute the gyro bias Jacobian terms - double df_1_dbw_1; - double df_1_dbw_2; - double df_1_dbw_3; - - double df_2_dbw_1; - double df_2_dbw_2; - double df_2_dbw_3; - - double df_3_dbw_1; - double df_3_dbw_2; - double df_3_dbw_3; - - double df_4_dbw_1; - double df_4_dbw_2; - double df_4_dbw_3; - - if (small_w) { - double df_1_dw_mag = -(pow(delta_t, 5) / 15); - df_1_dbw_1 = w_1 * df_1_dw_mag; - df_1_dbw_2 = w_2 * df_1_dw_mag; - df_1_dbw_3 = w_3 * df_1_dw_mag; - - double df_2_dw_mag = (pow(delta_t, 6) / 72); - df_2_dbw_1 = w_1 * df_2_dw_mag; - df_2_dbw_2 = w_2 * df_2_dw_mag; - df_2_dbw_3 = w_3 * df_2_dw_mag; - - double df_3_dw_mag = -(pow(delta_t, 4) / 12); - df_3_dbw_1 = w_1 * df_3_dw_mag; - df_3_dbw_2 = w_2 * df_3_dw_mag; - df_3_dbw_3 = w_3 * df_3_dw_mag; - - double df_4_dw_mag = (pow(delta_t, 5) / 60); - df_4_dbw_1 = w_1 * df_4_dw_mag; - df_4_dbw_2 = w_2 * df_4_dw_mag; - df_4_dbw_3 = w_3 * df_4_dw_mag; - } else { - double df_1_dw_mag = (pow(w_dt, 2) * sin_wt - 3 * sin_wt + 3 * w_dt * cos_wt) / pow(mag_w, 5); - df_1_dbw_1 = w_1 * df_1_dw_mag; - df_1_dbw_2 = w_2 * df_1_dw_mag; - df_1_dbw_3 = w_3 * df_1_dw_mag; - - double df_2_dw_mag = (pow(w_dt, 2) - 4 * cos_wt - 4 * w_dt * sin_wt + pow(w_dt, 2) * cos_wt + 4) / (pow(mag_w, 6)); - df_2_dbw_1 = w_1 * df_2_dw_mag; - df_2_dbw_2 = w_2 * df_2_dw_mag; - df_2_dbw_3 = w_3 * df_2_dw_mag; - - double df_3_dw_mag = (2 * (cos_wt - 1) + w_dt * sin_wt) / (pow(mag_w, 4)); - df_3_dbw_1 = w_1 * df_3_dw_mag; - df_3_dbw_2 = w_2 * df_3_dw_mag; - df_3_dbw_3 = w_3 * df_3_dw_mag; - - double df_4_dw_mag = (2 * w_dt + w_dt * cos_wt - 3 * sin_wt) / (pow(mag_w, 5)); - df_4_dbw_1 = w_1 * df_4_dw_mag; - df_4_dbw_2 = w_2 * df_4_dw_mag; - df_4_dbw_3 = w_3 * df_4_dw_mag; - } - - // Update alpha and beta gyro bias Jacobians - J_a += J_b * delta_t; - J_a.block(0, 0, 3, 1) += - (d_R_bw_1 * alpha_arg + R_tau12k * (df_1_dbw_1 * w_x - f_1 * e_1x + df_2_dbw_1 * w_x_2 - f_2 * (e_1x * w_x + w_x * e_1x))) * a_hat; - J_a.block(0, 1, 3, 1) += - (d_R_bw_2 * alpha_arg + R_tau12k * (df_1_dbw_2 * w_x - f_1 * e_2x + df_2_dbw_2 * w_x_2 - f_2 * (e_2x * w_x + w_x * e_2x))) * a_hat; - J_a.block(0, 2, 3, 1) += - (d_R_bw_3 * alpha_arg + R_tau12k * (df_1_dbw_3 * w_x - f_1 * e_3x + df_2_dbw_3 * w_x_2 - f_2 * (e_3x * w_x + w_x * e_3x))) * a_hat; - J_b.block(0, 0, 3, 1) += - (d_R_bw_1 * Beta_arg + R_tau12k * (df_3_dbw_1 * w_x - f_3 * e_1x + df_4_dbw_1 * w_x_2 - f_4 * (e_1x * w_x + w_x * e_1x))) * a_hat; - J_b.block(0, 1, 3, 1) += - (d_R_bw_2 * Beta_arg + R_tau12k * (df_3_dbw_2 * w_x - f_3 * e_2x + df_4_dbw_2 * w_x_2 - f_4 * (e_2x * w_x + w_x * e_2x))) * a_hat; - J_b.block(0, 2, 3, 1) += - (d_R_bw_3 * Beta_arg + R_tau12k * (df_3_dbw_3 * w_x - f_3 * e_3x + df_4_dbw_3 * w_x_2 - f_4 * (e_3x * w_x + w_x * e_3x))) * a_hat; - - //========================================================================== - // MEASUREMENT COVARIANCE - //========================================================================== - - // Going to need orientation at intermediate time i.e. at .5*dt; - Eigen::Matrix R_mid = - small_w ? eye3 - .5 * delta_t * w_x + (pow(.5 * delta_t, 2) / 2) * w_x_2 - : eye3 - (sin(mag_w * .5 * delta_t) / mag_w) * w_x + ((1.0 - cos(mag_w * .5 * delta_t)) / (pow(mag_w, 2.0))) * w_x_2; - R_mid = R_mid * R_k2tau; - - // Compute covariance (in this implementation, we use RK4) - // k1------------------------------------------------------------------------------------------------- - - // Build state Jacobian - Eigen::Matrix F_k1 = Eigen::Matrix::Zero(); - F_k1.block(0, 0, 3, 3) = -w_x; - F_k1.block(0, 3, 3, 3) = -eye3; - F_k1.block(6, 0, 3, 3) = -R_k2tau.transpose() * a_x; - F_k1.block(6, 9, 3, 3) = -R_k2tau.transpose(); - F_k1.block(12, 6, 3, 3) = eye3; - - // Build noise Jacobian - Eigen::Matrix G_k1 = Eigen::Matrix::Zero(); - G_k1.block(0, 0, 3, 3) = -eye3; - G_k1.block(3, 3, 3, 3) = eye3; - G_k1.block(6, 6, 3, 3) = -R_k2tau.transpose(); - G_k1.block(9, 9, 3, 3) = eye3; - - // Get covariance derivative - Eigen::Matrix P_dot_k1 = F_k1 * P_meas + P_meas * F_k1.transpose() + G_k1 * Q_c * G_k1.transpose(); - - // k2------------------------------------------------------------------------------------------------- - - // Build state Jacobian - Eigen::Matrix F_k2 = Eigen::Matrix::Zero(); - F_k2.block(0, 0, 3, 3) = -w_x; - F_k2.block(0, 3, 3, 3) = -eye3; - F_k2.block(6, 0, 3, 3) = -R_mid.transpose() * a_x; - F_k2.block(6, 9, 3, 3) = -R_mid.transpose(); - F_k2.block(12, 6, 3, 3) = eye3; - - // Build noise Jacobian - Eigen::Matrix G_k2 = Eigen::Matrix::Zero(); - G_k2.block(0, 0, 3, 3) = -eye3; - G_k2.block(3, 3, 3, 3) = eye3; - G_k2.block(6, 6, 3, 3) = -R_mid.transpose(); - G_k2.block(9, 9, 3, 3) = eye3; - - // Get covariance derivative - Eigen::Matrix P_k2 = P_meas + P_dot_k1 * delta_t / 2.0; - Eigen::Matrix P_dot_k2 = F_k2 * P_k2 + P_k2 * F_k2.transpose() + G_k2 * Q_c * G_k2.transpose(); - - // k3------------------------------------------------------------------------------------------------- - - // Our state and noise Jacobians are the same as k2 - // Since k2 and k3 correspond to the same estimates for the midpoint - Eigen::Matrix F_k3 = F_k2; - Eigen::Matrix G_k3 = G_k2; - - // Get covariance derivative - Eigen::Matrix P_k3 = P_meas + P_dot_k2 * delta_t / 2.0; - Eigen::Matrix P_dot_k3 = F_k3 * P_k3 + P_k3 * F_k3.transpose() + G_k3 * Q_c * G_k3.transpose(); - - // k4------------------------------------------------------------------------------------------------- - - // Build state Jacobian - Eigen::Matrix F_k4 = Eigen::Matrix::Zero(); - F_k4.block(0, 0, 3, 3) = -w_x; - F_k4.block(0, 3, 3, 3) = -eye3; - F_k4.block(6, 0, 3, 3) = -R_k2tau1.transpose() * a_x; - F_k4.block(6, 9, 3, 3) = -R_k2tau1.transpose(); - F_k4.block(12, 6, 3, 3) = eye3; - - // Build noise Jacobian - Eigen::Matrix G_k4 = Eigen::Matrix::Zero(); - G_k4.block(0, 0, 3, 3) = -eye3; - G_k4.block(3, 3, 3, 3) = eye3; - G_k4.block(6, 6, 3, 3) = -R_k2tau1.transpose(); - G_k4.block(9, 9, 3, 3) = eye3; - - // Get covariance derivative - Eigen::Matrix P_k4 = P_meas + P_dot_k3 * delta_t; - Eigen::Matrix P_dot_k4 = F_k4 * P_k4 + P_k4 * F_k4.transpose() + G_k4 * Q_c * G_k4.transpose(); - - // done------------------------------------------------------------------------------------------------- - - // Collect covariance solution - // Ensure it is positive definite - P_meas += (delta_t / 6.0) * (P_dot_k1 + 2.0 * P_dot_k2 + 2.0 * P_dot_k3 + P_dot_k4); - P_meas = 0.5 * (P_meas + P_meas.transpose()); - - // Update rotation mean - // Note we had to wait to do this, since we use the old orientation in our covariance calculation - R_k2tau = R_k2tau1; - q_k2tau = rot_2_quat(R_k2tau); - } + Eigen::Matrix a_m_1 = Eigen::Matrix::Zero()); }; } // namespace ov_core diff --git a/ov_core/src/cpi/CpiV2.cpp b/ov_core/src/cpi/CpiV2.cpp new file mode 100644 index 000000000..10e280631 --- /dev/null +++ b/ov_core/src/cpi/CpiV2.cpp @@ -0,0 +1,387 @@ + +/* + * MIT License + * Copyright (c) 2018 Kevin Eckenhoff + * Copyright (c) 2018 Patrick Geneva + * Copyright (c) 2018 Guoquan Huang + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include "CpiV2.h" + +#include "utils/quat_ops.h" + +using namespace ov_core; + +void CpiV2::feed_IMU(double t_0, double t_1, Eigen::Matrix w_m_0, Eigen::Matrix a_m_0, + Eigen::Matrix w_m_1, Eigen::Matrix a_m_1) { + + // Get time difference + double delta_t = t_1 - t_0; + DT += delta_t; + + // If no time has passed do nothing + if (delta_t == 0) { + return; + } + + // Get estimated imu readings + Eigen::Matrix w_hat = w_m_0 - b_w_lin; + Eigen::Matrix a_hat = a_m_0 - b_a_lin - R_k2tau * quat_2_Rot(q_k_lin) * grav; + + // If averaging, average + // Note: we will average the LOCAL acceleration after getting the relative rotation + if (imu_avg) { + w_hat += w_m_1 - b_w_lin; + w_hat = 0.5 * w_hat; + } + + // Get angle change w*dt + Eigen::Matrix w_hatdt = w_hat * delta_t; + + // Get entries of w_hat + double w_1 = w_hat(0, 0); + double w_2 = w_hat(1, 0); + double w_3 = w_hat(2, 0); + + // Get magnitude of w and wdt + double mag_w = w_hat.norm(); + double w_dt = mag_w * delta_t; + + // Threshold to determine if equations will be unstable + bool small_w = (mag_w < 0.008726646); + + // Get some of the variables used in the preintegration equations + double dt_2 = pow(delta_t, 2); + double cos_wt = cos(w_dt); + double sin_wt = sin(w_dt); + + Eigen::Matrix w_x = skew_x(w_hat); + Eigen::Matrix w_tx = skew_x(w_hatdt); + Eigen::Matrix w_x_2 = w_x * w_x; + + //========================================================================== + // MEASUREMENT MEANS + //========================================================================== + + // Get relative rotation + Eigen::Matrix R_tau2tau1 = small_w ? eye3 - delta_t * w_x + (pow(delta_t, 2) / 2) * w_x_2 + : eye3 - (sin_wt / mag_w) * w_x + ((1.0 - cos_wt) / (pow(mag_w, 2.0))) * w_x_2; + + // Updated roation and its transpose + Eigen::Matrix R_k2tau1 = R_tau2tau1 * R_k2tau; + Eigen::Matrix R_tau12k = R_k2tau1.transpose(); + + // If averaging, average the LOCAL acceleration + if (imu_avg) { + a_hat += a_m_1 - b_a_lin - R_k2tau1 * quat_2_Rot(q_k_lin) * grav; + a_hat = 0.5 * a_hat; + } + Eigen::Matrix a_x = skew_x(a_hat); + + // Intermediate variables for evaluating the measurement/bias Jacobian update + double f_1; + double f_2; + double f_3; + double f_4; + + if (small_w) { + f_1 = -(pow(delta_t, 3) / 3); + f_2 = (pow(delta_t, 4) / 8); + f_3 = -(pow(delta_t, 2) / 2); + f_4 = (pow(delta_t, 3) / 6); + } else { + f_1 = (w_dt * cos_wt - sin_wt) / (pow(mag_w, 3)); + f_2 = (pow(w_dt, 2) - 2 * cos_wt - 2 * w_dt * sin_wt + 2) / (2 * pow(mag_w, 4)); + f_3 = -(1 - cos_wt) / pow(mag_w, 2); + f_4 = (w_dt - sin_wt) / pow(mag_w, 3); + } + + // Compute the main part of our analytical means + Eigen::Matrix alpha_arg = ((dt_2 / 2.0) * eye3 + f_1 * w_x + f_2 * w_x_2); + Eigen::Matrix Beta_arg = (delta_t * eye3 + f_3 * w_x + f_4 * w_x_2); + + // Matrices that will multiply the a_hat in the update expressions + Eigen::Matrix H_al = R_tau12k * alpha_arg; + Eigen::Matrix H_be = R_tau12k * Beta_arg; + + // Update the measurements + alpha_tau += beta_tau * delta_t + H_al * a_hat; + beta_tau += H_be * a_hat; + + //========================================================================== + // BIAS JACOBIANS (ANALYTICAL) + //========================================================================== + + // Get right Jacobian + Eigen::Matrix J_r_tau1 = + small_w ? eye3 - .5 * w_tx + (1.0 / 6.0) * w_tx * w_tx + : eye3 - ((1 - cos_wt) / (pow((w_dt), 2.0))) * w_tx + ((w_dt - sin_wt) / (pow(w_dt, 3.0))) * w_tx * w_tx; + + // Update orientation in respect to gyro bias Jacobians + Eigen::Matrix J_save = J_q; + J_q = R_tau2tau1 * J_q + J_r_tau1 * delta_t; + + // Update alpha and beta in respect to accel bias Jacobian + H_a -= H_al; + H_a += delta_t * H_b; + H_b -= H_be; + + // Update alpha and beta in respect to q_GtoLIN Jacobian + Eigen::Matrix g_k = quat_2_Rot(q_k_lin) * grav; + O_a += delta_t * O_b; + O_a += -H_al * R_k2tau * skew_x(g_k); + O_b += -H_be * R_k2tau * skew_x(g_k); + + // Derivatives of R_tau12k wrt bias_w entries + Eigen::MatrixXd d_R_bw_1 = -R_tau12k * skew_x(J_q * e_1); + Eigen::MatrixXd d_R_bw_2 = -R_tau12k * skew_x(J_q * e_2); + Eigen::MatrixXd d_R_bw_3 = -R_tau12k * skew_x(J_q * e_3); + + // Now compute the gyro bias Jacobian terms + double df_1_dbw_1; + double df_1_dbw_2; + double df_1_dbw_3; + + double df_2_dbw_1; + double df_2_dbw_2; + double df_2_dbw_3; + + double df_3_dbw_1; + double df_3_dbw_2; + double df_3_dbw_3; + + double df_4_dbw_1; + double df_4_dbw_2; + double df_4_dbw_3; + + if (small_w) { + double df_1_dw_mag = -(pow(delta_t, 5) / 15); + df_1_dbw_1 = w_1 * df_1_dw_mag; + df_1_dbw_2 = w_2 * df_1_dw_mag; + df_1_dbw_3 = w_3 * df_1_dw_mag; + + double df_2_dw_mag = (pow(delta_t, 6) / 72); + df_2_dbw_1 = w_1 * df_2_dw_mag; + df_2_dbw_2 = w_2 * df_2_dw_mag; + df_2_dbw_3 = w_3 * df_2_dw_mag; + + double df_3_dw_mag = -(pow(delta_t, 4) / 12); + df_3_dbw_1 = w_1 * df_3_dw_mag; + df_3_dbw_2 = w_2 * df_3_dw_mag; + df_3_dbw_3 = w_3 * df_3_dw_mag; + + double df_4_dw_mag = (pow(delta_t, 5) / 60); + df_4_dbw_1 = w_1 * df_4_dw_mag; + df_4_dbw_2 = w_2 * df_4_dw_mag; + df_4_dbw_3 = w_3 * df_4_dw_mag; + } else { + double df_1_dw_mag = (pow(w_dt, 2) * sin_wt - 3 * sin_wt + 3 * w_dt * cos_wt) / pow(mag_w, 5); + df_1_dbw_1 = w_1 * df_1_dw_mag; + df_1_dbw_2 = w_2 * df_1_dw_mag; + df_1_dbw_3 = w_3 * df_1_dw_mag; + + double df_2_dw_mag = (pow(w_dt, 2) - 4 * cos_wt - 4 * w_dt * sin_wt + pow(w_dt, 2) * cos_wt + 4) / (pow(mag_w, 6)); + df_2_dbw_1 = w_1 * df_2_dw_mag; + df_2_dbw_2 = w_2 * df_2_dw_mag; + df_2_dbw_3 = w_3 * df_2_dw_mag; + + double df_3_dw_mag = (2 * (cos_wt - 1) + w_dt * sin_wt) / (pow(mag_w, 4)); + df_3_dbw_1 = w_1 * df_3_dw_mag; + df_3_dbw_2 = w_2 * df_3_dw_mag; + df_3_dbw_3 = w_3 * df_3_dw_mag; + + double df_4_dw_mag = (2 * w_dt + w_dt * cos_wt - 3 * sin_wt) / (pow(mag_w, 5)); + df_4_dbw_1 = w_1 * df_4_dw_mag; + df_4_dbw_2 = w_2 * df_4_dw_mag; + df_4_dbw_3 = w_3 * df_4_dw_mag; + } + + // Gravity rotated into the "tau'th" frame (i.e. start of the measurement interval) + Eigen::Matrix g_tau = R_k2tau * quat_2_Rot(q_k_lin) * grav; + + // Update gyro bias Jacobians + J_a += J_b * delta_t; + J_a.block(0, 0, 3, 1) += + (d_R_bw_1 * alpha_arg + R_tau12k * (df_1_dbw_1 * w_x - f_1 * e_1x + df_2_dbw_1 * w_x_2 - f_2 * (e_1x * w_x + w_x * e_1x))) * a_hat - + H_al * skew_x((J_save * e_1)) * g_tau; + J_a.block(0, 1, 3, 1) += + (d_R_bw_2 * alpha_arg + R_tau12k * (df_1_dbw_2 * w_x - f_1 * e_2x + df_2_dbw_2 * w_x_2 - f_2 * (e_2x * w_x + w_x * e_2x))) * a_hat - + H_al * skew_x((J_save * e_2)) * g_tau; + J_a.block(0, 2, 3, 1) += + (d_R_bw_3 * alpha_arg + R_tau12k * (df_1_dbw_3 * w_x - f_1 * e_3x + df_2_dbw_3 * w_x_2 - f_2 * (e_3x * w_x + w_x * e_3x))) * a_hat - + H_al * skew_x((J_save * e_3)) * g_tau; + J_b.block(0, 0, 3, 1) += + (d_R_bw_1 * Beta_arg + R_tau12k * (df_3_dbw_1 * w_x - f_3 * e_1x + df_4_dbw_1 * w_x_2 - f_4 * (e_1x * w_x + w_x * e_1x))) * a_hat - + -H_be * skew_x((J_save * e_1)) * g_tau; + J_b.block(0, 1, 3, 1) += + (d_R_bw_2 * Beta_arg + R_tau12k * (df_3_dbw_2 * w_x - f_3 * e_2x + df_4_dbw_2 * w_x_2 - f_4 * (e_2x * w_x + w_x * e_2x))) * a_hat - + H_be * skew_x((J_save * e_2)) * g_tau; + J_b.block(0, 2, 3, 1) += + (d_R_bw_3 * Beta_arg + R_tau12k * (df_3_dbw_3 * w_x - f_3 * e_3x + df_4_dbw_3 * w_x_2 - f_4 * (e_3x * w_x + w_x * e_3x))) * a_hat - + H_be * skew_x((J_save * e_3)) * g_tau; + + //========================================================================== + // MEASUREMENT COVARIANCE + //========================================================================== + + // Going to need orientation at intermediate time i.e. at .5*dt; + Eigen::Matrix R_G_to_k = quat_2_Rot(q_k_lin); + double dt_mid = delta_t / 2.0; + double w_dt_mid = mag_w * dt_mid; + Eigen::Matrix R_mid; + + // The middle of this interval (i.e., rotation from k to mid) + R_mid = small_w ? eye3 - dt_mid * w_x + (pow(dt_mid, 2) / 2) * w_x_2 + : eye3 - (sin(w_dt_mid) / mag_w) * w_x + ((1.0 - cos(w_dt_mid)) / (pow(mag_w, 2.0))) * w_x_2; + R_mid = R_mid * R_k2tau; + + // Compute covariance (in this implementation, we use RK4) + // k1------------------------------------------------------------------------------------------------- + + // Build state Jacobian + Eigen::Matrix F_k1 = Eigen::Matrix::Zero(); + F_k1.block(0, 0, 3, 3) = -w_x; + F_k1.block(0, 3, 3, 3) = -eye3; + F_k1.block(6, 0, 3, 3) = -R_k2tau.transpose() * a_x; + F_k1.block(6, 9, 3, 3) = -R_k2tau.transpose(); + F_k1.block(6, 15, 3, 3) = -R_k2tau.transpose() * skew_x(R_k2tau * R_G_to_k * grav); + F_k1.block(6, 18, 3, 3) = -R_k2tau.transpose() * R_k2tau * skew_x(R_G_to_k * grav); + F_k1.block(12, 6, 3, 3) = eye3; + + // Build noise Jacobian + Eigen::Matrix G_k1 = Eigen::Matrix::Zero(); + G_k1.block(0, 0, 3, 3) = -eye3; + G_k1.block(3, 3, 3, 3) = eye3; + G_k1.block(6, 6, 3, 3) = -R_k2tau.transpose(); + G_k1.block(9, 9, 3, 3) = eye3; + + // Get state transition and covariance derivative + Eigen::Matrix Phi_dot_k1 = F_k1; + Eigen::Matrix P_dot_k1 = F_k1 * P_big + P_big * F_k1.transpose() + G_k1 * Q_c * G_k1.transpose(); + + // k2------------------------------------------------------------------------------------------------- + + // Build state Jacobian + Eigen::Matrix F_k2 = Eigen::Matrix::Zero(); + F_k2.block(0, 0, 3, 3) = -w_x; + F_k2.block(0, 3, 3, 3) = -eye3; + F_k2.block(6, 0, 3, 3) = -R_mid.transpose() * a_x; + F_k2.block(6, 9, 3, 3) = -R_mid.transpose(); + F_k2.block(6, 15, 3, 3) = -R_mid.transpose() * skew_x(R_k2tau * R_G_to_k * grav); + F_k2.block(6, 18, 3, 3) = -R_mid.transpose() * R_k2tau * skew_x(R_G_to_k * grav); + F_k2.block(12, 6, 3, 3) = eye3; + + // Build noise Jacobian + Eigen::Matrix G_k2 = Eigen::Matrix::Zero(); + G_k2.block(0, 0, 3, 3) = -eye3; + G_k2.block(3, 3, 3, 3) = eye3; + G_k2.block(6, 6, 3, 3) = -R_mid.transpose(); + G_k2.block(9, 9, 3, 3) = eye3; + + // Get state transition and covariance derivative + Eigen::Matrix Phi_k2 = Eigen::Matrix::Identity() + Phi_dot_k1 * dt_mid; + Eigen::Matrix P_k2 = P_big + P_dot_k1 * dt_mid; + Eigen::Matrix Phi_dot_k2 = F_k2 * Phi_k2; + Eigen::Matrix P_dot_k2 = F_k2 * P_k2 + P_k2 * F_k2.transpose() + G_k2 * Q_c * G_k2.transpose(); + + // k3------------------------------------------------------------------------------------------------- + + // Our state and noise Jacobians are the same as k2 + // Since k2 and k3 correspond to the same estimates for the midpoint + Eigen::Matrix F_k3 = F_k2; + Eigen::Matrix G_k3 = G_k2; + + // Get state transition and covariance derivative + Eigen::Matrix Phi_k3 = Eigen::Matrix::Identity() + Phi_dot_k2 * dt_mid; + Eigen::Matrix P_k3 = P_big + P_dot_k2 * dt_mid; + Eigen::Matrix Phi_dot_k3 = F_k3 * Phi_k3; + Eigen::Matrix P_dot_k3 = F_k3 * P_k3 + P_k3 * F_k3.transpose() + G_k3 * Q_c * G_k3.transpose(); + + // k4------------------------------------------------------------------------------------------------- + + // Build state Jacobian + Eigen::Matrix F_k4 = Eigen::Matrix::Zero(); + F_k4.block(0, 0, 3, 3) = -w_x; + F_k4.block(0, 3, 3, 3) = -eye3; + F_k4.block(6, 0, 3, 3) = -R_k2tau1.transpose() * a_x; + F_k4.block(6, 9, 3, 3) = -R_k2tau1.transpose(); + F_k4.block(6, 15, 3, 3) = -R_k2tau1.transpose() * skew_x(R_k2tau * R_G_to_k * grav); + F_k4.block(6, 18, 3, 3) = -R_k2tau1.transpose() * R_k2tau * skew_x(R_G_to_k * grav); + F_k4.block(12, 6, 3, 3) = eye3; + + // Build noise Jacobian + Eigen::Matrix G_k4 = Eigen::Matrix::Zero(); + G_k4.block(0, 0, 3, 3) = -eye3; + G_k4.block(3, 3, 3, 3) = eye3; + G_k4.block(6, 6, 3, 3) = -R_k2tau1.transpose(); + G_k4.block(9, 9, 3, 3) = eye3; + + // Get state transition and covariance derivative + Eigen::Matrix Phi_k4 = Eigen::Matrix::Identity() + Phi_dot_k3 * delta_t; + Eigen::Matrix P_k4 = P_big + P_dot_k3 * delta_t; + Eigen::Matrix Phi_dot_k4 = F_k4 * Phi_k4; + Eigen::Matrix P_dot_k4 = F_k4 * P_k4 + P_k4 * F_k4.transpose() + G_k4 * Q_c * G_k4.transpose(); + + // done------------------------------------------------------------------------------------------------- + + // Collect covariance solution + // Ensure it is positive definite + P_big += (delta_t / 6.0) * (P_dot_k1 + 2.0 * P_dot_k2 + 2.0 * P_dot_k3 + P_dot_k4); + P_big = 0.5 * (P_big + P_big.transpose()); + + // Calculate the state transition from time k to tau + Eigen::Matrix Phi = + Eigen::Matrix::Identity() + (delta_t / 6.0) * (Phi_dot_k1 + 2.0 * Phi_dot_k2 + 2.0 * Phi_dot_k3 + Phi_dot_k4); + + //========================================================================== + // CLONE TO NEW SAMPLE TIME AND MARGINALIZE OLD SAMPLE TIME + //========================================================================== + + // Matrix that performs the clone and mariginalization + Eigen::Matrix B_k = Eigen::Matrix::Identity(); + B_k.block(15, 15, 3, 3).setZero(); + B_k.block(15, 0, 3, 3) = Eigen::Matrix::Identity(); + + // Change our measurement covariance and state transition + P_big = B_k * P_big * B_k.transpose(); + P_big = 0.5 * (P_big + P_big.transpose()); + Discrete_J_b = B_k * Phi * Discrete_J_b; + + // Our measurement covariance is the top 15x15 of our large covariance + P_meas = P_big.block(0, 0, 15, 15); + + // If we are using the state transition Jacobian, then we should overwrite the analytical versions + // Note: we flip the sign for J_q to match the Model 1 derivation + if (state_transition_jacobians) { + J_q = -Discrete_J_b.block(0, 3, 3, 3); + J_a = Discrete_J_b.block(12, 3, 3, 3); + J_b = Discrete_J_b.block(6, 3, 3, 3); + H_a = Discrete_J_b.block(12, 9, 3, 3); + H_b = Discrete_J_b.block(6, 9, 3, 3); + O_a = Discrete_J_b.block(12, 18, 3, 3); + O_b = Discrete_J_b.block(6, 18, 3, 3); + } + + // Update rotation mean + // Note we had to wait to do this, since we use the old orientation in our covariance calculation + R_k2tau = R_k2tau1; + q_k2tau = rot_2_quat(R_k2tau); +} diff --git a/ov_core/src/cpi/CpiV2.h b/ov_core/src/cpi/CpiV2.h index 8a358501e..44594c124 100644 --- a/ov_core/src/cpi/CpiV2.h +++ b/ov_core/src/cpi/CpiV2.h @@ -27,7 +27,7 @@ */ #include "CpiBase.h" -#include "utils/quat_ops.h" + #include namespace ov_core { @@ -79,6 +79,8 @@ class CpiV2 : public CpiBase { CpiV2(double sigma_w, double sigma_wb, double sigma_a, double sigma_ab, bool imu_avg_ = false) : CpiBase(sigma_w, sigma_wb, sigma_a, sigma_ab, imu_avg_) {} + virtual ~CpiV2() {} + /** * @brief Our precompound function for Model 2 * @param[in] t_0 first IMU timestamp @@ -94,360 +96,7 @@ class CpiV2 : public CpiBase { */ void feed_IMU(double t_0, double t_1, Eigen::Matrix w_m_0, Eigen::Matrix a_m_0, Eigen::Matrix w_m_1 = Eigen::Matrix::Zero(), - Eigen::Matrix a_m_1 = Eigen::Matrix::Zero()) { - - // Get time difference - double delta_t = t_1 - t_0; - DT += delta_t; - - // If no time has passed do nothing - if (delta_t == 0) { - return; - } - - // Get estimated imu readings - Eigen::Matrix w_hat = w_m_0 - b_w_lin; - Eigen::Matrix a_hat = a_m_0 - b_a_lin - R_k2tau * quat_2_Rot(q_k_lin) * grav; - - // If averaging, average - // Note: we will average the LOCAL acceleration after getting the relative rotation - if (imu_avg) { - w_hat += w_m_1 - b_w_lin; - w_hat = 0.5 * w_hat; - } - - // Get angle change w*dt - Eigen::Matrix w_hatdt = w_hat * delta_t; - - // Get entries of w_hat - double w_1 = w_hat(0, 0); - double w_2 = w_hat(1, 0); - double w_3 = w_hat(2, 0); - - // Get magnitude of w and wdt - double mag_w = w_hat.norm(); - double w_dt = mag_w * delta_t; - - // Threshold to determine if equations will be unstable - bool small_w = (mag_w < 0.008726646); - - // Get some of the variables used in the preintegration equations - double dt_2 = pow(delta_t, 2); - double cos_wt = cos(w_dt); - double sin_wt = sin(w_dt); - - Eigen::Matrix w_x = skew_x(w_hat); - Eigen::Matrix w_tx = skew_x(w_hatdt); - Eigen::Matrix w_x_2 = w_x * w_x; - - //========================================================================== - // MEASUREMENT MEANS - //========================================================================== - - // Get relative rotation - Eigen::Matrix R_tau2tau1 = small_w ? eye3 - delta_t * w_x + (pow(delta_t, 2) / 2) * w_x_2 - : eye3 - (sin_wt / mag_w) * w_x + ((1.0 - cos_wt) / (pow(mag_w, 2.0))) * w_x_2; - - // Updated roation and its transpose - Eigen::Matrix R_k2tau1 = R_tau2tau1 * R_k2tau; - Eigen::Matrix R_tau12k = R_k2tau1.transpose(); - - // If averaging, average the LOCAL acceleration - if (imu_avg) { - a_hat += a_m_1 - b_a_lin - R_k2tau1 * quat_2_Rot(q_k_lin) * grav; - a_hat = 0.5 * a_hat; - } - Eigen::Matrix a_x = skew_x(a_hat); - - // Intermediate variables for evaluating the measurement/bias Jacobian update - double f_1; - double f_2; - double f_3; - double f_4; - - if (small_w) { - f_1 = -(pow(delta_t, 3) / 3); - f_2 = (pow(delta_t, 4) / 8); - f_3 = -(pow(delta_t, 2) / 2); - f_4 = (pow(delta_t, 3) / 6); - } else { - f_1 = (w_dt * cos_wt - sin_wt) / (pow(mag_w, 3)); - f_2 = (pow(w_dt, 2) - 2 * cos_wt - 2 * w_dt * sin_wt + 2) / (2 * pow(mag_w, 4)); - f_3 = -(1 - cos_wt) / pow(mag_w, 2); - f_4 = (w_dt - sin_wt) / pow(mag_w, 3); - } - - // Compute the main part of our analytical means - Eigen::Matrix alpha_arg = ((dt_2 / 2.0) * eye3 + f_1 * w_x + f_2 * w_x_2); - Eigen::Matrix Beta_arg = (delta_t * eye3 + f_3 * w_x + f_4 * w_x_2); - - // Matrices that will multiply the a_hat in the update expressions - Eigen::Matrix H_al = R_tau12k * alpha_arg; - Eigen::Matrix H_be = R_tau12k * Beta_arg; - - // Update the measurements - alpha_tau += beta_tau * delta_t + H_al * a_hat; - beta_tau += H_be * a_hat; - - //========================================================================== - // BIAS JACOBIANS (ANALYTICAL) - //========================================================================== - - // Get right Jacobian - Eigen::Matrix J_r_tau1 = - small_w ? eye3 - .5 * w_tx + (1.0 / 6.0) * w_tx * w_tx - : eye3 - ((1 - cos_wt) / (pow((w_dt), 2.0))) * w_tx + ((w_dt - sin_wt) / (pow(w_dt, 3.0))) * w_tx * w_tx; - - // Update orientation in respect to gyro bias Jacobians - Eigen::Matrix J_save = J_q; - J_q = R_tau2tau1 * J_q + J_r_tau1 * delta_t; - - // Update alpha and beta in respect to accel bias Jacobian - H_a -= H_al; - H_a += delta_t * H_b; - H_b -= H_be; - - // Update alpha and beta in respect to q_GtoLIN Jacobian - Eigen::Matrix g_k = quat_2_Rot(q_k_lin) * grav; - O_a += delta_t * O_b; - O_a += -H_al * R_k2tau * skew_x(g_k); - O_b += -H_be * R_k2tau * skew_x(g_k); - - // Derivatives of R_tau12k wrt bias_w entries - Eigen::MatrixXd d_R_bw_1 = -R_tau12k * skew_x(J_q * e_1); - Eigen::MatrixXd d_R_bw_2 = -R_tau12k * skew_x(J_q * e_2); - Eigen::MatrixXd d_R_bw_3 = -R_tau12k * skew_x(J_q * e_3); - - // Now compute the gyro bias Jacobian terms - double df_1_dbw_1; - double df_1_dbw_2; - double df_1_dbw_3; - - double df_2_dbw_1; - double df_2_dbw_2; - double df_2_dbw_3; - - double df_3_dbw_1; - double df_3_dbw_2; - double df_3_dbw_3; - - double df_4_dbw_1; - double df_4_dbw_2; - double df_4_dbw_3; - - if (small_w) { - double df_1_dw_mag = -(pow(delta_t, 5) / 15); - df_1_dbw_1 = w_1 * df_1_dw_mag; - df_1_dbw_2 = w_2 * df_1_dw_mag; - df_1_dbw_3 = w_3 * df_1_dw_mag; - - double df_2_dw_mag = (pow(delta_t, 6) / 72); - df_2_dbw_1 = w_1 * df_2_dw_mag; - df_2_dbw_2 = w_2 * df_2_dw_mag; - df_2_dbw_3 = w_3 * df_2_dw_mag; - - double df_3_dw_mag = -(pow(delta_t, 4) / 12); - df_3_dbw_1 = w_1 * df_3_dw_mag; - df_3_dbw_2 = w_2 * df_3_dw_mag; - df_3_dbw_3 = w_3 * df_3_dw_mag; - - double df_4_dw_mag = (pow(delta_t, 5) / 60); - df_4_dbw_1 = w_1 * df_4_dw_mag; - df_4_dbw_2 = w_2 * df_4_dw_mag; - df_4_dbw_3 = w_3 * df_4_dw_mag; - } else { - double df_1_dw_mag = (pow(w_dt, 2) * sin_wt - 3 * sin_wt + 3 * w_dt * cos_wt) / pow(mag_w, 5); - df_1_dbw_1 = w_1 * df_1_dw_mag; - df_1_dbw_2 = w_2 * df_1_dw_mag; - df_1_dbw_3 = w_3 * df_1_dw_mag; - - double df_2_dw_mag = (pow(w_dt, 2) - 4 * cos_wt - 4 * w_dt * sin_wt + pow(w_dt, 2) * cos_wt + 4) / (pow(mag_w, 6)); - df_2_dbw_1 = w_1 * df_2_dw_mag; - df_2_dbw_2 = w_2 * df_2_dw_mag; - df_2_dbw_3 = w_3 * df_2_dw_mag; - - double df_3_dw_mag = (2 * (cos_wt - 1) + w_dt * sin_wt) / (pow(mag_w, 4)); - df_3_dbw_1 = w_1 * df_3_dw_mag; - df_3_dbw_2 = w_2 * df_3_dw_mag; - df_3_dbw_3 = w_3 * df_3_dw_mag; - - double df_4_dw_mag = (2 * w_dt + w_dt * cos_wt - 3 * sin_wt) / (pow(mag_w, 5)); - df_4_dbw_1 = w_1 * df_4_dw_mag; - df_4_dbw_2 = w_2 * df_4_dw_mag; - df_4_dbw_3 = w_3 * df_4_dw_mag; - } - - // Gravity rotated into the "tau'th" frame (i.e. start of the measurement interval) - Eigen::Matrix g_tau = R_k2tau * quat_2_Rot(q_k_lin) * grav; - - // Update gyro bias Jacobians - J_a += J_b * delta_t; - J_a.block(0, 0, 3, 1) += - (d_R_bw_1 * alpha_arg + R_tau12k * (df_1_dbw_1 * w_x - f_1 * e_1x + df_2_dbw_1 * w_x_2 - f_2 * (e_1x * w_x + w_x * e_1x))) * a_hat - - H_al * skew_x((J_save * e_1)) * g_tau; - J_a.block(0, 1, 3, 1) += - (d_R_bw_2 * alpha_arg + R_tau12k * (df_1_dbw_2 * w_x - f_1 * e_2x + df_2_dbw_2 * w_x_2 - f_2 * (e_2x * w_x + w_x * e_2x))) * a_hat - - H_al * skew_x((J_save * e_2)) * g_tau; - J_a.block(0, 2, 3, 1) += - (d_R_bw_3 * alpha_arg + R_tau12k * (df_1_dbw_3 * w_x - f_1 * e_3x + df_2_dbw_3 * w_x_2 - f_2 * (e_3x * w_x + w_x * e_3x))) * a_hat - - H_al * skew_x((J_save * e_3)) * g_tau; - J_b.block(0, 0, 3, 1) += - (d_R_bw_1 * Beta_arg + R_tau12k * (df_3_dbw_1 * w_x - f_3 * e_1x + df_4_dbw_1 * w_x_2 - f_4 * (e_1x * w_x + w_x * e_1x))) * a_hat - - -H_be * skew_x((J_save * e_1)) * g_tau; - J_b.block(0, 1, 3, 1) += - (d_R_bw_2 * Beta_arg + R_tau12k * (df_3_dbw_2 * w_x - f_3 * e_2x + df_4_dbw_2 * w_x_2 - f_4 * (e_2x * w_x + w_x * e_2x))) * a_hat - - H_be * skew_x((J_save * e_2)) * g_tau; - J_b.block(0, 2, 3, 1) += - (d_R_bw_3 * Beta_arg + R_tau12k * (df_3_dbw_3 * w_x - f_3 * e_3x + df_4_dbw_3 * w_x_2 - f_4 * (e_3x * w_x + w_x * e_3x))) * a_hat - - H_be * skew_x((J_save * e_3)) * g_tau; - - //========================================================================== - // MEASUREMENT COVARIANCE - //========================================================================== - - // Going to need orientation at intermediate time i.e. at .5*dt; - Eigen::Matrix R_G_to_k = quat_2_Rot(q_k_lin); - double dt_mid = delta_t / 2.0; - double w_dt_mid = mag_w * dt_mid; - Eigen::Matrix R_mid; - - // The middle of this interval (i.e., rotation from k to mid) - R_mid = small_w ? eye3 - dt_mid * w_x + (pow(dt_mid, 2) / 2) * w_x_2 - : eye3 - (sin(w_dt_mid) / mag_w) * w_x + ((1.0 - cos(w_dt_mid)) / (pow(mag_w, 2.0))) * w_x_2; - R_mid = R_mid * R_k2tau; - - // Compute covariance (in this implementation, we use RK4) - // k1------------------------------------------------------------------------------------------------- - - // Build state Jacobian - Eigen::Matrix F_k1 = Eigen::Matrix::Zero(); - F_k1.block(0, 0, 3, 3) = -w_x; - F_k1.block(0, 3, 3, 3) = -eye3; - F_k1.block(6, 0, 3, 3) = -R_k2tau.transpose() * a_x; - F_k1.block(6, 9, 3, 3) = -R_k2tau.transpose(); - F_k1.block(6, 15, 3, 3) = -R_k2tau.transpose() * skew_x(R_k2tau * R_G_to_k * grav); - F_k1.block(6, 18, 3, 3) = -R_k2tau.transpose() * R_k2tau * skew_x(R_G_to_k * grav); - F_k1.block(12, 6, 3, 3) = eye3; - - // Build noise Jacobian - Eigen::Matrix G_k1 = Eigen::Matrix::Zero(); - G_k1.block(0, 0, 3, 3) = -eye3; - G_k1.block(3, 3, 3, 3) = eye3; - G_k1.block(6, 6, 3, 3) = -R_k2tau.transpose(); - G_k1.block(9, 9, 3, 3) = eye3; - - // Get state transition and covariance derivative - Eigen::Matrix Phi_dot_k1 = F_k1; - Eigen::Matrix P_dot_k1 = F_k1 * P_big + P_big * F_k1.transpose() + G_k1 * Q_c * G_k1.transpose(); - - // k2------------------------------------------------------------------------------------------------- - - // Build state Jacobian - Eigen::Matrix F_k2 = Eigen::Matrix::Zero(); - F_k2.block(0, 0, 3, 3) = -w_x; - F_k2.block(0, 3, 3, 3) = -eye3; - F_k2.block(6, 0, 3, 3) = -R_mid.transpose() * a_x; - F_k2.block(6, 9, 3, 3) = -R_mid.transpose(); - F_k2.block(6, 15, 3, 3) = -R_mid.transpose() * skew_x(R_k2tau * R_G_to_k * grav); - F_k2.block(6, 18, 3, 3) = -R_mid.transpose() * R_k2tau * skew_x(R_G_to_k * grav); - F_k2.block(12, 6, 3, 3) = eye3; - - // Build noise Jacobian - Eigen::Matrix G_k2 = Eigen::Matrix::Zero(); - G_k2.block(0, 0, 3, 3) = -eye3; - G_k2.block(3, 3, 3, 3) = eye3; - G_k2.block(6, 6, 3, 3) = -R_mid.transpose(); - G_k2.block(9, 9, 3, 3) = eye3; - - // Get state transition and covariance derivative - Eigen::Matrix Phi_k2 = Eigen::Matrix::Identity() + Phi_dot_k1 * dt_mid; - Eigen::Matrix P_k2 = P_big + P_dot_k1 * dt_mid; - Eigen::Matrix Phi_dot_k2 = F_k2 * Phi_k2; - Eigen::Matrix P_dot_k2 = F_k2 * P_k2 + P_k2 * F_k2.transpose() + G_k2 * Q_c * G_k2.transpose(); - - // k3------------------------------------------------------------------------------------------------- - - // Our state and noise Jacobians are the same as k2 - // Since k2 and k3 correspond to the same estimates for the midpoint - Eigen::Matrix F_k3 = F_k2; - Eigen::Matrix G_k3 = G_k2; - - // Get state transition and covariance derivative - Eigen::Matrix Phi_k3 = Eigen::Matrix::Identity() + Phi_dot_k2 * dt_mid; - Eigen::Matrix P_k3 = P_big + P_dot_k2 * dt_mid; - Eigen::Matrix Phi_dot_k3 = F_k3 * Phi_k3; - Eigen::Matrix P_dot_k3 = F_k3 * P_k3 + P_k3 * F_k3.transpose() + G_k3 * Q_c * G_k3.transpose(); - - // k4------------------------------------------------------------------------------------------------- - - // Build state Jacobian - Eigen::Matrix F_k4 = Eigen::Matrix::Zero(); - F_k4.block(0, 0, 3, 3) = -w_x; - F_k4.block(0, 3, 3, 3) = -eye3; - F_k4.block(6, 0, 3, 3) = -R_k2tau1.transpose() * a_x; - F_k4.block(6, 9, 3, 3) = -R_k2tau1.transpose(); - F_k4.block(6, 15, 3, 3) = -R_k2tau1.transpose() * skew_x(R_k2tau * R_G_to_k * grav); - F_k4.block(6, 18, 3, 3) = -R_k2tau1.transpose() * R_k2tau * skew_x(R_G_to_k * grav); - F_k4.block(12, 6, 3, 3) = eye3; - - // Build noise Jacobian - Eigen::Matrix G_k4 = Eigen::Matrix::Zero(); - G_k4.block(0, 0, 3, 3) = -eye3; - G_k4.block(3, 3, 3, 3) = eye3; - G_k4.block(6, 6, 3, 3) = -R_k2tau1.transpose(); - G_k4.block(9, 9, 3, 3) = eye3; - - // Get state transition and covariance derivative - Eigen::Matrix Phi_k4 = Eigen::Matrix::Identity() + Phi_dot_k3 * delta_t; - Eigen::Matrix P_k4 = P_big + P_dot_k3 * delta_t; - Eigen::Matrix Phi_dot_k4 = F_k4 * Phi_k4; - Eigen::Matrix P_dot_k4 = F_k4 * P_k4 + P_k4 * F_k4.transpose() + G_k4 * Q_c * G_k4.transpose(); - - // done------------------------------------------------------------------------------------------------- - - // Collect covariance solution - // Ensure it is positive definite - P_big += (delta_t / 6.0) * (P_dot_k1 + 2.0 * P_dot_k2 + 2.0 * P_dot_k3 + P_dot_k4); - P_big = 0.5 * (P_big + P_big.transpose()); - - // Calculate the state transition from time k to tau - Eigen::Matrix Phi = - Eigen::Matrix::Identity() + (delta_t / 6.0) * (Phi_dot_k1 + 2.0 * Phi_dot_k2 + 2.0 * Phi_dot_k3 + Phi_dot_k4); - - //========================================================================== - // CLONE TO NEW SAMPLE TIME AND MARGINALIZE OLD SAMPLE TIME - //========================================================================== - - // Matrix that performs the clone and mariginalization - Eigen::Matrix B_k = Eigen::Matrix::Identity(); - B_k.block(15, 15, 3, 3).setZero(); - B_k.block(15, 0, 3, 3) = Eigen::Matrix::Identity(); - - // Change our measurement covariance and state transition - P_big = B_k * P_big * B_k.transpose(); - P_big = 0.5 * (P_big + P_big.transpose()); - Discrete_J_b = B_k * Phi * Discrete_J_b; - - // Our measurement covariance is the top 15x15 of our large covariance - P_meas = P_big.block(0, 0, 15, 15); - - // If we are using the state transition Jacobian, then we should overwrite the analytical versions - // Note: we flip the sign for J_q to match the Model 1 derivation - if (state_transition_jacobians) { - J_q = -Discrete_J_b.block(0, 3, 3, 3); - J_a = Discrete_J_b.block(12, 3, 3, 3); - J_b = Discrete_J_b.block(6, 3, 3, 3); - H_a = Discrete_J_b.block(12, 9, 3, 3); - H_b = Discrete_J_b.block(6, 9, 3, 3); - O_a = Discrete_J_b.block(12, 18, 3, 3); - O_b = Discrete_J_b.block(6, 18, 3, 3); - } - - // Update rotation mean - // Note we had to wait to do this, since we use the old orientation in our covariance calculation - R_k2tau = R_k2tau1; - q_k2tau = rot_2_quat(R_k2tau); - } + Eigen::Matrix a_m_1 = Eigen::Matrix::Zero()); }; } // namespace ov_core diff --git a/ov_core/src/feat/FeatureDatabase.cpp b/ov_core/src/feat/FeatureDatabase.cpp new file mode 100644 index 000000000..5cb8de5ce --- /dev/null +++ b/ov_core/src/feat/FeatureDatabase.cpp @@ -0,0 +1,321 @@ +/* + * OpenVINS: An Open Platform for Visual-Inertial Research + * Copyright (C) 2018-2022 Patrick Geneva + * Copyright (C) 2018-2022 Guoquan Huang + * Copyright (C) 2018-2022 OpenVINS Contributors + * Copyright (C) 2018-2019 Kevin Eckenhoff + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include "FeatureDatabase.h" + +#include "Feature.h" +#include "utils/print.h" + +using namespace ov_core; + +std::shared_ptr FeatureDatabase::get_feature(size_t id, bool remove) { + std::lock_guard lck(mtx); + if (features_idlookup.find(id) != features_idlookup.end()) { + std::shared_ptr temp = features_idlookup.at(id); + if (remove) + features_idlookup.erase(id); + return temp; + } else { + return nullptr; + } +} + +bool FeatureDatabase::get_feature_clone(size_t id, Feature &feat) { + std::lock_guard lck(mtx); + if (features_idlookup.find(id) == features_idlookup.end()) + return false; + // TODO: should probably have a copy constructor function in feature class + std::shared_ptr temp = features_idlookup.at(id); + feat.featid = temp->featid; + feat.to_delete = temp->to_delete; + feat.uvs = temp->uvs; + feat.uvs_norm = temp->uvs_norm; + feat.timestamps = temp->timestamps; + feat.anchor_cam_id = temp->anchor_cam_id; + feat.anchor_clone_timestamp = temp->anchor_clone_timestamp; + feat.p_FinA = temp->p_FinA; + feat.p_FinG = temp->p_FinG; + return true; +} + +void FeatureDatabase::update_feature(size_t id, double timestamp, size_t cam_id, float u, float v, float u_n, float v_n) { + + // Find this feature using the ID lookup + std::lock_guard lck(mtx); + if (features_idlookup.find(id) != features_idlookup.end()) { + // Get our feature + std::shared_ptr feat = features_idlookup.at(id); + // Append this new information to it! + feat->uvs[cam_id].push_back(Eigen::Vector2f(u, v)); + feat->uvs_norm[cam_id].push_back(Eigen::Vector2f(u_n, v_n)); + feat->timestamps[cam_id].push_back(timestamp); + return; + } + + // Debug info + // PRINT_DEBUG("featdb - adding new feature %d",(int)id); + + // Else we have not found the feature, so lets make it be a new one! + std::shared_ptr feat = std::make_shared(); + feat->featid = id; + feat->uvs[cam_id].push_back(Eigen::Vector2f(u, v)); + feat->uvs_norm[cam_id].push_back(Eigen::Vector2f(u_n, v_n)); + feat->timestamps[cam_id].push_back(timestamp); + + // Append this new feature into our database + features_idlookup[id] = feat; +} + +std::vector> FeatureDatabase::features_not_containing_newer(double timestamp, bool remove, bool skip_deleted) { + + // Our vector of features that do not have measurements after the specified time + std::vector> feats_old; + + // Now lets loop through all features, and just make sure they are not old + std::lock_guard lck(mtx); + for (auto it = features_idlookup.begin(); it != features_idlookup.end();) { + // Skip if already deleted + if (skip_deleted && (*it).second->to_delete) { + it++; + continue; + } + // Loop through each camera + // If we have a measurement greater-than or equal to the specified, this measurement is find + bool has_newer_measurement = false; + for (auto const &pair : (*it).second->timestamps) { + has_newer_measurement = (!pair.second.empty() && pair.second.at(pair.second.size() - 1) >= timestamp); + if (has_newer_measurement) { + break; + } + } + // If it is not being actively tracked, then it is old + if (!has_newer_measurement) { + feats_old.push_back((*it).second); + if (remove) + features_idlookup.erase(it++); + else + it++; + } else { + it++; + } + } + + // Debugging + // PRINT_DEBUG("feature db size = %u\n", features_idlookup.size()) + + // Return the old features + return feats_old; +} + +std::vector> FeatureDatabase::features_containing_older(double timestamp, bool remove, bool skip_deleted) { + + // Our vector of old features + std::vector> feats_old; + + // Now lets loop through all features, and just make sure they are not old + std::lock_guard lck(mtx); + for (auto it = features_idlookup.begin(); it != features_idlookup.end();) { + // Skip if already deleted + if (skip_deleted && (*it).second->to_delete) { + it++; + continue; + } + // Loop through each camera + // Check if we have at least one time older then the requested + bool found_containing_older = false; + for (auto const &pair : (*it).second->timestamps) { + found_containing_older = (!pair.second.empty() && pair.second.at(0) < timestamp); + if (found_containing_older) { + break; + } + } + // If it has an older timestamp, then add it + if (found_containing_older) { + feats_old.push_back((*it).second); + if (remove) + features_idlookup.erase(it++); + else + it++; + } else { + it++; + } + } + + // Debugging + // PRINT_DEBUG("feature db size = %u\n", features_idlookup.size()) + + // Return the old features + return feats_old; +} + +std::vector> FeatureDatabase::features_containing(double timestamp, bool remove, bool skip_deleted) { + + // Our vector of old features + std::vector> feats_has_timestamp; + + // Now lets loop through all features, and just make sure they are not + std::lock_guard lck(mtx); + for (auto it = features_idlookup.begin(); it != features_idlookup.end();) { + // Skip if already deleted + if (skip_deleted && (*it).second->to_delete) { + it++; + continue; + } + // Boolean if it has the timestamp + // Break out if we found a single timestamp that is equal to the specified time + bool has_timestamp = false; + for (auto const &pair : (*it).second->timestamps) { + has_timestamp = (std::find(pair.second.begin(), pair.second.end(), timestamp) != pair.second.end()); + if (has_timestamp) { + break; + } + } + // Remove this feature if it contains the specified timestamp + if (has_timestamp) { + feats_has_timestamp.push_back((*it).second); + if (remove) + features_idlookup.erase(it++); + else + it++; + } else { + it++; + } + } + + // Debugging + // PRINT_DEBUG("feature db size = %u\n", features_idlookup.size()) + // PRINT_DEBUG("return vector = %u\n", feats_has_timestamp.size()) + + // Return the features + return feats_has_timestamp; +} + +void FeatureDatabase::cleanup() { + // Loop through all features + // int sizebefore = (int)features_idlookup.size(); + std::lock_guard lck(mtx); + for (auto it = features_idlookup.begin(); it != features_idlookup.end();) { + // If delete flag is set, then delete it + if ((*it).second->to_delete) { + features_idlookup.erase(it++); + } else { + it++; + } + } + // PRINT_DEBUG("feat db = %d -> %d\n", sizebefore, (int)features_idlookup.size() << std::endl; +} + +void FeatureDatabase::cleanup_measurements(double timestamp) { + std::lock_guard lck(mtx); + for (auto it = features_idlookup.begin(); it != features_idlookup.end();) { + // Remove the older measurements + (*it).second->clean_older_measurements(timestamp); + // Count how many measurements + int ct_meas = 0; + for (const auto &pair : (*it).second->timestamps) { + ct_meas += (int)(pair.second.size()); + } + // If delete flag is set, then delete it + if (ct_meas < 1) { + features_idlookup.erase(it++); + } else { + it++; + } + } +} + +void FeatureDatabase::cleanup_measurements_exact(double timestamp) { + std::lock_guard lck(mtx); + std::vector timestamps = {timestamp}; + for (auto it = features_idlookup.begin(); it != features_idlookup.end();) { + // Remove the older measurements + (*it).second->clean_invalid_measurements(timestamps); + // Count how many measurements + int ct_meas = 0; + for (const auto &pair : (*it).second->timestamps) { + ct_meas += (int)(pair.second.size()); + } + // If delete flag is set, then delete it + if (ct_meas < 1) { + features_idlookup.erase(it++); + } else { + it++; + } + } +} + +double FeatureDatabase::get_oldest_timestamp() { + std::lock_guard lck(mtx); + double oldest_time = -1; + for (auto const &feat : features_idlookup) { + for (auto const &camtimepair : feat.second->timestamps) { + if (!camtimepair.second.empty() && (oldest_time == -1 || oldest_time < camtimepair.second.at(0))) { + oldest_time = camtimepair.second.at(0); + } + } + } + return oldest_time; +} + +void FeatureDatabase::append_new_measurements(const std::shared_ptr &database) { + std::lock_guard lck(mtx); + + // Loop through the other database's internal database + // int sizebefore = (int)features_idlookup.size(); + for (const auto &feat : database->get_internal_data()) { + if (features_idlookup.find(feat.first) != features_idlookup.end()) { + + // For this feature, now try to append the new measurement data + std::shared_ptr temp = features_idlookup.at(feat.first); + for (const auto × : feat.second->timestamps) { + // Append the whole camera vector is not seen + // Otherwise need to loop through each and append + size_t cam_id = times.first; + if (temp->timestamps.find(cam_id) == temp->timestamps.end()) { + temp->timestamps[cam_id] = feat.second->timestamps.at(cam_id); + temp->uvs[cam_id] = feat.second->uvs.at(cam_id); + temp->uvs_norm[cam_id] = feat.second->uvs_norm.at(cam_id); + } else { + auto temp_times = temp->timestamps.at(cam_id); + for (size_t i = 0; i < feat.second->timestamps.at(cam_id).size(); i++) { + double time_to_find = feat.second->timestamps.at(cam_id).at(i); + if (std::find(temp_times.begin(), temp_times.end(), time_to_find) == temp_times.end()) { + temp->timestamps.at(cam_id).push_back(feat.second->timestamps.at(cam_id).at(i)); + temp->uvs.at(cam_id).push_back(feat.second->uvs.at(cam_id).at(i)); + temp->uvs_norm.at(cam_id).push_back(feat.second->uvs_norm.at(cam_id).at(i)); + } + } + } + } + + } else { + + // Else we have not found the feature, so lets make it be a new one! + std::shared_ptr temp = std::make_shared(); + temp->featid = feat.second->featid; + temp->timestamps = feat.second->timestamps; + temp->uvs = feat.second->uvs; + temp->uvs_norm = feat.second->uvs_norm; + features_idlookup[feat.first] = temp; + } + } + // PRINT_DEBUG("feat db = %d -> %d\n", sizebefore, (int)features_idlookup.size() << std::endl; +} diff --git a/ov_core/src/feat/FeatureDatabase.h b/ov_core/src/feat/FeatureDatabase.h index 46c65fcf0..7eae4de5c 100644 --- a/ov_core/src/feat/FeatureDatabase.h +++ b/ov_core/src/feat/FeatureDatabase.h @@ -25,13 +25,13 @@ #include #include #include +#include #include -#include "Feature.h" -#include "utils/print.h" - namespace ov_core { +class Feature; + /** * @brief Database containing features we are currently tracking. * @@ -65,17 +65,15 @@ class FeatureDatabase { * @param remove Set to true if you want to remove the feature from the database (you will need to handle the freeing of memory) * @return Either a feature object, or null if it is not in the database. */ - std::shared_ptr get_feature(size_t id, bool remove = false) { - std::lock_guard lck(mtx); - if (features_idlookup.find(id) != features_idlookup.end()) { - std::shared_ptr temp = features_idlookup.at(id); - if (remove) - features_idlookup.erase(id); - return temp; - } else { - return nullptr; - } - } + std::shared_ptr get_feature(size_t id, bool remove = false); + + /** + * @brief Get a specified feature clone (pointer is thread safe) + * @param id What feature we want to get + * @param feat Feature with data in it + * @return True if the feature was found + */ + bool get_feature_clone(size_t id, Feature &feat); /** * @brief Update a feature object @@ -90,33 +88,7 @@ class FeatureDatabase { * This will update a given feature based on the passed ID it has. * It will create a new feature, if it is an ID that we have not seen before. */ - void update_feature(size_t id, double timestamp, size_t cam_id, float u, float v, float u_n, float v_n) { - - // Find this feature using the ID lookup - std::lock_guard lck(mtx); - if (features_idlookup.find(id) != features_idlookup.end()) { - // Get our feature - std::shared_ptr feat = features_idlookup.at(id); - // Append this new information to it! - feat->uvs[cam_id].push_back(Eigen::Vector2f(u, v)); - feat->uvs_norm[cam_id].push_back(Eigen::Vector2f(u_n, v_n)); - feat->timestamps[cam_id].push_back(timestamp); - return; - } - - // Debug info - // PRINT_DEBUG("featdb - adding new feature %d",(int)id); - - // Else we have not found the feature, so lets make it be a new one! - std::shared_ptr feat = std::make_shared(); - feat->featid = id; - feat->uvs[cam_id].push_back(Eigen::Vector2f(u, v)); - feat->uvs_norm[cam_id].push_back(Eigen::Vector2f(u_n, v_n)); - feat->timestamps[cam_id].push_back(timestamp); - - // Append this new feature into our database - features_idlookup[id] = feat; - } + void update_feature(size_t id, double timestamp, size_t cam_id, float u, float v, float u_n, float v_n); /** * @brief Get features that do not have newer measurement then the specified time. @@ -125,46 +97,7 @@ class FeatureDatabase { * For example this could be used to get features that have not been successfully tracked into the newest frame. * All features returned will not have any measurements occurring at a time greater then the specified. */ - std::vector> features_not_containing_newer(double timestamp, bool remove = false, bool skip_deleted = false) { - - // Our vector of features that do not have measurements after the specified time - std::vector> feats_old; - - // Now lets loop through all features, and just make sure they are not old - std::lock_guard lck(mtx); - for (auto it = features_idlookup.begin(); it != features_idlookup.end();) { - // Skip if already deleted - if (skip_deleted && (*it).second->to_delete) { - it++; - continue; - } - // Loop through each camera - // If we have a measurement greater-than or equal to the specified, this measurement is find - bool has_newer_measurement = false; - for (auto const &pair : (*it).second->timestamps) { - has_newer_measurement = (!pair.second.empty() && pair.second.at(pair.second.size() - 1) >= timestamp); - if (has_newer_measurement) { - break; - } - } - // If it is not being actively tracked, then it is old - if (!has_newer_measurement) { - feats_old.push_back((*it).second); - if (remove) - features_idlookup.erase(it++); - else - it++; - } else { - it++; - } - } - - // Debugging - // PRINT_DEBUG("feature db size = %u\n", features_idlookup.size()) - - // Return the old features - return feats_old; - } + std::vector> features_not_containing_newer(double timestamp, bool remove = false, bool skip_deleted = false); /** * @brief Get features that has measurements older then the specified time. @@ -172,46 +105,7 @@ class FeatureDatabase { * This will collect all features that have measurements occurring before the specified timestamp. * For example, we would want to remove all features older then the last clone/state in our sliding window. */ - std::vector> features_containing_older(double timestamp, bool remove = false, bool skip_deleted = false) { - - // Our vector of old features - std::vector> feats_old; - - // Now lets loop through all features, and just make sure they are not old - std::lock_guard lck(mtx); - for (auto it = features_idlookup.begin(); it != features_idlookup.end();) { - // Skip if already deleted - if (skip_deleted && (*it).second->to_delete) { - it++; - continue; - } - // Loop through each camera - // Check if we have at least one time older then the requested - bool found_containing_older = false; - for (auto const &pair : (*it).second->timestamps) { - found_containing_older = (!pair.second.empty() && pair.second.at(0) < timestamp); - if (found_containing_older) { - break; - } - } - // If it has an older timestamp, then add it - if (found_containing_older) { - feats_old.push_back((*it).second); - if (remove) - features_idlookup.erase(it++); - else - it++; - } else { - it++; - } - } - - // Debugging - // PRINT_DEBUG("feature db size = %u\n", features_idlookup.size()) - - // Return the old features - return feats_old; - } + std::vector> features_containing_older(double timestamp, bool remove = false, bool skip_deleted = false); /** * @brief Get features that has measurements at the specified time. @@ -219,112 +113,24 @@ class FeatureDatabase { * This function will return all features that have the specified time in them. * This would be used to get all features that occurred at a specific clone/state. */ - std::vector> features_containing(double timestamp, bool remove = false, bool skip_deleted = false) { - - // Our vector of old features - std::vector> feats_has_timestamp; - - // Now lets loop through all features, and just make sure they are not - std::lock_guard lck(mtx); - for (auto it = features_idlookup.begin(); it != features_idlookup.end();) { - // Skip if already deleted - if (skip_deleted && (*it).second->to_delete) { - it++; - continue; - } - // Boolean if it has the timestamp - // Break out if we found a single timestamp that is equal to the specified time - bool has_timestamp = false; - for (auto const &pair : (*it).second->timestamps) { - has_timestamp = (std::find(pair.second.begin(), pair.second.end(), timestamp) != pair.second.end()); - if (has_timestamp) { - break; - } - } - // Remove this feature if it contains the specified timestamp - if (has_timestamp) { - feats_has_timestamp.push_back((*it).second); - if (remove) - features_idlookup.erase(it++); - else - it++; - } else { - it++; - } - } - - // Debugging - // PRINT_DEBUG("feature db size = %u\n", features_idlookup.size()) - // PRINT_DEBUG("return vector = %u\n", feats_has_timestamp.size()) - - // Return the features - return feats_has_timestamp; - } + std::vector> features_containing(double timestamp, bool remove = false, bool skip_deleted = false); /** * @brief This function will delete all features that have been used up. * * If a feature was unable to be used, it will still remain since it will not have a delete flag set */ - void cleanup() { - // Loop through all features - // int sizebefore = (int)features_idlookup.size(); - std::lock_guard lck(mtx); - for (auto it = features_idlookup.begin(); it != features_idlookup.end();) { - // If delete flag is set, then delete it - if ((*it).second->to_delete) { - features_idlookup.erase(it++); - } else { - it++; - } - } - // PRINT_DEBUG("feat db = %d -> %d\n", sizebefore, (int)features_idlookup.size() << std::endl; - } + void cleanup(); /** * @brief This function will delete all feature measurements that are older then the specified timestamp */ - void cleanup_measurements(double timestamp) { - std::lock_guard lck(mtx); - for (auto it = features_idlookup.begin(); it != features_idlookup.end();) { - // Remove the older measurements - (*it).second->clean_older_measurements(timestamp); - // Count how many measurements - int ct_meas = 0; - for (const auto &pair : (*it).second->timestamps) { - ct_meas += (int)(pair.second.size()); - } - // If delete flag is set, then delete it - if (ct_meas < 1) { - features_idlookup.erase(it++); - } else { - it++; - } - } - } + void cleanup_measurements(double timestamp); /** * @brief This function will delete all feature measurements that are at the specified timestamp */ - void cleanup_measurements_exact(double timestamp) { - std::lock_guard lck(mtx); - std::vector timestamps = {timestamp}; - for (auto it = features_idlookup.begin(); it != features_idlookup.end();) { - // Remove the older measurements - (*it).second->clean_invalid_measurements(timestamps); - // Count how many measurements - int ct_meas = 0; - for (const auto &pair : (*it).second->timestamps) { - ct_meas += (int)(pair.second.size()); - } - // If delete flag is set, then delete it - if (ct_meas < 1) { - features_idlookup.erase(it++); - } else { - it++; - } - } - } + void cleanup_measurements_exact(double timestamp); /** * @brief Returns the size of the feature database @@ -345,66 +151,12 @@ class FeatureDatabase { /** * @brief Gets the oldest time in the database */ - double get_oldest_timestamp() { - std::lock_guard lck(mtx); - double oldest_time = -1; - for (auto const &feat : features_idlookup) { - for (auto const &camtimepair : feat.second->timestamps) { - if (!camtimepair.second.empty() && (oldest_time == -1 || oldest_time < camtimepair.second.at(0))) { - oldest_time = camtimepair.second.at(0); - } - } - } - return oldest_time; - } + double get_oldest_timestamp(); /** * @brief Will update the passed database with this database's latest feature information. */ - void append_new_measurements(const std::shared_ptr &database) { - std::lock_guard lck(mtx); - - // Loop through the other database's internal database - // int sizebefore = (int)features_idlookup.size(); - for (const auto &feat : database->get_internal_data()) { - if (features_idlookup.find(feat.first) != features_idlookup.end()) { - - // For this feature, now try to append the new measurement data - std::shared_ptr temp = features_idlookup.at(feat.first); - for (const auto × : feat.second->timestamps) { - // Append the whole camera vector is not seen - // Otherwise need to loop through each and append - size_t cam_id = times.first; - if (temp->timestamps.find(cam_id) == temp->timestamps.end()) { - temp->timestamps[cam_id] = feat.second->timestamps.at(cam_id); - temp->uvs[cam_id] = feat.second->uvs.at(cam_id); - temp->uvs_norm[cam_id] = feat.second->uvs_norm.at(cam_id); - } else { - auto temp_times = temp->timestamps.at(cam_id); - for (size_t i = 0; i < feat.second->timestamps.at(cam_id).size(); i++) { - double time_to_find = feat.second->timestamps.at(cam_id).at(i); - if (std::find(temp_times.begin(), temp_times.end(), time_to_find) == temp_times.end()) { - temp->timestamps.at(cam_id).push_back(feat.second->timestamps.at(cam_id).at(i)); - temp->uvs.at(cam_id).push_back(feat.second->uvs.at(cam_id).at(i)); - temp->uvs_norm.at(cam_id).push_back(feat.second->uvs_norm.at(cam_id).at(i)); - } - } - } - } - - } else { - - // Else we have not found the feature, so lets make it be a new one! - std::shared_ptr temp = std::make_shared(); - temp->featid = feat.second->featid; - temp->timestamps = feat.second->timestamps; - temp->uvs = feat.second->uvs; - temp->uvs_norm = feat.second->uvs_norm; - features_idlookup[feat.first] = temp; - } - } - // PRINT_DEBUG("feat db = %d -> %d\n", sizebefore, (int)features_idlookup.size() << std::endl; - } + void append_new_measurements(const std::shared_ptr &database); protected: /// Mutex lock for our map diff --git a/ov_core/src/feat/FeatureHelper.h b/ov_core/src/feat/FeatureHelper.h index 1ca9a5c75..6e08aa0d9 100644 --- a/ov_core/src/feat/FeatureHelper.h +++ b/ov_core/src/feat/FeatureHelper.h @@ -117,8 +117,11 @@ class FeatureHelper { * @param disp_mean Average raw disparity * @param disp_var Variance of the disparities * @param total_feats Total number of common features + * @param newest_time Only compute disparity for ones older (-1 to disable) + * @param oldest_time Only compute disparity for ones newer (-1 to disable) */ - static void compute_disparity(std::shared_ptr db, double &disp_mean, double &disp_var, int &total_feats) { + static void compute_disparity(std::shared_ptr db, double &disp_mean, double &disp_var, int &total_feats, + double newest_time = 1, double oldest_time = 1) { // Compute the disparity std::vector disparities; @@ -129,10 +132,29 @@ class FeatureHelper { if (campairs.second.size() < 2) continue; - // Now lets calculate the disparity + // Now lets calculate the disparity (assumes time array is monotonic) size_t camid = campairs.first; - Eigen::Vector2f uv0 = feat.second->uvs.at(camid).at(0).block(0, 0, 2, 1); - Eigen::Vector2f uv1 = feat.second->uvs.at(camid).at(campairs.second.size() - 1).block(0, 0, 2, 1); + bool found0 = false; + bool found1 = false; + Eigen::Vector2f uv0 = Eigen::Vector2f::Zero(); + Eigen::Vector2f uv1 = Eigen::Vector2f::Zero(); + for (size_t idx = 0; idx < feat.second->timestamps.at(camid).size(); idx++) { + double time = feat.second->timestamps.at(camid).at(idx); + if ((oldest_time == -1 || time > oldest_time) && !found0) { + uv0 = feat.second->uvs.at(camid).at(idx).block(0, 0, 2, 1); + found0 = true; + continue; + } + if ((newest_time == -1 || time < newest_time) && found0) { + uv1 = feat.second->uvs.at(camid).at(idx).block(0, 0, 2, 1); + found1 = true; + continue; + } + } + + // If we found both an old and a new time, then we are good! + if (!found0 || !found1) + continue; disparities.push_back((uv1 - uv0).norm()); } } diff --git a/ov_core/src/feat/FeatureInitializer.cpp b/ov_core/src/feat/FeatureInitializer.cpp index d9b58d92d..a30c1405e 100644 --- a/ov_core/src/feat/FeatureInitializer.cpp +++ b/ov_core/src/feat/FeatureInitializer.cpp @@ -21,9 +21,14 @@ #include "FeatureInitializer.h" +#include "Feature.h" +#include "utils/print.h" +#include "utils/quat_ops.h" + using namespace ov_core; -bool FeatureInitializer::single_triangulation(Feature *feat, std::unordered_map> &clonesCAM) { +bool FeatureInitializer::single_triangulation(std::shared_ptr feat, + std::unordered_map> &clonesCAM) { // Total number of measurements // Also set the first measurement to be the anchor frame @@ -83,7 +88,7 @@ bool FeatureInitializer::single_triangulation(Feature *feat, std::unordered_map< Eigen::MatrixXd p_f = A.colPivHouseholderQr().solve(b); // Check A and p_f - Eigen::JacobiSVD svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV); + Eigen::JacobiSVD svd(A); Eigen::MatrixXd singularValues; singularValues.resize(svd.singularValues().rows(), 1); singularValues = svd.singularValues(); @@ -106,7 +111,7 @@ bool FeatureInitializer::single_triangulation(Feature *feat, std::unordered_map< return true; } -bool FeatureInitializer::single_triangulation_1d(Feature *feat, +bool FeatureInitializer::single_triangulation_1d(std::shared_ptr feat, std::unordered_map> &clonesCAM) { // Total number of measurements @@ -189,7 +194,8 @@ bool FeatureInitializer::single_triangulation_1d(Feature *feat, return true; } -bool FeatureInitializer::single_gaussnewton(Feature *feat, std::unordered_map> &clonesCAM) { +bool FeatureInitializer::single_gaussnewton(std::shared_ptr feat, + std::unordered_map> &clonesCAM) { // Get into inverse depth double rho = 1 / feat->p_FinA(2); @@ -368,8 +374,8 @@ bool FeatureInitializer::single_gaussnewton(Feature *feat, std::unordered_map> &clonesCAM, Feature *feat, - double alpha, double beta, double rho) { +double FeatureInitializer::compute_error(std::unordered_map> &clonesCAM, + std::shared_ptr feat, double alpha, double beta, double rho) { // Total error double err = 0; @@ -414,4 +420,4 @@ double FeatureInitializer::compute_error(std::unordered_map -#include "Feature.h" #include "FeatureInitializerOptions.h" -#include "utils/print.h" -#include "utils/quat_ops.h" namespace ov_core { +class Feature; + /** * @brief Class that triangulates feature * @@ -98,7 +97,7 @@ class FeatureInitializer { * in global frame) * @return Returns false if it fails to triangulate (based on the thresholds) */ - bool single_triangulation(Feature *feat, std::unordered_map> &clonesCAM); + bool single_triangulation(std::shared_ptr feat, std::unordered_map> &clonesCAM); /** * @brief Uses a linear triangulation to get initial estimate for the feature, treating the anchor observation as a true bearing. @@ -111,7 +110,7 @@ class FeatureInitializer { * in global frame) * @return Returns false if it fails to triangulate (based on the thresholds) */ - bool single_triangulation_1d(Feature *feat, std::unordered_map> &clonesCAM); + bool single_triangulation_1d(std::shared_ptr feat, std::unordered_map> &clonesCAM); /** * @brief Uses a nonlinear triangulation to refine initial linear estimate of the feature @@ -120,7 +119,7 @@ class FeatureInitializer { * in global frame) * @return Returns false if it fails to be optimize (based on the thresholds) */ - bool single_gaussnewton(Feature *feat, std::unordered_map> &clonesCAM); + bool single_gaussnewton(std::shared_ptr feat, std::unordered_map> &clonesCAM); /** * @brief Gets the current configuration of the feature initializer @@ -140,8 +139,8 @@ class FeatureInitializer { * @param beta y/z in anchor * @param rho 1/z inverse depth */ - double compute_error(std::unordered_map> &clonesCAM, Feature *feat, double alpha, - double beta, double rho); + double compute_error(std::unordered_map> &clonesCAM, std::shared_ptr feat, + double alpha, double beta, double rho); }; } // namespace ov_core diff --git a/ov_core/src/test_tracking.cpp b/ov_core/src/test_tracking.cpp index f0fe5c9b0..566c1c025 100644 --- a/ov_core/src/test_tracking.cpp +++ b/ov_core/src/test_tracking.cpp @@ -35,6 +35,8 @@ #include #include "cam/CamRadtan.h" +#include "feat/Feature.h" +#include "feat/FeatureDatabase.h" #include "track/TrackAruco.h" #include "track/TrackDescriptor.h" #include "track/TrackKLT.h" @@ -56,6 +58,9 @@ int clone_states = 10; std::deque clonetimes; ros::Time time_start; +// How many cameras we will do visual tracking on (mono=1, stereo=2) +int max_cameras = 2; + // Our master function for tracking void handle_stereo(double time0, double time1, cv::Mat img0, cv::Mat img1); @@ -78,7 +83,7 @@ int main(int argc, char **argv) { parser->set_node_handler(nh); // Verbosity - std::string verbosity = "INFO"; + std::string verbosity = "DEBUG"; parser->parse_config("verbosity", verbosity); ov_core::Printer::setPrintLevel(verbosity); @@ -91,7 +96,7 @@ int main(int argc, char **argv) { // Location of the ROS bag we want to read in std::string path_to_bag; - nh->param("path_bag", path_to_bag, "/home/patrick/datasets/eth/V1_01_easy.bag"); + nh->param("path_bag", path_to_bag, "/home/patrick/datasets/euroc_mav/V1_01_easy.bag"); // nh->param("path_bag", path_to_bag, "/home/patrick/datasets/open_vins/aruco_room_01.bag"); PRINT_INFO("ros bag path is: %s\n", path_to_bag.c_str()); @@ -105,16 +110,21 @@ int main(int argc, char **argv) { //=================================================================================== //=================================================================================== + // This will globally set the thread count we will use + // -1 will reset to the system default threading (usually the num of cores) + cv::setNumThreads(4); + // Parameters for our extractor - int num_pts = 400; + int num_pts = 200; int num_aruco = 1024; - int fast_threshold = 15; - int grid_x = 9; - int grid_y = 7; + int fast_threshold = 20; + int grid_x = 5; + int grid_y = 3; int min_px_dist = 10; double knn_ratio = 0.70; bool do_downsizing = false; bool use_stereo = false; + parser->parse_config("max_cameras", max_cameras, false); parser->parse_config("num_pts", num_pts, false); parser->parse_config("num_aruco", num_aruco, false); parser->parse_config("clone_states", clone_states, false); @@ -145,6 +155,7 @@ int main(int argc, char **argv) { } // Debug print! + PRINT_DEBUG("max cameras: %d\n", max_cameras); PRINT_DEBUG("max features: %d\n", num_pts); PRINT_DEBUG("max aruco: %d\n", num_aruco); PRINT_DEBUG("clone states: %d\n", clone_states); @@ -152,6 +163,7 @@ int main(int argc, char **argv) { PRINT_DEBUG("fast threshold: %d\n", fast_threshold); PRINT_DEBUG("min pixel distance: %d\n", min_px_dist); PRINT_DEBUG("downsize aruco image: %d\n", do_downsizing); + PRINT_DEBUG("stereo tracking: %d\n", use_stereo); // Fake camera info (we don't need this, as we are not using the normalized coordinates for anything) std::unordered_map> cameras; @@ -164,10 +176,10 @@ int main(int argc, char **argv) { } // Lets make a feature extractor - extractor = new TrackKLT(cameras, num_pts, num_aruco, !use_stereo, method, fast_threshold, grid_x, grid_y, min_px_dist); - // extractor = new TrackDescriptor(cameras, num_pts, num_aruco, !use_stereo, method, fast_threshold, grid_x, grid_y, min_px_dist, + extractor = new TrackKLT(cameras, num_pts, num_aruco, use_stereo, method, fast_threshold, grid_x, grid_y, min_px_dist); + // extractor = new TrackDescriptor(cameras, num_pts, num_aruco, use_stereo, method, fast_threshold, grid_x, grid_y, min_px_dist, // knn_ratio); - // extractor = new TrackAruco(cameras, num_aruco, !use_stereo, method, do_downsizing); + // extractor = new TrackAruco(cameras, num_aruco, use_stereo, method, do_downsizing); //=================================================================================== //=================================================================================== @@ -293,11 +305,13 @@ void handle_stereo(double time0, double time1, cv::Mat img0, cv::Mat img1) { ov_core::CameraData message; message.timestamp = time0; message.sensor_ids.push_back(0); - message.sensor_ids.push_back(1); message.images.push_back(img0); - message.images.push_back(img1); - message.masks.push_back(mask); message.masks.push_back(mask); + if (max_cameras == 2) { + message.sensor_ids.push_back(1); + message.images.push_back(img1); + message.masks.push_back(mask); + } extractor->feed_new_camera(message); // Display the resulting tracks diff --git a/ov_core/src/test_webcam.cpp b/ov_core/src/test_webcam.cpp index 7c2693925..7611cb80d 100644 --- a/ov_core/src/test_webcam.cpp +++ b/ov_core/src/test_webcam.cpp @@ -20,6 +20,7 @@ */ #include +#include #include #include #include @@ -34,6 +35,8 @@ #include #include "cam/CamRadtan.h" +#include "feat/Feature.h" +#include "feat/FeatureDatabase.h" #include "track/TrackAruco.h" #include "track/TrackDescriptor.h" #include "track/TrackKLT.h" @@ -49,6 +52,9 @@ using namespace ov_core; // Our feature extractor TrackBase *extractor; +// Define the function to be called when ctrl-c (SIGINT) is sent to process +void signal_callback_handler(int signum) { std::exit(signum); } + // Main function int main(int argc, char **argv) { @@ -158,7 +164,12 @@ int main(int argc, char **argv) { // Loop forever until we break out double current_time = 0.0; std::deque clonetimes; + signal(SIGINT, signal_callback_handler); +#if ROS_AVAILABLE == 1 + while (ros::ok()) { +#else while (true) { +#endif // Get the next frame (and fake advance time forward) cv::Mat frame; diff --git a/ov_core/src/track/Grider_GRID.h b/ov_core/src/track/Grider_GRID.h new file mode 100644 index 000000000..0e4f7ff7e --- /dev/null +++ b/ov_core/src/track/Grider_GRID.h @@ -0,0 +1,185 @@ +/* + * OpenVINS: An Open Platform for Visual-Inertial Research + * Copyright (C) 2018-2022 Patrick Geneva + * Copyright (C) 2018-2022 Guoquan Huang + * Copyright (C) 2018-2022 OpenVINS Contributors + * Copyright (C) 2018-2019 Kevin Eckenhoff + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#ifndef OV_CORE_GRIDER_GRID_H +#define OV_CORE_GRIDER_GRID_H + +#include +#include +#include +#include + +#include +#include +#include + +#include "utils/opencv_lambda_body.h" + +namespace ov_core { + +/** + * @brief Extracts FAST features in a grid pattern. + * + * As compared to just extracting fast features over the entire image, + * we want to have as uniform of extractions as possible over the image plane. + * Thus we split the image into a bunch of small grids, and extract points in each. + * We then pick enough top points in each grid so that we have the total number of desired points. + */ +class Grider_GRID { + +public: + /** + * @brief Compare keypoints based on their response value. + * @param first First keypoint + * @param second Second keypoint + * + * We want to have the keypoints with the highest values! + * See: https://stackoverflow.com/a/10910921 + */ + static bool compare_response(cv::KeyPoint first, cv::KeyPoint second) { return first.response > second.response; } + + /** + * @brief This function will perform grid extraction using FAST. + * @param img Image we will do FAST extraction on + * @param mask Region of the image we do not want to extract features in (255 = do not detect features) + * @param valid_locs Valid 2d grid locations we will extract in (instead of the whole image) + * @param pts vector of extracted points we will return + * @param num_features max number of features we want to extract + * @param grid_x size of grid in the x-direction / u-direction + * @param grid_y size of grid in the y-direction / v-direction + * @param threshold FAST threshold paramter (10 is a good value normally) + * @param nonmaxSuppression if FAST should perform non-max suppression (true normally) + * + * Given a specified grid size, this will try to extract fast features from each grid. + * It will then return the best from each grid in the return vector. + */ + static void perform_griding(const cv::Mat &img, const cv::Mat &mask, const std::vector> &valid_locs, + std::vector &pts, int num_features, int grid_x, int grid_y, int threshold, + bool nonmaxSuppression) { + + // Return if there is nothing to extract + if (valid_locs.empty()) + return; + + // We want to have equally distributed features + // NOTE: If we have more grids than number of total points, we calc the biggest grid we can do + // NOTE: Thus if we extract 1 point per grid we have + // NOTE: -> 1 = num_features / (grid_x * grid_y) + // NOTE: -> grid_x = ratio * grid_y (keep the original grid ratio) + // NOTE: -> grid_y = sqrt(num_features / ratio) + if (num_features < grid_x * grid_y) { + double ratio = (double)grid_x / (double)grid_y; + grid_y = std::ceil(std::sqrt(num_features / ratio)); + grid_x = std::ceil(grid_y * ratio); + } + int num_features_grid = (int)((double)num_features / (double)(grid_x * grid_y)) + 1; + assert(grid_x > 0); + assert(grid_y > 0); + assert(num_features_grid > 0); + + // Calculate the size our extraction boxes should be + int size_x = img.cols / grid_x; + int size_y = img.rows / grid_y; + + // Make sure our sizes are not zero + assert(size_x > 0); + assert(size_y > 0); + + // Parallelize our 2d grid extraction!! + std::vector> collection(valid_locs.size()); + parallel_for_(cv::Range(0, (int)valid_locs.size()), LambdaBody([&](const cv::Range &range) { + for (int r = range.start; r < range.end; r++) { + + // Calculate what cell xy value we are in + auto grid = valid_locs.at(r); + int x = grid.first * size_x; + int y = grid.second * size_y; + + // Skip if we are out of bounds + if (x + size_x > img.cols || y + size_y > img.rows) + continue; + + // Calculate where we should be extracting from + cv::Rect img_roi = cv::Rect(x, y, size_x, size_y); + + // Extract FAST features for this part of the image + std::vector pts_new; + cv::FAST(img(img_roi), pts_new, threshold, nonmaxSuppression); + + // Now lets get the top number from this + std::sort(pts_new.begin(), pts_new.end(), Grider_FAST::compare_response); + + // Append the "best" ones to our vector + // Note that we need to "correct" the point u,v since we extracted it in a ROI + // So we should append the location of that ROI in the image + for (size_t i = 0; i < (size_t)num_features_grid && i < pts_new.size(); i++) { + + // Create keypoint + cv::KeyPoint pt_cor = pts_new.at(i); + pt_cor.pt.x += (float)x; + pt_cor.pt.y += (float)y; + + // Reject if out of bounds (shouldn't be possible...) + if ((int)pt_cor.pt.x < 0 || (int)pt_cor.pt.x > img.cols || (int)pt_cor.pt.y < 0 || (int)pt_cor.pt.y > img.rows) + continue; + + // Check if it is in the mask region + // NOTE: mask has max value of 255 (white) if it should be removed + if (mask.at((int)pt_cor.pt.y, (int)pt_cor.pt.x) > 127) + continue; + collection.at(r).push_back(pt_cor); + } + } + })); + + // Combine all the collections into our single vector + for (size_t r = 0; r < collection.size(); r++) { + pts.insert(pts.end(), collection.at(r).begin(), collection.at(r).end()); + } + + // Return if no points + if (pts.empty()) + return; + + // Sub-pixel refinement parameters + cv::Size win_size = cv::Size(5, 5); + cv::Size zero_zone = cv::Size(-1, -1); + cv::TermCriteria term_crit = cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 20, 0.001); + + // Get vector of points + std::vector pts_refined; + for (size_t i = 0; i < pts.size(); i++) { + pts_refined.push_back(pts.at(i).pt); + } + + // Finally get sub-pixel for all extracted features + cv::cornerSubPix(img, pts_refined, win_size, zero_zone, term_crit); + + // Save the refined points! + for (size_t i = 0; i < pts.size(); i++) { + pts.at(i).pt = pts_refined.at(i); + } + } +}; + +} // namespace ov_core + +#endif /* OV_CORE_GRIDER_GRID_H */ diff --git a/ov_core/src/track/TrackAruco.cpp b/ov_core/src/track/TrackAruco.cpp index fa88f5f10..95880b32e 100644 --- a/ov_core/src/track/TrackAruco.cpp +++ b/ov_core/src/track/TrackAruco.cpp @@ -20,7 +20,11 @@ */ #include "TrackAruco.h" -#include "utils/print.h" + +#include "cam/CamBase.h" +#include "feat/Feature.h" +#include "feat/FeatureDatabase.h" +#include "utils/opencv_lambda_body.h" using namespace ov_core; @@ -147,10 +151,13 @@ void TrackAruco::perform_tracking(double timestamp, const cv::Mat &imgin, size_t } // Move forward in time - img_last[cam_id] = img; - img_mask_last[cam_id] = maskin; - ids_last[cam_id] = ids_new; - pts_last[cam_id] = pts_new; + { + std::lock_guard lckv(mtx_last_vars); + img_last[cam_id] = img; + img_mask_last[cam_id] = maskin; + ids_last[cam_id] = ids_new; + pts_last[cam_id] = pts_new; + } rT3 = boost::posix_time::microsec_clock::local_time(); // Timing information @@ -163,12 +170,14 @@ void TrackAruco::display_active(cv::Mat &img_out, int r1, int g1, int b1, int r2 // Cache the images to prevent other threads from editing while we viz (which can be slow) std::map img_last_cache, img_mask_last_cache; - for (auto const &pair : img_last) { - img_last_cache.insert({pair.first, pair.second.clone()}); - } - for (auto const &pair : img_mask_last) { - img_mask_last_cache.insert({pair.first, pair.second.clone()}); + { + std::lock_guard lckv(mtx_last_vars); + img_last_cache = img_last; + img_mask_last_cache = img_mask_last; } + auto ids_aruco_cache = ids_aruco; + auto corners_cache = corners; + auto rejects_cache = rejects; // Get the largest width and height int max_width = -1; @@ -181,7 +190,7 @@ void TrackAruco::display_active(cv::Mat &img_out, int r1, int g1, int b1, int r2 } // Return if we didn't have a last image - if (max_width == -1 || max_height == -1) + if (img_last_cache.empty() || max_width == -1 || max_height == -1) return; // If the image is "small" thus we shoudl use smaller display codes @@ -198,8 +207,6 @@ void TrackAruco::display_active(cv::Mat &img_out, int r1, int g1, int b1, int r2 // Loop through each image, and draw int index_cam = 0; for (auto const &pair : img_last_cache) { - // Lock this image - std::lock_guard lck(mtx_feeds.at(pair.first)); // select the subset of the image cv::Mat img_temp; if (image_new) @@ -207,10 +214,10 @@ void TrackAruco::display_active(cv::Mat &img_out, int r1, int g1, int b1, int r2 else img_temp = img_out(cv::Rect(max_width * index_cam, 0, max_width, max_height)); // draw... - if (!ids_aruco[pair.first].empty()) - cv::aruco::drawDetectedMarkers(img_temp, corners[pair.first], ids_aruco[pair.first]); - if (!rejects[pair.first].empty()) - cv::aruco::drawDetectedMarkers(img_temp, rejects[pair.first], cv::noArray(), cv::Scalar(100, 0, 255)); + if (!ids_aruco_cache[pair.first].empty()) + cv::aruco::drawDetectedMarkers(img_temp, corners_cache[pair.first], ids_aruco_cache[pair.first]); + if (!rejects_cache[pair.first].empty()) + cv::aruco::drawDetectedMarkers(img_temp, rejects_cache[pair.first], cv::noArray(), cv::Scalar(100, 0, 255)); // Draw what camera this is auto txtpt = (is_small) ? cv::Point(10, 30) : cv::Point(30, 60); if (overlay == "") { diff --git a/ov_core/src/track/TrackAruco.h b/ov_core/src/track/TrackAruco.h index daa777060..367a0acdc 100644 --- a/ov_core/src/track/TrackAruco.h +++ b/ov_core/src/track/TrackAruco.h @@ -27,7 +27,6 @@ #endif #include "TrackBase.h" -#include "utils/print.h" namespace ov_core { @@ -48,13 +47,13 @@ class TrackAruco : public TrackBase { * @brief Public constructor with configuration variables * @param cameras camera calibration object which has all camera intrinsics in it * @param numaruco the max id of the arucotags, we don't use any tags greater than this value even if we extract them - * @param binocular if we should do binocular feature tracking or stereo if there are multiple cameras + * @param stereo if we should do stereo feature tracking or binocular * @param histmethod what type of histogram pre-processing should be done (histogram eq?) * @param downsize we can scale the image by 1/2 to increase Aruco tag extraction speed */ - explicit TrackAruco(std::unordered_map> cameras, int numaruco, bool binocular, - HistogramMethod histmethod, bool downsize) - : TrackBase(cameras, 0, numaruco, binocular, histmethod), max_tag_id(numaruco), do_downsizing(downsize) { + explicit TrackAruco(std::unordered_map> cameras, int numaruco, bool stereo, HistogramMethod histmethod, + bool downsize) + : TrackBase(cameras, 0, numaruco, stereo, histmethod), max_tag_id(numaruco), do_downsizing(downsize) { #if ENABLE_ARUCO_TAGS aruco_dict = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_1000); aruco_params = cv::aruco::DetectorParameters::create(); @@ -70,7 +69,7 @@ class TrackAruco : public TrackBase { * @brief Process a new image * @param message Contains our timestamp, images, and camera ids */ - void feed_new_camera(const CameraData &message); + void feed_new_camera(const CameraData &message) override; #if ENABLE_ARUCO_TAGS /** diff --git a/ov_core/src/track/TrackBase.cpp b/ov_core/src/track/TrackBase.cpp index 91c05c1b3..9b74b24eb 100644 --- a/ov_core/src/track/TrackBase.cpp +++ b/ov_core/src/track/TrackBase.cpp @@ -21,17 +21,35 @@ #include "TrackBase.h" +#include "cam/CamBase.h" +#include "feat/Feature.h" +#include "feat/FeatureDatabase.h" + using namespace ov_core; +TrackBase::TrackBase(std::unordered_map> cameras, int numfeats, int numaruco, bool stereo, + HistogramMethod histmethod) + : camera_calib(cameras), database(new FeatureDatabase()), num_features(numfeats), use_stereo(stereo), histogram_method(histmethod) { + // Our current feature ID should be larger then the number of aruco tags we have (each has 4 corners) + currid = 4 * (size_t)numaruco + 1; + // Create our mutex array based on the number of cameras we have + // See https://stackoverflow.com/a/24170141/7718197 + if (mtx_feeds.empty() || mtx_feeds.size() != camera_calib.size()) { + std::vector list(camera_calib.size()); + mtx_feeds.swap(list); + } +} + void TrackBase::display_active(cv::Mat &img_out, int r1, int g1, int b1, int r2, int g2, int b2, std::string overlay) { // Cache the images to prevent other threads from editing while we viz (which can be slow) std::map img_last_cache, img_mask_last_cache; - for (auto const &pair : img_last) { - img_last_cache.insert({pair.first, pair.second.clone()}); - } - for (auto const &pair : img_mask_last) { - img_mask_last_cache.insert({pair.first, pair.second.clone()}); + std::unordered_map> pts_last_cache; + { + std::lock_guard lckv(mtx_last_vars); + img_last_cache = img_last; + img_mask_last_cache = img_mask_last; + pts_last_cache = pts_last; } // Get the largest width and height @@ -45,7 +63,7 @@ void TrackBase::display_active(cv::Mat &img_out, int r1, int g1, int b1, int r2, } // Return if we didn't have a last image - if (max_width == -1 || max_height == -1) + if (img_last_cache.empty() || max_width == -1 || max_height == -1) return; // If the image is "small" thus we should use smaller display codes @@ -62,8 +80,6 @@ void TrackBase::display_active(cv::Mat &img_out, int r1, int g1, int b1, int r2, // Loop through each image, and draw int index_cam = 0; for (auto const &pair : img_last_cache) { - // Lock this image - std::lock_guard lck(mtx_feeds.at(pair.first)); // select the subset of the image cv::Mat img_temp; if (image_new) @@ -71,9 +87,9 @@ void TrackBase::display_active(cv::Mat &img_out, int r1, int g1, int b1, int r2, else img_temp = img_out(cv::Rect(max_width * index_cam, 0, max_width, max_height)); // draw, loop through all keypoints - for (size_t i = 0; i < pts_last[pair.first].size(); i++) { + for (size_t i = 0; i < pts_last_cache[pair.first].size(); i++) { // Get bounding pts for our boxes - cv::Point2f pt_l = pts_last[pair.first].at(i).pt; + cv::Point2f pt_l = pts_last_cache[pair.first].at(i).pt; // Draw the extracted points and ID cv::circle(img_temp, pt_l, (is_small) ? 1 : 2, cv::Scalar(r1, g1, b1), cv::FILLED); // cv::putText(img_out, std::to_string(ids_left_last.at(i)), pt_l, cv::FONT_HERSHEY_SIMPLEX,0.5,cv::Scalar(0,0,255),1,cv::LINE_AA); @@ -105,11 +121,14 @@ void TrackBase::display_history(cv::Mat &img_out, int r1, int g1, int b1, int r2 // Cache the images to prevent other threads from editing while we viz (which can be slow) std::map img_last_cache, img_mask_last_cache; - for (auto const &pair : img_last) { - img_last_cache.insert({pair.first, pair.second.clone()}); - } - for (auto const &pair : img_mask_last) { - img_mask_last_cache.insert({pair.first, pair.second.clone()}); + std::unordered_map> pts_last_cache; + std::unordered_map> ids_last_cache; + { + std::lock_guard lckv(mtx_last_vars); + img_last_cache = img_last; + img_mask_last_cache = img_mask_last; + pts_last_cache = pts_last; + ids_last_cache = ids_last; } // Get the largest width and height @@ -123,7 +142,7 @@ void TrackBase::display_history(cv::Mat &img_out, int r1, int g1, int b1, int r2 } // Return if we didn't have a last image - if (max_width == -1 || max_height == -1) + if (img_last_cache.empty() || max_width == -1 || max_height == -1) return; // If the image is "small" thus we shoudl use smaller display codes @@ -143,8 +162,6 @@ void TrackBase::display_history(cv::Mat &img_out, int r1, int g1, int b1, int r2 // Loop through each image, and draw int index_cam = 0; for (auto const &pair : img_last_cache) { - // Lock this image - std::lock_guard lck(mtx_feeds.at(pair.first)); // select the subset of the image cv::Mat img_temp; if (image_new) @@ -152,40 +169,41 @@ void TrackBase::display_history(cv::Mat &img_out, int r1, int g1, int b1, int r2 else img_temp = img_out(cv::Rect(max_width * index_cam, 0, max_width, max_height)); // draw, loop through all keypoints - for (size_t i = 0; i < ids_last[pair.first].size(); i++) { + for (size_t i = 0; i < ids_last_cache[pair.first].size(); i++) { // If a highlighted point, then put a nice box around it - if (std::find(highlighted.begin(), highlighted.end(), ids_last[pair.first].at(i)) != highlighted.end()) { - cv::Point2f pt_c = pts_last[pair.first].at(i).pt; + if (std::find(highlighted.begin(), highlighted.end(), ids_last_cache[pair.first].at(i)) != highlighted.end()) { + cv::Point2f pt_c = pts_last_cache[pair.first].at(i).pt; cv::Point2f pt_l_top = cv::Point2f(pt_c.x - ((is_small) ? 3 : 5), pt_c.y - ((is_small) ? 3 : 5)); cv::Point2f pt_l_bot = cv::Point2f(pt_c.x + ((is_small) ? 3 : 5), pt_c.y + ((is_small) ? 3 : 5)); cv::rectangle(img_temp, pt_l_top, pt_l_bot, cv::Scalar(0, 255, 0), 1); cv::circle(img_temp, pt_c, (is_small) ? 1 : 2, cv::Scalar(0, 255, 0), cv::FILLED); } // Get the feature from the database - std::shared_ptr feat = database->get_feature(ids_last[pair.first].at(i)); - // Skip if the feature is null - if (feat == nullptr || feat->uvs[pair.first].empty() || feat->to_delete) + Feature feat; + if (!database->get_feature_clone(ids_last_cache[pair.first].at(i), feat)) + continue; + if (feat.uvs.empty() || feat.uvs[pair.first].empty() || feat.to_delete) continue; // Draw the history of this point (start at the last inserted one) - for (size_t z = feat->uvs[pair.first].size() - 1; z > 0; z--) { + for (size_t z = feat.uvs[pair.first].size() - 1; z > 0; z--) { // Check if we have reached the max - if (feat->uvs[pair.first].size() - z > maxtracks) + if (feat.uvs[pair.first].size() - z > maxtracks) break; // Calculate what color we are drawing in - bool is_stereo = (feat->uvs.size() > 1); - int color_r = (is_stereo ? b2 : r2) - (int)((is_stereo ? b1 : r1) / feat->uvs[pair.first].size() * z); - int color_g = (is_stereo ? r2 : g2) - (int)((is_stereo ? r1 : g1) / feat->uvs[pair.first].size() * z); - int color_b = (is_stereo ? g2 : b2) - (int)((is_stereo ? g1 : b1) / feat->uvs[pair.first].size() * z); + bool is_stereo = (feat.uvs.size() > 1); + int color_r = (is_stereo ? b2 : r2) - (int)((is_stereo ? b1 : r1) / feat.uvs[pair.first].size() * z); + int color_g = (is_stereo ? r2 : g2) - (int)((is_stereo ? r1 : g1) / feat.uvs[pair.first].size() * z); + int color_b = (is_stereo ? g2 : b2) - (int)((is_stereo ? g1 : b1) / feat.uvs[pair.first].size() * z); // Draw current point - cv::Point2f pt_c(feat->uvs[pair.first].at(z)(0), feat->uvs[pair.first].at(z)(1)); + cv::Point2f pt_c(feat.uvs[pair.first].at(z)(0), feat.uvs[pair.first].at(z)(1)); cv::circle(img_temp, pt_c, (is_small) ? 1 : 2, cv::Scalar(color_r, color_g, color_b), cv::FILLED); // If there is a next point, then display the line from this point to the next - if (z + 1 < feat->uvs[pair.first].size()) { - cv::Point2f pt_n(feat->uvs[pair.first].at(z + 1)(0), feat->uvs[pair.first].at(z + 1)(1)); + if (z + 1 < feat.uvs[pair.first].size()) { + cv::Point2f pt_n(feat.uvs[pair.first].at(z + 1)(0), feat.uvs[pair.first].at(z + 1)(1)); cv::line(img_temp, pt_c, pt_n, cv::Scalar(color_r, color_g, color_b)); } // If the first point, display the ID - if (z == feat->uvs[pair.first].size() - 1) { + if (z == feat.uvs[pair.first].size() - 1) { // cv::putText(img_out0, std::to_string(feat->featid), pt_c, cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 255), 1, // cv::LINE_AA); cv::circle(img_out0, pt_c, 2, cv::Scalar(color,color,255), CV_FILLED); } @@ -208,3 +226,23 @@ void TrackBase::display_history(cv::Mat &img_out, int r1, int g1, int b1, int r2 index_cam++; } } + +void TrackBase::change_feat_id(size_t id_old, size_t id_new) { + + // If found in db then replace + if (database->get_internal_data().find(id_old) != database->get_internal_data().end()) { + std::shared_ptr feat = database->get_internal_data().at(id_old); + database->get_internal_data().erase(id_old); + feat->featid = id_new; + database->get_internal_data().insert({id_new, feat}); + } + + // Update current track IDs + for (auto &cam_ids_pair : ids_last) { + for (size_t i = 0; i < cam_ids_pair.second.size(); i++) { + if (cam_ids_pair.second.at(i) == id_old) { + ids_last.at(cam_ids_pair.first).at(i) = id_new; + } + } + } +} \ No newline at end of file diff --git a/ov_core/src/track/TrackBase.h b/ov_core/src/track/TrackBase.h index 7d13b0976..fdb9578d6 100644 --- a/ov_core/src/track/TrackBase.h +++ b/ov_core/src/track/TrackBase.h @@ -33,15 +33,16 @@ #include #include -#include "Grider_FAST.h" -#include "cam/CamBase.h" -#include "feat/FeatureDatabase.h" #include "utils/colors.h" -#include "utils/opencv_lambda_body.h" +#include "utils/print.h" #include "utils/sensor_data.h" namespace ov_core { +class Feature; +class CamBase; +class FeatureDatabase; + /** * @brief Visual feature tracking base class * @@ -85,17 +86,7 @@ class TrackBase { * @param histmethod what type of histogram pre-processing should be done (histogram eq?) */ TrackBase(std::unordered_map> cameras, int numfeats, int numaruco, bool stereo, - HistogramMethod histmethod) - : camera_calib(cameras), database(new FeatureDatabase()), num_features(numfeats), use_stereo(stereo), histogram_method(histmethod) { - // Our current feature ID should be larger then the number of aruco tags we have (each has 4 corners) - currid = 4 * (size_t)numaruco + 1; - // Create our mutex array based on the number of cameras we have - // See https://stackoverflow.com/a/24170141/7718197 - if (mtx_feeds.empty() || mtx_feeds.size() != camera_calib.size()) { - std::vector list(camera_calib.size()); - mtx_feeds.swap(list); - } - } + HistogramMethod histmethod); virtual ~TrackBase() {} @@ -140,25 +131,7 @@ class TrackBase { * @param id_old Old id we want to change * @param id_new Id we want to change the old id to */ - void change_feat_id(size_t id_old, size_t id_new) { - - // If found in db then replace - if (database->get_internal_data().find(id_old) != database->get_internal_data().end()) { - std::shared_ptr feat = database->get_internal_data().at(id_old); - database->get_internal_data().erase(id_old); - feat->featid = id_new; - database->get_internal_data().insert({id_new, feat}); - } - - // Update current track IDs - for (auto &cam_ids_pair : ids_last) { - for (size_t i = 0; i < cam_ids_pair.second.size(); i++) { - if (cam_ids_pair.second.at(i) == id_old) { - ids_last.at(cam_ids_pair.first).at(i) = id_new; - } - } - } - } + void change_feat_id(size_t id_old, size_t id_new); /// Getter method for number of active features int get_num_features() { return num_features; } @@ -188,6 +161,9 @@ class TrackBase { /// Mutexs for our last set of image storage (img_last, pts_last, and ids_last) std::vector mtx_feeds; + /// Mutex for editing the *_last variables + std::mutex mtx_last_vars; + /// Last set of images (use map so all trackers render in the same order) std::map img_last; diff --git a/ov_core/src/track/TrackDescriptor.cpp b/ov_core/src/track/TrackDescriptor.cpp index 386291aa6..934468354 100644 --- a/ov_core/src/track/TrackDescriptor.cpp +++ b/ov_core/src/track/TrackDescriptor.cpp @@ -20,7 +20,13 @@ */ #include "TrackDescriptor.h" -#include "utils/print.h" + +#include + +#include "Grider_FAST.h" +#include "cam/CamBase.h" +#include "feat/Feature.h" +#include "feat/FeatureDatabase.h" using namespace ov_core; @@ -79,9 +85,16 @@ void TrackDescriptor::feed_monocular(const CameraData &message, size_t msg_id) { // If we are the first frame (or have lost tracking), initialize our descriptors if (pts_last.find(cam_id) == pts_last.end() || pts_last[cam_id].empty()) { - perform_detection_monocular(img, mask, pts_last[cam_id], desc_last[cam_id], ids_last[cam_id]); + std::vector good_left; + std::vector good_ids_left; + cv::Mat good_desc_left; + perform_detection_monocular(img, mask, good_left, good_desc_left, good_ids_left); + std::lock_guard lckv(mtx_last_vars); img_last[cam_id] = img; img_mask_last[cam_id] = mask; + pts_last[cam_id] = good_left; + ids_last[cam_id] = good_ids_left; + desc_last[cam_id] = good_desc_left; return; } @@ -145,11 +158,14 @@ void TrackDescriptor::feed_monocular(const CameraData &message, size_t msg_id) { // PRINT_DEBUG("LtoL = %d | good = %d | fromlast = %d\n",(int)matches_ll.size(),(int)good_left.size(),num_tracklast); // Move forward in time - img_last[cam_id] = img; - img_mask_last[cam_id] = mask; - pts_last[cam_id] = good_left; - ids_last[cam_id] = good_ids_left; - desc_last[cam_id] = good_desc_left; + { + std::lock_guard lckv(mtx_last_vars); + img_last[cam_id] = img; + img_mask_last[cam_id] = mask; + pts_last[cam_id] = good_left; + ids_last[cam_id] = good_ids_left; + desc_last[cam_id] = good_desc_left; + } rT5 = boost::posix_time::microsec_clock::local_time(); // Our timing information @@ -191,13 +207,22 @@ void TrackDescriptor::feed_stereo(const CameraData &message, size_t msg_id_left, // If we are the first frame (or have lost tracking), initialize our descriptors if (pts_last[cam_id_left].empty() || pts_last[cam_id_right].empty()) { - perform_detection_stereo(img_left, img_right, mask_left, mask_right, pts_last[cam_id_left], pts_last[cam_id_right], - desc_last[cam_id_left], desc_last[cam_id_right], cam_id_left, cam_id_right, ids_last[cam_id_left], - ids_last[cam_id_right]); + std::vector good_left, good_right; + std::vector good_ids_left, good_ids_right; + cv::Mat good_desc_left, good_desc_right; + perform_detection_stereo(img_left, img_right, mask_left, mask_right, good_left, good_right, good_desc_left, good_desc_right, + cam_id_left, cam_id_right, good_ids_left, good_ids_right); + std::lock_guard lckv(mtx_last_vars); img_last[cam_id_left] = img_left; img_last[cam_id_right] = img_right; img_mask_last[cam_id_left] = mask_left; img_mask_last[cam_id_right] = mask_right; + pts_last[cam_id_left] = good_left; + pts_last[cam_id_right] = good_right; + ids_last[cam_id_left] = good_ids_left; + ids_last[cam_id_right] = good_ids_right; + desc_last[cam_id_left] = good_desc_left; + desc_last[cam_id_right] = good_desc_right; return; } @@ -302,16 +327,19 @@ void TrackDescriptor::feed_stereo(const CameraData &message, size_t msg_id_left, // (int)matches_rr.size(),(int)ids_left_new.size(),(int)good_left.size(),num_tracklast); // Move forward in time - img_last[cam_id_left] = img_left; - img_last[cam_id_right] = img_right; - img_mask_last[cam_id_left] = mask_left; - img_mask_last[cam_id_right] = mask_right; - pts_last[cam_id_left] = good_left; - pts_last[cam_id_right] = good_right; - ids_last[cam_id_left] = good_ids_left; - ids_last[cam_id_right] = good_ids_right; - desc_last[cam_id_left] = good_desc_left; - desc_last[cam_id_right] = good_desc_right; + { + std::lock_guard lckv(mtx_last_vars); + img_last[cam_id_left] = img_left; + img_last[cam_id_right] = img_right; + img_mask_last[cam_id_left] = mask_left; + img_mask_last[cam_id_right] = mask_right; + pts_last[cam_id_left] = good_left; + pts_last[cam_id_right] = good_right; + ids_last[cam_id_left] = good_ids_left; + ids_last[cam_id_right] = good_ids_right; + desc_last[cam_id_left] = good_desc_left; + desc_last[cam_id_right] = good_desc_right; + } rT5 = boost::posix_time::microsec_clock::local_time(); // Our timing information @@ -447,8 +475,8 @@ void TrackDescriptor::perform_detection_stereo(const cv::Mat &img0, const cv::Ma } } -void TrackDescriptor::robust_match(std::vector &pts0, std::vector pts1, cv::Mat &desc0, cv::Mat &desc1, - size_t id0, size_t id1, std::vector &matches) { +void TrackDescriptor::robust_match(const std::vector &pts0, const std::vector &pts1, const cv::Mat &desc0, + const cv::Mat &desc1, size_t id0, size_t id1, std::vector &matches) { // Our 1to2 and 2to1 match vectors std::vector> matches0to1, matches1to0; diff --git a/ov_core/src/track/TrackDescriptor.h b/ov_core/src/track/TrackDescriptor.h index 1daf9084d..e36f564b5 100644 --- a/ov_core/src/track/TrackDescriptor.h +++ b/ov_core/src/track/TrackDescriptor.h @@ -22,8 +22,6 @@ #ifndef OV_CORE_TRACK_DESC_H #define OV_CORE_TRACK_DESC_H -#include - #include "TrackBase.h" namespace ov_core { @@ -44,7 +42,7 @@ class TrackDescriptor : public TrackBase { * @param cameras camera calibration object which has all camera intrinsics in it * @param numfeats number of features we want want to track (i.e. track 200 points from frame to frame) * @param numaruco the max id of the arucotags, so we ensure that we start our non-auroc features above this value - * @param binocular if we should do binocular feature tracking or stereo if there are multiple cameras + * @param stereo if we should do stereo feature tracking or binocular * @param histmethod what type of histogram pre-processing should be done (histogram eq?) * @param fast_threshold FAST detection threshold * @param gridx size of grid in the x-direction / u-direction @@ -52,16 +50,16 @@ class TrackDescriptor : public TrackBase { * @param minpxdist features need to be at least this number pixels away from each other * @param knnratio matching ratio needed (smaller value forces top two descriptors during match to be more different) */ - explicit TrackDescriptor(std::unordered_map> cameras, int numfeats, int numaruco, bool binocular, + explicit TrackDescriptor(std::unordered_map> cameras, int numfeats, int numaruco, bool stereo, HistogramMethod histmethod, int fast_threshold, int gridx, int gridy, int minpxdist, double knnratio) - : TrackBase(cameras, numfeats, numaruco, binocular, histmethod), threshold(fast_threshold), grid_x(gridx), grid_y(gridy), + : TrackBase(cameras, numfeats, numaruco, stereo, histmethod), threshold(fast_threshold), grid_x(gridx), grid_y(gridy), min_px_dist(minpxdist), knn_ratio(knnratio) {} /** * @brief Process a new image * @param message Contains our timestamp, images, and camera ids */ - void feed_new_camera(const CameraData &message); + void feed_new_camera(const CameraData &message) override; protected: /** @@ -134,8 +132,8 @@ class TrackDescriptor : public TrackBase { * Original code is from the "RobustMatcher" in the opencv examples, and seems to give very good results in the matches. * https://github.com/opencv/opencv/blob/master/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/RobustMatcher.cpp */ - void robust_match(std::vector &pts0, std::vector pts1, cv::Mat &desc0, cv::Mat &desc1, size_t id0, size_t id1, - std::vector &matches); + void robust_match(const std::vector &pts0, const std::vector &pts1, const cv::Mat &desc0, + const cv::Mat &desc1, size_t id0, size_t id1, std::vector &matches); // Helper functions for the robust_match function // Original code is from the "RobustMatcher" in the opencv examples diff --git a/ov_core/src/track/TrackKLT.cpp b/ov_core/src/track/TrackKLT.cpp index b7e8f8604..e5b25209c 100644 --- a/ov_core/src/track/TrackKLT.cpp +++ b/ov_core/src/track/TrackKLT.cpp @@ -20,6 +20,13 @@ */ #include "TrackKLT.h" + +#include "Grider_FAST.h" +#include "Grider_GRID.h" +#include "cam/CamBase.h" +#include "feat/Feature.h" +#include "feat/FeatureDatabase.h" +#include "utils/opencv_lambda_body.h" #include "utils/print.h" using namespace ov_core; @@ -35,9 +42,41 @@ void TrackKLT::feed_new_camera(const CameraData &message) { std::exit(EXIT_FAILURE); } + // Preprocessing steps that we do not parallelize + // NOTE: DO NOT PARALLELIZE THESE! + // NOTE: These seem to be much slower if you parallelize them... + rT1 = boost::posix_time::microsec_clock::local_time(); + size_t num_images = message.images.size(); + for (size_t msg_id = 0; msg_id < num_images; msg_id++) { + + // Lock this data feed for this camera + size_t cam_id = message.sensor_ids.at(msg_id); + std::lock_guard lck(mtx_feeds.at(cam_id)); + + // Histogram equalize + cv::Mat img; + if (histogram_method == HistogramMethod::HISTOGRAM) { + cv::equalizeHist(message.images.at(msg_id), img); + } else if (histogram_method == HistogramMethod::CLAHE) { + double eq_clip_limit = 10.0; + cv::Size eq_win_size = cv::Size(8, 8); + cv::Ptr clahe = cv::createCLAHE(eq_clip_limit, eq_win_size); + clahe->apply(message.images.at(msg_id), img); + } else { + img = message.images.at(msg_id); + } + + // Extract image pyramid + std::vector imgpyr; + cv::buildOpticalFlowPyramid(img, imgpyr, win_size, pyr_levels); + + // Save! + img_curr[cam_id] = img; + img_pyramid_curr[cam_id] = imgpyr; + } + // Either call our stereo or monocular version // If we are doing binocular tracking, then we should parallize our tracking - size_t num_images = message.images.size(); if (num_images == 1) { feed_monocular(message, 0); } else if (num_images == 2 && use_stereo) { @@ -56,60 +95,52 @@ void TrackKLT::feed_new_camera(const CameraData &message) { void TrackKLT::feed_monocular(const CameraData &message, size_t msg_id) { - // Start timing - rT1 = boost::posix_time::microsec_clock::local_time(); - // Lock this data feed for this camera size_t cam_id = message.sensor_ids.at(msg_id); std::lock_guard lck(mtx_feeds.at(cam_id)); - // Histogram equalize - cv::Mat img, mask; - if (histogram_method == HistogramMethod::HISTOGRAM) { - cv::equalizeHist(message.images.at(msg_id), img); - } else if (histogram_method == HistogramMethod::CLAHE) { - double eq_clip_limit = 10.0; - cv::Size eq_win_size = cv::Size(8, 8); - cv::Ptr clahe = cv::createCLAHE(eq_clip_limit, eq_win_size); - clahe->apply(message.images.at(msg_id), img); - } else { - img = message.images.at(msg_id); - } - mask = message.masks.at(msg_id); - - // Extract the new image pyramid - std::vector imgpyr; - cv::buildOpticalFlowPyramid(img, imgpyr, win_size, pyr_levels); + // Get our image objects for this image + cv::Mat img = img_curr.at(cam_id); + std::vector imgpyr = img_pyramid_curr.at(cam_id); + cv::Mat mask = message.masks.at(msg_id); rT2 = boost::posix_time::microsec_clock::local_time(); // If we didn't have any successful tracks last time, just extract this time // This also handles, the tracking initalization on the first call to this extractor if (pts_last[cam_id].empty()) { // Detect new features - perform_detection_monocular(imgpyr, mask, pts_last[cam_id], ids_last[cam_id]); + std::vector good_left; + std::vector good_ids_left; + perform_detection_monocular(imgpyr, mask, good_left, good_ids_left); // Save the current image and pyramid + std::lock_guard lckv(mtx_last_vars); img_last[cam_id] = img; img_pyramid_last[cam_id] = imgpyr; img_mask_last[cam_id] = mask; + pts_last[cam_id] = good_left; + ids_last[cam_id] = good_ids_left; return; } // First we should make that the last images have enough features so we can do KLT // This will "top-off" our number of tracks so always have a constant number - perform_detection_monocular(img_pyramid_last[cam_id], img_mask_last[cam_id], pts_last[cam_id], ids_last[cam_id]); + auto pts_left_old = pts_last[cam_id]; + auto ids_left_old = ids_last[cam_id]; + perform_detection_monocular(img_pyramid_last[cam_id], img_mask_last[cam_id], pts_left_old, ids_left_old); rT3 = boost::posix_time::microsec_clock::local_time(); // Our return success masks, and predicted new features std::vector mask_ll; - std::vector pts_left_new = pts_last[cam_id]; + std::vector pts_left_new = pts_left_old; // Lets track temporally - perform_matching(img_pyramid_last[cam_id], imgpyr, pts_last[cam_id], pts_left_new, cam_id, cam_id, mask_ll); - assert(pts_left_new.size() == ids_last[cam_id].size()); + perform_matching(img_pyramid_last[cam_id], imgpyr, pts_left_old, pts_left_new, cam_id, cam_id, mask_ll); + assert(pts_left_new.size() == ids_left_old.size()); rT4 = boost::posix_time::microsec_clock::local_time(); // If any of our mask is empty, that means we didn't have enough to do ransac, so just return if (mask_ll.empty()) { + std::lock_guard lckv(mtx_last_vars); img_last[cam_id] = img; img_pyramid_last[cam_id] = imgpyr; img_mask_last[cam_id] = mask; @@ -136,7 +167,7 @@ void TrackKLT::feed_monocular(const CameraData &message, size_t msg_id) { // If it is a good track, and also tracked from left to right if (mask_ll[i]) { good_left.push_back(pts_left_new[i]); - good_ids_left.push_back(ids_last[cam_id][i]); + good_ids_left.push_back(ids_left_old[i]); } } @@ -147,14 +178,17 @@ void TrackKLT::feed_monocular(const CameraData &message, size_t msg_id) { } // Move forward in time - img_last[cam_id] = img; - img_pyramid_last[cam_id] = imgpyr; - img_mask_last[cam_id] = mask; - pts_last[cam_id] = good_left; - ids_last[cam_id] = good_ids_left; + { + std::lock_guard lckv(mtx_last_vars); + img_last[cam_id] = img; + img_pyramid_last[cam_id] = imgpyr; + img_mask_last[cam_id] = mask; + pts_last[cam_id] = good_left; + ids_last[cam_id] = good_ids_left; + } rT5 = boost::posix_time::microsec_clock::local_time(); - // Timing information + // // Timing information // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for pyramid\n", (rT2 - rT1).total_microseconds() * 1e-6); // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for detection\n", (rT3 - rT2).total_microseconds() * 1e-6); // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for temporal klt\n", (rT4 - rT3).total_microseconds() * 1e-6); @@ -165,77 +199,66 @@ void TrackKLT::feed_monocular(const CameraData &message, size_t msg_id) { void TrackKLT::feed_stereo(const CameraData &message, size_t msg_id_left, size_t msg_id_right) { - // Start timing - rT1 = boost::posix_time::microsec_clock::local_time(); - // Lock this data feed for this camera size_t cam_id_left = message.sensor_ids.at(msg_id_left); size_t cam_id_right = message.sensor_ids.at(msg_id_right); std::lock_guard lck1(mtx_feeds.at(cam_id_left)); std::lock_guard lck2(mtx_feeds.at(cam_id_right)); - // Histogram equalize images - cv::Mat img_left, img_right, mask_left, mask_right; - if (histogram_method == HistogramMethod::HISTOGRAM) { - cv::equalizeHist(message.images.at(msg_id_left), img_left); - cv::equalizeHist(message.images.at(msg_id_right), img_right); - } else if (histogram_method == HistogramMethod::CLAHE) { - double eq_clip_limit = 10.0; - cv::Size eq_win_size = cv::Size(8, 8); - cv::Ptr clahe = cv::createCLAHE(eq_clip_limit, eq_win_size); - clahe->apply(message.images.at(msg_id_left), img_left); - clahe->apply(message.images.at(msg_id_right), img_right); - } else { - img_left = message.images.at(msg_id_left); - img_right = message.images.at(msg_id_right); - } - mask_left = message.masks.at(msg_id_left); - mask_right = message.masks.at(msg_id_right); - - // Extract image pyramids - std::vector imgpyr_left, imgpyr_right; - parallel_for_(cv::Range(0, 2), LambdaBody([&](const cv::Range &range) { - for (int i = range.start; i < range.end; i++) { - bool is_left = (i == 0); - cv::buildOpticalFlowPyramid(is_left ? img_left : img_right, is_left ? imgpyr_left : imgpyr_right, win_size, pyr_levels); - } - })); + // Get our image objects for this image + cv::Mat img_left = img_curr.at(cam_id_left); + cv::Mat img_right = img_curr.at(cam_id_right); + std::vector imgpyr_left = img_pyramid_curr.at(cam_id_left); + std::vector imgpyr_right = img_pyramid_curr.at(cam_id_right); + cv::Mat mask_left = message.masks.at(msg_id_left); + cv::Mat mask_right = message.masks.at(msg_id_right); rT2 = boost::posix_time::microsec_clock::local_time(); // If we didn't have any successful tracks last time, just extract this time // This also handles, the tracking initalization on the first call to this extractor if (pts_last[cam_id_left].empty() && pts_last[cam_id_right].empty()) { // Track into the new image - perform_detection_stereo(imgpyr_left, imgpyr_right, mask_left, mask_right, cam_id_left, cam_id_right, pts_last[cam_id_left], - pts_last[cam_id_right], ids_last[cam_id_left], ids_last[cam_id_right]); + std::vector good_left, good_right; + std::vector good_ids_left, good_ids_right; + perform_detection_stereo(imgpyr_left, imgpyr_right, mask_left, mask_right, cam_id_left, cam_id_right, good_left, good_right, + good_ids_left, good_ids_right); // Save the current image and pyramid + std::lock_guard lckv(mtx_last_vars); img_last[cam_id_left] = img_left; img_last[cam_id_right] = img_right; img_pyramid_last[cam_id_left] = imgpyr_left; img_pyramid_last[cam_id_right] = imgpyr_right; img_mask_last[cam_id_left] = mask_left; img_mask_last[cam_id_right] = mask_right; + pts_last[cam_id_left] = good_left; + pts_last[cam_id_right] = good_right; + ids_last[cam_id_left] = good_ids_left; + ids_last[cam_id_right] = good_ids_right; return; } // First we should make that the last images have enough features so we can do KLT // This will "top-off" our number of tracks so always have a constant number + auto pts_left_old = pts_last[cam_id_left]; + auto pts_right_old = pts_last[cam_id_right]; + auto ids_left_old = ids_last[cam_id_left]; + auto ids_right_old = ids_last[cam_id_right]; perform_detection_stereo(img_pyramid_last[cam_id_left], img_pyramid_last[cam_id_right], img_mask_last[cam_id_left], - img_mask_last[cam_id_right], cam_id_left, cam_id_right, pts_last[cam_id_left], pts_last[cam_id_right], - ids_last[cam_id_left], ids_last[cam_id_right]); + img_mask_last[cam_id_right], cam_id_left, cam_id_right, pts_left_old, pts_right_old, ids_left_old, + ids_right_old); rT3 = boost::posix_time::microsec_clock::local_time(); // Our return success masks, and predicted new features std::vector mask_ll, mask_rr; - std::vector pts_left_new = pts_last[cam_id_left]; - std::vector pts_right_new = pts_last[cam_id_right]; + std::vector pts_left_new = pts_left_old; + std::vector pts_right_new = pts_right_old; // Lets track temporally parallel_for_(cv::Range(0, 2), LambdaBody([&](const cv::Range &range) { for (int i = range.start; i < range.end; i++) { bool is_left = (i == 0); perform_matching(img_pyramid_last[is_left ? cam_id_left : cam_id_right], is_left ? imgpyr_left : imgpyr_right, - pts_last[is_left ? cam_id_left : cam_id_right], is_left ? pts_left_new : pts_right_new, + is_left ? pts_left_old : pts_right_old, is_left ? pts_left_new : pts_right_new, is_left ? cam_id_left : cam_id_right, is_left ? cam_id_left : cam_id_right, is_left ? mask_ll : mask_rr); } @@ -257,6 +280,7 @@ void TrackKLT::feed_stereo(const CameraData &message, size_t msg_id_left, size_t // If any of our masks are empty, that means we didn't have enough to do ransac, so just return if (mask_ll.empty() && mask_rr.empty()) { + std::lock_guard lckv(mtx_last_vars); img_last[cam_id_left] = img_left; img_last[cam_id_right] = img_right; img_pyramid_last[cam_id_left] = imgpyr_left; @@ -284,8 +308,8 @@ void TrackKLT::feed_stereo(const CameraData &message, size_t msg_id_left, size_t // See if we have the same feature in the right bool found_right = false; size_t index_right = 0; - for (size_t n = 0; n < ids_last[cam_id_right].size(); n++) { - if (ids_last[cam_id_left].at(i) == ids_last[cam_id_right].at(n)) { + for (size_t n = 0; n < ids_right_old.size(); n++) { + if (ids_left_old.at(i) == ids_right_old.at(n)) { found_right = true; index_right = n; break; @@ -300,13 +324,13 @@ void TrackKLT::feed_stereo(const CameraData &message, size_t msg_id_left, size_t continue; good_left.push_back(pts_left_new.at(i)); good_right.push_back(pts_right_new.at(index_right)); - good_ids_left.push_back(ids_last[cam_id_left].at(i)); - good_ids_right.push_back(ids_last[cam_id_right].at(index_right)); - // PRINT_DEBUG("adding to stereo - %u , %u\n", ids_last[cam_id_left].at(i), ids_last[cam_id_right].at(index_right)); + good_ids_left.push_back(ids_left_old.at(i)); + good_ids_right.push_back(ids_right_old.at(index_right)); + // PRINT_DEBUG("adding to stereo - %u , %u\n", ids_left_old.at(i), ids_right_old.at(index_right)); } else if (mask_ll[i]) { good_left.push_back(pts_left_new.at(i)); - good_ids_left.push_back(ids_last[cam_id_left].at(i)); - // PRINT_DEBUG("adding to left - %u \n", ids_last[cam_id_left].at(i)); + good_ids_left.push_back(ids_left_old.at(i)); + // PRINT_DEBUG("adding to left - %u \n",ids_left_old.at(i)); } } @@ -317,12 +341,12 @@ void TrackKLT::feed_stereo(const CameraData &message, size_t msg_id_left, size_t (int)pts_right_new.at(i).pt.y >= img_right.rows) continue; // See if we have the same feature in the right - bool added_already = (std::find(good_ids_right.begin(), good_ids_right.end(), ids_last[cam_id_right].at(i)) != good_ids_right.end()); + bool added_already = (std::find(good_ids_right.begin(), good_ids_right.end(), ids_right_old.at(i)) != good_ids_right.end()); // If it has not already been added as a good feature, add it as a mono track if (mask_rr[i] && !added_already) { good_right.push_back(pts_right_new.at(i)); - good_ids_right.push_back(ids_last[cam_id_right].at(i)); - // PRINT_DEBUG("adding to right - %u \n", ids_last[cam_id_right].at(i)); + good_ids_right.push_back(ids_right_old.at(i)); + // PRINT_DEBUG("adding to right - %u \n", ids_right_old.at(i)); } } @@ -339,25 +363,29 @@ void TrackKLT::feed_stereo(const CameraData &message, size_t msg_id_left, size_t } // Move forward in time - img_last[cam_id_left] = img_left; - img_last[cam_id_right] = img_right; - img_pyramid_last[cam_id_left] = imgpyr_left; - img_pyramid_last[cam_id_right] = imgpyr_right; - img_mask_last[cam_id_left] = mask_left; - img_mask_last[cam_id_right] = mask_right; - pts_last[cam_id_left] = good_left; - pts_last[cam_id_right] = good_right; - ids_last[cam_id_left] = good_ids_left; - ids_last[cam_id_right] = good_ids_right; + { + std::lock_guard lckv(mtx_last_vars); + img_last[cam_id_left] = img_left; + img_last[cam_id_right] = img_right; + img_pyramid_last[cam_id_left] = imgpyr_left; + img_pyramid_last[cam_id_right] = imgpyr_right; + img_mask_last[cam_id_left] = mask_left; + img_mask_last[cam_id_right] = mask_right; + pts_last[cam_id_left] = good_left; + pts_last[cam_id_right] = good_right; + ids_last[cam_id_left] = good_ids_left; + ids_last[cam_id_right] = good_ids_right; + } rT6 = boost::posix_time::microsec_clock::local_time(); - // Timing information - // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for pyramid\n",(rT2-rT1).total_microseconds() * 1e-6); - // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for detection\n",(rT3-rT2).total_microseconds() * 1e-6); - // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for temporal klt\n",(rT4-rT3).total_microseconds() * 1e-6); - // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for stereo klt\n",(rT5-rT4).total_microseconds() * 1e-6); - // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for feature DB update (%d features)\n",(rT6-rT5).total_microseconds() * 1e-6, - // (int)good_left.size()); PRINT_DEBUG("[TIME-KLT]: %.4f seconds for total\n",(rT6-rT1).total_microseconds() * 1e-6); + // // Timing information + // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for pyramid\n", (rT2 - rT1).total_microseconds() * 1e-6); + // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for detection\n", (rT3 - rT2).total_microseconds() * 1e-6); + // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for temporal klt\n", (rT4 - rT3).total_microseconds() * 1e-6); + // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for stereo klt\n", (rT5 - rT4).total_microseconds() * 1e-6); + // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for feature DB update (%d features)\n", (rT6 - rT5).total_microseconds() * 1e-6, + // (int)good_left.size()); + // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for total\n", (rT6 - rT1).total_microseconds() * 1e-6); } void TrackKLT::perform_detection_monocular(const std::vector &img0pyr, const cv::Mat &mask0, std::vector &pts0, @@ -365,9 +393,14 @@ void TrackKLT::perform_detection_monocular(const std::vector &img0pyr, // Create a 2D occupancy grid for this current image // Note that we scale this down, so that each grid point is equal to a set of pixels - // This means that we will reject points that less then grid_px_size points away then existing features - cv::Size size((int)((float)img0pyr.at(0).cols / (float)min_px_dist), (int)((float)img0pyr.at(0).rows / (float)min_px_dist)); - cv::Mat grid_2d = cv::Mat::zeros(size, CV_8UC1); + // This means that we will reject points that less than grid_px_size points away then existing features + cv::Size size_close((int)((float)img0pyr.at(0).cols / (float)min_px_dist), + (int)((float)img0pyr.at(0).rows / (float)min_px_dist)); // width x height + cv::Mat grid_2d_close = cv::Mat::zeros(size_close, CV_8UC1); + float size_x = (float)img0pyr.at(0).cols / (float)grid_x; + float size_y = (float)img0pyr.at(0).rows / (float)grid_y; + cv::Size size_grid(grid_x, grid_y); // width x height + cv::Mat grid_2d_grid = cv::Mat::zeros(size_grid, CV_8UC1); cv::Mat mask0_updated = mask0.clone(); auto it0 = pts0.begin(); auto it1 = ids0.begin(); @@ -376,16 +409,30 @@ void TrackKLT::perform_detection_monocular(const std::vector &img0pyr, cv::KeyPoint kpt = *it0; int x = (int)kpt.pt.x; int y = (int)kpt.pt.y; - int x_grid = (int)(kpt.pt.x / (float)min_px_dist); - int y_grid = (int)(kpt.pt.y / (float)min_px_dist); - if (x_grid < 0 || x_grid >= size.width || y_grid < 0 || y_grid >= size.height || x < 0 || x >= img0pyr.at(0).cols || y < 0 || - y >= img0pyr.at(0).rows) { + int edge = 10; + if (x < edge || x >= img0pyr.at(0).cols - edge || y < edge || y >= img0pyr.at(0).rows - edge) { + it0 = pts0.erase(it0); + it1 = ids0.erase(it1); + continue; + } + // Calculate mask coordinates for close points + int x_close = (int)(kpt.pt.x / (float)min_px_dist); + int y_close = (int)(kpt.pt.y / (float)min_px_dist); + if (x_close < 0 || x_close >= size_close.width || y_close < 0 || y_close >= size_close.height) { + it0 = pts0.erase(it0); + it1 = ids0.erase(it1); + continue; + } + // Calculate what grid cell this feature is in + int x_grid = std::floor(kpt.pt.x / size_x); + int y_grid = std::floor(kpt.pt.y / size_y); + if (x_grid < 0 || x_grid >= size_grid.width || y_grid < 0 || y_grid >= size_grid.height) { it0 = pts0.erase(it0); it1 = ids0.erase(it1); continue; } // Check if this keypoint is near another point - if (grid_2d.at(y_grid, x_grid) > 127) { + if (grid_2d_close.at(y_close, x_close) > 127) { it0 = pts0.erase(it0); it1 = ids0.erase(it1); continue; @@ -398,7 +445,10 @@ void TrackKLT::perform_detection_monocular(const std::vector &img0pyr, continue; } // Else we are good, move forward to the next point - grid_2d.at(y_grid, x_grid) = 255; + grid_2d_close.at(y_close, x_close) = 255; + if (grid_2d_grid.at(y_grid, x_grid) < 255) { + grid_2d_grid.at(y_grid, x_grid) += 1; + } // Append this to the local mask of the image if (x - min_px_dist >= 0 && x + min_px_dist < img0pyr.at(0).cols && y - min_px_dist >= 0 && y + min_px_dist < img0pyr.at(0).rows) { cv::Point pt1(x - min_px_dist, y - min_px_dist); @@ -410,15 +460,34 @@ void TrackKLT::perform_detection_monocular(const std::vector &img0pyr, } // First compute how many more features we need to extract from this image - int num_featsneeded = num_features - (int)pts0.size(); - // If we don't need any features, just return - if (num_featsneeded < std::min(75, (int)(0.2 * num_features))) + double min_feat_percent = 0.50; + int num_featsneeded = num_features - (int)pts0.size(); + if (num_featsneeded < std::min(20, (int)(min_feat_percent * num_features))) return; - // Extract our features (use fast with griding) + // This is old extraction code that would extract from the whole image + // This can be slow as this will recompute extractions for grid areas that we have max features already + // std::vector pts0_ext; + // Grider_FAST::perform_griding(img0pyr.at(0), mask0_updated, pts0_ext, num_features, grid_x, grid_y, threshold, true); + + // We also check a downsampled mask such that we don't extract in areas where it is all masked! + cv::Mat mask0_grid; + cv::resize(mask0, mask0_grid, size_grid, 0.0, 0.0, cv::INTER_NEAREST); + + // Create grids we need to extract from and then extract our features (use fast with griding) + int num_features_grid = (int)((double)num_features / (double)(grid_x * grid_y)) + 1; + int num_features_grid_req = std::max(1, (int)(min_feat_percent * num_features_grid)); + std::vector> valid_locs; + for (int x = 0; x < grid_2d_grid.cols; x++) { + for (int y = 0; y < grid_2d_grid.rows; y++) { + if ((int)grid_2d_grid.at(y, x) < num_features_grid_req && (int)mask0_grid.at(y, x) != 255) { + valid_locs.emplace_back(x, y); + } + } + } std::vector pts0_ext; - Grider_FAST::perform_griding(img0pyr.at(0), mask0_updated, pts0_ext, num_features, grid_x, grid_y, threshold, true); + Grider_GRID::perform_griding(img0pyr.at(0), mask0_updated, valid_locs, pts0_ext, num_features, grid_x, grid_y, threshold, true); // Now, reject features that are close a current feature std::vector kpts0_new; @@ -427,15 +496,15 @@ void TrackKLT::perform_detection_monocular(const std::vector &img0pyr, // Check that it is in bounds int x_grid = (int)(kpt.pt.x / (float)min_px_dist); int y_grid = (int)(kpt.pt.y / (float)min_px_dist); - if (x_grid < 0 || x_grid >= size.width || y_grid < 0 || y_grid >= size.height) + if (x_grid < 0 || x_grid >= size_close.width || y_grid < 0 || y_grid >= size_close.height) continue; // See if there is a point at this location - if (grid_2d.at(y_grid, x_grid) > 127) + if (grid_2d_close.at(y_grid, x_grid) > 127) continue; // Else lets add it! kpts0_new.push_back(kpt); pts0_new.push_back(kpt.pt); - grid_2d.at(y_grid, x_grid) = 255; + grid_2d_close.at(y_grid, x_grid) = 255; } // Loop through and record only ones that are valid @@ -461,8 +530,13 @@ void TrackKLT::perform_detection_stereo(const std::vector &img0pyr, con // Create a 2D occupancy grid for this current image // Note that we scale this down, so that each grid point is equal to a set of pixels // This means that we will reject points that less then grid_px_size points away then existing features - cv::Size size0((int)((float)img0pyr.at(0).cols / (float)min_px_dist), (int)((float)img0pyr.at(0).rows / (float)min_px_dist)); - cv::Mat grid_2d_0 = cv::Mat::zeros(size0, CV_8UC1); + cv::Size size_close0((int)((float)img0pyr.at(0).cols / (float)min_px_dist), + (int)((float)img0pyr.at(0).rows / (float)min_px_dist)); // width x height + cv::Mat grid_2d_close0 = cv::Mat::zeros(size_close0, CV_8UC1); + float size_x0 = (float)img0pyr.at(0).cols / (float)grid_x; + float size_y0 = (float)img0pyr.at(0).rows / (float)grid_y; + cv::Size size_grid0(grid_x, grid_y); // width x height + cv::Mat grid_2d_grid0 = cv::Mat::zeros(size_grid0, CV_8UC1); cv::Mat mask0_updated = mask0.clone(); auto it0 = pts0.begin(); auto it1 = ids0.begin(); @@ -471,16 +545,30 @@ void TrackKLT::perform_detection_stereo(const std::vector &img0pyr, con cv::KeyPoint kpt = *it0; int x = (int)kpt.pt.x; int y = (int)kpt.pt.y; - int x_grid = (int)(kpt.pt.x / (float)min_px_dist); - int y_grid = (int)(kpt.pt.y / (float)min_px_dist); - if (x_grid < 0 || x_grid >= size0.width || y_grid < 0 || y_grid >= size0.height || x < 0 || x >= img0pyr.at(0).cols || y < 0 || - y >= img0pyr.at(0).rows) { + int edge = 10; + if (x < edge || x >= img0pyr.at(0).cols - edge || y < edge || y >= img0pyr.at(0).rows - edge) { + it0 = pts0.erase(it0); + it1 = ids0.erase(it1); + continue; + } + // Calculate mask coordinates for close points + int x_close = (int)(kpt.pt.x / (float)min_px_dist); + int y_close = (int)(kpt.pt.y / (float)min_px_dist); + if (x_close < 0 || x_close >= size_close0.width || y_close < 0 || y_close >= size_close0.height) { + it0 = pts0.erase(it0); + it1 = ids0.erase(it1); + continue; + } + // Calculate what grid cell this feature is in + int x_grid = std::floor(kpt.pt.x / size_x0); + int y_grid = std::floor(kpt.pt.y / size_y0); + if (x_grid < 0 || x_grid >= size_grid0.width || y_grid < 0 || y_grid >= size_grid0.height) { it0 = pts0.erase(it0); it1 = ids0.erase(it1); continue; } // Check if this keypoint is near another point - if (grid_2d_0.at(y_grid, x_grid) > 127) { + if (grid_2d_close0.at(y_close, x_close) > 127) { it0 = pts0.erase(it0); it1 = ids0.erase(it1); continue; @@ -493,7 +581,10 @@ void TrackKLT::perform_detection_stereo(const std::vector &img0pyr, con continue; } // Else we are good, move forward to the next point - grid_2d_0.at(y_grid, x_grid) = 255; + grid_2d_close0.at(y_close, x_close) = 255; + if (grid_2d_grid0.at(y_grid, x_grid) < 255) { + grid_2d_grid0.at(y_grid, x_grid) += 1; + } // Append this to the local mask of the image if (x - min_px_dist >= 0 && x + min_px_dist < img0pyr.at(0).cols && y - min_px_dist >= 0 && y + min_px_dist < img0pyr.at(0).rows) { cv::Point pt1(x - min_px_dist, y - min_px_dist); @@ -505,16 +596,36 @@ void TrackKLT::perform_detection_stereo(const std::vector &img0pyr, con } // First compute how many more features we need to extract from this image + double min_feat_percent = 0.50; int num_featsneeded_0 = num_features - (int)pts0.size(); // LEFT: if we need features we should extract them in the current frame // LEFT: we will also try to track them from this frame over to the right frame // LEFT: in the case that we have two features that are the same, then we should merge them - if (num_featsneeded_0 > std::min(75, (int)(0.2 * num_features))) { - - // Extract our features (use fast with griding) + if (num_featsneeded_0 > std::min(20, (int)(min_feat_percent * num_features))) { + + // This is old extraction code that would extract from the whole image + // This can be slow as this will recompute extractions for grid areas that we have max features already + // std::vector pts0_ext; + // Grider_FAST::perform_griding(img0pyr.at(0), mask0_updated, pts0_ext, num_features, grid_x, grid_y, threshold, true); + + // We also check a downsampled mask such that we don't extract in areas where it is all masked! + cv::Mat mask0_grid; + cv::resize(mask0, mask0_grid, size_grid0, 0.0, 0.0, cv::INTER_NEAREST); + + // Create grids we need to extract from and then extract our features (use fast with griding) + int num_features_grid = (int)((double)num_features / (double)(grid_x * grid_y)) + 1; + int num_features_grid_req = std::max(1, (int)(min_feat_percent * num_features_grid)); + std::vector> valid_locs; + for (int x = 0; x < grid_2d_grid0.cols; x++) { + for (int y = 0; y < grid_2d_grid0.rows; y++) { + if ((int)grid_2d_grid0.at(y, x) < num_features_grid_req && (int)mask0_grid.at(y, x) != 255) { + valid_locs.emplace_back(x, y); + } + } + } std::vector pts0_ext; - Grider_FAST::perform_griding(img0pyr.at(0), mask0_updated, pts0_ext, num_features, grid_x, grid_y, threshold, true); + Grider_GRID::perform_griding(img0pyr.at(0), mask0_updated, valid_locs, pts0_ext, num_features, grid_x, grid_y, threshold, true); // Now, reject features that are close a current feature std::vector kpts0_new; @@ -523,13 +634,13 @@ void TrackKLT::perform_detection_stereo(const std::vector &img0pyr, con // Check that it is in bounds int x_grid = (int)(kpt.pt.x / (float)min_px_dist); int y_grid = (int)(kpt.pt.y / (float)min_px_dist); - if (x_grid < 0 || x_grid >= size0.width || y_grid < 0 || y_grid >= size0.height) + if (x_grid < 0 || x_grid >= size_close0.width || y_grid < 0 || y_grid >= size_close0.height) continue; // See if there is a point at this location - if (grid_2d_0.at(y_grid, x_grid) > 127) + if (grid_2d_close0.at(y_grid, x_grid) > 127) continue; // Else lets add it! - grid_2d_0.at(y_grid, x_grid) = 255; + grid_2d_close0.at(y_grid, x_grid) = 255; kpts0_new.push_back(kpt); pts0_new.push_back(kpt.pt); } @@ -552,7 +663,7 @@ void TrackKLT::perform_detection_stereo(const std::vector &img0pyr, con std::vector mask; // perform_matching(img0pyr, img1pyr, kpts0_new, kpts1_new, cam_id_left, cam_id_right, mask); std::vector error; - cv::TermCriteria term_crit = cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 10, 0.01); + cv::TermCriteria term_crit = cv::TermCriteria(cv::TermCriteria::COUNT | cv::TermCriteria::EPS, 30, 0.01); cv::calcOpticalFlowPyrLK(img0pyr, img1pyr, pts0_new, pts1_new, mask, error, win_size, pyr_levels, term_crit, cv::OPTFLOW_USE_INITIAL_FLOW); @@ -601,8 +712,12 @@ void TrackKLT::perform_detection_stereo(const std::vector &img0pyr, con // RIGHT: Now summarise the number of tracks in the right image // RIGHT: We will try to extract some monocular features if we have the room // RIGHT: This will also remove features if there are multiple in the same location - cv::Size size1((int)((float)img1pyr.at(0).cols / (float)min_px_dist), (int)((float)img1pyr.at(0).rows / (float)min_px_dist)); - cv::Mat grid_2d_1 = cv::Mat::zeros(size1, CV_8UC1); + cv::Size size_close1((int)((float)img1pyr.at(0).cols / (float)min_px_dist), (int)((float)img1pyr.at(0).rows / (float)min_px_dist)); + cv::Mat grid_2d_close1 = cv::Mat::zeros(size_close1, CV_8UC1); + float size_x1 = (float)img1pyr.at(0).cols / (float)grid_x; + float size_y1 = (float)img1pyr.at(0).rows / (float)grid_y; + cv::Size size_grid1(grid_x, grid_y); // width x height + cv::Mat grid_2d_grid1 = cv::Mat::zeros(size_grid1, CV_8UC1); it0 = pts1.begin(); it1 = ids1.begin(); while (it0 != pts1.end()) { @@ -610,10 +725,24 @@ void TrackKLT::perform_detection_stereo(const std::vector &img0pyr, con cv::KeyPoint kpt = *it0; int x = (int)kpt.pt.x; int y = (int)kpt.pt.y; - int x_grid = (int)(kpt.pt.x / (float)min_px_dist); - int y_grid = (int)(kpt.pt.y / (float)min_px_dist); - if (x_grid < 0 || x_grid >= size1.width || y_grid < 0 || y_grid >= size1.height || x < 0 || x >= img1pyr.at(0).cols || y < 0 || - y >= img1pyr.at(0).rows) { + int edge = 10; + if (x < edge || x >= img1pyr.at(0).cols - edge || y < edge || y >= img1pyr.at(0).rows - edge) { + it0 = pts1.erase(it0); + it1 = ids1.erase(it1); + continue; + } + // Calculate mask coordinates for close points + int x_close = (int)(kpt.pt.x / (float)min_px_dist); + int y_close = (int)(kpt.pt.y / (float)min_px_dist); + if (x_close < 0 || x_close >= size_close1.width || y_close < 0 || y_close >= size_close1.height) { + it0 = pts1.erase(it0); + it1 = ids1.erase(it1); + continue; + } + // Calculate what grid cell this feature is in + int x_grid = std::floor(kpt.pt.x / size_x1); + int y_grid = std::floor(kpt.pt.y / size_y1); + if (x_grid < 0 || x_grid >= size_grid1.width || y_grid < 0 || y_grid >= size_grid1.height) { it0 = pts1.erase(it0); it1 = ids1.erase(it1); continue; @@ -623,11 +752,12 @@ void TrackKLT::perform_detection_stereo(const std::vector &img0pyr, con // Check if this keypoint is near another point // NOTE: if it is *not* a stereo point, then we will not delete the feature // NOTE: this means we might have a mono and stereo feature near each other, but that is ok - if (grid_2d_1.at(y_grid, x_grid) > 127 && !is_stereo) { + if (grid_2d_close1.at(y_grid, x_grid) > 127 && !is_stereo) { it0 = pts1.erase(it0); it1 = ids1.erase(it1); continue; } + // Now check if it is in a mask area or not // NOTE: mask has max value of 255 (white) if it should be if (mask1.at(y, x) > 127) { @@ -636,7 +766,10 @@ void TrackKLT::perform_detection_stereo(const std::vector &img0pyr, con continue; } // Else we are good, move forward to the next point - grid_2d_1.at(y_grid, x_grid) = 255; + grid_2d_close1.at(y_grid, x_grid) = 255; + if (grid_2d_grid1.at(y_grid, x_grid) < 255) { + grid_2d_grid1.at(y_grid, x_grid) += 1; + } it0++; it1++; } @@ -644,27 +777,46 @@ void TrackKLT::perform_detection_stereo(const std::vector &img0pyr, con // RIGHT: if we need features we should extract them in the current frame // RIGHT: note that we don't track them to the left as we already did left->right tracking above int num_featsneeded_1 = num_features - (int)pts1.size(); - if (num_featsneeded_1 > std::min(75, (int)(0.2 * num_features))) { - - // Extract our features (use fast with griding) + if (num_featsneeded_1 > std::min(20, (int)(min_feat_percent * num_features))) { + + // This is old extraction code that would extract from the whole image + // This can be slow as this will recompute extractions for grid areas that we have max features already + // std::vector pts1_ext; + // Grider_FAST::perform_griding(img1pyr.at(0), mask1, pts1_ext, num_features, grid_x, grid_y, threshold, true); + + // We also check a downsampled mask such that we don't extract in areas where it is all masked! + cv::Mat mask1_grid; + cv::resize(mask1, mask1_grid, size_grid1, 0.0, 0.0, cv::INTER_NEAREST); + + // Create grids we need to extract from and then extract our features (use fast with griding) + int num_features_grid = (int)((double)num_features / (double)(grid_x * grid_y)) + 1; + int num_features_grid_req = std::max(1, (int)(min_feat_percent * num_features_grid)); + std::vector> valid_locs; + for (int x = 0; x < grid_2d_grid1.cols; x++) { + for (int y = 0; y < grid_2d_grid1.rows; y++) { + if ((int)grid_2d_grid1.at(y, x) < num_features_grid_req && (int)mask1_grid.at(y, x) != 255) { + valid_locs.emplace_back(x, y); + } + } + } std::vector pts1_ext; - Grider_FAST::perform_griding(img1pyr.at(0), mask1, pts1_ext, num_features, grid_x, grid_y, threshold, true); + Grider_GRID::perform_griding(img1pyr.at(0), mask1, valid_locs, pts1_ext, num_features, grid_x, grid_y, threshold, true); // Now, reject features that are close a current feature for (auto &kpt : pts1_ext) { // Check that it is in bounds int x_grid = (int)(kpt.pt.x / (float)min_px_dist); int y_grid = (int)(kpt.pt.y / (float)min_px_dist); - if (x_grid < 0 || x_grid >= size1.width || y_grid < 0 || y_grid >= size1.height) + if (x_grid < 0 || x_grid >= size_close1.width || y_grid < 0 || y_grid >= size_close1.height) continue; // See if there is a point at this location - if (grid_2d_1.at(y_grid, x_grid) > 127) + if (grid_2d_close1.at(y_grid, x_grid) > 127) continue; // Else lets add it! pts1.push_back(kpt); size_t temp = ++currid; ids1.push_back(temp); - grid_2d_1.at(y_grid, x_grid) = 255; + grid_2d_close1.at(y_grid, x_grid) = 255; } } } @@ -697,7 +849,7 @@ void TrackKLT::perform_matching(const std::vector &img0pyr, const std:: // Now do KLT tracking to get the valid new points std::vector mask_klt; std::vector error; - cv::TermCriteria term_crit = cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 20, 0.01); + cv::TermCriteria term_crit = cv::TermCriteria(cv::TermCriteria::COUNT | cv::TermCriteria::EPS, 30, 0.01); cv::calcOpticalFlowPyrLK(img0pyr, img1pyr, pts0, pts1, mask_klt, error, win_size, pyr_levels, term_crit, cv::OPTFLOW_USE_INITIAL_FLOW); // Normalize these points, so we can then do ransac @@ -713,7 +865,7 @@ void TrackKLT::perform_matching(const std::vector &img0pyr, const std:: double max_focallength_img0 = std::max(camera_calib.at(id0)->get_K()(0, 0), camera_calib.at(id0)->get_K()(1, 1)); double max_focallength_img1 = std::max(camera_calib.at(id1)->get_K()(0, 0), camera_calib.at(id1)->get_K()(1, 1)); double max_focallength = std::max(max_focallength_img0, max_focallength_img1); - cv::findFundamentalMat(pts0_n, pts1_n, cv::FM_RANSAC, 1.0 / max_focallength, 0.999, mask_rsc); + cv::findFundamentalMat(pts0_n, pts1_n, cv::FM_RANSAC, 2.0 / max_focallength, 0.999, mask_rsc); // Loop through and record only ones that are valid for (size_t i = 0; i < mask_klt.size(); i++) { diff --git a/ov_core/src/track/TrackKLT.h b/ov_core/src/track/TrackKLT.h index f6112edb4..46366f88e 100644 --- a/ov_core/src/track/TrackKLT.h +++ b/ov_core/src/track/TrackKLT.h @@ -44,23 +44,23 @@ class TrackKLT : public TrackBase { * @param cameras camera calibration object which has all camera intrinsics in it * @param numfeats number of features we want want to track (i.e. track 200 points from frame to frame) * @param numaruco the max id of the arucotags, so we ensure that we start our non-auroc features above this value - * @param binocular if we should do binocular feature tracking or stereo if there are multiple cameras + * @param stereo if we should do stereo feature tracking or binocular * @param histmethod what type of histogram pre-processing should be done (histogram eq?) * @param fast_threshold FAST detection threshold * @param gridx size of grid in the x-direction / u-direction * @param gridy size of grid in the y-direction / v-direction * @param minpxdist features need to be at least this number pixels away from each other */ - explicit TrackKLT(std::unordered_map> cameras, int numfeats, int numaruco, bool binocular, + explicit TrackKLT(std::unordered_map> cameras, int numfeats, int numaruco, bool stereo, HistogramMethod histmethod, int fast_threshold, int gridx, int gridy, int minpxdist) - : TrackBase(cameras, numfeats, numaruco, binocular, histmethod), threshold(fast_threshold), grid_x(gridx), grid_y(gridy), + : TrackBase(cameras, numfeats, numaruco, stereo, histmethod), threshold(fast_threshold), grid_x(gridx), grid_y(gridy), min_px_dist(minpxdist) {} /** * @brief Process a new image * @param message Contains our timestamp, images, and camera ids */ - void feed_new_camera(const CameraData &message); + void feed_new_camera(const CameraData &message) override; protected: /** @@ -140,11 +140,13 @@ class TrackKLT : public TrackBase { int min_px_dist; // How many pyramid levels to track - int pyr_levels = 3; - cv::Size win_size = cv::Size(20, 20); + int pyr_levels = 5; + cv::Size win_size = cv::Size(15, 15); // Last set of image pyramids std::map> img_pyramid_last; + std::map img_curr; + std::map> img_pyramid_curr; }; } // namespace ov_core diff --git a/ov_core/src/track/TrackSIM.cpp b/ov_core/src/track/TrackSIM.cpp index ff3e4cc53..900a048dd 100644 --- a/ov_core/src/track/TrackSIM.cpp +++ b/ov_core/src/track/TrackSIM.cpp @@ -21,6 +21,10 @@ #include "TrackSIM.h" +#include "cam/CamBase.h" +#include "feat/Feature.h" +#include "feat/FeatureDatabase.h" + using namespace ov_core; void TrackSIM::feed_measurement_simulation(double timestamp, const std::vector &camids, @@ -64,9 +68,12 @@ void TrackSIM::feed_measurement_simulation(double timestamp, const std::vectorh(); // Move forward in time - img_last[cam_id] = cv::Mat::zeros(cv::Size(width, height), CV_8UC1); - img_mask_last[cam_id] = cv::Mat::zeros(cv::Size(width, height), CV_8UC1); - pts_last[cam_id] = good_left; - ids_last[cam_id] = good_ids_left; + { + std::lock_guard lckv(mtx_last_vars); + img_last[cam_id] = cv::Mat::zeros(cv::Size(width, height), CV_8UC1); + img_mask_last[cam_id] = cv::Mat::zeros(cv::Size(width, height), CV_8UC1); + pts_last[cam_id] = good_left; + ids_last[cam_id] = good_ids_left; + } } } diff --git a/ov_core/src/track/TrackSIM.h b/ov_core/src/track/TrackSIM.h index 0f0af9fc1..b6db7b1f2 100644 --- a/ov_core/src/track/TrackSIM.h +++ b/ov_core/src/track/TrackSIM.h @@ -23,7 +23,6 @@ #define OV_CORE_TRACK_SIM_H #include "TrackBase.h" -#include "utils/print.h" namespace ov_core { diff --git a/ov_core/src/utils/print.h b/ov_core/src/utils/print.h index be789c2c3..00fa120e8 100644 --- a/ov_core/src/utils/print.h +++ b/ov_core/src/utils/print.h @@ -99,4 +99,15 @@ class Printer { #define PRINT_WARNING(x...) ov_core::Printer::debugPrint(ov_core::Printer::PrintLevel::WARNING, __FILE__, TOSTRING(__LINE__), x); #define PRINT_ERROR(x...) ov_core::Printer::debugPrint(ov_core::Printer::PrintLevel::ERROR, __FILE__, TOSTRING(__LINE__), x); +// Assert that will always be here in release builds also +// TODO: place this in a better header, just putting here for now... +#define assert_r(EX) (void)((EX) || (__assert(#EX, __FILE__, __LINE__), 0)) +#ifdef __cplusplus +extern "C" { +#endif +extern void __assert(const char *msg, const char *file, int line); +#ifdef __cplusplus +}; +#endif + #endif /* OV_CORE_PRINT_H */ diff --git a/ov_core/src/utils/quat_ops.h b/ov_core/src/utils/quat_ops.h index d595e71cb..5b027bc66 100644 --- a/ov_core/src/utils/quat_ops.h +++ b/ov_core/src/utils/quat_ops.h @@ -537,7 +537,7 @@ inline Eigen::Matrix Jr_so3(const Eigen::Matrix &w) inline Eigen::Matrix rot2rpy(const Eigen::Matrix &rot) { Eigen::Matrix rpy; rpy(1, 0) = atan2(-rot(2, 0), sqrt(rot(0, 0) * rot(0, 0) + rot(1, 0) * rot(1, 0))); - if (std::abs(cos(rpy(1, 0)) > 1.0e-12)) { + if (std::abs(cos(rpy(1, 0))) > 1.0e-12) { rpy(2, 0) = atan2(rot(1, 0) / cos(rpy(1, 0)), rot(0, 0) / cos(rpy(1, 0))); rpy(0, 0) = atan2(rot(2, 1) / cos(rpy(1, 0)), rot(2, 2) / cos(rpy(1, 0))); } else { diff --git a/ov_eval/CMakeLists.txt b/ov_eval/CMakeLists.txt index 27abb8920..b212058fb 100644 --- a/ov_eval/CMakeLists.txt +++ b/ov_eval/CMakeLists.txt @@ -25,6 +25,11 @@ set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) +# Default build as release +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE "Release" CACHE STRING "Choose the type of build, options are: Debug Release RelWithDebInfo MinSizeRel." FORCE) +endif() + # Enable compile optimizations set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops") diff --git a/ov_eval/src/calc/ResultTrajectory.cpp b/ov_eval/src/calc/ResultTrajectory.cpp index dabb36eae..d772765e3 100644 --- a/ov_eval/src/calc/ResultTrajectory.cpp +++ b/ov_eval/src/calc/ResultTrajectory.cpp @@ -253,8 +253,8 @@ void ResultTrajectory::calculate_nees(Statistics &nees_ori, Statistics &nees_pos // Calculate orientation error // NOTE: we define our error as e_R = -Log(R*Rhat^T) - Eigen::Matrix3d e_R = - ov_core::quat_2_Rot(gt_poses.at(i).block(3, 0, 4, 1)) * ov_core::quat_2_Rot(est_poses.at(i).block(3, 0, 4, 1)).transpose(); + Eigen::Matrix3d e_R = ov_core::quat_2_Rot(gt_poses_aignedtoEST.at(i).block(3, 0, 4, 1)) * + ov_core::quat_2_Rot(est_poses.at(i).block(3, 0, 4, 1)).transpose(); Eigen::Vector3d errori = -ov_core::log_so3(e_R); // Eigen::Vector4d e_q = Math::quat_multiply(gt_poses_aignedtoEST.at(i).block(3,0,4,1),Math::Inv(est_poses.at(i).block(3,0,4,1))); // Eigen::Vector3d errori = 2*e_q.block(0,0,3,1); diff --git a/ov_init/CMakeLists.txt b/ov_init/CMakeLists.txt index 7df06547a..da96cc151 100644 --- a/ov_init/CMakeLists.txt +++ b/ov_init/CMakeLists.txt @@ -33,6 +33,11 @@ set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) +# Default build as release +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE "Release" CACHE STRING "Choose the type of build, options are: Debug Release RelWithDebInfo MinSizeRel." FORCE) +endif() + # Enable compile optimizations set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops") diff --git a/ov_init/cmake/ROS1.cmake b/ov_init/cmake/ROS1.cmake index 731469383..34916e93a 100644 --- a/ov_init/cmake/ROS1.cmake +++ b/ov_init/cmake/ROS1.cmake @@ -36,7 +36,7 @@ list(APPEND thirdparty_libraries ${OpenCV_LIBRARIES} ${CERES_LIBRARIES} ${catkin_LIBRARIES} -) + ) # If we are not building with ROS then we need to manually link to its headers # This isn't that elegant of a way, but this at least allows for building without ROS @@ -60,11 +60,15 @@ endif () list(APPEND LIBRARY_SOURCES src/dummy.cpp + src/ceres/Factor_GenericPrior.cpp + src/ceres/Factor_ImageReprojCalib.cpp + src/ceres/Factor_ImuCPIv1.cpp + src/ceres/State_JPLQuatLocal.cpp src/init/InertialInitializer.cpp src/dynamic/DynamicInitializer.cpp src/static/StaticInitializer.cpp - src/sim/Simulator.cpp -) + src/sim/SimulatorInit.cpp + ) file(GLOB_RECURSE LIBRARY_HEADERS "src/*.h") add_library(ov_init_lib SHARED ${LIBRARY_SOURCES} ${LIBRARY_HEADERS}) target_link_libraries(ov_init_lib ${thirdparty_libraries}) @@ -73,11 +77,11 @@ install(TARGETS ov_init_lib ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) + ) install(DIRECTORY src/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" -) + ) ################################################## @@ -90,7 +94,7 @@ install(TARGETS test_simulation ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) + ) add_executable(test_dynamic_mle src/test_dynamic_mle.cpp) target_link_libraries(test_dynamic_mle ov_init_lib ${thirdparty_libraries}) @@ -98,7 +102,7 @@ install(TARGETS test_dynamic_mle ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) + ) add_executable(test_dynamic_init src/test_dynamic_init.cpp) target_link_libraries(test_dynamic_init ov_init_lib ${thirdparty_libraries}) @@ -106,6 +110,6 @@ install(TARGETS test_dynamic_init ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) + ) diff --git a/ov_init/cmake/ROS2.cmake b/ov_init/cmake/ROS2.cmake index cdfc841c1..7e440d697 100644 --- a/ov_init/cmake/ROS2.cmake +++ b/ov_init/cmake/ROS2.cmake @@ -26,7 +26,7 @@ list(APPEND thirdparty_libraries ${Boost_LIBRARIES} ${CERES_LIBRARIES} ${OpenCV_LIBRARIES} -) + ) ################################################## # Make the shared library @@ -34,11 +34,15 @@ list(APPEND thirdparty_libraries list(APPEND LIBRARY_SOURCES src/dummy.cpp + src/ceres/Factor_GenericPrior.cpp + src/ceres/Factor_ImageReprojCalib.cpp + src/ceres/Factor_ImuCPIv1.cpp + src/ceres/State_JPLQuatLocal.cpp src/init/InertialInitializer.cpp src/dynamic/DynamicInitializer.cpp src/static/StaticInitializer.cpp - src/sim/Simulator.cpp -) + src/sim/SimulatorInit.cpp + ) file(GLOB_RECURSE LIBRARY_HEADERS "src/*.h") add_library(ov_init_lib SHARED ${LIBRARY_SOURCES} ${LIBRARY_HEADERS}) ament_target_dependencies(ov_init_lib rclcpp ov_core cv_bridge) @@ -48,11 +52,11 @@ install(TARGETS ov_init_lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin PUBLIC_HEADER DESTINATION include -) + ) install(DIRECTORY src/ DESTINATION include FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" -) + ) ament_export_include_directories(include) ament_export_libraries(ov_init_lib) diff --git a/ov_init/src/ceres/Factor_GenericPrior.cpp b/ov_init/src/ceres/Factor_GenericPrior.cpp new file mode 100644 index 000000000..bd73f1fa5 --- /dev/null +++ b/ov_init/src/ceres/Factor_GenericPrior.cpp @@ -0,0 +1,163 @@ +/* + * OpenVINS: An Open Platform for Visual-Inertial Research + * Copyright (C) 2018-2022 Patrick Geneva + * Copyright (C) 2018-2022 Guoquan Huang + * Copyright (C) 2018-2022 OpenVINS Contributors + * Copyright (C) 2018-2019 Kevin Eckenhoff + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include "Factor_GenericPrior.h" + +#include "utils/quat_ops.h" + +using namespace ov_init; + +Factor_GenericPrior::Factor_GenericPrior(const Eigen::MatrixXd &x_lin_, const std::vector &x_type_, + const Eigen::MatrixXd &prior_Info, const Eigen::MatrixXd &prior_grad) + : x_lin(x_lin_), x_type(x_type_) { + + // First assert that our state and variables are of the correct size + int state_size = 0; + int state_error_size = 0; + for (auto const &str : x_type_) { + if (str == "quat") { + state_size += 4; + state_error_size += 3; + } else if (str == "quat_yaw") { + state_size += 4; + state_error_size += 1; + } else if (str == "vec3") { + state_size += 3; + state_error_size += 3; + } else if (str == "vec8") { + state_size += 8; + state_error_size += 8; + } else { + std::cerr << "type - " << str << " not implemented in prior" << std::endl; + std::exit(EXIT_FAILURE); + } + } + assert(x_lin.rows() == state_size); + assert(x_lin.cols() == 1); + assert(prior_Info.rows() == state_error_size); + assert(prior_Info.cols() == state_error_size); + assert(prior_grad.rows() == state_error_size); + assert(prior_grad.cols() == 1); + + // Now lets base-compute the square-root information and constant term b + // Comes from the form: cost = A * (x - x_lin) + b + Eigen::LLT lltOfI(prior_Info); + sqrtI = lltOfI.matrixL().transpose(); + Eigen::MatrixXd I = Eigen::MatrixXd::Identity(prior_Info.rows(), prior_Info.rows()); + b = sqrtI.triangularView().solve(I) * prior_grad; + + // Check that we have a valid matrix that we can get the information of + if (std::isnan(prior_Info.norm()) || std::isnan(sqrtI.norm()) || std::isnan(b.norm())) { + std::cerr << "prior_Info - " << std::endl << prior_Info << std::endl << std::endl; + std::cerr << "prior_Info_inv - " << std::endl << prior_Info.inverse() << std::endl << std::endl; + std::cerr << "b - " << std::endl << b << std::endl << std::endl; + std::exit(EXIT_FAILURE); + } + + // Set the number of measurements, and the block sized + set_num_residuals(state_error_size); + for (auto const &str : x_type_) { + if (str == "quat") + mutable_parameter_block_sizes()->push_back(4); + if (str == "quat_yaw") + mutable_parameter_block_sizes()->push_back(4); + if (str == "vec3") + mutable_parameter_block_sizes()->push_back(3); + if (str == "vec8") + mutable_parameter_block_sizes()->push_back(8); + } +} + +bool Factor_GenericPrior::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const { + + // Location in our state and output residual + int local_it = 0; + int global_it = 0; + Eigen::MatrixXd res = Eigen::MatrixXd::Zero(num_residuals(), 1); + + // Loop through each state and calculate its residual and Jacobian + for (size_t i = 0; i < x_type.size(); i++) { + if (x_type[i] == "quat") { + Eigen::Vector4d q_i = Eigen::Map(parameters[i]); + Eigen::Matrix3d R_i = ov_core::quat_2_Rot(q_i); + Eigen::Matrix3d R_lin = ov_core::quat_2_Rot(x_lin.block(global_it, 0, 4, 1)); + Eigen::Vector3d theta_err = ov_core::log_so3(R_i.transpose() * R_lin); + res.block(local_it, 0, 3, 1) = -theta_err; + if (jacobians && jacobians[i]) { + Eigen::Map> jacobian(jacobians[i], num_residuals(), 4); + jacobian.setZero(); + Eigen::Matrix3d Jr_inv = ov_core::Jr_so3(theta_err).inverse(); + Eigen::Matrix3d H_theta = -Jr_inv * R_lin.transpose(); + jacobian.block(0, 0, num_residuals(), 3) = sqrtI.block(0, local_it, num_residuals(), 3) * H_theta; + } + global_it += 4; + local_it += 3; + } else if (x_type[i] == "quat_yaw") { + Eigen::Vector3d ez = Eigen::Vector3d(0.0, 0.0, 1.0); + Eigen::Vector4d q_i = Eigen::Map(parameters[i]); + Eigen::Matrix3d R_i = ov_core::quat_2_Rot(q_i); + Eigen::Matrix3d R_lin = ov_core::quat_2_Rot(x_lin.block(global_it, 0, 4, 1)); + Eigen::Vector3d theta_err = ov_core::log_so3(R_i.transpose() * R_lin); + res(local_it, 0) = -(ez.transpose() * theta_err)(0, 0); + if (jacobians && jacobians[i]) { + Eigen::Map> jacobian(jacobians[i], num_residuals(), 4); + jacobian.setZero(); + Eigen::Matrix3d Jr_inv = ov_core::Jr_so3(theta_err).inverse(); + Eigen::Matrix H_theta = -ez.transpose() * (Jr_inv * R_lin.transpose()); + jacobian.block(0, 0, num_residuals(), 3) = sqrtI.block(0, local_it, num_residuals(), 1) * H_theta; + } + global_it += 4; + local_it += 1; + } else if (x_type[i] == "vec3") { + Eigen::Matrix p_i = Eigen::Map>(parameters[i]); + res.block(local_it, 0, 3, 1) = p_i - x_lin.block(global_it, 0, 3, 1); + if (jacobians && jacobians[i]) { + Eigen::Map> jacobian(jacobians[i], num_residuals(), 3); + jacobian.block(0, 0, num_residuals(), 3) = sqrtI.block(0, local_it, num_residuals(), 3); + } + global_it += 3; + local_it += 3; + } else if (x_type[i] == "vec8") { + Eigen::Matrix p_i = Eigen::Map>(parameters[i]); + res.block(local_it, 0, 8, 1) = p_i - x_lin.block(global_it, 0, 8, 1); + if (jacobians && jacobians[i]) { + Eigen::Map> jacobian(jacobians[i], num_residuals(), 8); + jacobian.block(0, 0, num_residuals(), 8) = sqrtI.block(0, local_it, num_residuals(), 8); + } + global_it += 8; + local_it += 8; + } else { + std::cerr << "type - " << x_type[i] << " not implemented in prior" << std::endl; + std::exit(EXIT_FAILURE); + } + } + + // Now that we have done x - x_lin we need to multiply by sqrtI and add b to get full cost + // Jacobians will already have sqrtI applied to them... + res = sqrtI * res; + res += b; + + // Store the residuals into ceres + for (int i = 0; i < res.rows(); i++) { + residuals[i] = res(i, 0); + } + return true; +} \ No newline at end of file diff --git a/ov_init/src/ceres/Factor_GenericPrior.h b/ov_init/src/ceres/Factor_GenericPrior.h index 086ed7223..c650c9ebc 100644 --- a/ov_init/src/ceres/Factor_GenericPrior.h +++ b/ov_init/src/ceres/Factor_GenericPrior.h @@ -24,8 +24,6 @@ #include -#include "utils/quat_ops.h" - namespace ov_init { /** @@ -89,146 +87,14 @@ class Factor_GenericPrior : public ceres::CostFunction { * @brief Default constructor */ Factor_GenericPrior(const Eigen::MatrixXd &x_lin_, const std::vector &x_type_, const Eigen::MatrixXd &prior_Info, - const Eigen::MatrixXd &prior_grad) - : x_lin(x_lin_), x_type(x_type_) { - - // First assert that our state and variables are of the correct size - int state_size = 0; - int state_error_size = 0; - for (auto const &str : x_type_) { - if (str == "quat") { - state_size += 4; - state_error_size += 3; - } else if (str == "quat_yaw") { - state_size += 4; - state_error_size += 1; - } else if (str == "vec3") { - state_size += 3; - state_error_size += 3; - } else if (str == "vec8") { - state_size += 8; - state_error_size += 8; - } else { - std::cerr << "type - " << str << " not implemented in prior" << std::endl; - std::exit(EXIT_FAILURE); - } - } - assert(x_lin.rows() == state_size); - assert(x_lin.cols() == 1); - assert(prior_Info.rows() == state_error_size); - assert(prior_Info.cols() == state_error_size); - assert(prior_grad.rows() == state_error_size); - assert(prior_grad.cols() == 1); - - // Now lets base-compute the square-root information and constant term b - // Comes from the form: cost = A * (x - x_lin) + b - Eigen::LLT lltOfI(prior_Info); - sqrtI = lltOfI.matrixL().transpose(); - Eigen::MatrixXd I = Eigen::MatrixXd::Identity(prior_Info.rows(), prior_Info.rows()); - b = sqrtI.triangularView().solve(I) * prior_grad; - - // Check that we have a valid matrix that we can get the information of - if (std::isnan(prior_Info.norm()) || std::isnan(sqrtI.norm()) || std::isnan(b.norm())) { - std::cerr << "prior_Info - " << std::endl << prior_Info << std::endl << std::endl; - std::cerr << "prior_Info_inv - " << std::endl << prior_Info.inverse() << std::endl << std::endl; - std::cerr << "b - " << std::endl << b << std::endl << std::endl; - std::exit(EXIT_FAILURE); - } - - // Set the number of measurements, and the block sized - set_num_residuals(state_error_size); - for (auto const &str : x_type_) { - if (str == "quat") - mutable_parameter_block_sizes()->push_back(4); - if (str == "quat_yaw") - mutable_parameter_block_sizes()->push_back(4); - if (str == "vec3") - mutable_parameter_block_sizes()->push_back(3); - if (str == "vec8") - mutable_parameter_block_sizes()->push_back(8); - } - } + const Eigen::MatrixXd &prior_grad); virtual ~Factor_GenericPrior() {} /** * @brief Error residual and Jacobian calculation */ - bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const override { - - // Location in our state and output residual - int local_it = 0; - int global_it = 0; - Eigen::MatrixXd res = Eigen::MatrixXd::Zero(num_residuals(), 1); - - // Loop through each state and calculate its residual and Jacobian - for (size_t i = 0; i < x_type.size(); i++) { - if (x_type[i] == "quat") { - Eigen::Vector4d q_i = Eigen::Map(parameters[i]); - Eigen::Matrix3d R_i = ov_core::quat_2_Rot(q_i); - Eigen::Matrix3d R_lin = ov_core::quat_2_Rot(x_lin.block(global_it, 0, 4, 1)); - Eigen::Vector3d theta_err = ov_core::log_so3(R_i.transpose() * R_lin); - res.block(local_it, 0, 3, 1) = -theta_err; - if (jacobians && jacobians[i]) { - Eigen::Map> jacobian(jacobians[i], num_residuals(), 4); - jacobian.setZero(); - Eigen::Matrix3d Jr_inv = ov_core::Jr_so3(theta_err).inverse(); - Eigen::Matrix3d H_theta = -Jr_inv * R_lin.transpose(); - jacobian.block(0, 0, num_residuals(), 3) = sqrtI.block(0, local_it, num_residuals(), 3) * H_theta; - } - global_it += 4; - local_it += 3; - } else if (x_type[i] == "quat_yaw") { - Eigen::Vector3d ez = Eigen::Vector3d(0.0, 0.0, 1.0); - Eigen::Vector4d q_i = Eigen::Map(parameters[i]); - Eigen::Matrix3d R_i = ov_core::quat_2_Rot(q_i); - Eigen::Matrix3d R_lin = ov_core::quat_2_Rot(x_lin.block(global_it, 0, 4, 1)); - Eigen::Vector3d theta_err = ov_core::log_so3(R_i.transpose() * R_lin); - res(local_it, 0) = -(ez.transpose() * theta_err)(0, 0); - if (jacobians && jacobians[i]) { - Eigen::Map> jacobian(jacobians[i], num_residuals(), 4); - jacobian.setZero(); - Eigen::Matrix3d Jr_inv = ov_core::Jr_so3(theta_err).inverse(); - Eigen::Matrix H_theta = -ez.transpose() * (Jr_inv * R_lin.transpose()); - jacobian.block(0, 0, num_residuals(), 3) = sqrtI.block(0, local_it, num_residuals(), 1) * H_theta; - } - global_it += 4; - local_it += 1; - } else if (x_type[i] == "vec3") { - Eigen::Matrix p_i = Eigen::Map>(parameters[i]); - res.block(local_it, 0, 3, 1) = p_i - x_lin.block(global_it, 0, 3, 1); - if (jacobians && jacobians[i]) { - Eigen::Map> jacobian(jacobians[i], num_residuals(), 3); - jacobian.block(0, 0, num_residuals(), 3) = sqrtI.block(0, local_it, num_residuals(), 3); - } - global_it += 3; - local_it += 3; - } else if (x_type[i] == "vec8") { - Eigen::Matrix p_i = Eigen::Map>(parameters[i]); - res.block(local_it, 0, 8, 1) = p_i - x_lin.block(global_it, 0, 8, 1); - if (jacobians && jacobians[i]) { - Eigen::Map> jacobian(jacobians[i], num_residuals(), 8); - jacobian.block(0, 0, num_residuals(), 8) = sqrtI.block(0, local_it, num_residuals(), 8); - } - global_it += 8; - local_it += 8; - } else { - std::cerr << "type - " << x_type[i] << " not implemented in prior" << std::endl; - std::exit(EXIT_FAILURE); - } - } - - // Now that we have done x - x_lin we need to multiply by sqrtI and add b to get full cost - // Jacobians will already have sqrtI applied to them... - res = sqrtI * res; - res += b; - - // Store the residuals into ceres - for (int i = 0; i < res.rows(); i++) { - residuals[i] = res(i, 0); - } - return true; - } + bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const override; }; } // namespace ov_init diff --git a/ov_init/src/ceres/Factor_ImageReprojCalib.cpp b/ov_init/src/ceres/Factor_ImageReprojCalib.cpp new file mode 100644 index 000000000..b10655cbc --- /dev/null +++ b/ov_init/src/ceres/Factor_ImageReprojCalib.cpp @@ -0,0 +1,152 @@ +/* + * OpenVINS: An Open Platform for Visual-Inertial Research + * Copyright (C) 2018-2022 Patrick Geneva + * Copyright (C) 2018-2022 Guoquan Huang + * Copyright (C) 2018-2022 OpenVINS Contributors + * Copyright (C) 2018-2019 Kevin Eckenhoff + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include "Factor_ImageReprojCalib.h" + +#include "utils/quat_ops.h" + +using namespace ov_init; + +Factor_ImageReprojCalib::Factor_ImageReprojCalib(const Eigen::Vector2d &uv_meas_, double pix_sigma_, bool is_fisheye_) + : uv_meas(uv_meas_), pix_sigma(pix_sigma_), is_fisheye(is_fisheye_) { + + // Square root information inverse + sqrtQ = Eigen::Matrix::Identity(); + sqrtQ(0, 0) *= 1.0 / pix_sigma; + sqrtQ(1, 1) *= 1.0 / pix_sigma; + + // Parameters we are a function of + set_num_residuals(2); + mutable_parameter_block_sizes()->push_back(4); // q_GtoIi + mutable_parameter_block_sizes()->push_back(3); // p_IiinG + mutable_parameter_block_sizes()->push_back(3); // p_FinG + mutable_parameter_block_sizes()->push_back(4); // q_ItoC + mutable_parameter_block_sizes()->push_back(3); // p_IinC + mutable_parameter_block_sizes()->push_back(8); // focal, center, distortion +} + +bool Factor_ImageReprojCalib::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const { + + // Recover the current state from our parameters + Eigen::Vector4d q_GtoIi = Eigen::Map(parameters[0]); + Eigen::Matrix3d R_GtoIi = ov_core::quat_2_Rot(q_GtoIi); + Eigen::Vector3d p_IiinG = Eigen::Map(parameters[1]); + Eigen::Vector3d p_FinG = Eigen::Map(parameters[2]); + Eigen::Vector4d q_ItoC = Eigen::Map(parameters[3]); + Eigen::Matrix3d R_ItoC = ov_core::quat_2_Rot(q_ItoC); + Eigen::Vector3d p_IinC = Eigen::Map(parameters[4]); + + // order: f_x & f_y & c_x & c_y & k_1 & k_2 & k_3 & k_4 + Eigen::Matrix camera_vals = Eigen::Map>(parameters[5]); + + // Transform the feature into the current camera frame of reference + Eigen::Vector3d p_FinIi = R_GtoIi * (p_FinG - p_IiinG); + Eigen::Vector3d p_FinCi = R_ItoC * p_FinIi + p_IinC; + + // Normalized projected feature bearing + Eigen::Vector2d uv_norm; + uv_norm << p_FinCi(0) / p_FinCi(2), p_FinCi(1) / p_FinCi(2); + + // Square-root information and gate + Eigen::Matrix sqrtQ_gate = gate * sqrtQ; + + // Get the distorted raw image coordinate using the camera model + // Also if jacobians are requested, then compute derivatives + Eigen::Vector2d uv_dist; + Eigen::MatrixXd H_dz_dzn, H_dz_dzeta; + if (is_fisheye) { + ov_core::CamEqui cam(0, 0); + cam.set_value(camera_vals); + uv_dist = cam.distort_d(uv_norm); + if (jacobians) { + cam.compute_distort_jacobian(uv_norm, H_dz_dzn, H_dz_dzeta); + H_dz_dzn = sqrtQ_gate * H_dz_dzn; + H_dz_dzeta = sqrtQ_gate * H_dz_dzeta; + } + } else { + ov_core::CamRadtan cam(0, 0); + cam.set_value(camera_vals); + uv_dist = cam.distort_d(uv_norm); + if (jacobians) { + cam.compute_distort_jacobian(uv_norm, H_dz_dzn, H_dz_dzeta); + H_dz_dzn = sqrtQ_gate * H_dz_dzn; + H_dz_dzeta = sqrtQ_gate * H_dz_dzeta; + } + } + + // Compute residual + // NOTE: we make this negative ceres cost function is only min||f(x)||^2 + // NOTE: thus since we have found the derivative of uv_meas = f(x) + n + // NOTE: we need to reformulate into a zero error constraint 0 = f(x) + n - uv_meas + // NOTE: if it was the other way (0 = uv_meas - f(x) - n) all the Jacobians would need to be flipped + Eigen::Vector2d res = uv_dist - uv_meas; + res = sqrtQ_gate * res; + residuals[0] = res(0); + residuals[1] = res(1); + + // Compute jacobians if requested by ceres + if (jacobians) { + + // Normalized coordinates in respect to projection function + Eigen::MatrixXd H_dzn_dpfc = Eigen::MatrixXd::Zero(2, 3); + H_dzn_dpfc << 1.0 / p_FinCi(2), 0, -p_FinCi(0) / std::pow(p_FinCi(2), 2), 0, 1.0 / p_FinCi(2), -p_FinCi(1) / std::pow(p_FinCi(2), 2); + Eigen::MatrixXd H_dz_dpfc = H_dz_dzn * H_dzn_dpfc; + + // Jacobian wrt q_GtoIi + if (jacobians[0]) { + Eigen::Map> jacobian(jacobians[0]); + jacobian.block(0, 0, 2, 3) = H_dz_dpfc * R_ItoC * ov_core::skew_x(p_FinIi); + jacobian.block(0, 3, 2, 1).setZero(); + } + + // Jacobian wrt p_IiinG + if (jacobians[1]) { + Eigen::Map> jacobian(jacobians[1]); + jacobian.block(0, 0, 2, 3) = -H_dz_dpfc * R_ItoC * R_GtoIi; + } + + // Jacobian wrt feature p_FinG + if (jacobians[2]) { + Eigen::Map> jacobian(jacobians[2]); + jacobian.block(0, 0, 2, 3) = H_dz_dpfc * R_ItoC * R_GtoIi; + } + + // Jacbian wrt IMU-camera transform q_ItoC + if (jacobians[3]) { + Eigen::Map> jacobian(jacobians[3]); + jacobian.block(0, 0, 2, 3) = H_dz_dpfc * ov_core::skew_x(R_ItoC * p_FinIi); + jacobian.block(0, 3, 2, 1).setZero(); + } + + // Jacbian wrt IMU-camera transform p_IinC + if (jacobians[4]) { + Eigen::Map> jacobian(jacobians[4]); + jacobian.block(0, 0, 2, 3) = H_dz_dpfc; + } + + // Jacbian wrt camera intrinsic + if (jacobians[5]) { + Eigen::Map> jacobian(jacobians[5]); + jacobian.block(0, 0, 2, 8) = H_dz_dzeta; + } + } + return true; +} \ No newline at end of file diff --git a/ov_init/src/ceres/Factor_ImageReprojCalib.h b/ov_init/src/ceres/Factor_ImageReprojCalib.h index 0ae115b4e..af4810fac 100644 --- a/ov_init/src/ceres/Factor_ImageReprojCalib.h +++ b/ov_init/src/ceres/Factor_ImageReprojCalib.h @@ -59,23 +59,7 @@ class Factor_ImageReprojCalib : public ceres::CostFunction { * @param pix_sigma_ Raw pixel measurement uncertainty (typically 1) * @param is_fisheye_ If this raw pixel camera uses fisheye distortion */ - Factor_ImageReprojCalib(const Eigen::Vector2d &uv_meas_, double pix_sigma_, bool is_fisheye_) - : uv_meas(uv_meas_), pix_sigma(pix_sigma_), is_fisheye(is_fisheye_) { - - // Square root information inverse - sqrtQ = Eigen::Matrix::Identity(); - sqrtQ(0, 0) *= 1.0 / pix_sigma; - sqrtQ(1, 1) *= 1.0 / pix_sigma; - - // Parameters we are a function of - set_num_residuals(2); - mutable_parameter_block_sizes()->push_back(4); // q_GtoIi - mutable_parameter_block_sizes()->push_back(3); // p_IiinG - mutable_parameter_block_sizes()->push_back(3); // p_FinG - mutable_parameter_block_sizes()->push_back(4); // q_ItoC - mutable_parameter_block_sizes()->push_back(3); // p_IinC - mutable_parameter_block_sizes()->push_back(8); // focal, center, distortion - } + Factor_ImageReprojCalib(const Eigen::Vector2d &uv_meas_, double pix_sigma_, bool is_fisheye_); virtual ~Factor_ImageReprojCalib() {} @@ -87,113 +71,7 @@ class Factor_ImageReprojCalib : public ceres::CostFunction { * The normalized pixel coordinates are found and then distorted using the camera distortion model. * See the @ref update-feat page for more details. */ - bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const override { - - // Recover the current state from our parameters - Eigen::Vector4d q_GtoIi = Eigen::Map(parameters[0]); - Eigen::Matrix3d R_GtoIi = ov_core::quat_2_Rot(q_GtoIi); - Eigen::Vector3d p_IiinG = Eigen::Map(parameters[1]); - Eigen::Vector3d p_FinG = Eigen::Map(parameters[2]); - Eigen::Vector4d q_ItoC = Eigen::Map(parameters[3]); - Eigen::Matrix3d R_ItoC = ov_core::quat_2_Rot(q_ItoC); - Eigen::Vector3d p_IinC = Eigen::Map(parameters[4]); - - // order: f_x & f_y & c_x & c_y & k_1 & k_2 & k_3 & k_4 - Eigen::Matrix camera_vals = Eigen::Map>(parameters[5]); - - // Transform the feature into the current camera frame of reference - Eigen::Vector3d p_FinIi = R_GtoIi * (p_FinG - p_IiinG); - Eigen::Vector3d p_FinCi = R_ItoC * p_FinIi + p_IinC; - - // Normalized projected feature bearing - Eigen::Vector2d uv_norm; - uv_norm << p_FinCi(0) / p_FinCi(2), p_FinCi(1) / p_FinCi(2); - - // Square-root information and gate - Eigen::Matrix sqrtQ_gate = gate * sqrtQ; - - // Get the distorted raw image coordinate using the camera model - // Also if jacobians are requested, then compute derivatives - Eigen::Vector2d uv_dist; - Eigen::MatrixXd H_dz_dzn, H_dz_dzeta; - if (is_fisheye) { - ov_core::CamEqui cam(0, 0); - cam.set_value(camera_vals); - uv_dist = cam.distort_d(uv_norm); - if (jacobians) { - cam.compute_distort_jacobian(uv_norm, H_dz_dzn, H_dz_dzeta); - H_dz_dzn = sqrtQ_gate * H_dz_dzn; - H_dz_dzeta = sqrtQ_gate * H_dz_dzeta; - } - } else { - ov_core::CamRadtan cam(0, 0); - cam.set_value(camera_vals); - uv_dist = cam.distort_d(uv_norm); - if (jacobians) { - cam.compute_distort_jacobian(uv_norm, H_dz_dzn, H_dz_dzeta); - H_dz_dzn = sqrtQ_gate * H_dz_dzn; - H_dz_dzeta = sqrtQ_gate * H_dz_dzeta; - } - } - - // Compute residual - // NOTE: we make this negative ceres cost function is only min||f(x)||^2 - // NOTE: thus since we have found the derivative of uv_meas = f(x) + n - // NOTE: we need to reformulate into a zero error constraint 0 = f(x) + n - uv_meas - // NOTE: if it was the other way (0 = uv_meas - f(x) - n) all the Jacobians would need to be flipped - Eigen::Vector2d res = uv_dist - uv_meas; - res = sqrtQ_gate * res; - residuals[0] = res(0); - residuals[1] = res(1); - - // Compute jacobians if requested by ceres - if (jacobians) { - - // Normalized coordinates in respect to projection function - Eigen::MatrixXd H_dzn_dpfc = Eigen::MatrixXd::Zero(2, 3); - H_dzn_dpfc << 1.0 / p_FinCi(2), 0, -p_FinCi(0) / std::pow(p_FinCi(2), 2), 0, 1.0 / p_FinCi(2), -p_FinCi(1) / std::pow(p_FinCi(2), 2); - Eigen::MatrixXd H_dz_dpfc = H_dz_dzn * H_dzn_dpfc; - - // Jacobian wrt q_GtoIi - if (jacobians[0]) { - Eigen::Map> jacobian(jacobians[0]); - jacobian.block(0, 0, 2, 3) = H_dz_dpfc * R_ItoC * ov_core::skew_x(p_FinIi); - jacobian.block(0, 3, 2, 1).setZero(); - } - - // Jacobian wrt p_IiinG - if (jacobians[1]) { - Eigen::Map> jacobian(jacobians[1]); - jacobian.block(0, 0, 2, 3) = -H_dz_dpfc * R_ItoC * R_GtoIi; - } - - // Jacobian wrt feature p_FinG - if (jacobians[2]) { - Eigen::Map> jacobian(jacobians[2]); - jacobian.block(0, 0, 2, 3) = H_dz_dpfc * R_ItoC * R_GtoIi; - } - - // Jacbian wrt IMU-camera transform q_ItoC - if (jacobians[3]) { - Eigen::Map> jacobian(jacobians[3]); - jacobian.block(0, 0, 2, 3) = H_dz_dpfc * ov_core::skew_x(R_ItoC * p_FinIi); - jacobian.block(0, 3, 2, 1).setZero(); - } - - // Jacbian wrt IMU-camera transform p_IinC - if (jacobians[4]) { - Eigen::Map> jacobian(jacobians[4]); - jacobian.block(0, 0, 2, 3) = H_dz_dpfc; - } - - // Jacbian wrt camera intrinsic - if (jacobians[5]) { - Eigen::Map> jacobian(jacobians[5]); - jacobian.block(0, 0, 2, 8) = H_dz_dzeta; - } - } - return true; - } + bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const override; }; } // namespace ov_init diff --git a/ov_init/src/ceres/Factor_ImuCPIv1.cpp b/ov_init/src/ceres/Factor_ImuCPIv1.cpp new file mode 100644 index 000000000..e354984c7 --- /dev/null +++ b/ov_init/src/ceres/Factor_ImuCPIv1.cpp @@ -0,0 +1,264 @@ +/* + * OpenVINS: An Open Platform for Visual-Inertial Research + * Copyright (C) 2018-2022 Patrick Geneva + * Copyright (C) 2018-2022 Guoquan Huang + * Copyright (C) 2018-2022 OpenVINS Contributors + * Copyright (C) 2018-2019 Kevin Eckenhoff + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include "Factor_ImuCPIv1.h" + +#include "utils/quat_ops.h" + +using namespace ov_init; + +Factor_ImuCPIv1::Factor_ImuCPIv1(double deltatime, Eigen::Vector3d &grav, Eigen::Vector3d &alpha, Eigen::Vector3d &beta, + Eigen::Vector4d &q_KtoK1, Eigen::Vector3d &ba_lin, Eigen::Vector3d &bg_lin, Eigen::Matrix3d &J_q, + Eigen::Matrix3d &J_beta, Eigen::Matrix3d &J_alpha, Eigen::Matrix3d &H_beta, Eigen::Matrix3d &H_alpha, + Eigen::Matrix &covariance) { + // Save measurements + this->alpha = alpha; + this->beta = beta; + this->q_breve = q_KtoK1; + this->dt = deltatime; + this->grav_save = grav; + + // Linearization points + this->b_a_lin_save = ba_lin; + this->b_w_lin_save = bg_lin; + + // Save bias jacobians + this->J_q = J_q; + this->J_a = J_alpha; + this->J_b = J_beta; + this->H_a = H_alpha; + this->H_b = H_beta; + + // Check that we have a valid covariance matrix that we can get the information of + Eigen::MatrixXd I = Eigen::MatrixXd::Identity(covariance.rows(), covariance.rows()); + Eigen::MatrixXd information = covariance.llt().solve(I); + if (std::isnan(information.norm())) { + std::cerr << "P - " << std::endl << covariance << std::endl << std::endl; + std::cerr << "Pinv - " << std::endl << covariance.inverse() << std::endl << std::endl; + std::exit(EXIT_FAILURE); + } + + // Get square-root of our information matrix + Eigen::LLT lltOfI(information); + sqrtI_save = lltOfI.matrixL().transpose(); + + // Set the number of measurements, and the block sized + set_num_residuals(15); + mutable_parameter_block_sizes()->push_back(4); // q_GtoI1 + mutable_parameter_block_sizes()->push_back(3); // bg_1 + mutable_parameter_block_sizes()->push_back(3); // v_I1inG + mutable_parameter_block_sizes()->push_back(3); // ba_1 + mutable_parameter_block_sizes()->push_back(3); // p_I1inG + mutable_parameter_block_sizes()->push_back(4); // q_GtoI2 + mutable_parameter_block_sizes()->push_back(3); // bg_2 + mutable_parameter_block_sizes()->push_back(3); // v_I2inG + mutable_parameter_block_sizes()->push_back(3); // ba_2 + mutable_parameter_block_sizes()->push_back(3); // p_I2inG +} + +bool Factor_ImuCPIv1::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const { + + // Get the local variables (these would be different if we relinearized) + Eigen::Vector3d gravity = grav_save; + Eigen::Matrix sqrtI = sqrtI_save; + Eigen::Vector3d b_w_lin = b_w_lin_save; + Eigen::Vector3d b_a_lin = b_a_lin_save; + + // Get the state estimates from the ceres parameters. + // Each clone is stored in the canonical VINS format: q, bw, v, ba, p + Eigen::Vector4d q_1 = Eigen::Map(parameters[0]); + Eigen::Matrix3d R_1 = ov_core::quat_2_Rot(q_1); + Eigen::Vector4d q_2 = Eigen::Map(parameters[5]); + + // Get bias_w + Eigen::Vector3d b_w1 = Eigen::Map(parameters[1]); + Eigen::Vector3d b_w2 = Eigen::Map(parameters[6]); + + // Get bias_a + Eigen::Vector3d b_a1 = Eigen::Map(parameters[3]); + Eigen::Vector3d b_a2 = Eigen::Map(parameters[8]); + + // Get velocity + Eigen::Vector3d v_1 = Eigen::Map(parameters[2]); + Eigen::Vector3d v_2 = Eigen::Map(parameters[7]); + + // Get positions + Eigen::Vector3d p_1 = Eigen::Map(parameters[4]); + Eigen::Vector3d p_2 = Eigen::Map(parameters[9]); + + // Get the change in clone 1's biases from the linearization points + Eigen::Vector3d dbw = b_w1 - b_w_lin; + Eigen::Vector3d dba = b_a1 - b_a_lin; + + // Quaternion associated with the bias w correction + // Eigen::Vector4d q_b= rot_2_quat(Exp(-J_q*dbw)); + Eigen::Vector4d q_b; + q_b.block(0, 0, 3, 1) = 0.5 * J_q * dbw; + q_b(3, 0) = 1.0; + q_b = q_b / q_b.norm(); + + // Relative orientation from state estimates + Eigen::Vector4d q_1_to_2 = ov_core::quat_multiply(q_2, ov_core::Inv(q_1)); + + // Intermediate quaternions for error/jacobian computation + Eigen::Vector4d q_res_minus = ov_core::quat_multiply(q_1_to_2, ov_core::Inv(q_breve)); + Eigen::Vector4d q_res_plus = ov_core::quat_multiply(q_res_minus, q_b); + + //================================================================================ + //================================================================================ + //================================================================================ + + // Computer residual + Eigen::Matrix res; + res.block(0, 0, 3, 1) = 2 * q_res_plus.block(0, 0, 3, 1); + res.block(3, 0, 3, 1) = b_w2 - b_w1; + res.block(6, 0, 3, 1) = R_1 * (v_2 - v_1 + gravity * dt) - J_b * dbw - H_b * dba - beta; + res.block(9, 0, 3, 1) = b_a2 - b_a1; + res.block(12, 0, 3, 1) = R_1 * (p_2 - p_1 - v_1 * dt + .5 * gravity * std::pow(dt, 2)) - J_a * dbw - H_a * dba - alpha; + res = sqrtI * res; + + // Store residuals + for (int i = 0; i < res.rows(); i++) { + residuals[i] = res(i, 0); + } + + //================================================================================ + //================================================================================ + //================================================================================ + + // Compute jacobians if asked + if (jacobians) { + + // Stores the total preintegrated jacobian into one spot + Eigen::Matrix Jacobian = Eigen::Matrix::Zero(); + + // Quick identity + Eigen::Matrix eye = Eigen::MatrixXd::Identity(3, 3); + Eigen::Matrix q_meas_plus = ov_core::quat_multiply(ov_core::Inv(q_breve), q_b); + + // Dtheta wrt theta 1 + Jacobian.block(0, 0, 3, 3) = -((q_1_to_2(3, 0) * eye - ov_core::skew_x(q_1_to_2.block(0, 0, 3, 1))) * + (q_meas_plus(3, 0) * eye + ov_core::skew_x(q_meas_plus.block(0, 0, 3, 1))) - + q_1_to_2.block(0, 0, 3, 1) * q_meas_plus.block(0, 0, 3, 1).transpose()); + + // Dtheta wrt theta 2 + Jacobian.block(0, 15, 3, 3) = q_res_plus(3, 0) * eye + ov_core::skew_x(q_res_plus.block(0, 0, 3, 1)); + + // Dtheta wrt bw 1 + Jacobian.block(0, 3, 3, 3) = (q_res_minus(3, 0) * eye - ov_core::skew_x(q_res_minus.block(0, 0, 3, 1))) * J_q; + + // Dbw wrt bw1 and bw2 + Jacobian.block(3, 3, 3, 3) = -eye; + Jacobian.block(3, 18, 3, 3) = eye; + + // Dvelocity wrt theta 1 + Jacobian.block(6, 0, 3, 3) = ov_core::skew_x(R_1 * (v_2 - v_1 + gravity * dt)); + + // Dvelocity wrt v 1 + Jacobian.block(6, 6, 3, 3) = -R_1; + + // Dvelocity wrt v 2 + Jacobian.block(6, 21, 3, 3) = R_1; + + // Dvelocity wrt bw 1 + Jacobian.block(6, 3, 3, 3) = -J_b; + + // Dvelocity wrt ba 1 + Jacobian.block(6, 9, 3, 3) = -H_b; + + // Dbw wrt ba1 and ba2 + Jacobian.block(9, 9, 3, 3) = -eye; + Jacobian.block(9, 24, 3, 3) = eye; + + // Dposition wrt theta 1 + Jacobian.block(12, 0, 3, 3) = ov_core::skew_x(R_1 * (p_2 - p_1 - v_1 * dt + .5 * gravity * std::pow(dt, 2))); + // Dposition wrt v 1 + Jacobian.block(12, 6, 3, 3) = -R_1 * dt; + // Dposition wrt p 1 + Jacobian.block(12, 12, 3, 3) = -R_1; + + // Dposition wrt p 2 + Jacobian.block(12, 27, 3, 3) = R_1; + + // Dposition wrt bw 1 + Jacobian.block(12, 3, 3, 3) = -J_a; + // Dposition wrt ba 1 + Jacobian.block(12, 9, 3, 3) = -H_a; + + // Apply sqrt info + Jacobian = sqrtI * Jacobian; + + // Now store jacobians + // Th1 + if (jacobians[0]) { + Eigen::Map> J_th1(jacobians[0], 15, 4); + J_th1.block(0, 0, 15, 3) = Jacobian.block(0, 0, 15, 3); + J_th1.block(0, 3, 15, 1).setZero(); + } + // Th2 + if (jacobians[5]) { + Eigen::Map> J_th2(jacobians[5], 15, 4); + J_th2.block(0, 0, 15, 3) = Jacobian.block(0, 15, 15, 3); + J_th2.block(0, 3, 15, 1).setZero(); + } + // bw1 + if (jacobians[1]) { + Eigen::Map> J_bw1(jacobians[1], 15, 3); + J_bw1.block(0, 0, 15, 3) = Jacobian.block(0, 3, 15, 3); + } + // bw2 + if (jacobians[6]) { + Eigen::Map> J_bw2(jacobians[6], 15, 3); + J_bw2.block(0, 0, 15, 3) = Jacobian.block(0, 18, 15, 3); + } + // v1 + if (jacobians[2]) { + Eigen::Map> J_v1(jacobians[2], 15, 3); + J_v1.block(0, 0, 15, 3) = Jacobian.block(0, 6, 15, 3); + } + // v2 + if (jacobians[7]) { + Eigen::Map> J_v2(jacobians[7], 15, 3); + J_v2.block(0, 0, 15, 3) = Jacobian.block(0, 21, 15, 3); + } + // ba1 + if (jacobians[3]) { + Eigen::Map> J_ba1(jacobians[3], 15, 3); + J_ba1.block(0, 0, 15, 3) = Jacobian.block(0, 9, 15, 3); + } + // ba2 + if (jacobians[8]) { + Eigen::Map> J_ba2(jacobians[8], 15, 3); + J_ba2.block(0, 0, 15, 3) = Jacobian.block(0, 24, 15, 3); + } + // p1 + if (jacobians[4]) { + Eigen::Map> J_p1(jacobians[4], 15, 3); + J_p1.block(0, 0, 15, 3) = Jacobian.block(0, 12, 15, 3); + } + // p2 + if (jacobians[9]) { + Eigen::Map> J_p2(jacobians[9], 15, 3); + J_p2.block(0, 0, 15, 3) = Jacobian.block(0, 27, 15, 3); + } + } + return true; +} diff --git a/ov_init/src/ceres/Factor_ImuCPIv1.h b/ov_init/src/ceres/Factor_ImuCPIv1.h index a4ecf5a12..37cd1f256 100644 --- a/ov_init/src/ceres/Factor_ImuCPIv1.h +++ b/ov_init/src/ceres/Factor_ImuCPIv1.h @@ -24,8 +24,6 @@ #include -#include "utils/quat_ops.h" - namespace ov_init { /** @@ -61,51 +59,7 @@ class Factor_ImuCPIv1 : public ceres::CostFunction { */ Factor_ImuCPIv1(double deltatime, Eigen::Vector3d &grav, Eigen::Vector3d &alpha, Eigen::Vector3d &beta, Eigen::Vector4d &q_KtoK1, Eigen::Vector3d &ba_lin, Eigen::Vector3d &bg_lin, Eigen::Matrix3d &J_q, Eigen::Matrix3d &J_beta, Eigen::Matrix3d &J_alpha, - Eigen::Matrix3d &H_beta, Eigen::Matrix3d &H_alpha, Eigen::Matrix &covariance) { - // Save measurements - this->alpha = alpha; - this->beta = beta; - this->q_breve = q_KtoK1; - this->dt = deltatime; - this->grav_save = grav; - - // Linearization points - this->b_a_lin_save = ba_lin; - this->b_w_lin_save = bg_lin; - - // Save bias jacobians - this->J_q = J_q; - this->J_a = J_alpha; - this->J_b = J_beta; - this->H_a = H_alpha; - this->H_b = H_beta; - - // Check that we have a valid covariance matrix that we can get the information of - Eigen::MatrixXd I = Eigen::MatrixXd::Identity(covariance.rows(), covariance.rows()); - Eigen::MatrixXd information = covariance.llt().solve(I); - if (std::isnan(information.norm())) { - std::cerr << "P - " << std::endl << covariance << std::endl << std::endl; - std::cerr << "Pinv - " << std::endl << covariance.inverse() << std::endl << std::endl; - std::exit(EXIT_FAILURE); - } - - // Get square-root of our information matrix - Eigen::LLT lltOfI(information); - sqrtI_save = lltOfI.matrixL().transpose(); - - // Set the number of measurements, and the block sized - set_num_residuals(15); - mutable_parameter_block_sizes()->push_back(4); // q_GtoI1 - mutable_parameter_block_sizes()->push_back(3); // bg_1 - mutable_parameter_block_sizes()->push_back(3); // v_I1inG - mutable_parameter_block_sizes()->push_back(3); // ba_1 - mutable_parameter_block_sizes()->push_back(3); // p_I1inG - mutable_parameter_block_sizes()->push_back(4); // q_GtoI2 - mutable_parameter_block_sizes()->push_back(3); // bg_2 - mutable_parameter_block_sizes()->push_back(3); // v_I2inG - mutable_parameter_block_sizes()->push_back(3); // ba_2 - mutable_parameter_block_sizes()->push_back(3); // p_I2inG - } + Eigen::Matrix3d &H_beta, Eigen::Matrix3d &H_alpha, Eigen::Matrix &covariance); virtual ~Factor_ImuCPIv1() {} @@ -116,194 +70,7 @@ class Factor_ImuCPIv1 : public ceres::CostFunction { * and the current state estimate. This also takes into account the * bias linearization point changes. */ - bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const override { - - // Get the local variables (these would be different if we relinearized) - Eigen::Vector3d gravity = grav_save; - Eigen::Matrix sqrtI = sqrtI_save; - Eigen::Vector3d b_w_lin = b_w_lin_save; - Eigen::Vector3d b_a_lin = b_a_lin_save; - - // Get the state estimates from the ceres parameters. - // Each clone is stored in the canonical VINS format: q, bw, v, ba, p - Eigen::Vector4d q_1 = Eigen::Map(parameters[0]); - Eigen::Matrix3d R_1 = ov_core::quat_2_Rot(q_1); - Eigen::Vector4d q_2 = Eigen::Map(parameters[5]); - - // Get bias_w - Eigen::Vector3d b_w1 = Eigen::Map(parameters[1]); - Eigen::Vector3d b_w2 = Eigen::Map(parameters[6]); - - // Get bias_a - Eigen::Vector3d b_a1 = Eigen::Map(parameters[3]); - Eigen::Vector3d b_a2 = Eigen::Map(parameters[8]); - - // Get velocity - Eigen::Vector3d v_1 = Eigen::Map(parameters[2]); - Eigen::Vector3d v_2 = Eigen::Map(parameters[7]); - - // Get positions - Eigen::Vector3d p_1 = Eigen::Map(parameters[4]); - Eigen::Vector3d p_2 = Eigen::Map(parameters[9]); - - // Get the change in clone 1's biases from the linearization points - Eigen::Vector3d dbw = b_w1 - b_w_lin; - Eigen::Vector3d dba = b_a1 - b_a_lin; - - // Quaternion associated with the bias w correction - // Eigen::Vector4d q_b= rot_2_quat(Exp(-J_q*dbw)); - Eigen::Vector4d q_b; - q_b.block(0, 0, 3, 1) = 0.5 * J_q * dbw; - q_b(3, 0) = 1.0; - q_b = q_b / q_b.norm(); - - // Relative orientation from state estimates - Eigen::Vector4d q_1_to_2 = ov_core::quat_multiply(q_2, ov_core::Inv(q_1)); - - // Intermediate quaternions for error/jacobian computation - Eigen::Vector4d q_res_minus = ov_core::quat_multiply(q_1_to_2, ov_core::Inv(q_breve)); - Eigen::Vector4d q_res_plus = ov_core::quat_multiply(q_res_minus, q_b); - - //================================================================================ - //================================================================================ - //================================================================================ - - // Computer residual - Eigen::Matrix res; - res.block(0, 0, 3, 1) = 2 * q_res_plus.block(0, 0, 3, 1); - res.block(3, 0, 3, 1) = b_w2 - b_w1; - res.block(6, 0, 3, 1) = R_1 * (v_2 - v_1 + gravity * dt) - J_b * dbw - H_b * dba - beta; - res.block(9, 0, 3, 1) = b_a2 - b_a1; - res.block(12, 0, 3, 1) = R_1 * (p_2 - p_1 - v_1 * dt + .5 * gravity * std::pow(dt, 2)) - J_a * dbw - H_a * dba - alpha; - res = sqrtI * res; - - // Store residuals - for (int i = 0; i < res.rows(); i++) { - residuals[i] = res(i, 0); - } - - //================================================================================ - //================================================================================ - //================================================================================ - - // Compute jacobians if asked - if (jacobians) { - - // Stores the total preintegrated jacobian into one spot - Eigen::Matrix Jacobian = Eigen::Matrix::Zero(); - - // Quick identity - Eigen::Matrix eye = Eigen::MatrixXd::Identity(3, 3); - Eigen::Matrix q_meas_plus = ov_core::quat_multiply(ov_core::Inv(q_breve), q_b); - - // Dtheta wrt theta 1 - Jacobian.block(0, 0, 3, 3) = -((q_1_to_2(3, 0) * eye - ov_core::skew_x(q_1_to_2.block(0, 0, 3, 1))) * - (q_meas_plus(3, 0) * eye + ov_core::skew_x(q_meas_plus.block(0, 0, 3, 1))) - - q_1_to_2.block(0, 0, 3, 1) * q_meas_plus.block(0, 0, 3, 1).transpose()); - - // Dtheta wrt theta 2 - Jacobian.block(0, 15, 3, 3) = q_res_plus(3, 0) * eye + ov_core::skew_x(q_res_plus.block(0, 0, 3, 1)); - - // Dtheta wrt bw 1 - Jacobian.block(0, 3, 3, 3) = (q_res_minus(3, 0) * eye - ov_core::skew_x(q_res_minus.block(0, 0, 3, 1))) * J_q; - - // Dbw wrt bw1 and bw2 - Jacobian.block(3, 3, 3, 3) = -eye; - Jacobian.block(3, 18, 3, 3) = eye; - - // Dvelocity wrt theta 1 - Jacobian.block(6, 0, 3, 3) = ov_core::skew_x(R_1 * (v_2 - v_1 + gravity * dt)); - - // Dvelocity wrt v 1 - Jacobian.block(6, 6, 3, 3) = -R_1; - - // Dvelocity wrt v 2 - Jacobian.block(6, 21, 3, 3) = R_1; - - // Dvelocity wrt bw 1 - Jacobian.block(6, 3, 3, 3) = -J_b; - - // Dvelocity wrt ba 1 - Jacobian.block(6, 9, 3, 3) = -H_b; - - // Dbw wrt ba1 and ba2 - Jacobian.block(9, 9, 3, 3) = -eye; - Jacobian.block(9, 24, 3, 3) = eye; - - // Dposition wrt theta 1 - Jacobian.block(12, 0, 3, 3) = ov_core::skew_x(R_1 * (p_2 - p_1 - v_1 * dt + .5 * gravity * std::pow(dt, 2))); - // Dposition wrt v 1 - Jacobian.block(12, 6, 3, 3) = -R_1 * dt; - // Dposition wrt p 1 - Jacobian.block(12, 12, 3, 3) = -R_1; - - // Dposition wrt p 2 - Jacobian.block(12, 27, 3, 3) = R_1; - - // Dposition wrt bw 1 - Jacobian.block(12, 3, 3, 3) = -J_a; - // Dposition wrt ba 1 - Jacobian.block(12, 9, 3, 3) = -H_a; - - // Apply sqrt info - Jacobian = sqrtI * Jacobian; - - // Now store jacobians - // Th1 - if (jacobians[0]) { - Eigen::Map> J_th1(jacobians[0], 15, 4); - J_th1.block(0, 0, 15, 3) = Jacobian.block(0, 0, 15, 3); - J_th1.block(0, 3, 15, 1).setZero(); - } - // Th2 - if (jacobians[5]) { - Eigen::Map> J_th2(jacobians[5], 15, 4); - J_th2.block(0, 0, 15, 3) = Jacobian.block(0, 15, 15, 3); - J_th2.block(0, 3, 15, 1).setZero(); - } - // bw1 - if (jacobians[1]) { - Eigen::Map> J_bw1(jacobians[1], 15, 3); - J_bw1.block(0, 0, 15, 3) = Jacobian.block(0, 3, 15, 3); - } - // bw2 - if (jacobians[6]) { - Eigen::Map> J_bw2(jacobians[6], 15, 3); - J_bw2.block(0, 0, 15, 3) = Jacobian.block(0, 18, 15, 3); - } - // v1 - if (jacobians[2]) { - Eigen::Map> J_v1(jacobians[2], 15, 3); - J_v1.block(0, 0, 15, 3) = Jacobian.block(0, 6, 15, 3); - } - // v2 - if (jacobians[7]) { - Eigen::Map> J_v2(jacobians[7], 15, 3); - J_v2.block(0, 0, 15, 3) = Jacobian.block(0, 21, 15, 3); - } - // ba1 - if (jacobians[3]) { - Eigen::Map> J_ba1(jacobians[3], 15, 3); - J_ba1.block(0, 0, 15, 3) = Jacobian.block(0, 9, 15, 3); - } - // ba2 - if (jacobians[8]) { - Eigen::Map> J_ba2(jacobians[8], 15, 3); - J_ba2.block(0, 0, 15, 3) = Jacobian.block(0, 24, 15, 3); - } - // p1 - if (jacobians[4]) { - Eigen::Map> J_p1(jacobians[4], 15, 3); - J_p1.block(0, 0, 15, 3) = Jacobian.block(0, 12, 15, 3); - } - // p2 - if (jacobians[9]) { - Eigen::Map> J_p2(jacobians[9], 15, 3); - J_p2.block(0, 0, 15, 3) = Jacobian.block(0, 27, 15, 3); - } - } - return true; - } + bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const override; }; } // namespace ov_init diff --git a/ov_init/src/ceres/State_JPLQuatLocal.cpp b/ov_init/src/ceres/State_JPLQuatLocal.cpp new file mode 100644 index 000000000..18076c9d4 --- /dev/null +++ b/ov_init/src/ceres/State_JPLQuatLocal.cpp @@ -0,0 +1,56 @@ +/* + * OpenVINS: An Open Platform for Visual-Inertial Research + * Copyright (C) 2018-2022 Patrick Geneva + * Copyright (C) 2018-2022 Guoquan Huang + * Copyright (C) 2018-2022 OpenVINS Contributors + * Copyright (C) 2018-2019 Kevin Eckenhoff + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include "State_JPLQuatLocal.h" + +#include "utils/quat_ops.h" + +using namespace ov_init; + +bool State_JPLQuatLocal::Plus(const double *x, const double *delta, double *x_plus_delta) const { + + // Apply the standard JPL update: q <-- [d_th/2; 1] (x) q + Eigen::Map q(x); + + // Get delta into eigen + Eigen::Map d_th(delta); + Eigen::Matrix d_q; + double theta = d_th.norm(); + if (theta < 1e-8) { + d_q << .5 * d_th, 1.0; + } else { + d_q.block(0, 0, 3, 1) = (d_th / theta) * std::sin(theta / 2); + d_q(3, 0) = std::cos(theta / 2); + } + d_q = ov_core::quatnorm(d_q); + + // Do the update + Eigen::Map q_plus(x_plus_delta); + q_plus = ov_core::quat_multiply(d_q, q); + return true; +} + +bool State_JPLQuatLocal::ComputeJacobian(const double *x, double *jacobian) const { + Eigen::Map> j(jacobian); + j.topRows<3>().setIdentity(); + j.bottomRows<1>().setZero(); + return true; +} diff --git a/ov_init/src/ceres/State_JPLQuatLocal.h b/ov_init/src/ceres/State_JPLQuatLocal.h index 8551b76bd..62b90dd1b 100644 --- a/ov_init/src/ceres/State_JPLQuatLocal.h +++ b/ov_init/src/ceres/State_JPLQuatLocal.h @@ -24,8 +24,6 @@ #include -#include "utils/quat_ops.h" - namespace ov_init { /** @@ -43,28 +41,7 @@ class State_JPLQuatLocal : public ceres::LocalParameterization { * \bar{q}=norm\Big(\begin{bmatrix} 0.5*\mathbf{\theta_{dx}} \\ 1 \end{bmatrix}\Big) \hat{\bar{q}} * @f] */ - bool Plus(const double *x, const double *delta, double *x_plus_delta) const override { - - // Apply the standard JPL update: q <-- [d_th/2; 1] (x) q - Eigen::Map q(x); - - // Get delta into eigen - Eigen::Map d_th(delta); - Eigen::Matrix d_q; - double theta = d_th.norm(); - if (theta < 1e-8) { - d_q << .5 * d_th, 1.0; - } else { - d_q.block(0, 0, 3, 1) = (d_th / theta) * std::sin(theta / 2); - d_q(3, 0) = std::cos(theta / 2); - } - d_q = ov_core::quatnorm(d_q); - - // Do the update - Eigen::Map q_plus(x_plus_delta); - q_plus = ov_core::quat_multiply(d_q, q); - return true; - } + bool Plus(const double *x, const double *delta, double *x_plus_delta) const override; /** * @brief Computes the jacobian in respect to the local parameterization @@ -77,12 +54,7 @@ class State_JPLQuatLocal : public ceres::LocalParameterization { * dr/dlocal= [ dr/dlocal, 0] * [I; 0]= dr/dlocal. * Therefore we here define dglobal/dlocal= [I; 0] */ - bool ComputeJacobian(const double *x, double *jacobian) const override { - Eigen::Map> j(jacobian); - j.topRows<3>().setIdentity(); - j.bottomRows<1>().setZero(); - return true; - } + bool ComputeJacobian(const double *x, double *jacobian) const override; int GlobalSize() const override { return 4; }; diff --git a/ov_init/src/dynamic/DynamicInitializer.cpp b/ov_init/src/dynamic/DynamicInitializer.cpp index 296bc8f34..ee0926f10 100644 --- a/ov_init/src/dynamic/DynamicInitializer.cpp +++ b/ov_init/src/dynamic/DynamicInitializer.cpp @@ -21,15 +21,29 @@ #include "DynamicInitializer.h" +#include "ceres/Factor_GenericPrior.h" +#include "ceres/Factor_ImageReprojCalib.h" +#include "ceres/Factor_ImuCPIv1.h" +#include "ceres/State_JPLQuatLocal.h" +#include "utils/helper.h" + +#include "cpi/CpiV1.h" +#include "feat/Feature.h" +#include "feat/FeatureDatabase.h" +#include "types/IMU.h" +#include "types/Landmark.h" +#include "utils/colors.h" +#include "utils/print.h" +#include "utils/quat_ops.h" +#include "utils/sensor_data.h" + using namespace ov_core; using namespace ov_type; using namespace ov_init; bool DynamicInitializer::initialize(double ×tamp, Eigen::MatrixXd &covariance, std::vector> &order, std::shared_ptr &_imu, std::map> &_clones_IMU, - std::unordered_map> &_features_SLAM, - std::unordered_map> &_calib_IMUtoCAM, - std::unordered_map> &_cam_intrinsics) { + std::unordered_map> &_features_SLAM) { // Get the newest and oldest timestamps we will try to initialize between! auto rT1 = boost::posix_time::microsec_clock::local_time(); @@ -395,15 +409,15 @@ bool DynamicInitializer::initialize(double ×tamp, Eigen::MatrixXd &covarian Eigen::MatrixXd Temp = A2.transpose() * (Eigen::MatrixXd::Identity(A1.rows(), A1.rows()) - A1 * A1A1_inv * A1.transpose()); Eigen::MatrixXd D = Temp * A2; Eigen::MatrixXd d = Temp * b; - Eigen::VectorXd coeff = InitializerHelper::compute_dongsi_coeff(D, d, params.gravity_mag); + Eigen::Matrix coeff = InitializerHelper::compute_dongsi_coeff(D, d, params.gravity_mag); // Create companion matrix of our polynomial // https://en.wikipedia.org/wiki/Companion_matrix assert(coeff(0) == 1); - Eigen::MatrixXd companion_matrix = Eigen::MatrixXd::Zero(coeff.rows() - 1, coeff.rows() - 1); + Eigen::Matrix companion_matrix = Eigen::Matrix::Zero(coeff.rows() - 1, coeff.rows() - 1); companion_matrix.diagonal(-1).setOnes(); companion_matrix.col(companion_matrix.cols() - 1) = -coeff.reverse().head(coeff.rows() - 1); - Eigen::JacobiSVD svd0(companion_matrix, Eigen::ComputeThinU | Eigen::ComputeThinV); + Eigen::JacobiSVD> svd0(companion_matrix); Eigen::MatrixXd singularValues0 = svd0.singularValues(); double cond0 = singularValues0(0) / singularValues0(singularValues0.rows() - 1); PRINT_DEBUG("[init-d]: CM cond = %.3f | rank = %d of %d (%4.3e thresh)\n", cond0, (int)svd0.rank(), (int)companion_matrix.cols(), @@ -414,7 +428,7 @@ bool DynamicInitializer::initialize(double ×tamp, Eigen::MatrixXd &covarian } // Find its eigenvalues (can be complex) - Eigen::EigenSolver solver(companion_matrix, false); + Eigen::EigenSolver> solver(companion_matrix, false); if (solver.info() != Eigen::Success) { PRINT_ERROR(RED "[init-d]: failed to compute the eigenvalue decomposition!!\n" RESET); return false; @@ -518,7 +532,7 @@ bool DynamicInitializer::initialize(double ×tamp, Eigen::MatrixXd &covarian continue; Eigen::Vector3d p_FinI0; if (size_feature == 1) { - assert(false); + assert_r(false); // p_FinI0 = x_hat(size_feature * map_features.at(feat.first) + 6, 0) * features_bearings.at(feat->featid); } else { p_FinI0 = x_hat.block(size_feature * A_index_features.at(feat.first) + 6, 0, 3, 1); @@ -993,52 +1007,52 @@ bool DynamicInitializer::initialize(double ×tamp, Eigen::MatrixXd &covarian Eigen::Matrix covtmp = Eigen::Matrix::Zero(); // block diagonal - assert(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_ori[state_index], covtmp.data())); + assert_r(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_ori[state_index], covtmp.data())); covariance.block(0, 0, 3, 3) = covtmp.eval(); - assert(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_pos[state_index], ceres_vars_pos[state_index], covtmp.data())); + assert_r(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_pos[state_index], ceres_vars_pos[state_index], covtmp.data())); covariance.block(3, 3, 3, 3) = covtmp.eval(); - assert(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_vel[state_index], ceres_vars_vel[state_index], covtmp.data())); + assert_r(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_vel[state_index], ceres_vars_vel[state_index], covtmp.data())); covariance.block(6, 6, 3, 3) = covtmp.eval(); - assert(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_bias_g[state_index], ceres_vars_bias_g[state_index], covtmp.data())); + assert_r(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_bias_g[state_index], ceres_vars_bias_g[state_index], covtmp.data())); covariance.block(9, 9, 3, 3) = covtmp.eval(); - assert(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_bias_a[state_index], ceres_vars_bias_a[state_index], covtmp.data())); + assert_r(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_bias_a[state_index], ceres_vars_bias_a[state_index], covtmp.data())); covariance.block(12, 12, 3, 3) = covtmp.eval(); // orientation - assert(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_pos[state_index], covtmp.data())); + assert_r(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_pos[state_index], covtmp.data())); covariance.block(0, 3, 3, 3) = covtmp.eval(); covariance.block(3, 0, 3, 3) = covtmp.transpose(); - assert(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_vel[state_index], covtmp.data())); + assert_r(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_vel[state_index], covtmp.data())); covariance.block(0, 6, 3, 3) = covtmp.eval(); covariance.block(6, 0, 3, 3) = covtmp.transpose().eval(); - assert(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_bias_g[state_index], covtmp.data())); + assert_r(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_bias_g[state_index], covtmp.data())); covariance.block(0, 9, 3, 3) = covtmp.eval(); covariance.block(9, 0, 3, 3) = covtmp.transpose().eval(); - assert(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_bias_a[state_index], covtmp.data())); + assert_r(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_ori[state_index], ceres_vars_bias_a[state_index], covtmp.data())); covariance.block(0, 12, 3, 3) = covtmp.eval(); covariance.block(12, 0, 3, 3) = covtmp.transpose().eval(); // position - assert(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_pos[state_index], ceres_vars_vel[state_index], covtmp.data())); + assert_r(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_pos[state_index], ceres_vars_vel[state_index], covtmp.data())); covariance.block(3, 6, 3, 3) = covtmp.eval(); covariance.block(6, 3, 3, 3) = covtmp.transpose().eval(); - assert(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_pos[state_index], ceres_vars_bias_g[state_index], covtmp.data())); + assert_r(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_pos[state_index], ceres_vars_bias_g[state_index], covtmp.data())); covariance.block(3, 9, 3, 3) = covtmp.eval(); covariance.block(9, 3, 3, 3) = covtmp.transpose().eval(); - assert(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_pos[state_index], ceres_vars_bias_a[state_index], covtmp.data())); + assert_r(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_pos[state_index], ceres_vars_bias_a[state_index], covtmp.data())); covariance.block(3, 12, 3, 3) = covtmp.eval(); covariance.block(12, 3, 3, 3) = covtmp.transpose().eval(); // velocity - assert(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_vel[state_index], ceres_vars_bias_g[state_index], covtmp.data())); + assert_r(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_vel[state_index], ceres_vars_bias_g[state_index], covtmp.data())); covariance.block(6, 9, 3, 3) = covtmp.eval(); covariance.block(9, 6, 3, 3) = covtmp.transpose().eval(); - assert(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_vel[state_index], ceres_vars_bias_a[state_index], covtmp.data())); + assert_r(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_vel[state_index], ceres_vars_bias_a[state_index], covtmp.data())); covariance.block(6, 12, 3, 3) = covtmp.eval(); covariance.block(12, 6, 3, 3) = covtmp.transpose().eval(); // bias_g - assert(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_bias_g[state_index], ceres_vars_bias_a[state_index], covtmp.data())); + assert_r(problem_cov.GetCovarianceBlockInTangentSpace(ceres_vars_bias_g[state_index], ceres_vars_bias_a[state_index], covtmp.data())); covariance.block(9, 12, 3, 3) = covtmp.eval(); covariance.block(12, 9, 3, 3) = covtmp.transpose().eval(); @@ -1063,22 +1077,6 @@ bool DynamicInitializer::initialize(double ×tamp, Eigen::MatrixXd &covarian _imu->set_value(x); _imu->set_fej(x); - // // Setting a static covariance (might not be valid, but we can guess) - // covariance = 1e-3 * Eigen::MatrixXd::Identity(15, 15); - // covariance.block(6, 6, 3, 3) = 0.1 * Eigen::Matrix3d::Identity(); - // covariance.block(9, 9, 3, 3) = 2e-3 * Eigen::Matrix3d::Identity(); - // covariance.block(12, 12, 3, 3) = 1e-2 * Eigen::Matrix3d::Identity(); - // - // // A VIO system has 4dof unobservabile directions which can be arbitrarily picked. - // // This means that on startup, we can fix the yaw and position to be 100 percent known. - // // Thus, after determining the global to current IMU orientation after initialization, we can propagate the global error - // // into the new IMU pose. In this case the position is directly equivalent, but the orientation needs to be propagated. - // covariance(0, 0) = 1e-3; - // covariance(1, 1) = 1e-3; - // covariance(2, 2) = 1e-5; - // covariance.block(0, 0, 3, 3) = _imu->Rot() * covariance.block(0, 0, 3, 3) * _imu->Rot().transpose(); - // covariance.block(3, 3, 3, 3) = 1e-5 * Eigen::Matrix3d::Identity(); - // Debug timing information about how long it took to initialize!! auto rT7 = boost::posix_time::microsec_clock::local_time(); PRINT_DEBUG("[TIME]: %.4f sec for prelim tests\n", (rT2 - rT1).total_microseconds() * 1e-6); diff --git a/ov_init/src/dynamic/DynamicInitializer.h b/ov_init/src/dynamic/DynamicInitializer.h index 1652f3a71..03b259e29 100644 --- a/ov_init/src/dynamic/DynamicInitializer.h +++ b/ov_init/src/dynamic/DynamicInitializer.h @@ -22,21 +22,19 @@ #ifndef OV_INIT_DYNAMICINITIALIZER_H #define OV_INIT_DYNAMICINITIALIZER_H -#include "ceres/Factor_GenericPrior.h" -#include "ceres/Factor_ImageReprojCalib.h" -#include "ceres/Factor_ImuCPIv1.h" -#include "ceres/State_JPLQuatLocal.h" #include "init/InertialInitializerOptions.h" -#include "utils/helper.h" -#include "cpi/CpiV1.h" -#include "feat/FeatureHelper.h" -#include "types/IMU.h" -#include "types/Landmark.h" -#include "utils/colors.h" -#include "utils/print.h" -#include "utils/quat_ops.h" -#include "utils/sensor_data.h" +namespace ov_core { +class FeatureDatabase; +struct ImuData; +} // namespace ov_core +namespace ov_type { +class Type; +class IMU; +class PoseJPL; +class Landmark; +class Vec; +} // namespace ov_type namespace ov_init { @@ -50,13 +48,15 @@ namespace ov_init { * 2. Construct linear system with features to recover velocity (solve with |g| constraint) * 3. Perform a large MLE with all calibration and recover the covariance. * - * Method is based on this work: + * Method is based on this work (see this [tech report](https://pgeneva.com/downloads/reports/tr_init.pdf) for a high level walk through): + * * > Dong-Si, Tue-Cuong, and Anastasios I. Mourikis. * > "Estimator initialization in vision-aided inertial navigation with unknown camera-IMU calibration." * > 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2012. * * - https://ieeexplore.ieee.org/abstract/document/6386235 * - https://tdongsi.github.io/download/pubs/2011_VIO_Init_TR.pdf + * - https://pgeneva.com/downloads/reports/tr_init.pdf * */ class DynamicInitializer { @@ -80,15 +80,11 @@ class DynamicInitializer { * @param _imu Pointer to the "active" IMU state (q_GtoI, p_IinG, v_IinG, bg, ba) * @param _clones_IMU Map between imaging times and clone poses (q_GtoIi, p_IiinG) * @param _features_SLAM Our current set of SLAM features (3d positions) - * @param _calib_IMUtoCAM Calibration poses for each camera (R_ItoC, p_IinC) - * @param _cam_intrinsics Camera intrinsics * @return True if we have successfully initialized our system */ bool initialize(double ×tamp, Eigen::MatrixXd &covariance, std::vector> &order, std::shared_ptr &_imu, std::map> &_clones_IMU, - std::unordered_map> &_features_SLAM, - std::unordered_map> &_calib_IMUtoCAM, - std::unordered_map> &_cam_intrinsics); + std::unordered_map> &_features_SLAM); private: /// Initialization parameters diff --git a/ov_init/src/init/InertialInitializer.cpp b/ov_init/src/init/InertialInitializer.cpp index 89c7f833e..6668bb66a 100644 --- a/ov_init/src/init/InertialInitializer.cpp +++ b/ov_init/src/init/InertialInitializer.cpp @@ -21,6 +21,16 @@ #include "InertialInitializer.h" +#include "dynamic/DynamicInitializer.h" +#include "static/StaticInitializer.h" + +#include "feat/FeatureHelper.h" +#include "types/Type.h" +#include "utils/colors.h" +#include "utils/print.h" +#include "utils/quat_ops.h" +#include "utils/sensor_data.h" + using namespace ov_core; using namespace ov_type; using namespace ov_init; @@ -36,6 +46,29 @@ InertialInitializer::InertialInitializer(InertialInitializerOptions ¶ms_, st init_dynamic = std::make_shared(params, _db, imu_data); } +void InertialInitializer::feed_imu(const ov_core::ImuData &message, double oldest_time) { + + // Append it to our vector + imu_data->emplace_back(message); + + // Sort our imu data (handles any out of order measurements) + // std::sort(imu_data->begin(), imu_data->end(), [](const IMUDATA i, const IMUDATA j) { + // return i.timestamp < j.timestamp; + //}); + + // Loop through and delete imu messages that are older than our requested time + if (oldest_time != -1) { + auto it0 = imu_data->begin(); + while (it0 != imu_data->end()) { + if (message.timestamp < oldest_time) { + it0 = imu_data->erase(it0); + } else { + it0++; + } + } + } +} + bool InertialInitializer::initialize(double ×tamp, Eigen::MatrixXd &covariance, std::vector> &order, std::shared_ptr t_imu, bool wait_for_jerk) { @@ -63,36 +96,46 @@ bool InertialInitializer::initialize(double ×tamp, Eigen::MatrixXd &covaria // Compute the disparity of the system at the current timestep // If disparity is zero or negative we will always use the static initializer - bool disparity_detected_moving = false; + bool disparity_detected_moving_1to0 = false; + bool disparity_detected_moving_2to1 = false; if (params.init_max_disparity > 0) { // Get the disparity statistics from this image to the previous - int num_features = 0; - double average_disparity = 0.0; - double variance_disparity = 0.0; - FeatureHelper::compute_disparity(_db, average_disparity, variance_disparity, num_features); + // Only compute the disparity for the oldest half of the initialization period + double newest_time_allowed = newest_cam_time - 0.5 * params.init_window_time; + int num_features0 = 0; + int num_features1 = 0; + double avg_disp0, avg_disp1; + double var_disp0, var_disp1; + FeatureHelper::compute_disparity(_db, avg_disp0, var_disp0, num_features0, newest_time_allowed); + FeatureHelper::compute_disparity(_db, avg_disp1, var_disp1, num_features1, newest_cam_time, newest_time_allowed); // Return if we can't compute the disparity - if (num_features < 10) { - PRINT_DEBUG(YELLOW "[init]: not enough features to compute disparity %d < 10\n" RESET, num_features); + int feat_thresh = 15; + if (num_features0 < feat_thresh || num_features1 < feat_thresh) { + PRINT_WARNING(YELLOW "[init]: not enough feats to compute disp: %d,%d < %d\n" RESET, num_features0, num_features1, feat_thresh); return false; } // Check if it passed our check! - PRINT_DEBUG(YELLOW "[init]: disparity of the platform is %.4f (%.4f threshold)\n" RESET, average_disparity, params.init_max_disparity); - disparity_detected_moving = (average_disparity > params.init_max_disparity); + PRINT_INFO(YELLOW "[init]: disparity is %.3f,%.3f (%.2f thresh)\n" RESET, avg_disp0, avg_disp1, params.init_max_disparity); + disparity_detected_moving_1to0 = (avg_disp0 > params.init_max_disparity); + disparity_detected_moving_2to1 = (avg_disp1 > params.init_max_disparity); } // Use our static initializer! - if (!disparity_detected_moving && params.init_imu_thresh > 0.0) { + // CASE1: if our disparity says we were static in last window and have moved in the newest, we have a jerk + // CASE2: if both disparities are below the threshold, then the platform has been stationary during both periods + bool has_jerk = (!disparity_detected_moving_1to0 && disparity_detected_moving_2to1); + bool is_still = (!disparity_detected_moving_1to0 && !disparity_detected_moving_2to1); + if (((has_jerk && wait_for_jerk) || (is_still && !wait_for_jerk)) && params.init_imu_thresh > 0.0) { PRINT_DEBUG(GREEN "[init]: USING STATIC INITIALIZER METHOD!\n" RESET); return init_static->initialize(timestamp, covariance, order, t_imu, wait_for_jerk); - } else { + } else if (params.init_dyn_use) { PRINT_DEBUG(GREEN "[init]: USING DYNAMIC INITIALIZER METHOD!\n" RESET); std::map> _clones_IMU; std::unordered_map> _features_SLAM; - std::unordered_map> _calib_IMUtoCAM; - std::unordered_map> _cam_intrinsics; - return init_dynamic->initialize(timestamp, covariance, order, t_imu, _clones_IMU, _features_SLAM, _calib_IMUtoCAM, _cam_intrinsics); + return init_dynamic->initialize(timestamp, covariance, order, t_imu, _clones_IMU, _features_SLAM); } + return false; } diff --git a/ov_init/src/init/InertialInitializer.h b/ov_init/src/init/InertialInitializer.h index a43c226f1..1c2153b65 100644 --- a/ov_init/src/init/InertialInitializer.h +++ b/ov_init/src/init/InertialInitializer.h @@ -22,18 +22,22 @@ #ifndef OV_INIT_INERTIALINITIALIZER_H #define OV_INIT_INERTIALINITIALIZER_H -#include "dynamic/DynamicInitializer.h" #include "init/InertialInitializerOptions.h" -#include "static/StaticInitializer.h" -#include "types/Type.h" -#include "utils/colors.h" -#include "utils/print.h" -#include "utils/quat_ops.h" -#include "utils/sensor_data.h" +namespace ov_core { +class FeatureDatabase; +struct ImuData; +} // namespace ov_core +namespace ov_type { +class Type; +class IMU; +} // namespace ov_type namespace ov_init { +class StaticInitializer; +class DynamicInitializer; + /** * @brief Initializer for visual-inertial system. * @@ -68,28 +72,7 @@ class InertialInitializer { * @param message Contains our timestamp and inertial information * @param oldest_time Time that we can discard measurements before */ - void feed_imu(const ov_core::ImuData &message, double oldest_time = -1) { - - // Append it to our vector - imu_data->emplace_back(message); - - // Sort our imu data (handles any out of order measurements) - // std::sort(imu_data->begin(), imu_data->end(), [](const IMUDATA i, const IMUDATA j) { - // return i.timestamp < j.timestamp; - //}); - - // Loop through and delete imu messages that are older than our requested time - if (oldest_time != -1) { - auto it0 = imu_data->begin(); - while (it0 != imu_data->end()) { - if (message.timestamp < oldest_time) { - it0 = imu_data->erase(it0); - } else { - it0++; - } - } - } - } + void feed_imu(const ov_core::ImuData &message, double oldest_time = -1); /** * @brief Try to get the initialized system diff --git a/ov_init/src/init/InertialInitializerOptions.h b/ov_init/src/init/InertialInitializerOptions.h index a896fe4bd..117801b74 100644 --- a/ov_init/src/init/InertialInitializerOptions.h +++ b/ov_init/src/init/InertialInitializerOptions.h @@ -72,6 +72,9 @@ struct InertialInitializerOptions { /// Number of features we should try to track int init_max_features = 50; + /// If we should perform dynamic initialization + bool init_dyn_use = false; + /// If we should optimize and recover the calibration in our MLE bool init_dyn_mle_opt_calib = false; @@ -125,6 +128,7 @@ struct InertialInitializerOptions { parser->parse_config("init_imu_thresh", init_imu_thresh); parser->parse_config("init_max_disparity", init_max_disparity); parser->parse_config("init_max_features", init_max_features); + parser->parse_config("init_dyn_use", init_dyn_use); parser->parse_config("init_dyn_mle_opt_calib", init_dyn_mle_opt_calib); parser->parse_config("init_dyn_mle_max_iter", init_dyn_mle_max_iter); parser->parse_config("init_dyn_mle_max_threads", init_dyn_mle_max_threads); @@ -152,6 +156,19 @@ struct InertialInitializerOptions { PRINT_ERROR(RED " init_max_features = %d\n" RESET, init_max_features); std::exit(EXIT_FAILURE); } + if (init_imu_thresh <= 0.0 && !init_dyn_use) { + PRINT_ERROR(RED "need to have an IMU threshold for static initialization!\n" RESET); + PRINT_ERROR(RED " init_imu_thresh = %.3f\n" RESET, init_imu_thresh); + PRINT_ERROR(RED " init_dyn_use = %d\n" RESET, init_dyn_use); + std::exit(EXIT_FAILURE); + } + if (init_max_disparity <= 0.0 && !init_dyn_use) { + PRINT_ERROR(RED "need to have an DISPARITY threshold for static initialization!\n" RESET); + PRINT_ERROR(RED " init_max_disparity = %.3f\n" RESET, init_max_disparity); + PRINT_ERROR(RED " init_dyn_use = %d\n" RESET, init_dyn_use); + std::exit(EXIT_FAILURE); + } + PRINT_DEBUG(" - init_dyn_use: %d\n", init_dyn_use); PRINT_DEBUG(" - init_dyn_mle_opt_calib: %d\n", init_dyn_mle_opt_calib); PRINT_DEBUG(" - init_dyn_mle_max_iter: %d\n", init_dyn_mle_max_iter); PRINT_DEBUG(" - init_dyn_mle_max_threads: %d\n", init_dyn_mle_max_threads); diff --git a/ov_init/src/sim/Simulator.cpp b/ov_init/src/sim/SimulatorInit.cpp similarity index 88% rename from ov_init/src/sim/Simulator.cpp rename to ov_init/src/sim/SimulatorInit.cpp index 2bd875ea4..ba5c23b5a 100644 --- a/ov_init/src/sim/Simulator.cpp +++ b/ov_init/src/sim/SimulatorInit.cpp @@ -19,12 +19,19 @@ * along with this program. If not, see . */ -#include "Simulator.h" +#include "SimulatorInit.h" + +#include "cam/CamBase.h" +#include "cam/CamEqui.h" +#include "cam/CamRadtan.h" +#include "sim/BsplineSE3.h" +#include "utils/colors.h" +#include "utils/dataset_reader.h" using namespace ov_core; using namespace ov_init; -Simulator::Simulator(InertialInitializerOptions ¶ms_) { +SimulatorInit::SimulatorInit(InertialInitializerOptions ¶ms_) { //=============================================================== //=============================================================== @@ -52,17 +59,18 @@ Simulator::Simulator(InertialInitializerOptions ¶ms_) { // Load the groundtruth trajectory and its spline DatasetReader::load_simulated_trajectory(params.sim_traj_path, traj_data); - spline.feed_trajectory(traj_data); + spline = std::make_shared(); + spline->feed_trajectory(traj_data); // Set all our timestamps as starting from the minimum spline time - timestamp = spline.get_start_time(); - timestamp_last_imu = spline.get_start_time(); - timestamp_last_cam = spline.get_start_time(); + timestamp = spline->get_start_time(); + timestamp_last_imu = spline->get_start_time(); + timestamp_last_cam = spline->get_start_time(); // Get the pose at the current timestep Eigen::Matrix3d R_GtoI_init; Eigen::Vector3d p_IinG_init; - bool success_pose_init = spline.get_pose(timestamp, R_GtoI_init, p_IinG_init); + bool success_pose_init = spline->get_pose(timestamp, R_GtoI_init, p_IinG_init); if (!success_pose_init) { PRINT_ERROR(RED "[SIM]: unable to find the first pose in the spline...\n" RESET); std::exit(EXIT_FAILURE); @@ -76,7 +84,7 @@ Simulator::Simulator(InertialInitializerOptions ¶ms_) { // Get the pose at the current timestep Eigen::Matrix3d R_GtoI; Eigen::Vector3d p_IinG; - bool success_pose = spline.get_pose(timestamp, R_GtoI, p_IinG); + bool success_pose = spline->get_pose(timestamp, R_GtoI, p_IinG); // Check if it fails if (!success_pose) { @@ -97,7 +105,7 @@ Simulator::Simulator(InertialInitializerOptions ¶ms_) { timestamp_last_cam += 1.0 / params.sim_freq_cam; } } - PRINT_DEBUG("[SIM]: moved %.3f seconds into the dataset where it starts moving\n", timestamp - spline.get_start_time()); + PRINT_DEBUG("[SIM]: moved %.3f seconds into the dataset where it starts moving\n", timestamp - spline->get_start_time()); // Append the current true bias to our history hist_true_bias_time.push_back(timestamp_last_imu - 1.0 / params.sim_freq_imu); @@ -157,7 +165,7 @@ Simulator::Simulator(InertialInitializerOptions ¶ms_) { for (int i = 0; i < params.num_cameras; i++) { // Reset the start time - double time_synth = spline.get_start_time(); + double time_synth = spline->get_start_time(); // Loop through each pose and generate our feature map in them!!!! while (true) { @@ -165,7 +173,7 @@ Simulator::Simulator(InertialInitializerOptions ¶ms_) { // Get the pose at the current timestep Eigen::Matrix3d R_GtoI; Eigen::Vector3d p_IinG; - bool success_pose = spline.get_pose(time_synth, R_GtoI, p_IinG); + bool success_pose = spline->get_pose(time_synth, R_GtoI, p_IinG); // We have finished generating features if (!success_pose) @@ -184,7 +192,7 @@ Simulator::Simulator(InertialInitializerOptions ¶ms_) { // Debug print PRINT_DEBUG("[SIM]: Generated %d map features in total over %d frames (camera %d)\n", (int)(featmap.size() - mapsize), - (int)((time_synth - spline.get_start_time()) / dt), i); + (int)((time_synth - spline->get_start_time()) / dt), i); mapsize = featmap.size(); } @@ -192,7 +200,7 @@ Simulator::Simulator(InertialInitializerOptions ¶ms_) { sleep(1); } -void Simulator::perturb_parameters(InertialInitializerOptions ¶ms_) { +void SimulatorInit::perturb_parameters(InertialInitializerOptions ¶ms_) { // One std generator std::normal_distribution w(0, 1); @@ -234,7 +242,7 @@ void Simulator::perturb_parameters(InertialInitializerOptions ¶ms_) { } } -bool Simulator::get_state(double desired_time, Eigen::Matrix &imustate) { +bool SimulatorInit::get_state(double desired_time, Eigen::Matrix &imustate) { // Set to default state imustate.setZero(); @@ -245,7 +253,7 @@ bool Simulator::get_state(double desired_time, Eigen::Matrix &imu Eigen::Vector3d p_IinG, w_IinI, v_IinG; // Get the pose, velocity, and acceleration - bool success_vel = spline.get_velocity(desired_time, R_GtoI, p_IinG, w_IinI, v_IinG); + bool success_vel = spline->get_velocity(desired_time, R_GtoI, p_IinG, w_IinI, v_IinG); // Find the bounding bias values bool success_bias = false; @@ -278,7 +286,7 @@ bool Simulator::get_state(double desired_time, Eigen::Matrix &imu return true; } -bool Simulator::get_next_imu(double &time_imu, Eigen::Vector3d &wm, Eigen::Vector3d &am) { +bool SimulatorInit::get_next_imu(double &time_imu, Eigen::Vector3d &wm, Eigen::Vector3d &am) { // Return if the camera measurement should go before us if (timestamp_last_cam + 1.0 / params.sim_freq_cam < timestamp_last_imu + 1.0 / params.sim_freq_imu) @@ -296,8 +304,8 @@ bool Simulator::get_next_imu(double &time_imu, Eigen::Vector3d &wm, Eigen::Vecto // Get the pose, velocity, and acceleration // NOTE: we get the acceleration between our two IMU // NOTE: this is because we are using a constant measurement model for integration - // bool success_accel = spline.get_acceleration(timestamp+0.5/freq_imu, R_GtoI, p_IinG, w_IinI, v_IinG, alpha_IinI, a_IinG); - bool success_accel = spline.get_acceleration(timestamp, R_GtoI, p_IinG, w_IinI, v_IinG, alpha_IinI, a_IinG); + // bool success_accel = spline->get_acceleration(timestamp+0.5/freq_imu, R_GtoI, p_IinG, w_IinI, v_IinG, alpha_IinI, a_IinG); + bool success_accel = spline->get_acceleration(timestamp, R_GtoI, p_IinG, w_IinI, v_IinG, alpha_IinI, a_IinG); // If failed, then that means we don't have any more spline // Thus we should stop the simulation @@ -339,8 +347,8 @@ bool Simulator::get_next_imu(double &time_imu, Eigen::Vector3d &wm, Eigen::Vecto return true; } -bool Simulator::get_next_cam(double &time_cam, std::vector &camids, - std::vector>> &feats) { +bool SimulatorInit::get_next_cam(double &time_cam, std::vector &camids, + std::vector>> &feats) { // Return if the imu measurement should go before us if (timestamp_last_imu + 1.0 / params.sim_freq_imu < timestamp_last_cam + 1.0 / params.sim_freq_cam) @@ -354,7 +362,7 @@ bool Simulator::get_next_cam(double &time_cam, std::vector &camids, // Get the pose at the current timestep Eigen::Matrix3d R_GtoI; Eigen::Vector3d p_IinG; - bool success_pose = spline.get_pose(timestamp, R_GtoI, p_IinG); + bool success_pose = spline->get_pose(timestamp, R_GtoI, p_IinG); // We have finished generating measurements if (!success_pose) { @@ -401,9 +409,9 @@ bool Simulator::get_next_cam(double &time_cam, std::vector &camids, return true; } -std::vector> Simulator::project_pointcloud(const Eigen::Matrix3d &R_GtoI, const Eigen::Vector3d &p_IinG, - int camid, - const std::unordered_map &feats) { +std::vector> +SimulatorInit::project_pointcloud(const Eigen::Matrix3d &R_GtoI, const Eigen::Vector3d &p_IinG, int camid, + const std::unordered_map &feats) { // Assert we have good camera assert(camid < params.num_cameras); @@ -450,8 +458,8 @@ std::vector> Simulator::project_pointcloud(co return uvs; } -void Simulator::generate_points(const Eigen::Matrix3d &R_GtoI, const Eigen::Vector3d &p_IinG, int camid, - std::unordered_map &feats, int numpts) { +void SimulatorInit::generate_points(const Eigen::Matrix3d &R_GtoI, const Eigen::Vector3d &p_IinG, int camid, + std::unordered_map &feats, int numpts) { // Assert we have good camera assert(camid < params.num_cameras); diff --git a/ov_init/src/sim/Simulator.h b/ov_init/src/sim/SimulatorInit.h similarity index 92% rename from ov_init/src/sim/Simulator.h rename to ov_init/src/sim/SimulatorInit.h index f440f4e5b..e7d10fba2 100644 --- a/ov_init/src/sim/Simulator.h +++ b/ov_init/src/sim/SimulatorInit.h @@ -19,8 +19,8 @@ * along with this program. If not, see . */ -#ifndef OV_INIT_SIMULATOR_H -#define OV_INIT_SIMULATOR_H +#ifndef OV_INIT_SIMULATORINIT_H +#define OV_INIT_SIMULATORINIT_H #include #include @@ -30,15 +30,12 @@ #include #include -#include "cam/CamBase.h" -#include "cam/CamEqui.h" -#include "cam/CamRadtan.h" -#include "sim/BsplineSE3.h" -#include "utils/colors.h" -#include "utils/dataset_reader.h" - #include "init/InertialInitializerOptions.h" +namespace ov_core { +class BsplineSE3; +} // namespace ov_core + namespace ov_init { /** @@ -52,14 +49,14 @@ namespace ov_init { * The user should specify the sensor rates that they desire along with the seeds of the random number generators. * */ -class Simulator { +class SimulatorInit { public: /** * @brief Default constructor, will load all configuration variables * @param params_ InertialInitializer parameters. Should have already been loaded from cmd. */ - Simulator(InertialInitializerOptions ¶ms_); + SimulatorInit(InertialInitializerOptions ¶ms_); /** * @brief Will get a set of perturbed parameters @@ -108,14 +105,6 @@ class Simulator { /// Returns the true 3d map of features std::unordered_map get_map() { return featmap; } - /// Returns the true 3d map of features - std::vector get_map_vec() { - std::vector feats; - for (auto const &feat : featmap) - feats.push_back(feat.second); - return feats; - } - /// Access function to get the true parameters (i.e. calibration and settings) InertialInitializerOptions get_true_parameters() { return params; } @@ -157,7 +146,7 @@ class Simulator { std::vector traj_data; /// Our b-spline trajectory - ov_core::BsplineSE3 spline; + std::shared_ptr spline; /// Our map of 3d features size_t id_map = 0; @@ -205,4 +194,4 @@ class Simulator { } // namespace ov_init -#endif // OV_INIT_SIMULATOR_H +#endif // OV_INIT_SIMULATORINIT_H diff --git a/ov_init/src/static/StaticInitializer.cpp b/ov_init/src/static/StaticInitializer.cpp index 961d917ab..e4a5fed62 100644 --- a/ov_init/src/static/StaticInitializer.cpp +++ b/ov_init/src/static/StaticInitializer.cpp @@ -21,6 +21,15 @@ #include "StaticInitializer.h" +#include "utils/helper.h" + +#include "feat/FeatureHelper.h" +#include "types/IMU.h" +#include "utils/colors.h" +#include "utils/print.h" +#include "utils/quat_ops.h" +#include "utils/sensor_data.h" + using namespace ov_core; using namespace ov_type; using namespace ov_init; @@ -86,26 +95,26 @@ bool StaticInitializer::initialize(double ×tamp, Eigen::MatrixXd &covarianc a_var_2to1 += (data.am - a_avg_2to1).dot(data.am - a_avg_2to1); } a_var_2to1 = std::sqrt(a_var_2to1 / ((int)window_2to1.size() - 1)); - // PRINT_DEBUG(BOLDGREEN "[init-s]: IMU excitation, %.4f,%.4f\n" RESET, a_var_1to0, a_var_2to1); + PRINT_DEBUG(YELLOW "[init-s]: IMU excitation stats: %.3f,%.3f\n" RESET, a_var_2to1, a_var_1to0); // If it is below the threshold and we want to wait till we detect a jerk if (a_var_1to0 < params.init_imu_thresh && wait_for_jerk) { - PRINT_DEBUG(YELLOW "[init-s]: no IMU excitation, below threshold %.4f < %.4f\n" RESET, a_var_1to0, params.init_imu_thresh); + PRINT_INFO(YELLOW "[init-s]: no IMU excitation, below threshold %.3f < %.3f\n" RESET, a_var_1to0, params.init_imu_thresh); return false; } // We should also check that the old state was below the threshold! // This is the case when we have started up moving, and thus we need to wait for a period of stationary motion if (a_var_2to1 > params.init_imu_thresh && wait_for_jerk) { - PRINT_DEBUG(YELLOW "[init-s]: to much IMU excitation, above threshold %.4f > %.4f\n" RESET, a_var_2to1, params.init_imu_thresh); + PRINT_INFO(YELLOW "[init-s]: to much IMU excitation, above threshold %.3f > %.3f\n" RESET, a_var_2to1, params.init_imu_thresh); return false; } // If it is above the threshold and we are not waiting for a jerk // Then we are not stationary (i.e. moving) so we should wait till we are if ((a_var_1to0 > params.init_imu_thresh || a_var_2to1 > params.init_imu_thresh) && !wait_for_jerk) { - PRINT_DEBUG(YELLOW "[init-s]: to much IMU excitation, above threshold %.4f,%.4f > %.4f\n" RESET, a_var_1to0, a_var_2to1, - params.init_imu_thresh); + PRINT_INFO(YELLOW "[init-s]: to much IMU excitation, above threshold %.3f,%.3f > %.3f\n" RESET, a_var_2to1, a_var_1to0, + params.init_imu_thresh); return false; } @@ -134,23 +143,22 @@ bool StaticInitializer::initialize(double ×tamp, Eigen::MatrixXd &covarianc // Create base covariance and its covariance ordering order.clear(); order.push_back(t_imu); - covariance = 1e-3 * Eigen::MatrixXd::Identity(t_imu->size(), t_imu->size()); + covariance = std::pow(0.02, 2) * Eigen::MatrixXd::Identity(t_imu->size(), t_imu->size()); + covariance.block(3, 3, 3, 3) = std::pow(0.017, 2) * Eigen::Matrix3d::Identity(); // q + covariance.block(3, 3, 3, 3) = std::pow(0.05, 2) * Eigen::Matrix3d::Identity(); // p + covariance.block(6, 6, 3, 3) = std::pow(0.01, 2) * Eigen::Matrix3d::Identity(); // v (static) - // Make velocity uncertainty a bit bigger - covariance.block(t_imu->v()->id(), t_imu->v()->id(), 3, 3) *= 2; - - // A VIO system has 4dof unobservabile directions which can be arbitrarily picked. + // A VIO system has 4dof unobservable directions which can be arbitrarily picked. // This means that on startup, we can fix the yaw and position to be 100 percent known. + // TODO: why can't we set these to zero and get a good NEES realworld result? // Thus, after determining the global to current IMU orientation after initialization, we can propagate the global error // into the new IMU pose. In this case the position is directly equivalent, but the orientation needs to be propagated. - covariance(t_imu->q()->id() + 2, t_imu->q()->id() + 2) = 0.0; - covariance.block(t_imu->p()->id(), t_imu->p()->id(), 3, 3).setZero(); - - // Propagate into the current local IMU frame + // We propagate the global orientation into the current local IMU frame // R_GtoI = R_GtoI*R_GtoG -> H = R_GtoI - Eigen::Matrix3d R_GtoI = quat_2_Rot(q_GtoI); - covariance.block(t_imu->q()->id(), t_imu->q()->id(), 3, 3) = - R_GtoI * covariance.block(t_imu->q()->id(), t_imu->q()->id(), 3, 3) * R_GtoI.transpose(); + // Eigen::Matrix3d R_GtoI = quat_2_Rot(q_GtoI); + // covariance(2, 2) = std::pow(1e-4, 2); + // covariance.block(0, 0, 3, 3) = R_GtoI * covariance.block(0, 0, 3, 3) * R_GtoI.transpose(); + // covariance.block(3, 3, 3, 3) = std::pow(1e-3, 2) * Eigen::Matrix3d::Identity(); // Return :D return true; diff --git a/ov_init/src/static/StaticInitializer.h b/ov_init/src/static/StaticInitializer.h index 74c298e83..9e8352bc7 100644 --- a/ov_init/src/static/StaticInitializer.h +++ b/ov_init/src/static/StaticInitializer.h @@ -23,14 +23,15 @@ #define OV_INIT_STATICINITIALIZER_H #include "init/InertialInitializerOptions.h" -#include "utils/helper.h" -#include "feat/FeatureHelper.h" -#include "types/IMU.h" -#include "utils/colors.h" -#include "utils/print.h" -#include "utils/quat_ops.h" -#include "utils/sensor_data.h" +namespace ov_core { +class FeatureDatabase; +struct ImuData; +} // namespace ov_core +namespace ov_type { +class Type; +class IMU; +} // namespace ov_type namespace ov_init { diff --git a/ov_init/src/test_dynamic_init.cpp b/ov_init/src/test_dynamic_init.cpp index 7a7b084be..9161ffb50 100644 --- a/ov_init/src/test_dynamic_init.cpp +++ b/ov_init/src/test_dynamic_init.cpp @@ -40,9 +40,12 @@ #include "dynamic/DynamicInitializer.h" #include "init/InertialInitializerOptions.h" -#include "sim/Simulator.h" +#include "sim/SimulatorInit.h" #include "track/TrackSIM.h" +#include "types/IMU.h" +#include "types/Landmark.h" +#include "types/PoseJPL.h" #include "utils/colors.h" #include "utils/sensor_data.h" @@ -114,7 +117,7 @@ int main(int argc, char **argv) { PRINT_ERROR(RED "unable to parse all parameters, please fix\n" RESET); std::exit(EXIT_FAILURE); } - Simulator sim(params); + SimulatorInit sim(params); // Our initialization class objects auto imu_readings = std::make_shared>(); @@ -171,13 +174,10 @@ int main(int argc, char **argv) { std::shared_ptr _imu; std::map> _clones_IMU; std::unordered_map> _features_SLAM; - std::unordered_map> _calib_IMUtoCAM; - std::unordered_map> _cam_intrinsics; // First we will try to make sure we have all the data required for our initialization boost::posix_time::ptime rT1 = boost::posix_time::microsec_clock::local_time(); - bool success = - initializer->initialize(timestamp, covariance, order, _imu, _clones_IMU, _features_SLAM, _calib_IMUtoCAM, _cam_intrinsics); + bool success = initializer->initialize(timestamp, covariance, order, _imu, _clones_IMU, _features_SLAM); boost::posix_time::ptime rT2 = boost::posix_time::microsec_clock::local_time(); double time = (rT2 - rT1).total_microseconds() * 1e-6; if (success) { @@ -188,7 +188,7 @@ int main(int argc, char **argv) { // First lets align the groundtruth state with the IMU state // NOTE: imu biases do not have to be corrected with the pos yaw alignment here... Eigen::Matrix gt_imu; - assert(sim.get_state(timestamp + sim.get_true_parameters().calib_camimu_dt, gt_imu)); + assert_r(sim.get_state(timestamp + sim.get_true_parameters().calib_camimu_dt, gt_imu)); Eigen::Matrix3d R_ESTtoGT_imu; Eigen::Vector3d t_ESTinGT_imu; align_posyaw_single(_imu->quat(), _imu->pos(), gt_imu.block(1, 0, 4, 1), gt_imu.block(5, 0, 3, 1), R_ESTtoGT_imu, t_ESTinGT_imu); @@ -248,7 +248,7 @@ int main(int argc, char **argv) { q_es_0 = oldestpose->quat(); p_es_0 = oldestpose->pos(); Eigen::Matrix gt_imustate_0; - assert(sim.get_state(oldestpose_time + sim.get_true_parameters().calib_camimu_dt, gt_imustate_0)); + assert_r(sim.get_state(oldestpose_time + sim.get_true_parameters().calib_camimu_dt, gt_imustate_0)); q_gt_0 = gt_imustate_0.block(1, 0, 4, 1); p_gt_0 = gt_imustate_0.block(5, 0, 3, 1); Eigen::Matrix3d R_ESTtoGT; @@ -273,7 +273,7 @@ int main(int argc, char **argv) { poseEST.pose.position.y = _pose.second->pos()(1, 0); poseEST.pose.position.z = _pose.second->pos()(2, 0); Eigen::Matrix gt_imustate; - assert(sim.get_state(_pose.first + sim.get_true_parameters().calib_camimu_dt, gt_imustate)); + assert_r(sim.get_state(_pose.first + sim.get_true_parameters().calib_camimu_dt, gt_imustate)); gt_imustate.block(1, 0, 4, 1) = ov_core::quat_multiply(gt_imustate.block(1, 0, 4, 1), ov_core::rot_2_quat(R_ESTtoGT)); gt_imustate.block(5, 0, 3, 1) = R_ESTtoGT.transpose() * (gt_imustate.block(5, 0, 3, 1) - t_ESTinGT); poseGT.header.stamp = ros::Time(_pose.first); @@ -334,8 +334,8 @@ int main(int argc, char **argv) { // Wait for user approval do { - cout << '\n' << "Press a key to continue..."; - } while (cin.get() != '\n'); + std::cout << '\n' << "Press a key to continue..."; + } while (std::cin.get() != '\n'); // Reset our tracker and simulator so we can try to init again if (params.sim_do_perturbation) { diff --git a/ov_init/src/test_dynamic_mle.cpp b/ov_init/src/test_dynamic_mle.cpp index e242b5e05..6d8dc88e3 100644 --- a/ov_init/src/test_dynamic_mle.cpp +++ b/ov_init/src/test_dynamic_mle.cpp @@ -40,17 +40,19 @@ #include #endif -#include "utils/colors.h" -#include "utils/sensor_data.h" - #include "ceres/Factor_GenericPrior.h" #include "ceres/Factor_ImageReprojCalib.h" #include "ceres/Factor_ImuCPIv1.h" #include "ceres/State_JPLQuatLocal.h" #include "init/InertialInitializerOptions.h" -#include "sim/Simulator.h" +#include "sim/SimulatorInit.h" #include "utils/helper.h" +#include "types/IMU.h" +#include "types/PoseJPL.h" +#include "utils/colors.h" +#include "utils/sensor_data.h" + using namespace ov_init; // Define the function to be called when ctrl-c (SIGINT) is sent to process @@ -101,7 +103,7 @@ int main(int argc, char **argv) { PRINT_ERROR(RED "unable to parse all parameters, please fix\n" RESET); std::exit(EXIT_FAILURE); } - Simulator sim(params); + SimulatorInit sim(params); //=================================================================================== //=================================================================================== @@ -202,14 +204,13 @@ int main(int argc, char **argv) { // TODO: do not initialize from the groundtruth pose double time1 = timestamp_k1 + sim.get_true_parameters().calib_camimu_dt; Eigen::Matrix gt_imustate; - assert(sim.get_state(time1, gt_imustate)); + assert_r(sim.get_state(time1, gt_imustate)); state_k1 = gt_imustate.block(1, 0, 16, 1); } else { // Get our previous state timestamp (newest time) and biases to integrate with assert(timestamp_k != -1); - // timestamp_k = map_states.rbegin()->first; Eigen::Vector4d quat_k; for (int i = 0; i < 4; i++) { quat_k(i) = ceres_vars_ori.at(map_states.at(timestamp_k))[i]; @@ -576,7 +577,7 @@ int main(int argc, char **argv) { poseEST.pose.position.y = ceres_vars_pos[statepair.second][1]; poseEST.pose.position.z = ceres_vars_pos[statepair.second][2]; Eigen::Matrix gt_imustate; - assert(sim.get_state(statepair.first + sim.get_true_parameters().calib_camimu_dt, gt_imustate)); + assert_r(sim.get_state(statepair.first + sim.get_true_parameters().calib_camimu_dt, gt_imustate)); poseGT.header.stamp = ros::Time(statepair.first); poseGT.header.frame_id = "global"; poseGT.pose.orientation.x = gt_imustate(1); diff --git a/ov_init/src/test_simulation.cpp b/ov_init/src/test_simulation.cpp index ba6dc0adb..992880999 100644 --- a/ov_init/src/test_simulation.cpp +++ b/ov_init/src/test_simulation.cpp @@ -35,7 +35,7 @@ #endif #include "init/InertialInitializerOptions.h" -#include "sim/Simulator.h" +#include "sim/SimulatorInit.h" using namespace ov_init; @@ -73,7 +73,7 @@ int main(int argc, char **argv) { InertialInitializerOptions params; params.print_and_load(parser); params.print_and_load_simulation(parser); - Simulator sim(params); + SimulatorInit sim(params); // Continue to simulate until we have processed all the measurements signal(SIGINT, signal_callback_handler); diff --git a/ov_init/src/utils/helper.h b/ov_init/src/utils/helper.h index 7bfd25201..baa40eb5f 100644 --- a/ov_init/src/utils/helper.h +++ b/ov_init/src/utils/helper.h @@ -23,7 +23,6 @@ #define OV_INIT_HELPER #include "cpi/CpiV1.h" -#include "feat/FeatureHelper.h" #include "types/IMU.h" namespace ov_init { @@ -144,26 +143,26 @@ class InitializerHelper { Eigen::Vector3d z_axis = gravity_inI / gravity_inI.norm(); Eigen::Vector3d x_axis, y_axis; Eigen::Vector3d e_1(1.0, 0.0, 0.0); - // Eigen::Vector3d e_2(0.0, 1.0, 0.0); - // double inner1 = e_1.dot(z_axis) / z_axis.norm(); - // double inner2 = e_2.dot(z_axis) / z_axis.norm(); - // if (fabs(inner1) >= fabs(inner2)) { - // x_axis = z_axis.cross(e_1); - // x_axis = x_axis / x_axis.norm(); - // y_axis = z_axis.cross(x_axis); - // y_axis = y_axis / y_axis.norm(); - // } else { - // x_axis = z_axis.cross(e_2); - // x_axis = x_axis / x_axis.norm(); - // y_axis = z_axis.cross(x_axis); - // y_axis = y_axis / y_axis.norm(); - // } + Eigen::Vector3d e_2(0.0, 1.0, 0.0); + double inner1 = e_1.dot(z_axis) / z_axis.norm(); + double inner2 = e_2.dot(z_axis) / z_axis.norm(); + if (fabs(inner1) >= fabs(inner2)) { + x_axis = z_axis.cross(e_1); + x_axis = x_axis / x_axis.norm(); + y_axis = z_axis.cross(x_axis); + y_axis = y_axis / y_axis.norm(); + } else { + x_axis = z_axis.cross(e_2); + x_axis = x_axis / x_axis.norm(); + y_axis = z_axis.cross(x_axis); + y_axis = y_axis / y_axis.norm(); + } // Original method - x_axis = e_1 - z_axis * z_axis.transpose() * e_1; - x_axis = x_axis / x_axis.norm(); - y_axis = ov_core::skew_x(z_axis) * x_axis; - y_axis = y_axis / y_axis.norm(); + // x_axis = e_1 - z_axis * z_axis.transpose() * e_1; + // x_axis = x_axis / x_axis.norm(); + // y_axis = ov_core::skew_x(z_axis) * x_axis; + // y_axis = y_axis / y_axis.norm(); // Rotation from our global (where gravity is only along the z-axis) to the local one R_GtoI.block(0, 0, 3, 1) = x_axis; @@ -181,7 +180,7 @@ class InitializerHelper { * @param gravity_mag Scalar size of gravity (normally is 9.81) * @return Coefficents from highest to the constant */ - static Eigen::VectorXd compute_dongsi_coeff(Eigen::MatrixXd &D, const Eigen::MatrixXd &d, double gravity_mag) { + static Eigen::Matrix compute_dongsi_coeff(Eigen::MatrixXd &D, const Eigen::MatrixXd &d, double gravity_mag) { // matlab constants assert(D.rows() == 3); @@ -201,7 +200,7 @@ class InitializerHelper { double g_sq = g * g; // Compute the coefficients - Eigen::VectorXd coeff = Eigen::VectorXd::Zero(7); + Eigen::Matrix coeff = Eigen::Matrix::Zero(); coeff(6) = -(-D1_1_sq * D2_2_sq * D3_3_sq * g_sq + D1_1_sq * D2_2_sq * d3_sq + 2 * D1_1_sq * D2_2 * D2_3 * D3_2 * D3_3 * g_sq - D1_1_sq * D2_2 * D2_3 * d2 * d3 - D1_1_sq * D2_2 * D3_2 * d2 * d3 - D1_1_sq * D2_3_sq * D3_2_sq * g_sq + diff --git a/ov_msckf/CMakeLists.txt b/ov_msckf/CMakeLists.txt index d62570fe0..51c3b7e42 100644 --- a/ov_msckf/CMakeLists.txt +++ b/ov_msckf/CMakeLists.txt @@ -27,6 +27,11 @@ set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) +# Default build as release +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE "Release" CACHE STRING "Choose the type of build, options are: Debug Release RelWithDebInfo MinSizeRel." FORCE) +endif() + # Enable compile optimizations set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops") diff --git a/ov_msckf/cmake/ROS1.cmake b/ov_msckf/cmake/ROS1.cmake index 08ce9f648..f5102d195 100644 --- a/ov_msckf/cmake/ROS1.cmake +++ b/ov_msckf/cmake/ROS1.cmake @@ -37,7 +37,7 @@ list(APPEND thirdparty_libraries ${OpenCV_LIBRARIES} ${CERES_LIBRARIES} ${catkin_LIBRARIES} -) + ) # If we are not building with ROS then we need to manually link to its headers # This isn't that elegant of a way, but this at least allows for building without ROS @@ -79,13 +79,14 @@ list(APPEND LIBRARY_SOURCES src/state/StateHelper.cpp src/state/Propagator.cpp src/core/VioManager.cpp + src/core/VioManagerHelper.cpp src/update/UpdaterHelper.cpp src/update/UpdaterMSCKF.cpp src/update/UpdaterSLAM.cpp src/update/UpdaterZeroVelocity.cpp -) + ) if (catkin_FOUND AND ENABLE_ROS) - list(APPEND LIBRARY_SOURCES src/ros/ROS1Visualizer.cpp) + list(APPEND LIBRARY_SOURCES src/ros/ROS1Visualizer.cpp src/ros/ROSVisualizerHelper.cpp) endif () file(GLOB_RECURSE LIBRARY_HEADERS "src/*.h") add_library(ov_msckf_lib SHARED ${LIBRARY_SOURCES} ${LIBRARY_HEADERS}) @@ -95,11 +96,11 @@ install(TARGETS ov_msckf_lib ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) + ) install(DIRECTORY src/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" -) + ) ################################################## # Make binary files! @@ -113,7 +114,7 @@ if (catkin_FOUND AND ENABLE_ROS) ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ) + ) add_executable(run_subscribe_msckf src/run_subscribe_msckf.cpp) target_link_libraries(run_subscribe_msckf ov_msckf_lib ${thirdparty_libraries}) @@ -121,7 +122,7 @@ if (catkin_FOUND AND ENABLE_ROS) ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ) + ) endif () @@ -131,7 +132,7 @@ install(TARGETS run_simulation ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) + ) add_executable(test_sim_meas src/test_sim_meas.cpp) target_link_libraries(test_sim_meas ov_msckf_lib ${thirdparty_libraries}) @@ -139,7 +140,7 @@ install(TARGETS test_sim_meas ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) + ) add_executable(test_sim_repeat src/test_sim_repeat.cpp) target_link_libraries(test_sim_repeat ov_msckf_lib ${thirdparty_libraries}) @@ -147,7 +148,7 @@ install(TARGETS test_sim_repeat ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) + ) ################################################## @@ -156,7 +157,7 @@ install(TARGETS test_sim_repeat install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch -) + ) diff --git a/ov_msckf/cmake/ROS2.cmake b/ov_msckf/cmake/ROS2.cmake index a1a9850ad..66db6124b 100644 --- a/ov_msckf/cmake/ROS2.cmake +++ b/ov_msckf/cmake/ROS2.cmake @@ -34,11 +34,11 @@ list(APPEND thirdparty_libraries ${Boost_LIBRARIES} ${CERES_LIBRARIES} ${OpenCV_LIBRARIES} -) + ) list(APPEND ament_libraries rclcpp tf2_ros - tf2_geometry_msgs + tf2_geometry_msgs std_msgs geometry_msgs sensor_msgs @@ -47,7 +47,7 @@ list(APPEND ament_libraries image_transport ov_core ov_init -) + ) ################################################## # Make the shared library @@ -60,12 +60,13 @@ list(APPEND LIBRARY_SOURCES src/state/StateHelper.cpp src/state/Propagator.cpp src/core/VioManager.cpp + src/core/VioManagerHelper.cpp src/update/UpdaterHelper.cpp src/update/UpdaterMSCKF.cpp src/update/UpdaterSLAM.cpp src/update/UpdaterZeroVelocity.cpp -) -list(APPEND LIBRARY_SOURCES src/ros/ROS2Visualizer.cpp) + ) +list(APPEND LIBRARY_SOURCES src/ros/ROS2Visualizer.cpp src/ros/ROSVisualizerHelper.cpp) file(GLOB_RECURSE LIBRARY_HEADERS "src/*.h") add_library(ov_msckf_lib SHARED ${LIBRARY_SOURCES} ${LIBRARY_HEADERS}) ament_target_dependencies(ov_msckf_lib ${ament_libraries}) @@ -75,11 +76,11 @@ install(TARGETS ov_msckf_lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin PUBLIC_HEADER DESTINATION include -) + ) install(DIRECTORY src/ DESTINATION include FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" -) + ) ament_export_include_directories(include) ament_export_libraries(ov_msckf_lib) diff --git a/ov_msckf/launch/serial.launch b/ov_msckf/launch/serial.launch index c11c7b18d..319f9fba4 100644 --- a/ov_msckf/launch/serial.launch +++ b/ov_msckf/launch/serial.launch @@ -9,9 +9,10 @@ - + - + + @@ -36,7 +37,6 @@ - diff --git a/ov_msckf/launch/subscribe.launch b/ov_msckf/launch/subscribe.launch index 1afc2478d..bc03bb8f1 100644 --- a/ov_msckf/launch/subscribe.launch +++ b/ov_msckf/launch/subscribe.launch @@ -9,9 +9,11 @@ - + + + @@ -45,7 +47,7 @@ - + diff --git a/ov_msckf/launch/subscribe.launch.py b/ov_msckf/launch/subscribe.launch.py index dfd9e7fd1..71ead5ee8 100644 --- a/ov_msckf/launch/subscribe.launch.py +++ b/ov_msckf/launch/subscribe.launch.py @@ -13,7 +13,7 @@ name="ov_enable", default_value="true", description="enable OpenVINS node" ), DeclareLaunchArgument( - name="rviz_enable", default_value="true", description="enable rviz node" + name="rviz_enable", default_value="false", description="enable rviz node" ), DeclareLaunchArgument( name="config", diff --git a/ov_msckf/scripts/run_ros_eth.sh b/ov_msckf/scripts/run_ros_eth.sh index f3cc03ea8..c67ad595c 100755 --- a/ov_msckf/scripts/run_ros_eth.sh +++ b/ov_msckf/scripts/run_ros_eth.sh @@ -8,7 +8,6 @@ source ${SCRIPT_DIR}/../../../../devel/setup.bash #============================================================= #============================================================= - # estimator configurations modes=( "mono" @@ -40,36 +39,39 @@ bagstarttimes=( "0" "0" "0" - "40" - "35" - "10" - "17" - "18" + "40" # 40 + "35" # 35 + "5" # 10 + "10" # 17 + "5" # 18 ) # location to save log files into -save_path1="/home/chuchu/test_ov/openvins_pra/exp_euroc/algorithms" -save_path2="/home/chuchu/test_ov/openvins_pra/exp_euroc/timings" -bag_path="/home/chuchu/datasets/euroc_mav/" +save_path1="/home/patrick/github/pubs_data/pgeneva/2022_openvins_test/exp_euroc/algorithms" +save_path2="/home/patrick/github/pubs_data/pgeneva/2022_openvins_test/exp_euroc/timings" +bag_path="/media/patrick/RPNG FLASH 3/euroc_mav/" +ov_ver="2.6.1" + + #============================================================= #============================================================= #============================================================= -# Loop through all modes -for h in "${!modes[@]}"; do # Loop through all datasets for i in "${!bagnames[@]}"; do +# Loop through all modes +for h in "${!modes[@]}"; do # Monte Carlo runs for this dataset # If you want more runs, change the below loop -for j in {00..04}; do +for j in {00..00}; do # start timing start_time="$(date -u +%s)" -filename_est="$save_path1/ov_2.4_${modes[h]}/${bagnames[i]}/${j}_estimate.txt" -filename_time="$save_path2/ov_2.4_${modes[h]}/${bagnames[i]}/${j}_timing.txt" +filename_est="$save_path1/ov_${ov_ver}_${modes[h]}/${bagnames[i]}/${j}_estimate.txt" +filename_time="$save_path2/ov_${ov_ver}_${modes[h]}/${bagnames[i]}/${j}_timing.txt" # number of cameras if [ "${modes[h]}" == "mono" ] @@ -89,12 +91,15 @@ then fi # run our ROS launch file (note we send console output to terminator) +# subscribe=live pub, serial=read from bag (fast) roslaunch ov_msckf serial.launch \ max_cameras:="$temp1" \ use_stereo:="$temp2" \ config:="euroc_mav" \ + dataset:="${bagnames[i]}" \ bag:="$bag_path/${bagnames[i]}.bag" \ bag_start:="${bagstarttimes[i]}" \ + dobag:="true" \ dosave:="true" \ path_est:="$filename_est" \ dotime:="true" \ diff --git a/ov_msckf/scripts/run_ros_kaist.sh b/ov_msckf/scripts/run_ros_kaist.sh new file mode 100755 index 000000000..0fe1f3e29 --- /dev/null +++ b/ov_msckf/scripts/run_ros_kaist.sh @@ -0,0 +1,107 @@ +#!/usr/bin/env bash + +# Source our workspace directory to load ENV variables +SCRIPT_DIR="$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )" +source ${SCRIPT_DIR}/../../../../devel/setup.bash + +#============================================================= +#============================================================= +#============================================================= + +# estimator configurations +modes=( +# "mono" # doesn't work... +# "binocular" # doesn't work... + "stereo" +) + +# dataset locations +bagnames=( + "urban28" + "urban32" +# "urban34" # too strong of sun... + "urban38" + "urban39" +) + +# how far we should start into the dataset +# this can be used to skip the initial sections +bagstarttimes=( + "0" + "0" +# "0" + "0" + "0" +) + +# location to save log files into +save_path1="/home/patrick/github/pubs_data/pgeneva/2022_openvins_test/exp_kaist/algorithms" +save_path2="/home/patrick/github/pubs_data/pgeneva/2022_openvins_test/exp_kaist/timings" +bag_path="/media/patrick/RPNG FLASH 3/kaist/" +ov_ver="2.6.1" + + + +#============================================================= +#============================================================= +#============================================================= + +# Loop through all datasets +for i in "${!bagnames[@]}"; do +# Loop through all modes +for h in "${!modes[@]}"; do + +# Monte Carlo runs for this dataset +# If you want more runs, change the below loop +for j in {00..00}; do + +# start timing +start_time="$(date -u +%s)" +filename_est="$save_path1/ov_${ov_ver}_${modes[h]}/${bagnames[i]}/${j}_estimate.txt" +filename_time="$save_path2/ov_${ov_ver}_${modes[h]}/${bagnames[i]}/${j}_timing.txt" + +# number of cameras +if [ "${modes[h]}" == "mono" ] +then + temp1="1" + temp2="true" +fi +if [ "${modes[h]}" == "binocular" ] +then + temp1="2" + temp2="false" +fi +if [ "${modes[h]}" == "stereo" ] +then + temp1="2" + temp2="true" +fi + +# run our ROS launch file (note we send console output to terminator) +# subscribe=live pub, serial=read from bag (fast) +roslaunch ov_msckf serial.launch \ + max_cameras:="$temp1" \ + use_stereo:="$temp2" \ + config:="kaist" \ + dataset:="${bagnames[i]}" \ + bag:="$bag_path/${bagnames[i]}.bag" \ + bag_start:="${bagstarttimes[i]}" \ + dobag:="true" \ + dosave:="true" \ + path_est:="$filename_est" \ + dotime:="true" \ + dolivetraj:="true" \ + path_time:="$filename_time" &> /dev/null + +# print out the time elapsed +end_time="$(date -u +%s)" +elapsed="$(($end_time-$start_time))" +echo "BASH: ${modes[h]} - ${bagnames[i]} - run $j took $elapsed seconds"; + +done + + +done +done + + diff --git a/ov_msckf/scripts/run_ros_kaistvio.sh b/ov_msckf/scripts/run_ros_kaistvio.sh index 76c50164a..6e34c154d 100755 --- a/ov_msckf/scripts/run_ros_kaistvio.sh +++ b/ov_msckf/scripts/run_ros_kaistvio.sh @@ -1,18 +1,17 @@ #!/usr/bin/env bash # Source our workspace directory to load ENV variables -source /home/patrick/workspace/catkin_ws_ov/devel/setup.bash - +SCRIPT_DIR="$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )" +source ${SCRIPT_DIR}/../../../../devel/setup.bash #============================================================= #============================================================= #============================================================= - # estimator configurations modes=( - "mono" - "binocular" +# "mono" # has scale issues, need to debug (imu intrinsics?)!! +# "binocular" # has scale issues, need to debug (imu intrinsics?)!! "stereo" ) @@ -47,31 +46,31 @@ bagstarttimes=( "0" ) +# location to save log files into +save_path1="/home/patrick/github/pubs_data/pgeneva/2022_openvins_test/exp_kaistvio/algorithms" +save_path2="/home/patrick/github/pubs_data/pgeneva/2022_openvins_test/exp_kaistvio/timings" +bag_path="/media/patrick/RPNG FLASH 3/kaist_vio/" +ov_ver="2.6.1" -# location to save log files into -save_path1="/home/patrick/github/pubs_data/pgeneva/2020_openvins_2.3.1/exp_kaistvio/algorithms" -save_path2="/home/patrick/github/pubs_data/pgeneva/2020_openvins_2.3.1/exp_kaistvio/timings" -bag_path="/media/patrick/RPNG\ FLASH\ 3/KAIST_VIO" #============================================================= #============================================================= #============================================================= - -# Loop through all modes -for h in "${!modes[@]}"; do # Loop through all datasets for i in "${!bagnames[@]}"; do +# Loop through all modes +for h in "${!modes[@]}"; do # Monte Carlo runs for this dataset # If you want more runs, change the below loop -for j in {00..04}; do +for j in {00..00}; do # start timing start_time="$(date -u +%s)" -filename_est="$save_path1/ov_2.3.3_${modes[h]}/${bagnames[i]}/${j}_estimate.txt" -filename_time="$save_path2/ov_2.3.3_${modes[h]}/${bagnames[i]}/${j}_timing.txt" +filename_est="$save_path1/ov_${ov_ver}_${modes[h]}/${bagnames[i]}/${j}_estimate.txt" +filename_time="$save_path2/ov_${ov_ver}_${modes[h]}/${bagnames[i]}/${j}_timing.txt" # number of cameras if [ "${modes[h]}" == "mono" ] @@ -91,17 +90,21 @@ then fi # run our ROS launch file (note we send console output to terminator) -roslaunch ov_msckf pgeneva_ros_kaistvio.launch \ +# subscribe=live pub, serial=read from bag (fast) +roslaunch ov_msckf serial.launch \ max_cameras:="$temp1" \ use_stereo:="$temp2" \ + config:="kaist_vio" \ + dataset:="${bagnames[i]}" \ bag:="$bag_path/${bagnames[i]}.bag" \ bag_start:="${bagstarttimes[i]}" \ + dobag:="true" \ dosave:="true" \ path_est:="$filename_est" \ dotime:="true" \ + dolivetraj:="true" \ path_time:="$filename_time" &> /dev/null - # print out the time elapsed end_time="$(date -u +%s)" elapsed="$(($end_time-$start_time))" diff --git a/ov_msckf/scripts/run_ros_tumvi.sh b/ov_msckf/scripts/run_ros_tumvi.sh index 87989c8bd..a6d580740 100755 --- a/ov_msckf/scripts/run_ros_tumvi.sh +++ b/ov_msckf/scripts/run_ros_tumvi.sh @@ -22,7 +22,7 @@ bagnames=( "dataset-room3_512_16" "dataset-room4_512_16" "dataset-room5_512_16" - "dataset-room6_512_16" +# "dataset-room6_512_16" # requires lower imu init threshold... ) # how far we should start into the dataset @@ -33,32 +33,34 @@ bagstarttimes=( "0" "0" "0" - "0" +# "0" # requires lower imu init threshold... ) # location to save log files into -save_path1="/home/chuchu/test_ov/openvins_pra/exp_tum/algorithms" -save_path2="/home/chuchu/test_ov/openvins_pra/exp_tum/timings" -bag_path="/home/chuchu/datasets/tum_vi" +save_path1="/home/patrick/github/pubs_data/pgeneva/2022_openvins_test/exp_tum/algorithms" +save_path2="/home/patrick/github/pubs_data/pgeneva/2022_openvins_test/exp_tum/timings" +bag_path="/media/patrick/RPNG FLASH 3/tum_vi/" +ov_ver="2.6.1" + #============================================================= #============================================================= #============================================================= -# TODO: Still need to test all, see how to get imu threshold in -# Loop through all modes -for h in "${!modes[@]}"; do + # Loop through all datasets for i in "${!bagnames[@]}"; do +# Loop through all modes +for h in "${!modes[@]}"; do # Monte Carlo runs for this dataset # If you want more runs, change the below loop -for j in {00..04}; do +for j in {00..00}; do # start timing start_time="$(date -u +%s)" -filename_est="$save_path1/ov_2.4_${modes[h]}/${bagnames[i]}/${j}_estimate.txt" -filename_time="$save_path2/ov_2.4_${modes[h]}/${bagnames[i]}/${j}_timing.txt" +filename_est="$save_path1/ov_${ov_ver}_${modes[h]}/${bagnames[i]}/${j}_estimate.txt" +filename_time="$save_path2/ov_${ov_ver}_${modes[h]}/${bagnames[i]}/${j}_timing.txt" # number of cameras if [ "${modes[h]}" == "mono" ] @@ -78,12 +80,15 @@ then fi # run our ROS launch file (note we send console output to terminator) +# subscribe=live pub, serial=read from bag (fast) roslaunch ov_msckf serial.launch \ max_cameras:="$temp1" \ use_stereo:="$temp2" \ config:="tum_vi" \ + dataset:="${bagnames[i]}" \ bag:="$bag_path/${bagnames[i]}.bag" \ bag_start:="${bagstarttimes[i]}" \ + dobag:="true" \ dosave:="true" \ path_est:="$filename_est" \ dotime:="true" \ diff --git a/ov_msckf/scripts/run_ros_uzhfpv.sh b/ov_msckf/scripts/run_ros_uzhfpv.sh index 9aa4a39cd..64fbc4ba1 100755 --- a/ov_msckf/scripts/run_ros_uzhfpv.sh +++ b/ov_msckf/scripts/run_ros_uzhfpv.sh @@ -4,12 +4,10 @@ SCRIPT_DIR="$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )" source ${SCRIPT_DIR}/../../../../devel/setup.bash - #============================================================= #============================================================= #============================================================= - # estimator configurations modes=( "mono" @@ -19,7 +17,7 @@ modes=( # dataset locations bagnames=( -# "indoor_forward_3_snapdragon_with_gt" # bag needs to end early as there is a hard landing + "indoor_forward_3_snapdragon_with_gt" # bag needs to end early as there is a hard landing.... "indoor_forward_5_snapdragon_with_gt" "indoor_forward_6_snapdragon_with_gt" "indoor_forward_7_snapdragon_with_gt" @@ -27,48 +25,41 @@ bagnames=( "indoor_forward_10_snapdragon_with_gt" "indoor_45_2_snapdragon_with_gt" "indoor_45_4_snapdragon_with_gt" -# "indoor_45_9_snapdragon_with_gt" # problem one, seems to fail part way in due to freefalling +# "indoor_45_9_snapdragon_with_gt" # problem one, seems to fail part way in due to freefalling? "indoor_45_12_snapdragon_with_gt" "indoor_45_13_snapdragon_with_gt" "indoor_45_14_snapdragon_with_gt" -# "outdoor_forward_1_snapdragon_with_gt" -# "outdoor_forward_3_snapdragon_with_gt" -# "outdoor_forward_5_snapdragon_with_gt" -# "outdoor_45_1_snapdragon_with_gt" + "outdoor_forward_1_snapdragon_with_gt" + "outdoor_forward_3_snapdragon_with_gt" + "outdoor_forward_5_snapdragon_with_gt" + "outdoor_45_1_snapdragon_with_gt" ) # what sensor configuration each dataset has -# 0: indoor forward facing -# 1: indoor 45 degree downward facing -# 2: outdoor forward facing -# 3: outdoor 45 degree downward facing -sensorconfig=( -# indoor forward -# "0" # bag needs to end early as there is a hard landing - "0" - "0" - "0" - "0" - "0" -# indoor 45 degree - "1" - "1" -# "1" # problem one, seems to fail part way in due to freefalling - "1" - "1" - "1" -# outdoor forward and 45 -# "2" -# "2" -# "2" -# "3" +config=( + "uzhfpv_indoor" # bag needs to end early as there is a hard landing.... + "uzhfpv_indoor" + "uzhfpv_indoor" + "uzhfpv_indoor" + "uzhfpv_indoor" + "uzhfpv_indoor" + "uzhfpv_indoor_45" + "uzhfpv_indoor_45" +# "uzhfpv_indoor_45" # problem one, seems to fail part way in due to freefalling? + "uzhfpv_indoor_45" + "uzhfpv_indoor_45" + "uzhfpv_indoor_45" + "uzhfpv_outdoor" + "uzhfpv_outdoor" + "uzhfpv_outdoor" + "uzhfpv_outdoor_45" ) # how far we should start into the dataset # this can be used to skip the initial sections bagstarttimes=( # indoor forward -# "25" + "0" # bag needs to end early as there is a hard landing.... "0" "0" "0" @@ -77,63 +68,42 @@ bagstarttimes=( # indoor 45 degree "0" "0" -# "17" # problem one, seems to fail part way in due to freefalling +# "0" # problem one, seems to fail part way in due to freefalling? "0" "0" "0" # outdoor forward and 45 -# "0" -# "0" -# "41" -# "21" -) - -# threshold for variance to detect if the unit has moved yet -imuthreshold=( -# indoor forward -# "5.0" - "0.5" - "0.5" - "0.5" - "0.5" - "0.5" -# indoor 45 degree - "0.5" - "0.5" -# "4.0" # problem one, seems to fail part way in due to freefalling - "0.5" - "0.5" - "0.5" -# outdoor forward and 45 -# "0.5" -# "0.5" -# "4.0" -# "4.0" + "0" + "0" + "0" + "0" ) # location to save log files into -save_path1="/home/patrick/github/pubs_data/pgeneva/2020_openvins_2.4/exp_uzhfpv/algorithms" -save_path2="/home/patrick/github/pubs_data/pgeneva/2020_openvins_2.4/exp_uzhfpv/timings" -bag_path="/media/patrick/RPNG\ FLASH\ 2/uzhfpv_newer" +save_path1="/home/patrick/github/pubs_data/pgeneva/2022_openvins_test/exp_uzhfpv/algorithms" +save_path2="/home/patrick/github/pubs_data/pgeneva/2022_openvins_test/exp_uzhfpv/timings" +bag_path="/media/patrick/RPNG FLASH 3/" +ov_ver="2.6.1" + + #============================================================= #============================================================= #============================================================= - -# Loop through all modes -for h in "${!modes[@]}"; do # Loop through all datasets for i in "${!bagnames[@]}"; do +# Loop through all modes +for h in "${!modes[@]}"; do # Monte Carlo runs for this dataset # If you want more runs, change the below loop -for j in {00..04}; do +for j in {00..00}; do # start timing start_time="$(date -u +%s)" -filename_est="$save_path1/ov_2.4_${modes[h]}/${bagnames[i]}/${j}_estimate.txt" -filename_time="$save_path2/ov_2.4_${modes[h]}/${bagnames[i]}/${j}_timing.txt" +filename_est="$save_path1/ov_${ov_ver}_${modes[h]}/${bagnames[i]}/${j}_estimate.txt" +filename_time="$save_path2/ov_${ov_ver}_${modes[h]}/${bagnames[i]}/${j}_timing.txt" # number of cameras if [ "${modes[h]}" == "mono" ] @@ -153,19 +123,21 @@ then fi # run our ROS launch file (note we send console output to terminator) -roslaunch ov_msckf pgeneva_ros_uzhfpv.launch \ +# subscribe=live pub, serial=read from bag (fast) +roslaunch ov_msckf serial.launch \ max_cameras:="$temp1" \ use_stereo:="$temp2" \ - bag:="$bag_path/${bagnames[i]}.bag" \ + config:="${config[i]}" \ + dataset:="${bagnames[i]}" \ + bag:="$bag_path/${config[i]}/${bagnames[i]}.bag" \ bag_start:="${bagstarttimes[i]}" \ - sensor_config:="${sensorconfig[i]}" \ - init_imu_thresh:="${imuthreshold[i]}" \ + dobag:="true" \ dosave:="true" \ path_est:="$filename_est" \ dotime:="true" \ + dolivetraj:="true" \ path_time:="$filename_time" &> /dev/null - # print out the time elapsed end_time="$(date -u +%s)" elapsed="$(($end_time-$start_time))" diff --git a/ov_msckf/scripts/run_sim_calib.sh b/ov_msckf/scripts/run_sim_calib.sh index 5cc0d463d..3848c1d97 100755 --- a/ov_msckf/scripts/run_sim_calib.sh +++ b/ov_msckf/scripts/run_sim_calib.sh @@ -45,13 +45,10 @@ for n in "${!sim_do_perturbation[@]}"; do # Monte Carlo runs for this dataset for j in {00..02}; do - filename_est="$save_path_est/calib_${sim_do_calibration[m]}/perturb_${sim_do_perturbation[n]}/${datasets[h]}/estimate_$j.txt" filename_gt="$save_path_gt/${datasets[h]}.txt" -#=============================================== -# Start Monte Carlo Simulations -#=============================================== +# launch the simulation script start_time="$(date -u +%s)" roslaunch ov_msckf simulation.launch \ seed:="$((10#$j + 1))" \ @@ -61,11 +58,13 @@ roslaunch ov_msckf simulation.launch \ dosave_pose:="true" \ path_est:="$filename_est" \ path_gt:="$filename_gt" &> /dev/null + + +# print out the time elapsed end_time="$(date -u +%s)" elapsed="$(($end_time-$start_time))" -echo "BASH: ${datasets[h]} - calib_${sim_do_calibration[m]} - perturb_${sim_do_perturbation[n]} - run $j took $elapsed seconds"; -#=============================================== -#=============================================== +echo "BASH: ${datasets[m]} - calib_${sim_do_calibration[m]} - perturb_${sim_do_perturbation[n]} - run $j took $elapsed seconds"; + done done diff --git a/ov_msckf/scripts/run_sim_cams.sh b/ov_msckf/scripts/run_sim_cams.sh index c90b73dfc..3b5bb8ca3 100755 --- a/ov_msckf/scripts/run_sim_cams.sh +++ b/ov_msckf/scripts/run_sim_cams.sh @@ -45,7 +45,7 @@ start_time="$(date -u +%s)" filename_est="$save_path_est/ov_v23_cam${cameras[i]}/${datasets[h]}/estimate_$j.txt" filename_gt="$save_path_gt/${datasets[h]}.txt" -# run our ROS launch file (note we send console output to terminator) +# launch the simulation script roslaunch ov_msckf simulation.launch \ seed:="$((10#$j + 1))" \ max_cameras:="${cameras[i]}" \ diff --git a/ov_msckf/scripts/run_sim_rep.sh b/ov_msckf/scripts/run_sim_rep.sh index 36ea0d416..0827b46c1 100755 --- a/ov_msckf/scripts/run_sim_rep.sh +++ b/ov_msckf/scripts/run_sim_rep.sh @@ -63,7 +63,7 @@ fi filename_est="$save_path_est/${representations[i]}$temp/${datasets[m]}/estimate_$j.txt" filename_gt="$save_path_gt/${datasets[m]}.txt" -# run our ROS launch file (note we send console output to terminator) +# launch the simulation script roslaunch ov_msckf simulation.launch \ seed:="$((10#$j + 1))" \ dataset:="${datasets[m]}.txt" \ diff --git a/ov_msckf/src/core/VioManager.cpp b/ov_msckf/src/core/VioManager.cpp index 7232aea97..4578d2570 100644 --- a/ov_msckf/src/core/VioManager.cpp +++ b/ov_msckf/src/core/VioManager.cpp @@ -21,6 +21,28 @@ #include "VioManager.h" +#include "feat/Feature.h" +#include "feat/FeatureDatabase.h" +#include "feat/FeatureInitializer.h" +#include "track/TrackAruco.h" +#include "track/TrackDescriptor.h" +#include "track/TrackKLT.h" +#include "track/TrackSIM.h" +#include "types/Landmark.h" +#include "types/LandmarkRepresentation.h" +#include "utils/opencv_lambda_body.h" +#include "utils/print.h" +#include "utils/sensor_data.h" + +#include "init/InertialInitializer.h" + +#include "state/Propagator.h" +#include "state/State.h" +#include "state/StateHelper.h" +#include "update/UpdaterMSCKF.h" +#include "update/UpdaterSLAM.h" +#include "update/UpdaterZeroVelocity.h" + using namespace ov_core; using namespace ov_type; using namespace ov_msckf; @@ -93,15 +115,17 @@ VioManager::VioManager(VioManagerOptions ¶ms_) : thread_init_running(false), // Let's make a feature extractor // NOTE: after we initialize we will increase the total number of feature tracks + // NOTE: we will split the total number of features over all cameras uniformly trackDATABASE = std::make_shared(); + int init_max_features = std::floor((double)params.init_options.init_max_features / (double)params.state_options.num_cameras); if (params.use_klt) { - trackFEATS = std::shared_ptr(new TrackKLT(state->_cam_intrinsics_cameras, params.init_options.init_max_features, + trackFEATS = std::shared_ptr(new TrackKLT(state->_cam_intrinsics_cameras, init_max_features, state->_options.max_aruco_features, params.use_stereo, params.histogram_method, params.fast_threshold, params.grid_x, params.grid_y, params.min_px_dist)); } else { trackFEATS = std::shared_ptr(new TrackDescriptor( - state->_cam_intrinsics_cameras, params.init_options.init_max_features, state->_options.max_aruco_features, params.use_stereo, - params.histogram_method, params.fast_threshold, params.grid_x, params.grid_y, params.min_px_dist, params.knn_ratio)); + state->_cam_intrinsics_cameras, init_max_features, state->_options.max_aruco_features, params.use_stereo, params.histogram_method, + params.fast_threshold, params.grid_x, params.grid_y, params.min_px_dist, params.knn_ratio)); } // Initialize our aruco tag extractor @@ -131,6 +155,27 @@ VioManager::VioManager(VioManagerOptions ¶ms_) : thread_init_running(false), active_tracks_initializer = std::make_shared(params.featinit_options); } +void VioManager::feed_measurement_imu(const ov_core::ImuData &message) { + + // The oldest time we need IMU with is the last clone + // We shouldn't really need the whole window, but if we go backwards in time we will + double oldest_time = state->margtimestep(); + if (oldest_time > state->_timestamp) { + oldest_time = -1; + } + propagator->feed_imu(message, oldest_time); + + // Push back to our initializer + if (!is_initialized_vio) { + initializer->feed_imu(message, oldest_time); + } + + // Push back to the zero velocity updater if we have it + if (is_initialized_vio && updaterZUPT != nullptr) { + updaterZUPT->feed_imu(message, oldest_time); + } +} + void VioManager::feed_measurement_simulation(double timestamp, const std::vector &camids, const std::vector>> &feats) { @@ -304,7 +349,7 @@ void VioManager::do_feature_propagate_update(const ov_core::CameraData &message) feats_lost = trackFEATS->get_feature_database()->features_not_containing_newer(state->_timestamp, false, true); // Don't need to get the oldest features until we reach our max number of clones - if ((int)state->_clones_IMU.size() > state->_options.max_clone_size) { + if ((int)state->_clones_IMU.size() > state->_options.max_clone_size || (int)state->_clones_IMU.size() > 5) { feats_marg = trackFEATS->get_feature_database()->features_containing(state->margtimestep(), false, true); if (trackARUCO != nullptr && message.timestamp - startup_time >= params.dt_slam_delay) { feats_slam = trackARUCO->get_feature_database()->features_containing(state->margtimestep(), false, true); @@ -610,288 +655,3 @@ void VioManager::do_feature_propagate_update(const ov_core::CameraData &message) } } } - -bool VioManager::try_to_initialize(const ov_core::CameraData &message) { - - // Directly return if the initialization thread is running - // Note that we lock on the queue since we could have finished an update - // And are using this queue to propagate the state forward. We should wait in this case - if (thread_init_running) { - std::lock_guard lck(camera_queue_init_mtx); - camera_queue_init.push_back(message.timestamp); - return false; - } - - // If the thread was a success, then return success! - if (thread_init_success) { - return true; - } - - // Run the initialization in a second thread so it can go as slow as it desires - thread_init_running = true; - std::thread thread([&] { - // Returns from our initializer - double timestamp; - Eigen::MatrixXd covariance; - std::vector> order; - auto init_rT1 = boost::posix_time::microsec_clock::local_time(); - - // Try to initialize the system - // We will wait for a jerk if we do not have the zero velocity update enabled - // Otherwise we can initialize right away as the zero velocity will handle the stationary case - bool wait_for_jerk = (updaterZUPT == nullptr); - bool success = initializer->initialize(timestamp, covariance, order, state->_imu, wait_for_jerk); - - // If we have initialized successfully we will set the covariance and state elements as needed - // TODO: set the clones and SLAM features here so we can start updating right away... - if (success) { - - // Set our covariance (state should already be set in the initializer) - StateHelper::set_initial_covariance(state, covariance, order); - - // Set the state time - state->_timestamp = timestamp; - startup_time = timestamp; - - // Cleanup any features older than the initialization time - // Also increase the number of features to the desired amount during estimation - trackFEATS->get_feature_database()->cleanup_measurements(state->_timestamp); - trackFEATS->set_num_features(params.num_pts); - if (trackARUCO != nullptr) { - trackARUCO->get_feature_database()->cleanup_measurements(state->_timestamp); - } - - // If we are moving then don't do zero velocity update4 - if (state->_imu->vel().norm() > params.zupt_max_velocity) { - has_moved_since_zupt = true; - } - - // Else we are good to go, print out our stats - auto init_rT2 = boost::posix_time::microsec_clock::local_time(); - PRINT_INFO(GREEN "[init]: successful initialization in %.4f seconds\n" RESET, (init_rT2 - init_rT1).total_microseconds() * 1e-6); - PRINT_INFO(GREEN "[init]: orientation = %.4f, %.4f, %.4f, %.4f\n" RESET, state->_imu->quat()(0), state->_imu->quat()(1), - state->_imu->quat()(2), state->_imu->quat()(3)); - PRINT_INFO(GREEN "[init]: bias gyro = %.4f, %.4f, %.4f\n" RESET, state->_imu->bias_g()(0), state->_imu->bias_g()(1), - state->_imu->bias_g()(2)); - PRINT_INFO(GREEN "[init]: velocity = %.4f, %.4f, %.4f\n" RESET, state->_imu->vel()(0), state->_imu->vel()(1), state->_imu->vel()(2)); - PRINT_INFO(GREEN "[init]: bias accel = %.4f, %.4f, %.4f\n" RESET, state->_imu->bias_a()(0), state->_imu->bias_a()(1), - state->_imu->bias_a()(2)); - PRINT_INFO(GREEN "[init]: position = %.4f, %.4f, %.4f\n" RESET, state->_imu->pos()(0), state->_imu->pos()(1), state->_imu->pos()(2)); - - // Remove any camera times that are order then the initialized time - // This can happen if the initialization has taken a while to perform - std::lock_guard lck(camera_queue_init_mtx); - std::vector camera_timestamps_to_init; - for (size_t i = 0; i < camera_queue_init.size(); i++) { - if (camera_queue_init.at(i) > timestamp) { - camera_timestamps_to_init.push_back(camera_queue_init.at(i)); - } - } - - // Now we have initialized we will propagate the state to the current timestep - // In general this should be ok as long as the initialization didn't take too long to perform - // Propagating over multiple seconds will become an issue if the initial biases are bad - size_t clone_rate = (size_t)((double)camera_timestamps_to_init.size() / (double)params.state_options.max_clone_size) + 1; - for (size_t i = 0; i < camera_timestamps_to_init.size(); i += clone_rate) { - propagator->propagate_and_clone(state, camera_timestamps_to_init.at(i)); - StateHelper::marginalize_old_clone(state); - } - PRINT_DEBUG(YELLOW "[init]: moved the state forward %.2f seconds\n" RESET, state->_timestamp - timestamp); - thread_init_success = true; - camera_queue_init.clear(); - - } else { - auto init_rT2 = boost::posix_time::microsec_clock::local_time(); - PRINT_DEBUG(YELLOW "[init]: failed initialization in %.4f seconds\n" RESET, (init_rT2 - init_rT1).total_microseconds() * 1e-6); - thread_init_success = false; - std::lock_guard lck(camera_queue_init_mtx); - camera_queue_init.clear(); - } - - // Finally, mark that the thread has finished running - thread_init_running = false; - }); - - // If we are single threaded, then run single threaded - // Otherwise detach this thread so it runs in the background! - if (!params.use_multi_threading) { - thread.join(); - } else { - thread.detach(); - } - return false; -} - -void VioManager::retriangulate_active_tracks(const ov_core::CameraData &message) { - - // Start timing - boost::posix_time::ptime retri_rT1, retri_rT2, retri_rT3, retri_rT4, retri_rT5; - retri_rT1 = boost::posix_time::microsec_clock::local_time(); - - // Clear old active track data - active_tracks_time = state->_timestamp; - active_image = message.images.at(0).clone(); - active_tracks_posinG.clear(); - active_tracks_uvd.clear(); - - // Get all features which are tracked in the current frame - // NOTE: This database should have all features from all trackers already in it - // NOTE: it also has the complete history so we shouldn't see jumps from deleting measurements - std::vector> active_features = trackDATABASE->features_containing_older(state->_timestamp); - - // 0. Get all timestamps our clones are at (and thus valid measurement times) - std::vector clonetimes; - for (const auto &clone_imu : state->_clones_IMU) { - clonetimes.emplace_back(clone_imu.first); - } - - // 1. Clean all feature measurements and make sure they all have valid clone times - // Also remove any that we are unable to triangulate (due to not having enough measurements) - auto it0 = active_features.begin(); - while (it0 != active_features.end()) { - - // Skip if it is a SLAM feature since it already is already going to be added - if (state->_features_SLAM.find((*it0)->featid) != state->_features_SLAM.end()) { - it0 = active_features.erase(it0); - continue; - } - - // Clean the feature - (*it0)->clean_old_measurements(clonetimes); - - // Count how many measurements - int ct_meas = 0; - for (const auto &pair : (*it0)->timestamps) { - ct_meas += (*it0)->timestamps[pair.first].size(); - } - - // Remove if we don't have enough and am not a SLAM feature which doesn't need triangulation - if (ct_meas < (int)std::max(4.0, std::floor(state->_options.max_clone_size * 2.0 / 5.0))) { - it0 = active_features.erase(it0); - } else { - it0++; - } - } - retri_rT2 = boost::posix_time::microsec_clock::local_time(); - - // Return if no features - if (active_features.empty() && state->_features_SLAM.empty()) - return; - - // 2. Create vector of cloned *CAMERA* poses at each of our clone timesteps - std::unordered_map> clones_cam; - for (const auto &clone_calib : state->_calib_IMUtoCAM) { - - // For this camera, create the vector of camera poses - std::unordered_map clones_cami; - for (const auto &clone_imu : state->_clones_IMU) { - - // Get current camera pose - Eigen::Matrix3d R_GtoCi = clone_calib.second->Rot() * clone_imu.second->Rot(); - Eigen::Vector3d p_CioinG = clone_imu.second->pos() - R_GtoCi.transpose() * clone_calib.second->pos(); - - // Append to our map - clones_cami.insert({clone_imu.first, FeatureInitializer::ClonePose(R_GtoCi, p_CioinG)}); - } - - // Append to our map - clones_cam.insert({clone_calib.first, clones_cami}); - } - retri_rT3 = boost::posix_time::microsec_clock::local_time(); - - // 3. Try to triangulate all features that have measurements - auto it1 = active_features.begin(); - while (it1 != active_features.end()) { - - // Triangulate the feature and remove if it fails - bool success_tri = true; - if (active_tracks_initializer->config().triangulate_1d) { - success_tri = active_tracks_initializer->single_triangulation_1d(it1->get(), clones_cam); - } else { - success_tri = active_tracks_initializer->single_triangulation(it1->get(), clones_cam); - } - - // Remove the feature if not a success - if (!success_tri) { - it1 = active_features.erase(it1); - continue; - } - it1++; - } - retri_rT4 = boost::posix_time::microsec_clock::local_time(); - - // Return if no features - if (active_features.empty() && state->_features_SLAM.empty()) - return; - - // Points which we have in the global frame - for (const auto &feat : active_features) { - active_tracks_posinG[feat->featid] = feat->p_FinG; - } - for (const auto &feat : state->_features_SLAM) { - Eigen::Vector3d p_FinG = feat.second->get_xyz(false); - if (LandmarkRepresentation::is_relative_representation(feat.second->_feat_representation)) { - // Assert that we have an anchor pose for this feature - assert(feat.second->_anchor_cam_id != -1); - // Get calibration for our anchor camera - Eigen::Matrix3d R_ItoC = state->_calib_IMUtoCAM.at(feat.second->_anchor_cam_id)->Rot(); - Eigen::Vector3d p_IinC = state->_calib_IMUtoCAM.at(feat.second->_anchor_cam_id)->pos(); - // Anchor pose orientation and position - Eigen::Matrix3d R_GtoI = state->_clones_IMU.at(feat.second->_anchor_clone_timestamp)->Rot(); - Eigen::Vector3d p_IinG = state->_clones_IMU.at(feat.second->_anchor_clone_timestamp)->pos(); - // Feature in the global frame - p_FinG = R_GtoI.transpose() * R_ItoC.transpose() * (feat.second->get_xyz(false) - p_IinC) + p_IinG; - } - active_tracks_posinG[feat.second->_featid] = p_FinG; - } - - // Calibration of the first camera (cam0) - std::shared_ptr distortion = state->_cam_intrinsics.at(0); - std::shared_ptr calibration = state->_calib_IMUtoCAM.at(0); - Eigen::Matrix R_ItoC = calibration->Rot(); - Eigen::Matrix p_IinC = calibration->pos(); - - // Get current IMU clone state - std::shared_ptr clone_Ii = state->_clones_IMU.at(active_tracks_time); - Eigen::Matrix3d R_GtoIi = clone_Ii->Rot(); - Eigen::Vector3d p_IiinG = clone_Ii->pos(); - - // 4. Next we can update our variable with the global position - // We also will project the features into the current frame - for (const auto &feat : active_tracks_posinG) { - - // Project the current feature into the current frame of reference - Eigen::Vector3d p_FinIi = R_GtoIi * (feat.second - p_IiinG); - Eigen::Vector3d p_FinCi = R_ItoC * p_FinIi + p_IinC; - double depth = p_FinCi(2); - Eigen::Vector2d uv_norm, uv_dist; - uv_norm << p_FinCi(0) / depth, p_FinCi(1) / depth; - uv_dist = state->_cam_intrinsics_cameras.at(0)->distort_d(uv_norm); - - // Skip if not valid (i.e. negative depth, or outside of image) - if (depth < 0.1) { - continue; - } - - // Skip if not valid (i.e. negative depth, or outside of image) - int width = state->_cam_intrinsics_cameras.at(0)->w(); - int height = state->_cam_intrinsics_cameras.at(0)->h(); - if (uv_dist(0) < 0 || (int)uv_dist(0) >= width || uv_dist(1) < 0 || (int)uv_dist(1) >= height) { - // PRINT_DEBUG("feat %zu -> depth = %.2f | u_d = %.2f | v_d = %.2f\n",(*it2)->featid,depth,uv_dist(0),uv_dist(1)); - continue; - } - - // Finally construct the uv and depth - Eigen::Vector3d uvd; - uvd << uv_dist, depth; - active_tracks_uvd.insert({feat.first, uvd}); - } - retri_rT5 = boost::posix_time::microsec_clock::local_time(); - - // Timing information - // PRINT_DEBUG(CYAN "[RETRI-TIME]: %.4f seconds for cleaning\n" RESET, (retri_rT2-retri_rT1).total_microseconds() * 1e-6); - // PRINT_DEBUG(CYAN "[RETRI-TIME]: %.4f seconds for triangulate setup\n" RESET, (retri_rT3-retri_rT2).total_microseconds() * 1e-6); - // PRINT_DEBUG(CYAN "[RETRI-TIME]: %.4f seconds for triangulation\n" RESET, (retri_rT4-retri_rT3).total_microseconds() * 1e-6); - // PRINT_DEBUG(CYAN "[RETRI-TIME]: %.4f seconds for re-projection\n" RESET, (retri_rT5-retri_rT4).total_microseconds() * 1e-6); - // PRINT_DEBUG(CYAN "[RETRI-TIME]: %.4f seconds total\n" RESET, (retri_rT5-retri_rT1).total_microseconds() * 1e-6); -} diff --git a/ov_msckf/src/core/VioManager.h b/ov_msckf/src/core/VioManager.h index 96c393f2c..d8aa30218 100644 --- a/ov_msckf/src/core/VioManager.h +++ b/ov_msckf/src/core/VioManager.h @@ -24,37 +24,34 @@ #include #include +#include #include #include #include +#include #include -#include "cam/CamBase.h" -#include "cam/CamEqui.h" -#include "cam/CamRadtan.h" -#include "track/TrackAruco.h" -#include "track/TrackDescriptor.h" -#include "track/TrackKLT.h" -#include "track/TrackSIM.h" -#include "types/Landmark.h" -#include "types/LandmarkRepresentation.h" -#include "utils/opencv_lambda_body.h" -#include "utils/print.h" -#include "utils/sensor_data.h" - -#include "init/InertialInitializer.h" - -#include "state/Propagator.h" -#include "state/State.h" -#include "state/StateHelper.h" -#include "update/UpdaterMSCKF.h" -#include "update/UpdaterSLAM.h" -#include "update/UpdaterZeroVelocity.h" - #include "VioManagerOptions.h" +namespace ov_core { +struct ImuData; +struct CameraData; +class TrackBase; +class FeatureInitializer; +} // namespace ov_core +namespace ov_init { +class InertialInitializer; +} // namespace ov_init + namespace ov_msckf { +class State; +class StateHelper; +class UpdaterMSCKF; +class UpdaterSLAM; +class UpdaterZeroVelocity; +class Propagator; + /** * @brief Core class that manages the entire system * @@ -75,26 +72,7 @@ class VioManager { * @brief Feed function for inertial data * @param message Contains our timestamp and inertial information */ - void feed_measurement_imu(const ov_core::ImuData &message) { - - // Get the oldest camera timestamp that we can remove IMU measurements before - // Then push back to our propagator and pass the IMU time we can delete up to - double oldest_time = trackFEATS->get_feature_database()->get_oldest_timestamp(); - if (oldest_time != -1) { - oldest_time += params.calib_camimu_dt; - } - propagator->feed_imu(message, oldest_time); - - // Push back to our initializer - if (!is_initialized_vio) { - initializer->feed_imu(message, oldest_time); - } - - // Push back to the zero velocity updater if we have it - if (is_initialized_vio && updaterZUPT != nullptr) { - updaterZUPT->feed_imu(message, oldest_time); - } - } + void feed_measurement_imu(const ov_core::ImuData &message); /** * @brief Feed function for camera measurements @@ -115,45 +93,7 @@ class VioManager { * @brief Given a state, this will initialize our IMU state. * @param imustate State in the MSCKF ordering: [time(sec),q_GtoI,p_IinG,v_IinG,b_gyro,b_accel] */ - void initialize_with_gt(Eigen::Matrix imustate) { - - // Initialize the system - state->_imu->set_value(imustate.block(1, 0, 16, 1)); - state->_imu->set_fej(imustate.block(1, 0, 16, 1)); - - // Fix the global yaw and position gauge freedoms - // TODO: Why does this break out simulation consistency metrics? - std::vector> order = {state->_imu}; - Eigen::MatrixXd Cov = 1e-4 * Eigen::MatrixXd::Identity(state->_imu->size(), state->_imu->size()); - // Cov.block(state->_imu->v()->id(), state->_imu->v()->id(), 3, 3) *= 10; - // Cov(state->_imu->q()->id() + 2, state->_imu->q()->id() + 2) = 0.0; - // Cov.block(state->_imu->p()->id(), state->_imu->p()->id(), 3, 3).setZero(); - // Cov.block(state->_imu->q()->id(), state->_imu->q()->id(), 3, 3) = - // state->_imu->Rot() * Cov.block(state->_imu->q()->id(), state->_imu->q()->id(), 3, 3) * state->_imu->Rot().transpose(); - StateHelper::set_initial_covariance(state, Cov, order); - - // Set the state time - state->_timestamp = imustate(0, 0); - startup_time = imustate(0, 0); - is_initialized_vio = true; - - // Cleanup any features older then the initialization time - trackFEATS->get_feature_database()->cleanup_measurements(state->_timestamp); - if (trackARUCO != nullptr) { - trackARUCO->get_feature_database()->cleanup_measurements(state->_timestamp); - } - - // Print what we init'ed with - PRINT_DEBUG(GREEN "[INIT]: INITIALIZED FROM GROUNDTRUTH FILE!!!!!\n" RESET); - PRINT_DEBUG(GREEN "[INIT]: orientation = %.4f, %.4f, %.4f, %.4f\n" RESET, state->_imu->quat()(0), state->_imu->quat()(1), - state->_imu->quat()(2), state->_imu->quat()(3)); - PRINT_DEBUG(GREEN "[INIT]: bias gyro = %.4f, %.4f, %.4f\n" RESET, state->_imu->bias_g()(0), state->_imu->bias_g()(1), - state->_imu->bias_g()(2)); - PRINT_DEBUG(GREEN "[INIT]: velocity = %.4f, %.4f, %.4f\n" RESET, state->_imu->vel()(0), state->_imu->vel()(1), state->_imu->vel()(2)); - PRINT_DEBUG(GREEN "[INIT]: bias accel = %.4f, %.4f, %.4f\n" RESET, state->_imu->bias_a()(0), state->_imu->bias_a()(1), - state->_imu->bias_a()(2)); - PRINT_DEBUG(GREEN "[INIT]: position = %.4f, %.4f, %.4f\n" RESET, state->_imu->pos()(0), state->_imu->pos()(1), state->_imu->pos()(2)); - } + void initialize_with_gt(Eigen::Matrix imustate); /// If we are initialized or not bool initialized() { return is_initialized_vio; } @@ -171,80 +111,16 @@ class VioManager { std::shared_ptr get_propagator() { return propagator; } /// Get a nice visualization image of what tracks we have - cv::Mat get_historical_viz_image() { - - // Build an id-list of what features we should highlight (i.e. SLAM) - std::vector highlighted_ids; - for (const auto &feat : state->_features_SLAM) { - highlighted_ids.push_back(feat.first); - } - - // Text we will overlay if needed - std::string overlay = (did_zupt_update) ? "zvupt" : ""; - overlay = (!is_initialized_vio) ? "init" : overlay; - - // Get the current active tracks - cv::Mat img_history; - trackFEATS->display_history(img_history, 255, 255, 0, 255, 255, 255, highlighted_ids, overlay); - if (trackARUCO != nullptr) { - trackARUCO->display_history(img_history, 0, 255, 255, 255, 255, 255, highlighted_ids, overlay); - // trackARUCO->display_active(img_history, 0, 255, 255, 255, 255, 255, overlay); - } - - // Finally return the image - return img_history; - } - - /// Returns 3d features used in the last update in global frame - std::vector get_good_features_MSCKF() { return good_features_MSCKF; } + cv::Mat get_historical_viz_image(); /// Returns 3d SLAM features in the global frame - std::vector get_features_SLAM() { - std::vector slam_feats; - for (auto &f : state->_features_SLAM) { - if ((int)f.first <= 4 * state->_options.max_aruco_features) - continue; - if (ov_type::LandmarkRepresentation::is_relative_representation(f.second->_feat_representation)) { - // Assert that we have an anchor pose for this feature - assert(f.second->_anchor_cam_id != -1); - // Get calibration for our anchor camera - Eigen::Matrix R_ItoC = state->_calib_IMUtoCAM.at(f.second->_anchor_cam_id)->Rot(); - Eigen::Matrix p_IinC = state->_calib_IMUtoCAM.at(f.second->_anchor_cam_id)->pos(); - // Anchor pose orientation and position - Eigen::Matrix R_GtoI = state->_clones_IMU.at(f.second->_anchor_clone_timestamp)->Rot(); - Eigen::Matrix p_IinG = state->_clones_IMU.at(f.second->_anchor_clone_timestamp)->pos(); - // Feature in the global frame - slam_feats.push_back(R_GtoI.transpose() * R_ItoC.transpose() * (f.second->get_xyz(false) - p_IinC) + p_IinG); - } else { - slam_feats.push_back(f.second->get_xyz(false)); - } - } - return slam_feats; - } + std::vector get_features_SLAM(); /// Returns 3d ARUCO features in the global frame - std::vector get_features_ARUCO() { - std::vector aruco_feats; - for (auto &f : state->_features_SLAM) { - if ((int)f.first > 4 * state->_options.max_aruco_features) - continue; - if (ov_type::LandmarkRepresentation::is_relative_representation(f.second->_feat_representation)) { - // Assert that we have an anchor pose for this feature - assert(f.second->_anchor_cam_id != -1); - // Get calibration for our anchor camera - Eigen::Matrix R_ItoC = state->_calib_IMUtoCAM.at(f.second->_anchor_cam_id)->Rot(); - Eigen::Matrix p_IinC = state->_calib_IMUtoCAM.at(f.second->_anchor_cam_id)->pos(); - // Anchor pose orientation and position - Eigen::Matrix R_GtoI = state->_clones_IMU.at(f.second->_anchor_clone_timestamp)->Rot(); - Eigen::Matrix p_IinG = state->_clones_IMU.at(f.second->_anchor_clone_timestamp)->pos(); - // Feature in the global frame - aruco_feats.push_back(R_GtoI.transpose() * R_ItoC.transpose() * (f.second->get_xyz(false) - p_IinC) + p_IinG); - } else { - aruco_feats.push_back(f.second->get_xyz(false)); - } - } - return aruco_feats; - } + std::vector get_features_ARUCO(); + + /// Returns 3d features used in the last update in global frame + std::vector get_good_features_MSCKF() { return good_features_MSCKF; } /// Return the image used when projecting the active tracks void get_active_image(double ×tamp, cv::Mat &image) { diff --git a/ov_msckf/src/core/VioManagerHelper.cpp b/ov_msckf/src/core/VioManagerHelper.cpp new file mode 100644 index 000000000..67e9e0275 --- /dev/null +++ b/ov_msckf/src/core/VioManagerHelper.cpp @@ -0,0 +1,436 @@ +/* + * OpenVINS: An Open Platform for Visual-Inertial Research + * Copyright (C) 2018-2022 Patrick Geneva + * Copyright (C) 2018-2022 Guoquan Huang + * Copyright (C) 2018-2022 OpenVINS Contributors + * Copyright (C) 2018-2019 Kevin Eckenhoff + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include "VioManager.h" + +#include "feat/Feature.h" +#include "feat/FeatureDatabase.h" +#include "feat/FeatureInitializer.h" +#include "types/LandmarkRepresentation.h" +#include "utils/print.h" + +#include "init/InertialInitializer.h" + +#include "state/Propagator.h" +#include "state/State.h" +#include "state/StateHelper.h" + +using namespace ov_core; +using namespace ov_type; +using namespace ov_msckf; + +void VioManager::initialize_with_gt(Eigen::Matrix imustate) { + + // Initialize the system + state->_imu->set_value(imustate.block(1, 0, 16, 1)); + state->_imu->set_fej(imustate.block(1, 0, 16, 1)); + + // Fix the global yaw and position gauge freedoms + // TODO: Why does this break out simulation consistency metrics? + std::vector> order = {state->_imu}; + Eigen::MatrixXd Cov = std::pow(0.02, 2) * Eigen::MatrixXd::Identity(state->_imu->size(), state->_imu->size()); + Cov.block(3, 3, 3, 3) = std::pow(0.017, 2) * Eigen::Matrix3d::Identity(); // q + Cov.block(3, 3, 3, 3) = std::pow(0.05, 2) * Eigen::Matrix3d::Identity(); // p + Cov.block(6, 6, 3, 3) = std::pow(0.01, 2) * Eigen::Matrix3d::Identity(); // v (static) + StateHelper::set_initial_covariance(state, Cov, order); + + // Set the state time + state->_timestamp = imustate(0, 0); + startup_time = imustate(0, 0); + is_initialized_vio = true; + + // Cleanup any features older then the initialization time + trackFEATS->get_feature_database()->cleanup_measurements(state->_timestamp); + if (trackARUCO != nullptr) { + trackARUCO->get_feature_database()->cleanup_measurements(state->_timestamp); + } + + // Print what we init'ed with + PRINT_DEBUG(GREEN "[INIT]: INITIALIZED FROM GROUNDTRUTH FILE!!!!!\n" RESET); + PRINT_DEBUG(GREEN "[INIT]: orientation = %.4f, %.4f, %.4f, %.4f\n" RESET, state->_imu->quat()(0), state->_imu->quat()(1), + state->_imu->quat()(2), state->_imu->quat()(3)); + PRINT_DEBUG(GREEN "[INIT]: bias gyro = %.4f, %.4f, %.4f\n" RESET, state->_imu->bias_g()(0), state->_imu->bias_g()(1), + state->_imu->bias_g()(2)); + PRINT_DEBUG(GREEN "[INIT]: velocity = %.4f, %.4f, %.4f\n" RESET, state->_imu->vel()(0), state->_imu->vel()(1), state->_imu->vel()(2)); + PRINT_DEBUG(GREEN "[INIT]: bias accel = %.4f, %.4f, %.4f\n" RESET, state->_imu->bias_a()(0), state->_imu->bias_a()(1), + state->_imu->bias_a()(2)); + PRINT_DEBUG(GREEN "[INIT]: position = %.4f, %.4f, %.4f\n" RESET, state->_imu->pos()(0), state->_imu->pos()(1), state->_imu->pos()(2)); +} + +bool VioManager::try_to_initialize(const ov_core::CameraData &message) { + + // Directly return if the initialization thread is running + // Note that we lock on the queue since we could have finished an update + // And are using this queue to propagate the state forward. We should wait in this case + if (thread_init_running) { + std::lock_guard lck(camera_queue_init_mtx); + camera_queue_init.push_back(message.timestamp); + return false; + } + + // If the thread was a success, then return success! + if (thread_init_success) { + return true; + } + + // Run the initialization in a second thread so it can go as slow as it desires + thread_init_running = true; + std::thread thread([&] { + // Returns from our initializer + double timestamp; + Eigen::MatrixXd covariance; + std::vector> order; + auto init_rT1 = boost::posix_time::microsec_clock::local_time(); + + // Try to initialize the system + // We will wait for a jerk if we do not have the zero velocity update enabled + // Otherwise we can initialize right away as the zero velocity will handle the stationary case + bool wait_for_jerk = (updaterZUPT == nullptr); + bool success = initializer->initialize(timestamp, covariance, order, state->_imu, wait_for_jerk); + + // If we have initialized successfully we will set the covariance and state elements as needed + // TODO: set the clones and SLAM features here so we can start updating right away... + if (success) { + + // Set our covariance (state should already be set in the initializer) + StateHelper::set_initial_covariance(state, covariance, order); + + // Set the state time + state->_timestamp = timestamp; + startup_time = timestamp; + + // Cleanup any features older than the initialization time + // Also increase the number of features to the desired amount during estimation + // NOTE: we will split the total number of features over all cameras uniformly + trackFEATS->get_feature_database()->cleanup_measurements(state->_timestamp); + trackFEATS->set_num_features(std::floor((double)params.num_pts / (double)params.state_options.num_cameras)); + if (trackARUCO != nullptr) { + trackARUCO->get_feature_database()->cleanup_measurements(state->_timestamp); + } + + // If we are moving then don't do zero velocity update4 + if (state->_imu->vel().norm() > params.zupt_max_velocity) { + has_moved_since_zupt = true; + } + + // Else we are good to go, print out our stats + auto init_rT2 = boost::posix_time::microsec_clock::local_time(); + PRINT_INFO(GREEN "[init]: successful initialization in %.4f seconds\n" RESET, (init_rT2 - init_rT1).total_microseconds() * 1e-6); + PRINT_INFO(GREEN "[init]: orientation = %.4f, %.4f, %.4f, %.4f\n" RESET, state->_imu->quat()(0), state->_imu->quat()(1), + state->_imu->quat()(2), state->_imu->quat()(3)); + PRINT_INFO(GREEN "[init]: bias gyro = %.4f, %.4f, %.4f\n" RESET, state->_imu->bias_g()(0), state->_imu->bias_g()(1), + state->_imu->bias_g()(2)); + PRINT_INFO(GREEN "[init]: velocity = %.4f, %.4f, %.4f\n" RESET, state->_imu->vel()(0), state->_imu->vel()(1), state->_imu->vel()(2)); + PRINT_INFO(GREEN "[init]: bias accel = %.4f, %.4f, %.4f\n" RESET, state->_imu->bias_a()(0), state->_imu->bias_a()(1), + state->_imu->bias_a()(2)); + PRINT_INFO(GREEN "[init]: position = %.4f, %.4f, %.4f\n" RESET, state->_imu->pos()(0), state->_imu->pos()(1), state->_imu->pos()(2)); + + // Remove any camera times that are order then the initialized time + // This can happen if the initialization has taken a while to perform + std::lock_guard lck(camera_queue_init_mtx); + std::vector camera_timestamps_to_init; + for (size_t i = 0; i < camera_queue_init.size(); i++) { + if (camera_queue_init.at(i) > timestamp) { + camera_timestamps_to_init.push_back(camera_queue_init.at(i)); + } + } + + // Now we have initialized we will propagate the state to the current timestep + // In general this should be ok as long as the initialization didn't take too long to perform + // Propagating over multiple seconds will become an issue if the initial biases are bad + size_t clone_rate = (size_t)((double)camera_timestamps_to_init.size() / (double)params.state_options.max_clone_size) + 1; + for (size_t i = 0; i < camera_timestamps_to_init.size(); i += clone_rate) { + propagator->propagate_and_clone(state, camera_timestamps_to_init.at(i)); + StateHelper::marginalize_old_clone(state); + } + PRINT_DEBUG(YELLOW "[init]: moved the state forward %.2f seconds\n" RESET, state->_timestamp - timestamp); + thread_init_success = true; + camera_queue_init.clear(); + + } else { + auto init_rT2 = boost::posix_time::microsec_clock::local_time(); + PRINT_DEBUG(YELLOW "[init]: failed initialization in %.4f seconds\n" RESET, (init_rT2 - init_rT1).total_microseconds() * 1e-6); + thread_init_success = false; + std::lock_guard lck(camera_queue_init_mtx); + camera_queue_init.clear(); + } + + // Finally, mark that the thread has finished running + thread_init_running = false; + }); + + // If we are single threaded, then run single threaded + // Otherwise detach this thread so it runs in the background! + if (!params.use_multi_threading) { + thread.join(); + } else { + thread.detach(); + } + return false; +} + +void VioManager::retriangulate_active_tracks(const ov_core::CameraData &message) { + + // Start timing + boost::posix_time::ptime retri_rT1, retri_rT2, retri_rT3, retri_rT4, retri_rT5; + retri_rT1 = boost::posix_time::microsec_clock::local_time(); + + // Clear old active track data + active_tracks_time = state->_timestamp; + active_image = message.images.at(0).clone(); + active_tracks_posinG.clear(); + active_tracks_uvd.clear(); + + // Get all features which are tracked in the current frame + // NOTE: This database should have all features from all trackers already in it + // NOTE: it also has the complete history so we shouldn't see jumps from deleting measurements + std::vector> active_features = trackDATABASE->features_containing_older(state->_timestamp); + + // 0. Get all timestamps our clones are at (and thus valid measurement times) + std::vector clonetimes; + for (const auto &clone_imu : state->_clones_IMU) { + clonetimes.emplace_back(clone_imu.first); + } + + // 1. Clean all feature measurements and make sure they all have valid clone times + // Also remove any that we are unable to triangulate (due to not having enough measurements) + auto it0 = active_features.begin(); + while (it0 != active_features.end()) { + + // Skip if it is a SLAM feature since it already is already going to be added + if (state->_features_SLAM.find((*it0)->featid) != state->_features_SLAM.end()) { + it0 = active_features.erase(it0); + continue; + } + + // Clean the feature + (*it0)->clean_old_measurements(clonetimes); + + // Count how many measurements + int ct_meas = 0; + for (const auto &pair : (*it0)->timestamps) { + ct_meas += (*it0)->timestamps[pair.first].size(); + } + + // Remove if we don't have enough and am not a SLAM feature which doesn't need triangulation + if (ct_meas < (int)std::max(4.0, std::floor(state->_options.max_clone_size * 2.0 / 5.0))) { + it0 = active_features.erase(it0); + } else { + it0++; + } + } + retri_rT2 = boost::posix_time::microsec_clock::local_time(); + + // Return if no features + if (active_features.empty() && state->_features_SLAM.empty()) + return; + + // 2. Create vector of cloned *CAMERA* poses at each of our clone timesteps + std::unordered_map> clones_cam; + for (const auto &clone_calib : state->_calib_IMUtoCAM) { + + // For this camera, create the vector of camera poses + std::unordered_map clones_cami; + for (const auto &clone_imu : state->_clones_IMU) { + + // Get current camera pose + Eigen::Matrix3d R_GtoCi = clone_calib.second->Rot() * clone_imu.second->Rot(); + Eigen::Vector3d p_CioinG = clone_imu.second->pos() - R_GtoCi.transpose() * clone_calib.second->pos(); + + // Append to our map + clones_cami.insert({clone_imu.first, FeatureInitializer::ClonePose(R_GtoCi, p_CioinG)}); + } + + // Append to our map + clones_cam.insert({clone_calib.first, clones_cami}); + } + retri_rT3 = boost::posix_time::microsec_clock::local_time(); + + // 3. Try to triangulate all features that have measurements + auto it1 = active_features.begin(); + while (it1 != active_features.end()) { + + // Triangulate the feature and remove if it fails + bool success_tri = true; + if (active_tracks_initializer->config().triangulate_1d) { + success_tri = active_tracks_initializer->single_triangulation_1d(*it1, clones_cam); + } else { + success_tri = active_tracks_initializer->single_triangulation(*it1, clones_cam); + } + + // Remove the feature if not a success + if (!success_tri) { + it1 = active_features.erase(it1); + continue; + } + it1++; + } + retri_rT4 = boost::posix_time::microsec_clock::local_time(); + + // Return if no features + if (active_features.empty() && state->_features_SLAM.empty()) + return; + + // Points which we have in the global frame + for (const auto &feat : active_features) { + active_tracks_posinG[feat->featid] = feat->p_FinG; + } + for (const auto &feat : state->_features_SLAM) { + Eigen::Vector3d p_FinG = feat.second->get_xyz(false); + if (LandmarkRepresentation::is_relative_representation(feat.second->_feat_representation)) { + // Assert that we have an anchor pose for this feature + assert(feat.second->_anchor_cam_id != -1); + // Get calibration for our anchor camera + Eigen::Matrix3d R_ItoC = state->_calib_IMUtoCAM.at(feat.second->_anchor_cam_id)->Rot(); + Eigen::Vector3d p_IinC = state->_calib_IMUtoCAM.at(feat.second->_anchor_cam_id)->pos(); + // Anchor pose orientation and position + Eigen::Matrix3d R_GtoI = state->_clones_IMU.at(feat.second->_anchor_clone_timestamp)->Rot(); + Eigen::Vector3d p_IinG = state->_clones_IMU.at(feat.second->_anchor_clone_timestamp)->pos(); + // Feature in the global frame + p_FinG = R_GtoI.transpose() * R_ItoC.transpose() * (feat.second->get_xyz(false) - p_IinC) + p_IinG; + } + active_tracks_posinG[feat.second->_featid] = p_FinG; + } + + // Calibration of the first camera (cam0) + std::shared_ptr distortion = state->_cam_intrinsics.at(0); + std::shared_ptr calibration = state->_calib_IMUtoCAM.at(0); + Eigen::Matrix R_ItoC = calibration->Rot(); + Eigen::Matrix p_IinC = calibration->pos(); + + // Get current IMU clone state + std::shared_ptr clone_Ii = state->_clones_IMU.at(active_tracks_time); + Eigen::Matrix3d R_GtoIi = clone_Ii->Rot(); + Eigen::Vector3d p_IiinG = clone_Ii->pos(); + + // 4. Next we can update our variable with the global position + // We also will project the features into the current frame + for (const auto &feat : active_tracks_posinG) { + + // Project the current feature into the current frame of reference + Eigen::Vector3d p_FinIi = R_GtoIi * (feat.second - p_IiinG); + Eigen::Vector3d p_FinCi = R_ItoC * p_FinIi + p_IinC; + double depth = p_FinCi(2); + Eigen::Vector2d uv_norm, uv_dist; + uv_norm << p_FinCi(0) / depth, p_FinCi(1) / depth; + uv_dist = state->_cam_intrinsics_cameras.at(0)->distort_d(uv_norm); + + // Skip if not valid (i.e. negative depth, or outside of image) + if (depth < 0.1) { + continue; + } + + // Skip if not valid (i.e. negative depth, or outside of image) + int width = state->_cam_intrinsics_cameras.at(0)->w(); + int height = state->_cam_intrinsics_cameras.at(0)->h(); + if (uv_dist(0) < 0 || (int)uv_dist(0) >= width || uv_dist(1) < 0 || (int)uv_dist(1) >= height) { + // PRINT_DEBUG("feat %zu -> depth = %.2f | u_d = %.2f | v_d = %.2f\n",(*it2)->featid,depth,uv_dist(0),uv_dist(1)); + continue; + } + + // Finally construct the uv and depth + Eigen::Vector3d uvd; + uvd << uv_dist, depth; + active_tracks_uvd.insert({feat.first, uvd}); + } + retri_rT5 = boost::posix_time::microsec_clock::local_time(); + + // Timing information + // PRINT_DEBUG(CYAN "[RETRI-TIME]: %.4f seconds for cleaning\n" RESET, (retri_rT2-retri_rT1).total_microseconds() * 1e-6); + // PRINT_DEBUG(CYAN "[RETRI-TIME]: %.4f seconds for triangulate setup\n" RESET, (retri_rT3-retri_rT2).total_microseconds() * 1e-6); + // PRINT_DEBUG(CYAN "[RETRI-TIME]: %.4f seconds for triangulation\n" RESET, (retri_rT4-retri_rT3).total_microseconds() * 1e-6); + // PRINT_DEBUG(CYAN "[RETRI-TIME]: %.4f seconds for re-projection\n" RESET, (retri_rT5-retri_rT4).total_microseconds() * 1e-6); + // PRINT_DEBUG(CYAN "[RETRI-TIME]: %.4f seconds total\n" RESET, (retri_rT5-retri_rT1).total_microseconds() * 1e-6); +} + +cv::Mat VioManager::get_historical_viz_image() { + + // Return if not ready yet + if (state == nullptr || trackFEATS == nullptr) + return cv::Mat(); + + // Build an id-list of what features we should highlight (i.e. SLAM) + std::vector highlighted_ids; + for (const auto &feat : state->_features_SLAM) { + highlighted_ids.push_back(feat.first); + } + + // Text we will overlay if needed + std::string overlay = (did_zupt_update) ? "zvupt" : ""; + overlay = (!is_initialized_vio) ? "init" : overlay; + + // Get the current active tracks + cv::Mat img_history; + trackFEATS->display_history(img_history, 255, 255, 0, 255, 255, 255, highlighted_ids, overlay); + if (trackARUCO != nullptr) { + trackARUCO->display_history(img_history, 0, 255, 255, 255, 255, 255, highlighted_ids, overlay); + // trackARUCO->display_active(img_history, 0, 255, 255, 255, 255, 255, overlay); + } + + // Finally return the image + return img_history; +} + +std::vector VioManager::get_features_SLAM() { + std::vector slam_feats; + for (auto &f : state->_features_SLAM) { + if ((int)f.first <= 4 * state->_options.max_aruco_features) + continue; + if (ov_type::LandmarkRepresentation::is_relative_representation(f.second->_feat_representation)) { + // Assert that we have an anchor pose for this feature + assert(f.second->_anchor_cam_id != -1); + // Get calibration for our anchor camera + Eigen::Matrix R_ItoC = state->_calib_IMUtoCAM.at(f.second->_anchor_cam_id)->Rot(); + Eigen::Matrix p_IinC = state->_calib_IMUtoCAM.at(f.second->_anchor_cam_id)->pos(); + // Anchor pose orientation and position + Eigen::Matrix R_GtoI = state->_clones_IMU.at(f.second->_anchor_clone_timestamp)->Rot(); + Eigen::Matrix p_IinG = state->_clones_IMU.at(f.second->_anchor_clone_timestamp)->pos(); + // Feature in the global frame + slam_feats.push_back(R_GtoI.transpose() * R_ItoC.transpose() * (f.second->get_xyz(false) - p_IinC) + p_IinG); + } else { + slam_feats.push_back(f.second->get_xyz(false)); + } + } + return slam_feats; +} + +std::vector VioManager::get_features_ARUCO() { + std::vector aruco_feats; + for (auto &f : state->_features_SLAM) { + if ((int)f.first > 4 * state->_options.max_aruco_features) + continue; + if (ov_type::LandmarkRepresentation::is_relative_representation(f.second->_feat_representation)) { + // Assert that we have an anchor pose for this feature + assert(f.second->_anchor_cam_id != -1); + // Get calibration for our anchor camera + Eigen::Matrix R_ItoC = state->_calib_IMUtoCAM.at(f.second->_anchor_cam_id)->Rot(); + Eigen::Matrix p_IinC = state->_calib_IMUtoCAM.at(f.second->_anchor_cam_id)->pos(); + // Anchor pose orientation and position + Eigen::Matrix R_GtoI = state->_clones_IMU.at(f.second->_anchor_clone_timestamp)->Rot(); + Eigen::Matrix p_IinG = state->_clones_IMU.at(f.second->_anchor_clone_timestamp)->pos(); + // Feature in the global frame + aruco_feats.push_back(R_GtoI.transpose() * R_ItoC.transpose() * (f.second->get_xyz(false) - p_IinC) + p_IinG); + } else { + aruco_feats.push_back(f.second->get_xyz(false)); + } + } + return aruco_feats; +} diff --git a/ov_msckf/src/core/VioManagerOptions.h b/ov_msckf/src/core/VioManagerOptions.h index 9c34674b0..2d6b04812 100644 --- a/ov_msckf/src/core/VioManagerOptions.h +++ b/ov_msckf/src/core/VioManagerOptions.h @@ -24,13 +24,14 @@ #include #include +#include #include #include #include -#include "state/Propagator.h" #include "state/StateOptions.h" #include "update/UpdaterOptions.h" +#include "utils/NoiseManager.h" #include "init/InertialInitializerOptions.h" @@ -130,7 +131,7 @@ struct VioManagerOptions { // NOISE / CHI2 ============================ /// IMU noise (gyroscope and accelerometer) - Propagator::NoiseManager imu_noises; + NoiseManager imu_noises; /// Update options for MSCKF features (pixel noise and chi2 multiplier) UpdaterOptions msckf_options; @@ -334,6 +335,9 @@ struct VioManagerOptions { /// If our front-end should try to use some multi-threading for stereo matching bool use_multi_threading = true; + /// If our ROS subscriber callbacks should be async (if sim and serial then this should be no!) + bool use_multi_threading_subs = false; + /// The number of points we should extract and track in *each* image frame. This highly effects the computation required for tracking. int num_pts = 150; @@ -375,6 +379,7 @@ struct VioManagerOptions { parser->parse_config("downsize_aruco", downsize_aruco); parser->parse_config("downsample_cameras", downsample_cameras); parser->parse_config("multi_threading", use_multi_threading); + parser->parse_config("multi_threading_subs", use_multi_threading_subs, false); parser->parse_config("num_pts", num_pts); parser->parse_config("fast_threshold", fast_threshold); parser->parse_config("grid_x", grid_x); @@ -405,6 +410,7 @@ struct VioManagerOptions { PRINT_DEBUG(" - downsize aruco: %d\n", downsize_aruco); PRINT_DEBUG(" - downsize cameras: %d\n", downsample_cameras); PRINT_DEBUG(" - use multi-threading: %d\n", use_multi_threading); + PRINT_DEBUG(" - use multi-threading subs: %d\n", use_multi_threading_subs); PRINT_DEBUG(" - num_pts: %d\n", num_pts); PRINT_DEBUG(" - fast threshold: %d\n", fast_threshold); PRINT_DEBUG(" - grid X by Y: %d by %d\n", grid_x, grid_y); diff --git a/ov_msckf/src/ros/ROS1Visualizer.cpp b/ov_msckf/src/ros/ROS1Visualizer.cpp index 5f312c385..4039b56af 100644 --- a/ov_msckf/src/ros/ROS1Visualizer.cpp +++ b/ov_msckf/src/ros/ROS1Visualizer.cpp @@ -21,6 +21,16 @@ #include "ROS1Visualizer.h" +#include "core/VioManager.h" +#include "ros/ROSVisualizerHelper.h" +#include "sim/Simulator.h" +#include "state/Propagator.h" +#include "state/State.h" +#include "state/StateHelper.h" +#include "utils/dataset_reader.h" +#include "utils/print.h" +#include "utils/sensor_data.h" + using namespace ov_core; using namespace ov_type; using namespace ov_msckf; @@ -75,6 +85,7 @@ ROS1Visualizer::ROS1Visualizer(std::shared_ptr nh, std::shared_ nh->param("publish_calibration_tf", publish_calibration_tf, true); // Load groundtruth if we have it and are not doing simulation + // NOTE: needs to be a csv ASL format file if (nh->hasParam("path_gt") && _sim == nullptr) { std::string path_to_gt; nh->param("path_gt", path_to_gt, ""); @@ -85,9 +96,8 @@ ROS1Visualizer::ROS1Visualizer(std::shared_ptr nh, std::shared_ } // Load if we should save the total state to file + // If so, then open the file and create folders as needed nh->param("save_total_state", save_total_state, false); - - // If the file is not open, then open the file if (save_total_state) { // files we will open @@ -105,7 +115,6 @@ ROS1Visualizer::ROS1Visualizer(std::shared_ptr nh, std::shared_ // Create folder path to this location if not exists boost::filesystem::create_directories(boost::filesystem::path(filepath_est.c_str()).parent_path()); boost::filesystem::create_directories(boost::filesystem::path(filepath_std.c_str()).parent_path()); - boost::filesystem::create_directories(boost::filesystem::path(filepath_gt.c_str()).parent_path()); // Open the files of_state_est.open(filepath_est.c_str()); @@ -117,10 +126,23 @@ ROS1Visualizer::ROS1Visualizer(std::shared_ptr nh, std::shared_ if (_sim != nullptr) { if (boost::filesystem::exists(filepath_gt)) boost::filesystem::remove(filepath_gt); + boost::filesystem::create_directories(boost::filesystem::path(filepath_gt.c_str()).parent_path()); of_state_gt.open(filepath_gt.c_str()); of_state_gt << "# timestamp(s) q p v bg ba cam_imu_dt num_cam cam0_k cam0_d cam0_rot cam0_trans .... etc" << std::endl; } } + + // Start thread for the image publishing + if (_app->get_params().use_multi_threading) { + std::thread thread([&] { + ros::Rate loop_rate(20); + while (ros::ok()) { + publish_images(); + loop_rate.sleep(); + } + }); + thread.detach(); + } } void ROS1Visualizer::setup_subscribers(std::shared_ptr parser) { @@ -176,11 +198,12 @@ void ROS1Visualizer::visualize() { last_visualization_timestamp = _app->get_state()->_timestamp; // Start timing - boost::posix_time::ptime rT0_1, rT0_2; - rT0_1 = boost::posix_time::microsec_clock::local_time(); + // boost::posix_time::ptime rT0_1, rT0_2; + // rT0_1 = boost::posix_time::microsec_clock::local_time(); - // publish current image - publish_images(); + // publish current image (only if not multi-threaded) + if (!_app->get_params().use_multi_threading) + publish_images(); // Return if we have not inited if (!_app->initialized()) @@ -206,13 +229,13 @@ void ROS1Visualizer::visualize() { // Save total state if (save_total_state) { - RosVisualizerHelper::sim_save_total_state_to_file(_app->get_state(), _sim, of_state_est, of_state_std, of_state_gt); + ROSVisualizerHelper::sim_save_total_state_to_file(_app->get_state(), _sim, of_state_est, of_state_std, of_state_gt); } // Print how much time it took to publish / displaying things - rT0_2 = boost::posix_time::microsec_clock::local_time(); - double time_total = (rT0_2 - rT0_1).total_microseconds() * 1e-6; - PRINT_DEBUG(BLUE "[TIME]: %.4f seconds for visualization\n" RESET, time_total); + // rT0_2 = boost::posix_time::microsec_clock::local_time(); + // double time_total = (rT0_2 - rT0_1).total_microseconds() * 1e-6; + // PRINT_DEBUG(BLUE "[TIME]: %.4f seconds for visualization\n" RESET, time_total); } void ROS1Visualizer::visualize_odometry(double timestamp) { @@ -277,7 +300,7 @@ void ROS1Visualizer::visualize_odometry(double timestamp) { // NOTE: a rotation from GtoI in JPL has the same xyzw as a ItoG Hamilton rotation auto odom_pose = std::make_shared(); odom_pose->set_value(state_plus.block(0, 0, 7, 1)); - tf::StampedTransform trans = RosVisualizerHelper::get_stamped_transform_from_pose(odom_pose, false); + tf::StampedTransform trans = ROSVisualizerHelper::get_stamped_transform_from_pose(odom_pose, false); trans.frame_id_ = "global"; trans.child_frame_id_ = "imu"; if (publish_global2imu_tf) { @@ -286,7 +309,7 @@ void ROS1Visualizer::visualize_odometry(double timestamp) { // Loop through each camera calibration and publish it for (const auto &calib : state->_calib_IMUtoCAM) { - tf::StampedTransform trans_calib = RosVisualizerHelper::get_stamped_transform_from_pose(calib.second, true); + tf::StampedTransform trans_calib = ROSVisualizerHelper::get_stamped_transform_from_pose(calib.second, true); trans_calib.frame_id_ = "imu"; trans_calib.child_frame_id_ = "cam" + std::to_string(calib.first); if (publish_calibration_tf) { @@ -385,12 +408,13 @@ void ROS1Visualizer::callback_inertial(const sensor_msgs::Imu::ConstPtr &msg) { double timestamp_imu_inC = message.timestamp - _app->get_state()->_calib_dt_CAMtoIMU->value()(0); while (!camera_queue.empty() && camera_queue.at(0).timestamp < timestamp_imu_inC) { auto rT0_1 = boost::posix_time::microsec_clock::local_time(); + double update_dt = 100.0 * (timestamp_imu_inC - camera_queue.at(0).timestamp); _app->feed_measurement_camera(camera_queue.at(0)); visualize(); camera_queue.pop_front(); auto rT0_2 = boost::posix_time::microsec_clock::local_time(); double time_total = (rT0_2 - rT0_1).total_microseconds() * 1e-6; - PRINT_INFO(BLUE "[TIME]: %.4f seconds total (%.1f hz)\n" RESET, time_total, 1.0 / time_total); + PRINT_INFO(BLUE "[TIME]: %.4f seconds total (%.1f hz, %.2f ms behind)\n" RESET, time_total, 1.0 / time_total, update_dt); } } thread_update_running = false; @@ -398,7 +422,7 @@ void ROS1Visualizer::callback_inertial(const sensor_msgs::Imu::ConstPtr &msg) { // If we are single threaded, then run single threaded // Otherwise detach this thread so it runs in the background! - if (!_app->get_params().use_multi_threading) { + if (!_app->get_params().use_multi_threading_subs) { thread.join(); } else { thread.detach(); @@ -560,12 +584,21 @@ void ROS1Visualizer::publish_state() { void ROS1Visualizer::publish_images() { + // Return if we have already visualized + if (_app->get_state() == nullptr) + return; + if (last_visualization_timestamp_image == _app->get_state()->_timestamp && _app->initialized()) + return; + last_visualization_timestamp_image = _app->get_state()->_timestamp; + // Check if we have subscribers if (it_pub_tracks.getNumSubscribers() == 0) return; // Get our image of history tracks cv::Mat img_history = _app->get_historical_viz_image(); + if (img_history.empty()) + return; // Create our message std_msgs::Header header; @@ -586,17 +619,17 @@ void ROS1Visualizer::publish_features() { // Get our good MSCKF features std::vector feats_msckf = _app->get_good_features_MSCKF(); - sensor_msgs::PointCloud2 cloud = RosVisualizerHelper::get_ros_pointcloud(feats_msckf); + sensor_msgs::PointCloud2 cloud = ROSVisualizerHelper::get_ros_pointcloud(feats_msckf); pub_points_msckf.publish(cloud); // Get our good SLAM features std::vector feats_slam = _app->get_features_SLAM(); - sensor_msgs::PointCloud2 cloud_SLAM = RosVisualizerHelper::get_ros_pointcloud(feats_slam); + sensor_msgs::PointCloud2 cloud_SLAM = ROSVisualizerHelper::get_ros_pointcloud(feats_slam); pub_points_slam.publish(cloud_SLAM); // Get our good ARUCO features std::vector feats_aruco = _app->get_features_ARUCO(); - sensor_msgs::PointCloud2 cloud_ARUCO = RosVisualizerHelper::get_ros_pointcloud(feats_aruco); + sensor_msgs::PointCloud2 cloud_ARUCO = ROSVisualizerHelper::get_ros_pointcloud(feats_aruco); pub_points_aruco.publish(cloud_ARUCO); // Skip the rest of we are not doing simulation @@ -605,7 +638,7 @@ void ROS1Visualizer::publish_features() { // Get our good SIMULATION features std::vector feats_sim = _sim->get_map_vec(); - sensor_msgs::PointCloud2 cloud_SIM = RosVisualizerHelper::get_ros_pointcloud(feats_sim); + sensor_msgs::PointCloud2 cloud_SIM = ROSVisualizerHelper::get_ros_pointcloud(feats_sim); pub_points_sim.publish(cloud_SIM); } diff --git a/ov_msckf/src/ros/ROS1Visualizer.h b/ov_msckf/src/ros/ROS1Visualizer.h index 0d31a46fa..68ea9e718 100644 --- a/ov_msckf/src/ros/ROS1Visualizer.h +++ b/ov_msckf/src/ros/ROS1Visualizer.h @@ -41,18 +41,26 @@ #include #include +#include +#include +#include +#include + +#include +#include #include #include -#include "core/VioManager.h" -#include "ros/RosVisualizerHelper.h" -#include "sim/Simulator.h" -#include "utils/dataset_reader.h" -#include "utils/print.h" -#include "utils/sensor_data.h" +namespace ov_core { +class YamlParser; +struct CameraData; +} // namespace ov_core namespace ov_msckf { +class VioManager; +class Simulator; + /** * @brief Helper class that will publish results onto the ROS framework. * @@ -176,6 +184,7 @@ class ROS1Visualizer { // Last timestamp we visualized at double last_visualization_timestamp = 0; + double last_visualization_timestamp_image = 0; // Our groundtruth states std::map> gt_states; @@ -187,7 +196,7 @@ class ROS1Visualizer { bool publish_calibration_tf = true; // Files and if we should save total state - bool save_total_state; + bool save_total_state = false; std::ofstream of_state_est, of_state_std, of_state_gt; }; diff --git a/ov_msckf/src/ros/ROS2Visualizer.cpp b/ov_msckf/src/ros/ROS2Visualizer.cpp index 661af90df..3220162c6 100644 --- a/ov_msckf/src/ros/ROS2Visualizer.cpp +++ b/ov_msckf/src/ros/ROS2Visualizer.cpp @@ -21,6 +21,16 @@ #include "ROS2Visualizer.h" +#include "core/VioManager.h" +#include "ros/ROSVisualizerHelper.h" +#include "sim/Simulator.h" +#include "state/Propagator.h" +#include "state/State.h" +#include "state/StateHelper.h" +#include "utils/dataset_reader.h" +#include "utils/print.h" +#include "utils/sensor_data.h" + using namespace ov_core; using namespace ov_type; using namespace ov_msckf; @@ -71,12 +81,15 @@ ROS2Visualizer::ROS2Visualizer(std::shared_ptr node, std::shared_p it_pub_loop_img_depth_color = it.advertise("/ov_msckf/loop_depth_colored", 2); // option to enable publishing of global to IMU transformation - node->declare_parameter("publish_global_to_imu_tf", true); - node->get_parameter("publish_global_to_imu_tf", publish_global2imu_tf); - node->declare_parameter("publish_calibration_tf", true); - node->get_parameter("publish_calibration_tf", publish_calibration_tf); + if (node->has_parameter("publish_global_to_imu_tf")) { + node->get_parameter("publish_global_to_imu_tf", publish_global2imu_tf); + } + if (node->has_parameter("publish_calibration_tf")) { + node->get_parameter("publish_calibration_tf", publish_calibration_tf); + } // Load groundtruth if we have it and are not doing simulation + // NOTE: needs to be a csv ASL format file std::string path_to_gt; bool has_gt = node->get_parameter("path_gt", path_to_gt); if (has_gt && _sim == nullptr && !path_to_gt.empty()) { @@ -85,20 +98,25 @@ ROS2Visualizer::ROS2Visualizer(std::shared_ptr node, std::shared_p } // Load if we should save the total state to file - node->declare_parameter("save_total_state", false); - node->get_parameter("save_total_state", save_total_state); - - // If the file is not open, then open the file + // If so, then open the file and create folders as needed + if (node->has_parameter("save_total_state")) { + node->get_parameter("save_total_state", save_total_state); + } if (save_total_state) { // files we will open - std::string filepath_est, filepath_std, filepath_gt; - node->declare_parameter("filepath_est", "state_estimate.txt"); - node->declare_parameter("filepath_std", "state_deviation.txt"); - node->declare_parameter("filepath_gt", "state_groundtruth.txt"); - node->get_parameter("filepath_est", filepath_est); - node->get_parameter("filepath_std", filepath_std); - node->get_parameter("filepath_gt", filepath_gt); + std::string filepath_est = "state_estimate.txt"; + std::string filepath_std = "state_deviation.txt"; + std::string filepath_gt = "state_groundtruth.txt"; + if (node->has_parameter("filepath_est")) { + node->get_parameter("filepath_est", filepath_est); + } + if (node->has_parameter("filepath_std")) { + node->get_parameter("filepath_std", filepath_std); + } + if (node->has_parameter("filepath_gt")) { + node->get_parameter("filepath_gt", filepath_gt); + } // If it exists, then delete it if (boost::filesystem::exists(filepath_est)) @@ -106,6 +124,10 @@ ROS2Visualizer::ROS2Visualizer(std::shared_ptr node, std::shared_p if (boost::filesystem::exists(filepath_std)) boost::filesystem::remove(filepath_std); + // Create folder path to this location if not exists + boost::filesystem::create_directories(boost::filesystem::path(filepath_est.c_str()).parent_path()); + boost::filesystem::create_directories(boost::filesystem::path(filepath_std.c_str()).parent_path()); + // Open the files of_state_est.open(filepath_est.c_str()); of_state_std.open(filepath_std.c_str()); @@ -116,10 +138,23 @@ ROS2Visualizer::ROS2Visualizer(std::shared_ptr node, std::shared_p if (_sim != nullptr) { if (boost::filesystem::exists(filepath_gt)) boost::filesystem::remove(filepath_gt); + boost::filesystem::create_directories(boost::filesystem::path(filepath_gt.c_str()).parent_path()); of_state_gt.open(filepath_gt.c_str()); of_state_gt << "# timestamp(s) q p v bg ba cam_imu_dt num_cam cam0_k cam0_d cam0_rot cam0_trans .... etc" << std::endl; } } + + // Start thread for the image publishing + if (_app->get_params().use_multi_threading) { + std::thread thread([&] { + rclcpp::Rate loop_rate(20); + while (rclcpp::ok()) { + publish_images(); + loop_rate.sleep(); + } + }); + thread.detach(); + } } void ROS2Visualizer::setup_subscribers(std::shared_ptr parser) { @@ -187,11 +222,12 @@ void ROS2Visualizer::visualize() { last_visualization_timestamp = _app->get_state()->_timestamp; // Start timing - boost::posix_time::ptime rT0_1, rT0_2; - rT0_1 = boost::posix_time::microsec_clock::local_time(); + // boost::posix_time::ptime rT0_1, rT0_2; + // rT0_1 = boost::posix_time::microsec_clock::local_time(); - // publish current image - publish_images(); + // publish current image (only if not multi-threaded) + if (!_app->get_params().use_multi_threading) + publish_images(); // Return if we have not inited if (!_app->initialized()) @@ -217,13 +253,13 @@ void ROS2Visualizer::visualize() { // Save total state if (save_total_state) { - RosVisualizerHelper::sim_save_total_state_to_file(_app->get_state(), _sim, of_state_est, of_state_std, of_state_gt); + ROSVisualizerHelper::sim_save_total_state_to_file(_app->get_state(), _sim, of_state_est, of_state_std, of_state_gt); } // Print how much time it took to publish / displaying things - rT0_2 = boost::posix_time::microsec_clock::local_time(); - double time_total = (rT0_2 - rT0_1).total_microseconds() * 1e-6; - PRINT_DEBUG(BLUE "[TIME]: %.4f seconds for visualization\n" RESET, time_total); + // rT0_2 = boost::posix_time::microsec_clock::local_time(); + // double time_total = (rT0_2 - rT0_1).total_microseconds() * 1e-6; + // PRINT_DEBUG(BLUE "[TIME]: %.4f seconds for visualization\n" RESET, time_total); } void ROS2Visualizer::visualize_odometry(double timestamp) { @@ -244,7 +280,7 @@ void ROS2Visualizer::visualize_odometry(double timestamp) { // Our odometry message nav_msgs::msg::Odometry odomIinM; - odomIinM.header.stamp = RosVisualizerHelper::get_time_from_seconds(timestamp); + odomIinM.header.stamp = ROSVisualizerHelper::get_time_from_seconds(timestamp); odomIinM.header.frame_id = "global"; // The POSE component (orientation and position) @@ -289,7 +325,7 @@ void ROS2Visualizer::visualize_odometry(double timestamp) { // NOTE: a rotation from GtoI in JPL has the same xyzw as a ItoG Hamilton rotation auto odom_pose = std::make_shared(); odom_pose->set_value(state_plus.block(0, 0, 7, 1)); - geometry_msgs::msg::TransformStamped trans = RosVisualizerHelper::get_stamped_transform_from_pose(_node, odom_pose, false); + geometry_msgs::msg::TransformStamped trans = ROSVisualizerHelper::get_stamped_transform_from_pose(_node, odom_pose, false); trans.header.stamp = _node->now(); trans.header.frame_id = "global"; trans.child_frame_id = "imu"; @@ -299,7 +335,7 @@ void ROS2Visualizer::visualize_odometry(double timestamp) { // Loop through each camera calibration and publish it for (const auto &calib : state->_calib_IMUtoCAM) { - geometry_msgs::msg::TransformStamped trans_calib = RosVisualizerHelper::get_stamped_transform_from_pose(_node, calib.second, true); + geometry_msgs::msg::TransformStamped trans_calib = ROSVisualizerHelper::get_stamped_transform_from_pose(_node, calib.second, true); trans_calib.header.stamp = _node->now(); trans_calib.header.frame_id = "imu"; trans_calib.child_frame_id = "cam" + std::to_string(calib.first); @@ -399,12 +435,13 @@ void ROS2Visualizer::callback_inertial(const sensor_msgs::msg::Imu::SharedPtr ms double timestamp_imu_inC = message.timestamp - _app->get_state()->_calib_dt_CAMtoIMU->value()(0); while (!camera_queue.empty() && camera_queue.at(0).timestamp < timestamp_imu_inC) { auto rT0_1 = boost::posix_time::microsec_clock::local_time(); + double update_dt = 100.0 * (timestamp_imu_inC - camera_queue.at(0).timestamp); _app->feed_measurement_camera(camera_queue.at(0)); visualize(); camera_queue.pop_front(); auto rT0_2 = boost::posix_time::microsec_clock::local_time(); double time_total = (rT0_2 - rT0_1).total_microseconds() * 1e-6; - PRINT_INFO(BLUE "[TIME]: %.4f seconds total (%.1f hz)\n" RESET, time_total, 1.0 / time_total); + PRINT_INFO(BLUE "[TIME]: %.4f seconds total (%.1f hz, %.2f ms behind)\n" RESET, time_total, 1.0 / time_total, update_dt); } } thread_update_running = false; @@ -412,7 +449,7 @@ void ROS2Visualizer::callback_inertial(const sensor_msgs::msg::Imu::SharedPtr ms // If we are single threaded, then run single threaded // Otherwise detach this thread so it runs in the background! - if (!_app->get_params().use_multi_threading) { + if (!_app->get_params().use_multi_threading_subs) { thread.join(); } else { thread.detach(); @@ -524,7 +561,7 @@ void ROS2Visualizer::publish_state() { // Create pose of IMU (note we use the bag time) geometry_msgs::msg::PoseWithCovarianceStamped poseIinM; - poseIinM.header.stamp = RosVisualizerHelper::get_time_from_seconds(timestamp_inI); + poseIinM.header.stamp = ROSVisualizerHelper::get_time_from_seconds(timestamp_inI); poseIinM.header.frame_id = "global"; poseIinM.pose.pose.orientation.x = state->_imu->quat()(0); poseIinM.pose.pose.orientation.y = state->_imu->quat()(1); @@ -569,12 +606,21 @@ void ROS2Visualizer::publish_state() { void ROS2Visualizer::publish_images() { + // Return if we have already visualized + if (_app->get_state() == nullptr) + return; + if (last_visualization_timestamp_image == _app->get_state()->_timestamp && _app->initialized()) + return; + last_visualization_timestamp_image = _app->get_state()->_timestamp; + // Check if we have subscribers if (it_pub_tracks.getNumSubscribers() == 0) return; // Get our image of history tracks cv::Mat img_history = _app->get_historical_viz_image(); + if (img_history.empty()) + return; // Create our message std_msgs::msg::Header header; @@ -595,17 +641,17 @@ void ROS2Visualizer::publish_features() { // Get our good MSCKF features std::vector feats_msckf = _app->get_good_features_MSCKF(); - sensor_msgs::msg::PointCloud2 cloud = RosVisualizerHelper::get_ros_pointcloud(_node, feats_msckf); + sensor_msgs::msg::PointCloud2 cloud = ROSVisualizerHelper::get_ros_pointcloud(_node, feats_msckf); pub_points_msckf->publish(cloud); // Get our good SLAM features std::vector feats_slam = _app->get_features_SLAM(); - sensor_msgs::msg::PointCloud2 cloud_SLAM = RosVisualizerHelper::get_ros_pointcloud(_node, feats_slam); + sensor_msgs::msg::PointCloud2 cloud_SLAM = ROSVisualizerHelper::get_ros_pointcloud(_node, feats_slam); pub_points_slam->publish(cloud_SLAM); // Get our good ARUCO features std::vector feats_aruco = _app->get_features_ARUCO(); - sensor_msgs::msg::PointCloud2 cloud_ARUCO = RosVisualizerHelper::get_ros_pointcloud(_node, feats_aruco); + sensor_msgs::msg::PointCloud2 cloud_ARUCO = ROSVisualizerHelper::get_ros_pointcloud(_node, feats_aruco); pub_points_aruco->publish(cloud_ARUCO); // Skip the rest of we are not doing simulation @@ -614,7 +660,7 @@ void ROS2Visualizer::publish_features() { // Get our good SIMULATION features std::vector feats_sim = _sim->get_map_vec(); - sensor_msgs::msg::PointCloud2 cloud_SIM = RosVisualizerHelper::get_ros_pointcloud(_node, feats_sim); + sensor_msgs::msg::PointCloud2 cloud_SIM = ROSVisualizerHelper::get_ros_pointcloud(_node, feats_sim); pub_points_sim->publish(cloud_SIM); } @@ -646,7 +692,7 @@ void ROS2Visualizer::publish_groundtruth() { // Create pose of IMU geometry_msgs::msg::PoseStamped poseIinM; - poseIinM.header.stamp = RosVisualizerHelper::get_time_from_seconds(timestamp_inI); + poseIinM.header.stamp = ROSVisualizerHelper::get_time_from_seconds(timestamp_inI); poseIinM.header.frame_id = "global"; poseIinM.pose.orientation.x = state_gt(1, 0); poseIinM.pose.orientation.y = state_gt(2, 0); @@ -764,7 +810,7 @@ void ROS2Visualizer::publish_loopclosure_information() { // Default header std_msgs::msg::Header header; - header.stamp = RosVisualizerHelper::get_time_from_seconds(active_tracks_time1); + header.stamp = ROSVisualizerHelper::get_time_from_seconds(active_tracks_time1); //====================================================== // Check if we have subscribers for the pose odometry, camera intrinsics, or extrinsics diff --git a/ov_msckf/src/ros/ROS2Visualizer.h b/ov_msckf/src/ros/ROS2Visualizer.h index 8278b63ae..9eced260d 100644 --- a/ov_msckf/src/ros/ROS2Visualizer.h +++ b/ov_msckf/src/ros/ROS2Visualizer.h @@ -45,18 +45,26 @@ #include #include +#include +#include +#include +#include + +#include +#include #include #include -#include "core/VioManager.h" -#include "ros/RosVisualizerHelper.h" -#include "sim/Simulator.h" -#include "utils/dataset_reader.h" -#include "utils/print.h" -#include "utils/sensor_data.h" +namespace ov_core { +class YamlParser; +struct CameraData; +} // namespace ov_core namespace ov_msckf { +class VioManager; +class Simulator; + /** * @brief Helper class that will publish results onto the ROS framework. * @@ -185,6 +193,7 @@ class ROS2Visualizer { // Last timestamp we visualized at double last_visualization_timestamp = 0; + double last_visualization_timestamp_image = 0; // Our groundtruth states std::map> gt_states; @@ -195,7 +204,7 @@ class ROS2Visualizer { bool publish_calibration_tf = true; // Files and if we should save total state - bool save_total_state; + bool save_total_state = false; std::ofstream of_state_est, of_state_std, of_state_gt; }; diff --git a/ov_msckf/src/ros/ROSVisualizerHelper.cpp b/ov_msckf/src/ros/ROSVisualizerHelper.cpp new file mode 100644 index 000000000..b29f9c89e --- /dev/null +++ b/ov_msckf/src/ros/ROSVisualizerHelper.cpp @@ -0,0 +1,304 @@ +/* + * OpenVINS: An Open Platform for Visual-Inertial Research + * Copyright (C) 2018-2022 Patrick Geneva + * Copyright (C) 2018-2022 Guoquan Huang + * Copyright (C) 2018-2022 OpenVINS Contributors + * Copyright (C) 2018-2019 Kevin Eckenhoff + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include "ROSVisualizerHelper.h" + +#include "core/VioManager.h" +#include "sim/Simulator.h" +#include "state/State.h" +#include "state/StateHelper.h" + +#include "types/PoseJPL.h" + +using namespace ov_msckf; +using namespace std; + +#if ROS_AVAILABLE == 1 +sensor_msgs::PointCloud2 ROSVisualizerHelper::get_ros_pointcloud(const std::vector &feats) { + + // Declare message and sizes + sensor_msgs::PointCloud2 cloud; + cloud.header.frame_id = "global"; + cloud.header.stamp = ros::Time::now(); + cloud.width = 3 * feats.size(); + cloud.height = 1; + cloud.is_bigendian = false; + cloud.is_dense = false; // there may be invalid points + + // Setup pointcloud fields + sensor_msgs::PointCloud2Modifier modifier(cloud); + modifier.setPointCloud2FieldsByString(1, "xyz"); + modifier.resize(3 * feats.size()); + + // Iterators + sensor_msgs::PointCloud2Iterator out_x(cloud, "x"); + sensor_msgs::PointCloud2Iterator out_y(cloud, "y"); + sensor_msgs::PointCloud2Iterator out_z(cloud, "z"); + + // Fill our iterators + for (const auto &pt : feats) { + *out_x = (float)pt(0); + ++out_x; + *out_y = (float)pt(1); + ++out_y; + *out_z = (float)pt(2); + ++out_z; + } + + return cloud; +} + +tf::StampedTransform ROSVisualizerHelper::get_stamped_transform_from_pose(const std::shared_ptr &pose, bool flip_trans) { + + // Need to flip the transform to the IMU frame + Eigen::Vector4d q_ItoC = pose->quat(); + Eigen::Vector3d p_CinI = pose->pos(); + if (flip_trans) { + p_CinI = -pose->Rot().transpose() * pose->pos(); + } + + // publish our transform on TF + // NOTE: since we use JPL we have an implicit conversion to Hamilton when we publish + // NOTE: a rotation from ItoC in JPL has the same xyzw as a CtoI Hamilton rotation + tf::StampedTransform trans; + trans.stamp_ = ros::Time::now(); + tf::Quaternion quat(q_ItoC(0), q_ItoC(1), q_ItoC(2), q_ItoC(3)); + trans.setRotation(quat); + tf::Vector3 orig(p_CinI(0), p_CinI(1), p_CinI(2)); + trans.setOrigin(orig); + return trans; +} +#endif + +#if ROS_AVAILABLE == 2 +sensor_msgs::msg::PointCloud2 ROSVisualizerHelper::get_ros_pointcloud(std::shared_ptr node, + const std::vector &feats) { + + // Declare message and sizes + sensor_msgs::msg::PointCloud2 cloud; + cloud.header.frame_id = "global"; + cloud.header.stamp = node->now(); + cloud.width = 3 * feats.size(); + cloud.height = 1; + cloud.is_bigendian = false; + cloud.is_dense = false; // there may be invalid points + + // Setup pointcloud fields + sensor_msgs::PointCloud2Modifier modifier(cloud); + modifier.setPointCloud2FieldsByString(1, "xyz"); + modifier.resize(3 * feats.size()); + + // Iterators + sensor_msgs::PointCloud2Iterator out_x(cloud, "x"); + sensor_msgs::PointCloud2Iterator out_y(cloud, "y"); + sensor_msgs::PointCloud2Iterator out_z(cloud, "z"); + + // Fill our iterators + for (const auto &pt : feats) { + *out_x = (float)pt(0); + ++out_x; + *out_y = (float)pt(1); + ++out_y; + *out_z = (float)pt(2); + ++out_z; + } + + return cloud; +} + +geometry_msgs::msg::TransformStamped ROSVisualizerHelper::get_stamped_transform_from_pose(std::shared_ptr node, + const std::shared_ptr &pose, + bool flip_trans) { + + // Need to flip the transform to the IMU frame + Eigen::Vector4d q_ItoC = pose->quat(); + Eigen::Vector3d p_CinI = pose->pos(); + if (flip_trans) { + p_CinI = -pose->Rot().transpose() * pose->pos(); + } + + // publish our transform on TF + // NOTE: since we use JPL we have an implicit conversion to Hamilton when we publish + // NOTE: a rotation from ItoC in JPL has the same xyzw as a CtoI Hamilton rotation + geometry_msgs::msg::TransformStamped trans; + trans.header.stamp = node->now(); + trans.transform.rotation.x = q_ItoC(0); + trans.transform.rotation.y = q_ItoC(1); + trans.transform.rotation.z = q_ItoC(2); + trans.transform.rotation.w = q_ItoC(3); + trans.transform.translation.x = p_CinI(0); + trans.transform.translation.y = p_CinI(1); + trans.transform.translation.z = p_CinI(2); + return trans; +} +#endif + +void ROSVisualizerHelper::sim_save_total_state_to_file(std::shared_ptr state, std::shared_ptr sim, + std::ofstream &of_state_est, std::ofstream &of_state_std, + std::ofstream &of_state_gt) { + + // We want to publish in the IMU clock frame + // The timestamp in the state will be the last camera time + double t_ItoC = state->_calib_dt_CAMtoIMU->value()(0); + double timestamp_inI = state->_timestamp + t_ItoC; + + // If we have our simulator, then save it to our groundtruth file + if (sim != nullptr) { + + // Note that we get the true time in the IMU clock frame + // NOTE: we record both the estimate and groundtruth with the same "true" timestamp if we are doing simulation + Eigen::Matrix state_gt; + timestamp_inI = state->_timestamp + sim->get_true_parameters().calib_camimu_dt; + if (sim->get_state(timestamp_inI, state_gt)) { + // STATE: write current true state + of_state_gt.precision(5); + of_state_gt.setf(std::ios::fixed, std::ios::floatfield); + of_state_gt << state_gt(0) << " "; + of_state_gt.precision(6); + of_state_gt << state_gt(1) << " " << state_gt(2) << " " << state_gt(3) << " " << state_gt(4) << " "; + of_state_gt << state_gt(5) << " " << state_gt(6) << " " << state_gt(7) << " "; + of_state_gt << state_gt(8) << " " << state_gt(9) << " " << state_gt(10) << " "; + of_state_gt << state_gt(11) << " " << state_gt(12) << " " << state_gt(13) << " "; + of_state_gt << state_gt(14) << " " << state_gt(15) << " " << state_gt(16) << " "; + + // TIMEOFF: Get the current true time offset + of_state_gt.precision(7); + of_state_gt << sim->get_true_parameters().calib_camimu_dt << " "; + of_state_gt.precision(0); + of_state_gt << state->_options.num_cameras << " "; + of_state_gt.precision(6); + + // CALIBRATION: Write the camera values to file + assert(state->_options.num_cameras == sim->get_true_parameters().state_options.num_cameras); + for (int i = 0; i < state->_options.num_cameras; i++) { + // Intrinsics values + of_state_gt << sim->get_true_parameters().camera_intrinsics.at(i)->get_value()(0) << " " + << sim->get_true_parameters().camera_intrinsics.at(i)->get_value()(1) << " " + << sim->get_true_parameters().camera_intrinsics.at(i)->get_value()(2) << " " + << sim->get_true_parameters().camera_intrinsics.at(i)->get_value()(3) << " "; + of_state_gt << sim->get_true_parameters().camera_intrinsics.at(i)->get_value()(4) << " " + << sim->get_true_parameters().camera_intrinsics.at(i)->get_value()(5) << " " + << sim->get_true_parameters().camera_intrinsics.at(i)->get_value()(6) << " " + << sim->get_true_parameters().camera_intrinsics.at(i)->get_value()(7) << " "; + // Rotation and position + of_state_gt << sim->get_true_parameters().camera_extrinsics.at(i)(0) << " " << sim->get_true_parameters().camera_extrinsics.at(i)(1) + << " " << sim->get_true_parameters().camera_extrinsics.at(i)(2) << " " + << sim->get_true_parameters().camera_extrinsics.at(i)(3) << " "; + of_state_gt << sim->get_true_parameters().camera_extrinsics.at(i)(4) << " " << sim->get_true_parameters().camera_extrinsics.at(i)(5) + << " " << sim->get_true_parameters().camera_extrinsics.at(i)(6) << " "; + } + + // New line + of_state_gt << endl; + } + } + + //========================================================================== + //========================================================================== + //========================================================================== + + // Get the covariance of the whole system + Eigen::MatrixXd cov = StateHelper::get_full_covariance(state); + + // STATE: Write the current state to file + of_state_est.precision(5); + of_state_est.setf(std::ios::fixed, std::ios::floatfield); + of_state_est << timestamp_inI << " "; + of_state_est.precision(6); + of_state_est << state->_imu->quat()(0) << " " << state->_imu->quat()(1) << " " << state->_imu->quat()(2) << " " << state->_imu->quat()(3) + << " "; + of_state_est << state->_imu->pos()(0) << " " << state->_imu->pos()(1) << " " << state->_imu->pos()(2) << " "; + of_state_est << state->_imu->vel()(0) << " " << state->_imu->vel()(1) << " " << state->_imu->vel()(2) << " "; + of_state_est << state->_imu->bias_g()(0) << " " << state->_imu->bias_g()(1) << " " << state->_imu->bias_g()(2) << " "; + of_state_est << state->_imu->bias_a()(0) << " " << state->_imu->bias_a()(1) << " " << state->_imu->bias_a()(2) << " "; + + // STATE: Write current uncertainty to file + of_state_std.precision(5); + of_state_std.setf(std::ios::fixed, std::ios::floatfield); + of_state_std << timestamp_inI << " "; + of_state_std.precision(6); + int id = state->_imu->q()->id(); + of_state_std << std::sqrt(cov(id + 0, id + 0)) << " " << std::sqrt(cov(id + 1, id + 1)) << " " << std::sqrt(cov(id + 2, id + 2)) << " "; + id = state->_imu->p()->id(); + of_state_std << std::sqrt(cov(id + 0, id + 0)) << " " << std::sqrt(cov(id + 1, id + 1)) << " " << std::sqrt(cov(id + 2, id + 2)) << " "; + id = state->_imu->v()->id(); + of_state_std << std::sqrt(cov(id + 0, id + 0)) << " " << std::sqrt(cov(id + 1, id + 1)) << " " << std::sqrt(cov(id + 2, id + 2)) << " "; + id = state->_imu->bg()->id(); + of_state_std << std::sqrt(cov(id + 0, id + 0)) << " " << std::sqrt(cov(id + 1, id + 1)) << " " << std::sqrt(cov(id + 2, id + 2)) << " "; + id = state->_imu->ba()->id(); + of_state_std << std::sqrt(cov(id + 0, id + 0)) << " " << std::sqrt(cov(id + 1, id + 1)) << " " << std::sqrt(cov(id + 2, id + 2)) << " "; + + // TIMEOFF: Get the current estimate time offset + of_state_est.precision(7); + of_state_est << state->_calib_dt_CAMtoIMU->value()(0) << " "; + of_state_est.precision(0); + of_state_est << state->_options.num_cameras << " "; + of_state_est.precision(6); + + // TIMEOFF: Get the current std values + if (state->_options.do_calib_camera_timeoffset) { + of_state_std << std::sqrt(cov(state->_calib_dt_CAMtoIMU->id(), state->_calib_dt_CAMtoIMU->id())) << " "; + } else { + of_state_std << 0.0 << " "; + } + of_state_std.precision(0); + of_state_std << state->_options.num_cameras << " "; + of_state_std.precision(6); + + // CALIBRATION: Write the camera values to file + for (int i = 0; i < state->_options.num_cameras; i++) { + // Intrinsics values + of_state_est << state->_cam_intrinsics.at(i)->value()(0) << " " << state->_cam_intrinsics.at(i)->value()(1) << " " + << state->_cam_intrinsics.at(i)->value()(2) << " " << state->_cam_intrinsics.at(i)->value()(3) << " "; + of_state_est << state->_cam_intrinsics.at(i)->value()(4) << " " << state->_cam_intrinsics.at(i)->value()(5) << " " + << state->_cam_intrinsics.at(i)->value()(6) << " " << state->_cam_intrinsics.at(i)->value()(7) << " "; + // Rotation and position + of_state_est << state->_calib_IMUtoCAM.at(i)->value()(0) << " " << state->_calib_IMUtoCAM.at(i)->value()(1) << " " + << state->_calib_IMUtoCAM.at(i)->value()(2) << " " << state->_calib_IMUtoCAM.at(i)->value()(3) << " "; + of_state_est << state->_calib_IMUtoCAM.at(i)->value()(4) << " " << state->_calib_IMUtoCAM.at(i)->value()(5) << " " + << state->_calib_IMUtoCAM.at(i)->value()(6) << " "; + // Covariance + if (state->_options.do_calib_camera_intrinsics) { + int index_in = state->_cam_intrinsics.at(i)->id(); + of_state_std << std::sqrt(cov(index_in + 0, index_in + 0)) << " " << std::sqrt(cov(index_in + 1, index_in + 1)) << " " + << std::sqrt(cov(index_in + 2, index_in + 2)) << " " << std::sqrt(cov(index_in + 3, index_in + 3)) << " "; + of_state_std << std::sqrt(cov(index_in + 4, index_in + 4)) << " " << std::sqrt(cov(index_in + 5, index_in + 5)) << " " + << std::sqrt(cov(index_in + 6, index_in + 6)) << " " << std::sqrt(cov(index_in + 7, index_in + 7)) << " "; + } else { + of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " " << 0.0 << " "; + of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " " << 0.0 << " "; + } + if (state->_options.do_calib_camera_pose) { + int index_ex = state->_calib_IMUtoCAM.at(i)->id(); + of_state_std << std::sqrt(cov(index_ex + 0, index_ex + 0)) << " " << std::sqrt(cov(index_ex + 1, index_ex + 1)) << " " + << std::sqrt(cov(index_ex + 2, index_ex + 2)) << " "; + of_state_std << std::sqrt(cov(index_ex + 3, index_ex + 3)) << " " << std::sqrt(cov(index_ex + 4, index_ex + 4)) << " " + << std::sqrt(cov(index_ex + 5, index_ex + 5)) << " "; + } else { + of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " "; + of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " "; + } + } + + // Done with the estimates! + of_state_est << endl; + of_state_std << endl; +} diff --git a/ov_msckf/src/ros/ROSVisualizerHelper.h b/ov_msckf/src/ros/ROSVisualizerHelper.h new file mode 100644 index 000000000..bb2acd691 --- /dev/null +++ b/ov_msckf/src/ros/ROSVisualizerHelper.h @@ -0,0 +1,140 @@ +/* + * OpenVINS: An Open Platform for Visual-Inertial Research + * Copyright (C) 2018-2022 Patrick Geneva + * Copyright (C) 2018-2022 Guoquan Huang + * Copyright (C) 2018-2022 OpenVINS Contributors + * Copyright (C) 2018-2019 Kevin Eckenhoff + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#ifndef OV_MSCKF_ROSVISUALIZER_HELPER_H +#define OV_MSCKF_ROSVISUALIZER_HELPER_H + +#include + +#if ROS_AVAILABLE == 1 +#include +#include +#include +#include +#elif ROS_AVAILABLE == 2 +#include +#include +#include +#include +#include +#endif + +namespace ov_type { +class PoseJPL; +} + +namespace ov_msckf { + +class State; +class VioManager; +class Simulator; + +/** + * @brief Helper class that handles some common versions into and out of ROS formats + */ +class ROSVisualizerHelper { + +public: +#if ROS_AVAILABLE == 1 + /** + * @brief Will visualize the system if we have new things + * @param feats Vector of features we will convert into ros format + * @return ROS pointcloud + */ + static sensor_msgs::PointCloud2 get_ros_pointcloud(const std::vector &feats); + + /** + * @brief Given a ov_type::PoseJPL this will convert into the ros format. + * + * NOTE: frame ids need to be handled externally! + * + * @param pose Pose with JPL quaternion (e.g. q_GtoI, p_IinG) + * @param flip_trans If we should flip / inverse the translation + * @return TF of our pose in global (e.g. q_ItoG, p_IinG) + */ + static tf::StampedTransform get_stamped_transform_from_pose(const std::shared_ptr &pose, bool flip_trans); +#endif + +#if ROS_AVAILABLE == 2 + /** + * @brief Will visualize the system if we have new things + * @param node ROS2 node pointer + * @param feats Vector of features we will convert into ros format + * @return ROS pointcloud + */ + static sensor_msgs::msg::PointCloud2 get_ros_pointcloud(std::shared_ptr node, const std::vector &feats); + + /** + * @brief Given a ov_type::PoseJPL this will convert into the ros format. + * + * NOTE: frame ids need to be handled externally! + * + * @param node ROS2 node pointer + * @param pose Pose with JPL quaternion (e.g. q_GtoI, p_IinG) + * @param flip_trans If we should flip / inverse the translation + * @return TF of our pose in global (e.g. q_ItoG, p_IinG) + */ + static geometry_msgs::msg::TransformStamped + get_stamped_transform_from_pose(std::shared_ptr node, const std::shared_ptr &pose, bool flip_trans); + + /** + * @brief Helper function that converts time in seconds to our rclcpp timestamp + * @param seconds Time in seconds + * @return Return ROS2 header timestamp + */ + static rclcpp::Time get_time_from_seconds(double seconds) { + + // ROS2 time class has no double constructor + // Extract compatible time from timestamp using ros1 implementation for now + uint32_t sec, nsec; + int64_t sec64 = static_cast(floor(seconds)); + if (sec64 < 0 || sec64 > std::numeric_limits::max()) + throw std::runtime_error("Time is out of dual 32-bit range"); + sec = static_cast(sec64); + nsec = static_cast(std::round((seconds - sec) * 1e9)); + + // Avoid rounding errors + sec += (nsec / 1000000000ul); + nsec %= 1000000000ul; + return rclcpp::Time(sec, nsec); + } + +#endif + + /** + * @brief Save current estimate state and groundtruth including calibration + * @param state Pointer to the state + * @param sim Pointer to the simulator (or null) + * @param of_state_est Output file for state estimate + * @param of_state_std Output file for covariance + * @param of_state_gt Output file for groundtruth (if we have it from sim) + */ + static void sim_save_total_state_to_file(std::shared_ptr state, std::shared_ptr sim, std::ofstream &of_state_est, + std::ofstream &of_state_std, std::ofstream &of_state_gt); + +private: + // Cannot create this class + ROSVisualizerHelper() = default; +}; + +} // namespace ov_msckf + +#endif // OV_MSCKF_ROSVISUALIZER_HELPER_H diff --git a/ov_msckf/src/ros/RosVisualizerHelper.h b/ov_msckf/src/ros/RosVisualizerHelper.h deleted file mode 100644 index 8d9b55044..000000000 --- a/ov_msckf/src/ros/RosVisualizerHelper.h +++ /dev/null @@ -1,388 +0,0 @@ -/* - * OpenVINS: An Open Platform for Visual-Inertial Research - * Copyright (C) 2018-2022 Patrick Geneva - * Copyright (C) 2018-2022 Guoquan Huang - * Copyright (C) 2018-2022 OpenVINS Contributors - * Copyright (C) 2018-2019 Kevin Eckenhoff - * - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see . - */ - -#ifndef OV_MSCKF_ROSVISUALIZER_HELPER_H -#define OV_MSCKF_ROSVISUALIZER_HELPER_H - -#if ROS_AVAILABLE == 1 -#include -#include -#include -#include -#elif ROS_AVAILABLE == 2 -#include -#include -#include -#include -#include -#endif - -#include "core/VioManager.h" -#include "sim/Simulator.h" - -namespace ov_msckf { - -/** - * @brief Helper class that handles some common versions into and out of ROS formats - */ -class RosVisualizerHelper { - -public: -#if ROS_AVAILABLE == 1 - /** - * @brief Will visualize the system if we have new things - * @param feats Vector of features we will convert into ros format - * @return ROS pointcloud - */ - static sensor_msgs::PointCloud2 get_ros_pointcloud(const std::vector &feats) { - - // Declare message and sizes - sensor_msgs::PointCloud2 cloud; - cloud.header.frame_id = "global"; - cloud.header.stamp = ros::Time::now(); - cloud.width = 3 * feats.size(); - cloud.height = 1; - cloud.is_bigendian = false; - cloud.is_dense = false; // there may be invalid points - - // Setup pointcloud fields - sensor_msgs::PointCloud2Modifier modifier(cloud); - modifier.setPointCloud2FieldsByString(1, "xyz"); - modifier.resize(3 * feats.size()); - - // Iterators - sensor_msgs::PointCloud2Iterator out_x(cloud, "x"); - sensor_msgs::PointCloud2Iterator out_y(cloud, "y"); - sensor_msgs::PointCloud2Iterator out_z(cloud, "z"); - - // Fill our iterators - for (const auto &pt : feats) { - *out_x = (float)pt(0); - ++out_x; - *out_y = (float)pt(1); - ++out_y; - *out_z = (float)pt(2); - ++out_z; - } - - return cloud; - } - - /** - * @brief Given a ov_type::PoseJPL this will convert into the ros format. - * - * NOTE: frame ids need to be handled externally! - * - * @param pose Pose with JPL quaternion (e.g. q_GtoI, p_IinG) - * @param flip_trans If we should flip / inverse the translation - * @return TF of our pose in global (e.g. q_ItoG, p_IinG) - */ - static tf::StampedTransform get_stamped_transform_from_pose(const std::shared_ptr &pose, bool flip_trans) { - - // Need to flip the transform to the IMU frame - Eigen::Vector4d q_ItoC = pose->quat(); - Eigen::Vector3d p_CinI = pose->pos(); - if (flip_trans) { - p_CinI = -pose->Rot().transpose() * pose->pos(); - } - - // publish our transform on TF - // NOTE: since we use JPL we have an implicit conversion to Hamilton when we publish - // NOTE: a rotation from ItoC in JPL has the same xyzw as a CtoI Hamilton rotation - tf::StampedTransform trans; - trans.stamp_ = ros::Time::now(); - tf::Quaternion quat(q_ItoC(0), q_ItoC(1), q_ItoC(2), q_ItoC(3)); - trans.setRotation(quat); - tf::Vector3 orig(p_CinI(0), p_CinI(1), p_CinI(2)); - trans.setOrigin(orig); - return trans; - } -#endif - -#if ROS_AVAILABLE == 2 - /** - * @brief Will visualize the system if we have new things - * @param node ROS2 node pointer - * @param feats Vector of features we will convert into ros format - * @return ROS pointcloud - */ - static sensor_msgs::msg::PointCloud2 get_ros_pointcloud(std::shared_ptr node, const std::vector &feats) { - - // Declare message and sizes - sensor_msgs::msg::PointCloud2 cloud; - cloud.header.frame_id = "global"; - cloud.header.stamp = node->now(); - cloud.width = 3 * feats.size(); - cloud.height = 1; - cloud.is_bigendian = false; - cloud.is_dense = false; // there may be invalid points - - // Setup pointcloud fields - sensor_msgs::PointCloud2Modifier modifier(cloud); - modifier.setPointCloud2FieldsByString(1, "xyz"); - modifier.resize(3 * feats.size()); - - // Iterators - sensor_msgs::PointCloud2Iterator out_x(cloud, "x"); - sensor_msgs::PointCloud2Iterator out_y(cloud, "y"); - sensor_msgs::PointCloud2Iterator out_z(cloud, "z"); - - // Fill our iterators - for (const auto &pt : feats) { - *out_x = (float)pt(0); - ++out_x; - *out_y = (float)pt(1); - ++out_y; - *out_z = (float)pt(2); - ++out_z; - } - - return cloud; - } - - /** - * @brief Given a ov_type::PoseJPL this will convert into the ros format. - * - * NOTE: frame ids need to be handled externally! - * - * @param node ROS2 node pointer - * @param pose Pose with JPL quaternion (e.g. q_GtoI, p_IinG) - * @param flip_trans If we should flip / inverse the translation - * @return TF of our pose in global (e.g. q_ItoG, p_IinG) - */ - static geometry_msgs::msg::TransformStamped - get_stamped_transform_from_pose(std::shared_ptr node, const std::shared_ptr &pose, bool flip_trans) { - - // Need to flip the transform to the IMU frame - Eigen::Vector4d q_ItoC = pose->quat(); - Eigen::Vector3d p_CinI = pose->pos(); - if (flip_trans) { - p_CinI = -pose->Rot().transpose() * pose->pos(); - } - - // publish our transform on TF - // NOTE: since we use JPL we have an implicit conversion to Hamilton when we publish - // NOTE: a rotation from ItoC in JPL has the same xyzw as a CtoI Hamilton rotation - geometry_msgs::msg::TransformStamped trans; - trans.header.stamp = node->now(); - trans.transform.rotation.x = q_ItoC(0); - trans.transform.rotation.y = q_ItoC(1); - trans.transform.rotation.z = q_ItoC(2); - trans.transform.rotation.w = q_ItoC(3); - trans.transform.translation.x = p_CinI(0); - trans.transform.translation.y = p_CinI(1); - trans.transform.translation.z = p_CinI(2); - return trans; - } - - /** - * @brief Helper function that converts time in seconds to our rclcpp timestamp - * @param seconds Time in seconds - * @return Return ROS2 header timestamp - */ - static rclcpp::Time get_time_from_seconds(double seconds) { - - // ROS2 time class has no double constructor - // Extract compatible time from timestamp using ros1 implementation for now - uint32_t sec, nsec; - int64_t sec64 = static_cast(floor(seconds)); - if (sec64 < 0 || sec64 > std::numeric_limits::max()) - throw std::runtime_error("Time is out of dual 32-bit range"); - sec = static_cast(sec64); - nsec = static_cast(std::round((seconds - sec) * 1e9)); - - // Avoid rounding errors - sec += (nsec / 1000000000ul); - nsec %= 1000000000ul; - return rclcpp::Time(sec, nsec); - } - -#endif - - /** - * @brief Save current estimate state and groundtruth including calibration - * @param state Pointer to the state - * @param sim Pointer to the simulator (or null) - * @param of_state_est Output file for state estimate - * @param of_state_std Output file for covariance - * @param of_state_gt Output file for groundtruth (if we have it from sim) - */ - static void sim_save_total_state_to_file(std::shared_ptr state, std::shared_ptr sim, std::ofstream &of_state_est, - std::ofstream &of_state_std, std::ofstream &of_state_gt) { - - // We want to publish in the IMU clock frame - // The timestamp in the state will be the last camera time - double t_ItoC = state->_calib_dt_CAMtoIMU->value()(0); - double timestamp_inI = state->_timestamp + t_ItoC; - - // If we have our simulator, then save it to our groundtruth file - if (sim != nullptr) { - - // Note that we get the true time in the IMU clock frame - // NOTE: we record both the estimate and groundtruth with the same "true" timestamp if we are doing simulation - Eigen::Matrix state_gt; - timestamp_inI = state->_timestamp + sim->get_true_parameters().calib_camimu_dt; - if (sim->get_state(timestamp_inI, state_gt)) { - // STATE: write current true state - of_state_gt.precision(5); - of_state_gt.setf(std::ios::fixed, std::ios::floatfield); - of_state_gt << state_gt(0) << " "; - of_state_gt.precision(6); - of_state_gt << state_gt(1) << " " << state_gt(2) << " " << state_gt(3) << " " << state_gt(4) << " "; - of_state_gt << state_gt(5) << " " << state_gt(6) << " " << state_gt(7) << " "; - of_state_gt << state_gt(8) << " " << state_gt(9) << " " << state_gt(10) << " "; - of_state_gt << state_gt(11) << " " << state_gt(12) << " " << state_gt(13) << " "; - of_state_gt << state_gt(14) << " " << state_gt(15) << " " << state_gt(16) << " "; - - // TIMEOFF: Get the current true time offset - of_state_gt.precision(7); - of_state_gt << sim->get_true_parameters().calib_camimu_dt << " "; - of_state_gt.precision(0); - of_state_gt << state->_options.num_cameras << " "; - of_state_gt.precision(6); - - // CALIBRATION: Write the camera values to file - assert(state->_options.num_cameras == sim->get_true_parameters().state_options.num_cameras); - for (int i = 0; i < state->_options.num_cameras; i++) { - // Intrinsics values - of_state_gt << sim->get_true_parameters().camera_intrinsics.at(i)->get_value()(0) << " " - << sim->get_true_parameters().camera_intrinsics.at(i)->get_value()(1) << " " - << sim->get_true_parameters().camera_intrinsics.at(i)->get_value()(2) << " " - << sim->get_true_parameters().camera_intrinsics.at(i)->get_value()(3) << " "; - of_state_gt << sim->get_true_parameters().camera_intrinsics.at(i)->get_value()(4) << " " - << sim->get_true_parameters().camera_intrinsics.at(i)->get_value()(5) << " " - << sim->get_true_parameters().camera_intrinsics.at(i)->get_value()(6) << " " - << sim->get_true_parameters().camera_intrinsics.at(i)->get_value()(7) << " "; - // Rotation and position - of_state_gt << sim->get_true_parameters().camera_extrinsics.at(i)(0) << " " - << sim->get_true_parameters().camera_extrinsics.at(i)(1) << " " - << sim->get_true_parameters().camera_extrinsics.at(i)(2) << " " - << sim->get_true_parameters().camera_extrinsics.at(i)(3) << " "; - of_state_gt << sim->get_true_parameters().camera_extrinsics.at(i)(4) << " " - << sim->get_true_parameters().camera_extrinsics.at(i)(5) << " " - << sim->get_true_parameters().camera_extrinsics.at(i)(6) << " "; - } - - // New line - of_state_gt << endl; - } - } - - //========================================================================== - //========================================================================== - //========================================================================== - - // Get the covariance of the whole system - Eigen::MatrixXd cov = StateHelper::get_full_covariance(state); - - // STATE: Write the current state to file - of_state_est.precision(5); - of_state_est.setf(std::ios::fixed, std::ios::floatfield); - of_state_est << timestamp_inI << " "; - of_state_est.precision(6); - of_state_est << state->_imu->quat()(0) << " " << state->_imu->quat()(1) << " " << state->_imu->quat()(2) << " " - << state->_imu->quat()(3) << " "; - of_state_est << state->_imu->pos()(0) << " " << state->_imu->pos()(1) << " " << state->_imu->pos()(2) << " "; - of_state_est << state->_imu->vel()(0) << " " << state->_imu->vel()(1) << " " << state->_imu->vel()(2) << " "; - of_state_est << state->_imu->bias_g()(0) << " " << state->_imu->bias_g()(1) << " " << state->_imu->bias_g()(2) << " "; - of_state_est << state->_imu->bias_a()(0) << " " << state->_imu->bias_a()(1) << " " << state->_imu->bias_a()(2) << " "; - - // STATE: Write current uncertainty to file - of_state_std.precision(5); - of_state_std.setf(std::ios::fixed, std::ios::floatfield); - of_state_std << timestamp_inI << " "; - of_state_std.precision(6); - int id = state->_imu->q()->id(); - of_state_std << std::sqrt(cov(id + 0, id + 0)) << " " << std::sqrt(cov(id + 1, id + 1)) << " " << std::sqrt(cov(id + 2, id + 2)) << " "; - id = state->_imu->p()->id(); - of_state_std << std::sqrt(cov(id + 0, id + 0)) << " " << std::sqrt(cov(id + 1, id + 1)) << " " << std::sqrt(cov(id + 2, id + 2)) << " "; - id = state->_imu->v()->id(); - of_state_std << std::sqrt(cov(id + 0, id + 0)) << " " << std::sqrt(cov(id + 1, id + 1)) << " " << std::sqrt(cov(id + 2, id + 2)) << " "; - id = state->_imu->bg()->id(); - of_state_std << std::sqrt(cov(id + 0, id + 0)) << " " << std::sqrt(cov(id + 1, id + 1)) << " " << std::sqrt(cov(id + 2, id + 2)) << " "; - id = state->_imu->ba()->id(); - of_state_std << std::sqrt(cov(id + 0, id + 0)) << " " << std::sqrt(cov(id + 1, id + 1)) << " " << std::sqrt(cov(id + 2, id + 2)) << " "; - - // TIMEOFF: Get the current estimate time offset - of_state_est.precision(7); - of_state_est << state->_calib_dt_CAMtoIMU->value()(0) << " "; - of_state_est.precision(0); - of_state_est << state->_options.num_cameras << " "; - of_state_est.precision(6); - - // TIMEOFF: Get the current std values - if (state->_options.do_calib_camera_timeoffset) { - of_state_std << std::sqrt(cov(state->_calib_dt_CAMtoIMU->id(), state->_calib_dt_CAMtoIMU->id())) << " "; - } else { - of_state_std << 0.0 << " "; - } - of_state_std.precision(0); - of_state_std << state->_options.num_cameras << " "; - of_state_std.precision(6); - - // CALIBRATION: Write the camera values to file - for (int i = 0; i < state->_options.num_cameras; i++) { - // Intrinsics values - of_state_est << state->_cam_intrinsics.at(i)->value()(0) << " " << state->_cam_intrinsics.at(i)->value()(1) << " " - << state->_cam_intrinsics.at(i)->value()(2) << " " << state->_cam_intrinsics.at(i)->value()(3) << " "; - of_state_est << state->_cam_intrinsics.at(i)->value()(4) << " " << state->_cam_intrinsics.at(i)->value()(5) << " " - << state->_cam_intrinsics.at(i)->value()(6) << " " << state->_cam_intrinsics.at(i)->value()(7) << " "; - // Rotation and position - of_state_est << state->_calib_IMUtoCAM.at(i)->value()(0) << " " << state->_calib_IMUtoCAM.at(i)->value()(1) << " " - << state->_calib_IMUtoCAM.at(i)->value()(2) << " " << state->_calib_IMUtoCAM.at(i)->value()(3) << " "; - of_state_est << state->_calib_IMUtoCAM.at(i)->value()(4) << " " << state->_calib_IMUtoCAM.at(i)->value()(5) << " " - << state->_calib_IMUtoCAM.at(i)->value()(6) << " "; - // Covariance - if (state->_options.do_calib_camera_intrinsics) { - int index_in = state->_cam_intrinsics.at(i)->id(); - of_state_std << std::sqrt(cov(index_in + 0, index_in + 0)) << " " << std::sqrt(cov(index_in + 1, index_in + 1)) << " " - << std::sqrt(cov(index_in + 2, index_in + 2)) << " " << std::sqrt(cov(index_in + 3, index_in + 3)) << " "; - of_state_std << std::sqrt(cov(index_in + 4, index_in + 4)) << " " << std::sqrt(cov(index_in + 5, index_in + 5)) << " " - << std::sqrt(cov(index_in + 6, index_in + 6)) << " " << std::sqrt(cov(index_in + 7, index_in + 7)) << " "; - } else { - of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " " << 0.0 << " "; - of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " " << 0.0 << " "; - } - if (state->_options.do_calib_camera_pose) { - int index_ex = state->_calib_IMUtoCAM.at(i)->id(); - of_state_std << std::sqrt(cov(index_ex + 0, index_ex + 0)) << " " << std::sqrt(cov(index_ex + 1, index_ex + 1)) << " " - << std::sqrt(cov(index_ex + 2, index_ex + 2)) << " "; - of_state_std << std::sqrt(cov(index_ex + 3, index_ex + 3)) << " " << std::sqrt(cov(index_ex + 4, index_ex + 4)) << " " - << std::sqrt(cov(index_ex + 5, index_ex + 5)) << " "; - } else { - of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " "; - of_state_std << 0.0 << " " << 0.0 << " " << 0.0 << " "; - } - } - - // Done with the estimates! - of_state_est << endl; - of_state_std << endl; - } - -private: - // Cannot create this class - RosVisualizerHelper() = default; -}; - -} // namespace ov_msckf - -#endif // OV_MSCKF_ROSVISUALIZER_HELPER_H diff --git a/ov_msckf/src/ros1_serial_msckf.cpp b/ov_msckf/src/ros1_serial_msckf.cpp index d42405ec3..8f9232390 100644 --- a/ov_msckf/src/ros1_serial_msckf.cpp +++ b/ov_msckf/src/ros1_serial_msckf.cpp @@ -63,13 +63,14 @@ int main(int argc, char **argv) { // Create our VIO system VioManagerOptions params; params.print_and_load(parser); - params.use_multi_threading = false; + // params.use_multi_threading = false; // uncomment if you want repeatability + params.use_multi_threading_subs = false; sys = std::make_shared(params); viz = std::make_shared(nh, sys); // Ensure we read in all parameters required if (!parser->successful()) { - PRINT_ERROR(RED "unable to parse all parameters, please fix\n" RESET); + PRINT_ERROR(RED "[SERIAL]: unable to parse all parameters, please fix\n" RESET); std::exit(EXIT_FAILURE); } @@ -81,44 +82,32 @@ int main(int argc, char **argv) { std::string topic_imu; nh->param("topic_imu", topic_imu, "/imu0"); parser->parse_external("relative_config_imu", "imu0", "rostopic", topic_imu); + PRINT_DEBUG("[SERIAL]: imu: %s\n", topic_imu.c_str()); - // Our camera topics (stereo pairs and non-stereo mono) - std::vector> topic_cameras; - if (params.state_options.num_cameras == 2) { - // Read in the topics - std::string cam_topic0, cam_topic1; - nh->param("topic_camera" + std::to_string(0), cam_topic0, "/cam" + std::to_string(0) + "/image_raw"); - nh->param("topic_camera" + std::to_string(1), cam_topic1, "/cam" + std::to_string(1) + "/image_raw"); - parser->parse_external("relative_config_imucam", "cam" + std::to_string(0), "rostopic", cam_topic0); - parser->parse_external("relative_config_imucam", "cam" + std::to_string(1), "rostopic", cam_topic1); - topic_cameras.emplace_back(0, cam_topic0); - topic_cameras.emplace_back(1, cam_topic1); - PRINT_DEBUG("serial cam (stereo): %s\n", cam_topic0.c_str()); - PRINT_DEBUG("serial cam (stereo): %s\n", cam_topic1.c_str()); - } else { - for (int i = 0; i < params.state_options.num_cameras; i++) { - // read in the topic - std::string cam_topic; - nh->param("topic_camera" + std::to_string(i), cam_topic, "/cam" + std::to_string(i) + "/image_raw"); - parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "rostopic", cam_topic); - topic_cameras.emplace_back(i, cam_topic); - PRINT_DEBUG("serial cam (mono): %s\n", cam_topic.c_str()); - } + // Our camera topics + std::vector topic_cameras; + for (int i = 0; i < params.state_options.num_cameras; i++) { + std::string cam_topic; + nh->param("topic_camera" + std::to_string(i), cam_topic, "/cam" + std::to_string(i) + "/image_raw"); + parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "rostopic", cam_topic); + topic_cameras.emplace_back(cam_topic); + PRINT_DEBUG("[SERIAL]: cam: %s\n", cam_topic.c_str()); } // Location of the ROS bag we want to read in std::string path_to_bag; nh->param("path_bag", path_to_bag, "/home/patrick/datasets/eth/V1_01_easy.bag"); - PRINT_DEBUG("ros bag path is: %s\n", path_to_bag.c_str()); + PRINT_DEBUG("[SERIAL]: ros bag path is: %s\n", path_to_bag.c_str()); // Load groundtruth if we have it + // NOTE: needs to be a csv ASL format file std::map> gt_states; if (nh->hasParam("path_gt")) { std::string path_to_gt; nh->param("path_gt", path_to_gt, ""); if (!path_to_gt.empty()) { ov_core::DatasetReader::load_gt_file(path_to_gt, gt_states); - PRINT_DEBUG("gt file path is: %s\n", path_to_gt.c_str()); + PRINT_DEBUG("[SERIAL]: gt file path is: %s\n", path_to_gt.c_str()); } } @@ -127,8 +116,8 @@ int main(int argc, char **argv) { double bag_start, bag_durr; nh->param("bag_start", bag_start, 0); nh->param("bag_durr", bag_durr, -1); - PRINT_DEBUG("bag start: %.1f\n", bag_start); - PRINT_DEBUG("bag duration: %.1f\n", bag_durr); + PRINT_DEBUG("[SERIAL]: bag start: %.1f\n", bag_start); + PRINT_DEBUG("[SERIAL]: bag duration: %.1f\n", bag_durr); //=================================================================================== //=================================================================================== @@ -155,193 +144,132 @@ int main(int argc, char **argv) { // Check to make sure we have data to play if (view.size() == 0) { - PRINT_ERROR(RED "No messages to play on specified topics. Exiting.\n" RESET); + PRINT_ERROR(RED "[SERIAL]: No messages to play on specified topics. Exiting.\n" RESET); ros::shutdown(); return EXIT_FAILURE; } - // Open our iterators - auto view_imu = std::make_shared(bag, rosbag::TopicQuery(topic_imu), time_init, time_finish); - auto view_imu_iter = view_imu->begin(); - std::vector> view_cameras; - std::vector view_cameras_iterators; - for (const auto &topic : topic_cameras) { - auto view_tmp = std::make_shared(bag, rosbag::TopicQuery(topic.second), time_init, time_finish); - view_cameras.push_back(view_tmp); - view_cameras_iterators.push_back(view_tmp->begin()); - } - - // Record the current measurement timestamps - sensor_msgs::Imu::ConstPtr msg_imu_current; - sensor_msgs::Imu::ConstPtr msg_imu_next; - std::vector msg_images_current; - std::vector msg_images_next; - msg_imu_current = view_imu_iter->instantiate(); - view_imu_iter++; - msg_imu_next = view_imu_iter->instantiate(); - for (int i = 0; i < params.state_options.num_cameras; i++) { - msg_images_current.emplace_back(view_cameras_iterators.at(i)->instantiate()); - view_cameras_iterators.at(i)++; - msg_images_next.emplace_back(view_cameras_iterators.at(i)->instantiate()); + // We going to loop through and collect a list of all messages + // This is done so we can access arbitrary points in the bag + // NOTE: if we instantiate messages here, this requires the whole bag to be read + // NOTE: thus we just check the topic which allows us to quickly loop through the index + // NOTE: see this PR https://github.com/ros/ros_comm/issues/117 + double max_camera_time = -1; + std::vector msgs; + for (const rosbag::MessageInstance &msg : view) { + if (!ros::ok()) + break; + if (msg.getTopic() == topic_imu) { + // if (msg.instantiate() == nullptr) { + // PRINT_ERROR(RED "[SERIAL]: IMU topic has unmatched message types!!\n" RESET); + // PRINT_ERROR(RED "[SERIAL]: Supports: sensor_msgs::Imu\n" RESET); + // return EXIT_FAILURE; + // } + msgs.push_back(msg); + } + for (int i = 0; i < params.state_options.num_cameras; i++) { + if (msg.getTopic() == topic_cameras.at(i)) { + // sensor_msgs::CompressedImage::ConstPtr img_c = msg.instantiate(); + // sensor_msgs::Image::ConstPtr img_i = msg.instantiate(); + // if (img_c == nullptr && img_i == nullptr) { + // PRINT_ERROR(RED "[SERIAL]: Image topic has unmatched message types!!\n" RESET); + // PRINT_ERROR(RED "[SERIAL]: Supports: sensor_msgs::Image and sensor_msgs::CompressedImage\n" RESET); + // return EXIT_FAILURE; + // } + msgs.push_back(msg); + max_camera_time = std::max(max_camera_time, msg.getTime().toSec()); + } + } } - - // Last camera message timestamps we have received (mapped by cam id) - std::map camera_last_timestamp; + PRINT_DEBUG("[SERIAL]: total of %zu messages!\n", msgs.size()); //=================================================================================== //=================================================================================== //=================================================================================== - while (ros::ok()) { + // Loop through our message array, and lets process them + std::set used_index; + for (int m = 0; m < (int)msgs.size(); m++) { - // Check if we should end since we have run out of measurements - bool should_stop = false; - if (view_imu_iter == view_imu->end()) { - should_stop = true; - } - for (int i = 0; i < params.state_options.num_cameras; i++) { - if (view_cameras_iterators.at(i) == view_cameras.at(i)->end()) { - should_stop = true; - } - } - if (should_stop) { + // End once we reach the last time, or skip if before beginning time (shouldn't happen) + if (!ros::ok() || msgs.at(m).getTime() > time_finish || msgs.at(m).getTime().toSec() > max_camera_time) break; - } + if (msgs.at(m).getTime() < time_init) + continue; - // We should process the IMU if all the current cameras are greater then its time - bool should_process_imu = false; - for (int i = 0; i < params.state_options.num_cameras; i++) { - double time_imu = msg_imu_current->header.stamp.toSec(); - double time_cam = msg_images_current.at(i)->header.stamp.toSec(); - if (time_imu <= time_cam) { - should_process_imu = true; - } - } - if (should_process_imu) { - viz->callback_inertial(msg_imu_current); - msg_imu_current = msg_imu_next; - view_imu_iter++; - msg_imu_next = view_imu_iter->instantiate(); + // Skip messages that we have already used + if (used_index.find(m) != used_index.end()) { + used_index.erase(m); continue; } - // If we are stereo, then we should collect both the left and right - if (params.state_options.num_cameras == 2) { - - // Now lets do some logic to find two images which are next to each other - // We want to ensure that our stereo pair are very close to occurring at the same time - bool have_found_pair = false; - while (!have_found_pair && view_cameras_iterators.at(0) != view_cameras.at(0)->end() && - view_cameras_iterators.at(1) != view_cameras.at(1)->end()) { - - // Get left and right cameras - auto msg_cam0 = msg_images_current.at(0); - auto msg_cam1 = msg_images_current.at(1); - auto msg_cam0_next = msg_images_next.at(0); - auto msg_cam1_next = msg_images_next.at(1); - - // timestamps - double time0 = msg_cam0->header.stamp.toSec(); - double time1 = msg_cam1->header.stamp.toSec(); - double time0_next = msg_cam0_next->header.stamp.toSec(); - double time1_next = msg_cam1_next->header.stamp.toSec(); - - // We will have a match if the current left and right images are closer then the next one - // Consider the case that we drop an image: - // (L1) L2 (R2) R3 <- current pointers are at L1 and R2 - // In this case, we dropped the R1 image, thus we should skip the L1 image - // We can check to see that L1 is further away compared to L2 from R2 - // Thus we should skip the L1 frame (advance the bag forward) and check this logic again! - if (std::abs(time1 - time0) < std::abs(time1_next - time0) && std::abs(time0 - time1) < std::abs(time0_next - time1)) { - have_found_pair = true; - } else if (std::abs(time1 - time0) >= std::abs(time1_next - time0)) { - // PRINT_WARNING("skipping cam1 (%.4f >= %.4f)",std::abs(time1-time0), std::abs(time1_next-time0)); - msg_images_current.at(1) = msg_images_next.at(1); - view_cameras_iterators.at(1)++; - if (view_cameras_iterators.at(1) != view_cameras.at(1)->end()) { - msg_images_next.at(1) = view_cameras_iterators.at(1)->instantiate(); - } - } else { - // PRINT_WARNING("skipping cam0 (%.4f >= %.4f)",std::abs(time0-time1), std::abs(time0_next-time1)); - msg_images_current.at(0) = msg_images_next.at(0); - view_cameras_iterators.at(0)++; - if (view_cameras_iterators.at(0) != view_cameras.at(0)->end()) { - msg_images_next.at(0) = view_cameras_iterators.at(0)->instantiate(); - } - } - } - - // Break out if we have ended - if (view_cameras_iterators.at(0) == view_cameras.at(0)->end() || view_cameras_iterators.at(1) == view_cameras.at(1)->end()) { - break; - } - - // Check if we should initialize using the groundtruth (always use left) - Eigen::Matrix imustate; - if (!gt_states.empty() && !sys->initialized() && - ov_core::DatasetReader::get_gt_state(msg_images_current.at(0)->header.stamp.toSec(), imustate, gt_states)) { - // biases are pretty bad normally, so zero them - // imustate.block(11,0,6,1).setZero(); - sys->initialize_with_gt(imustate); - } - - // Check if we should feed this into the system at the specified frequency - double timestamp = msg_images_current.at(0)->header.stamp.toSec(); - double time_delta = 1.0 / params.track_frequency; - if (camera_last_timestamp.find(0) == camera_last_timestamp.end() || timestamp >= camera_last_timestamp.at(0) + time_delta) { - camera_last_timestamp[0] = timestamp; - viz->callback_stereo(msg_images_current.at(0), msg_images_current.at(1), 0, 1); - } + // IMU processing + if (msgs.at(m).getTopic() == topic_imu) { + // PRINT_DEBUG("processing imu = %.3f sec\n", msgs.at(m).getTime().toSec() - time_init.toSec()); + viz->callback_inertial(msgs.at(m).instantiate()); + } - // Move forward in time - msg_images_current.at(0) = msg_images_next.at(0); - view_cameras_iterators.at(0)++; - if (view_cameras_iterators.at(0) != view_cameras.at(0)->end()) { - msg_images_next.at(0) = view_cameras_iterators.at(0)->instantiate(); - } - msg_images_current.at(1) = msg_images_next.at(1); - view_cameras_iterators.at(1)++; - if (view_cameras_iterators.at(1) != view_cameras.at(1)->end()) { - msg_images_next.at(1) = view_cameras_iterators.at(1)->instantiate(); + // Camera processing + for (int cam_id = 0; cam_id < params.state_options.num_cameras; cam_id++) { + + // Skip if this message is not a camera topic + if (msgs.at(m).getTopic() != topic_cameras.at(cam_id)) + continue; + + // We have a matching camera topic here, now find the other cameras for this time + // For each camera, we will find the nearest timestamp (within 0.02sec) that is greater than the current + // If we are unable, then this message should just be skipped since it isn't a sync'ed pair! + std::map camid_to_msg_index; + double meas_time = msgs.at(m).getTime().toSec(); + for (int cam_idt = 0; cam_idt < params.state_options.num_cameras; cam_idt++) { + if (cam_idt == cam_id) { + camid_to_msg_index.insert({cam_id, m}); + continue; + } + int cam_idt_idx = -1; + for (int mt = m; mt < (int)msgs.size(); mt++) { + if (msgs.at(mt).getTopic() != topic_cameras.at(cam_idt)) + continue; + if (std::abs(msgs.at(mt).getTime().toSec() - meas_time) < 0.02) + cam_idt_idx = mt; + break; + } + if (cam_idt_idx != -1) { + camid_to_msg_index.insert({cam_idt, cam_idt_idx}); + } } - } else { - - // Find the camera which should be processed (smallest time) - int smallest_cam = 0; - for (int i = 0; i < params.state_options.num_cameras; i++) { - double time_cam0 = msg_images_current.at(smallest_cam)->header.stamp.toSec(); - double time_cam1 = msg_images_current.at(i)->header.stamp.toSec(); - if (time_cam1 < time_cam0) { - smallest_cam = i; - } + // Skip processing if we were unable to find any messages + if ((int)camid_to_msg_index.size() != params.state_options.num_cameras) { + PRINT_DEBUG(YELLOW "[SERIAL]: Unable to find stereo pair for message %d at %.2f into bag (will skip!)\n" RESET, m, + meas_time - time_init.toSec()); + continue; } // Check if we should initialize using the groundtruth - auto msg_camera = msg_images_current.at(smallest_cam); Eigen::Matrix imustate; - if (!gt_states.empty() && !sys->initialized() && - ov_core::DatasetReader::get_gt_state(msg_camera->header.stamp.toSec(), imustate, gt_states)) { + if (!gt_states.empty() && !sys->initialized() && ov_core::DatasetReader::get_gt_state(meas_time, imustate, gt_states)) { // biases are pretty bad normally, so zero them // imustate.block(11,0,6,1).setZero(); sys->initialize_with_gt(imustate); } - // Check if we should feed this into the system at the specified frequency - double timestamp = msg_camera->header.stamp.toSec(); - double time_delta = 1.0 / params.track_frequency; - if (camera_last_timestamp.find(smallest_cam) == camera_last_timestamp.end() || - timestamp >= camera_last_timestamp.at(smallest_cam) + time_delta) { - camera_last_timestamp[smallest_cam] = timestamp; - viz->callback_monocular(msg_camera, smallest_cam); + // Pass our data into our visualizer callbacks! + // PRINT_DEBUG("processing cam = %.3f sec\n", msgs.at(m).getTime().toSec() - time_init.toSec()); + if (params.state_options.num_cameras == 1) { + viz->callback_monocular(msgs.at(camid_to_msg_index.at(0)).instantiate(), 0); + } else if (params.state_options.num_cameras == 2) { + auto msg0 = msgs.at(camid_to_msg_index.at(0)); + auto msg1 = msgs.at(camid_to_msg_index.at(1)); + used_index.insert(camid_to_msg_index.at(0)); // skip this message + used_index.insert(camid_to_msg_index.at(1)); // skip this message + viz->callback_stereo(msg0.instantiate(), msg1.instantiate(), 0, 1); + } else { + PRINT_ERROR(RED "[SERIAL]: We currently only support 1 or 2 camera serial input....\n" RESET); + return EXIT_FAILURE; } - // move forward - msg_images_current.at(smallest_cam) = msg_images_next.at(smallest_cam); - view_cameras_iterators.at(smallest_cam)++; - if (view_cameras_iterators.at(smallest_cam) != view_cameras.at(smallest_cam)->end()) { - msg_images_next.at(smallest_cam) = view_cameras_iterators.at(smallest_cam)->instantiate(); - } + break; } } diff --git a/ov_msckf/src/run_simulation.cpp b/ov_msckf/src/run_simulation.cpp index e5403a1bd..d335c80e6 100644 --- a/ov_msckf/src/run_simulation.cpp +++ b/ov_msckf/src/run_simulation.cpp @@ -91,7 +91,8 @@ int main(int argc, char **argv) { VioManagerOptions params; params.print_and_load(parser); params.print_and_load_simulation(parser); - params.use_multi_threading = false; + params.use_multi_threading = false; // for repeatability + params.use_multi_threading_subs = false; sim = std::make_shared(params); sys = std::make_shared(params); #if ROS_AVAILABLE == 1 @@ -111,8 +112,10 @@ int main(int argc, char **argv) { //=================================================================================== // Get initial state + // NOTE: we are getting it at the *next* timestep so we get the first IMU message + double next_imu_time = sim->current_timestamp() + 1.0 / params.sim_freq_imu; Eigen::Matrix imustate; - bool success = sim->get_state(sim->current_timestamp(), imustate); + bool success = sim->get_state(next_imu_time, imustate); if (!success) { PRINT_ERROR(RED "[SIM]: Could not initialize the filter to the first state\n" RESET); PRINT_ERROR(RED "[SIM]: Did the simulator load properly???\n" RESET); diff --git a/ov_msckf/src/run_subscribe_msckf.cpp b/ov_msckf/src/run_subscribe_msckf.cpp index a19337d57..420581520 100644 --- a/ov_msckf/src/run_subscribe_msckf.cpp +++ b/ov_msckf/src/run_subscribe_msckf.cpp @@ -82,6 +82,7 @@ int main(int argc, char **argv) { // Create our VIO system VioManagerOptions params; params.print_and_load(parser); + params.use_multi_threading_subs = true; sys = std::make_shared(params); #if ROS_AVAILABLE == 1 viz = std::make_shared(nh, sys); diff --git a/ov_msckf/src/sim/Simulator.cpp b/ov_msckf/src/sim/Simulator.cpp index 106311883..794511a44 100644 --- a/ov_msckf/src/sim/Simulator.cpp +++ b/ov_msckf/src/sim/Simulator.cpp @@ -21,6 +21,13 @@ #include "Simulator.h" +#include "cam/CamBase.h" +#include "cam/CamEqui.h" +#include "cam/CamRadtan.h" +#include "sim/BsplineSE3.h" +#include "utils/colors.h" +#include "utils/dataset_reader.h" + using namespace ov_core; using namespace ov_msckf; @@ -52,17 +59,18 @@ Simulator::Simulator(VioManagerOptions ¶ms_) { // Load the groundtruth trajectory and its spline DatasetReader::load_simulated_trajectory(params.sim_traj_path, traj_data); - spline.feed_trajectory(traj_data); + spline = std::make_shared(); + spline->feed_trajectory(traj_data); // Set all our timestamps as starting from the minimum spline time - timestamp = spline.get_start_time(); - timestamp_last_imu = spline.get_start_time(); - timestamp_last_cam = spline.get_start_time(); + timestamp = spline->get_start_time(); + timestamp_last_imu = spline->get_start_time(); + timestamp_last_cam = spline->get_start_time(); // Get the pose at the current timestep Eigen::Matrix3d R_GtoI_init; Eigen::Vector3d p_IinG_init; - bool success_pose_init = spline.get_pose(timestamp, R_GtoI_init, p_IinG_init); + bool success_pose_init = spline->get_pose(timestamp, R_GtoI_init, p_IinG_init); if (!success_pose_init) { PRINT_ERROR(RED "[SIM]: unable to find the first pose in the spline...\n" RESET); std::exit(EXIT_FAILURE); @@ -76,7 +84,7 @@ Simulator::Simulator(VioManagerOptions ¶ms_) { // Get the pose at the current timestep Eigen::Matrix3d R_GtoI; Eigen::Vector3d p_IinG; - bool success_pose = spline.get_pose(timestamp, R_GtoI, p_IinG); + bool success_pose = spline->get_pose(timestamp, R_GtoI, p_IinG); // Check if it fails if (!success_pose) { @@ -97,7 +105,7 @@ Simulator::Simulator(VioManagerOptions ¶ms_) { timestamp_last_cam += 1.0 / params.sim_freq_cam; } } - PRINT_DEBUG("[SIM]: moved %.3f seconds into the dataset where it starts moving\n", timestamp - spline.get_start_time()); + PRINT_DEBUG("[SIM]: moved %.3f seconds into the dataset where it starts moving\n", timestamp - spline->get_start_time()); // Append the current true bias to our history hist_true_bias_time.push_back(timestamp_last_imu - 1.0 / params.sim_freq_imu); @@ -106,6 +114,9 @@ Simulator::Simulator(VioManagerOptions ¶ms_) { hist_true_bias_time.push_back(timestamp_last_imu); hist_true_bias_accel.push_back(true_bias_accel); hist_true_bias_gyro.push_back(true_bias_gyro); + hist_true_bias_time.push_back(timestamp_last_imu + 1.0 / params.sim_freq_imu); + hist_true_bias_accel.push_back(true_bias_accel); + hist_true_bias_gyro.push_back(true_bias_gyro); // Our simulation is running is_running = true; @@ -159,7 +170,7 @@ Simulator::Simulator(VioManagerOptions ¶ms_) { for (int i = 0; i < params.state_options.num_cameras; i++) { // Reset the start time - double time_synth = spline.get_start_time(); + double time_synth = spline->get_start_time(); // Loop through each pose and generate our feature map in them!!!! while (true) { @@ -167,7 +178,7 @@ Simulator::Simulator(VioManagerOptions ¶ms_) { // Get the pose at the current timestep Eigen::Matrix3d R_GtoI; Eigen::Vector3d p_IinG; - bool success_pose = spline.get_pose(time_synth, R_GtoI, p_IinG); + bool success_pose = spline->get_pose(time_synth, R_GtoI, p_IinG); // We have finished generating features if (!success_pose) @@ -186,7 +197,7 @@ Simulator::Simulator(VioManagerOptions ¶ms_) { // Debug print PRINT_DEBUG("[SIM]: Generated %d map features in total over %d frames (camera %d)\n", (int)(featmap.size() - mapsize), - (int)((time_synth - spline.get_start_time()) / dt), i); + (int)((time_synth - spline->get_start_time()) / dt), i); mapsize = featmap.size(); } @@ -239,7 +250,7 @@ bool Simulator::get_state(double desired_time, Eigen::Matrix &imu Eigen::Vector3d p_IinG, w_IinI, v_IinG; // Get the pose, velocity, and acceleration - bool success_vel = spline.get_velocity(desired_time, R_GtoI, p_IinG, w_IinI, v_IinG); + bool success_vel = spline->get_velocity(desired_time, R_GtoI, p_IinG, w_IinI, v_IinG); // Find the bounding bias values bool success_bias = false; @@ -290,8 +301,8 @@ bool Simulator::get_next_imu(double &time_imu, Eigen::Vector3d &wm, Eigen::Vecto // Get the pose, velocity, and acceleration // NOTE: we get the acceleration between our two IMU // NOTE: this is because we are using a constant measurement model for integration - // bool success_accel = spline.get_acceleration(timestamp+0.5/freq_imu, R_GtoI, p_IinG, w_IinI, v_IinG, alpha_IinI, a_IinG); - bool success_accel = spline.get_acceleration(timestamp, R_GtoI, p_IinG, w_IinI, v_IinG, alpha_IinI, a_IinG); + // bool success_accel = spline->get_acceleration(timestamp+0.5/freq_imu, R_GtoI, p_IinG, w_IinI, v_IinG, alpha_IinI, a_IinG); + bool success_accel = spline->get_acceleration(timestamp, R_GtoI, p_IinG, w_IinI, v_IinG, alpha_IinI, a_IinG); // If failed, then that means we don't have any more spline // Thus we should stop the simulation @@ -306,9 +317,28 @@ bool Simulator::get_next_imu(double &time_imu, Eigen::Vector3d &wm, Eigen::Vecto gravity << 0.0, 0.0, params.gravity_mag; Eigen::Vector3d accel_inI = R_GtoI * (a_IinG + gravity); - // Now add noise to these measurements + // Calculate the bias values for this IMU reading + // NOTE: we skip the first ever bias since we have already appended it double dt = 1.0 / params.sim_freq_imu; std::normal_distribution w(0, 1); + if (has_skipped_first_bias) { + + // Move the biases forward in time + true_bias_gyro(0) += params.imu_noises.sigma_wb * std::sqrt(dt) * w(gen_meas_imu); + true_bias_gyro(1) += params.imu_noises.sigma_wb * std::sqrt(dt) * w(gen_meas_imu); + true_bias_gyro(2) += params.imu_noises.sigma_wb * std::sqrt(dt) * w(gen_meas_imu); + true_bias_accel(0) += params.imu_noises.sigma_ab * std::sqrt(dt) * w(gen_meas_imu); + true_bias_accel(1) += params.imu_noises.sigma_ab * std::sqrt(dt) * w(gen_meas_imu); + true_bias_accel(2) += params.imu_noises.sigma_ab * std::sqrt(dt) * w(gen_meas_imu); + + // Append the current true bias to our history + hist_true_bias_time.push_back(timestamp_last_imu); + hist_true_bias_gyro.push_back(true_bias_gyro); + hist_true_bias_accel.push_back(true_bias_accel); + } + has_skipped_first_bias = true; + + // Now add noise to these measurements wm(0) = omega_inI(0) + true_bias_gyro(0) + params.imu_noises.sigma_w / std::sqrt(dt) * w(gen_meas_imu); wm(1) = omega_inI(1) + true_bias_gyro(1) + params.imu_noises.sigma_w / std::sqrt(dt) * w(gen_meas_imu); wm(2) = omega_inI(2) + true_bias_gyro(2) + params.imu_noises.sigma_w / std::sqrt(dt) * w(gen_meas_imu); @@ -316,19 +346,6 @@ bool Simulator::get_next_imu(double &time_imu, Eigen::Vector3d &wm, Eigen::Vecto am(1) = accel_inI(1) + true_bias_accel(1) + params.imu_noises.sigma_a / std::sqrt(dt) * w(gen_meas_imu); am(2) = accel_inI(2) + true_bias_accel(2) + params.imu_noises.sigma_a / std::sqrt(dt) * w(gen_meas_imu); - // Move the biases forward in time - true_bias_gyro(0) += params.imu_noises.sigma_wb * std::sqrt(dt) * w(gen_meas_imu); - true_bias_gyro(1) += params.imu_noises.sigma_wb * std::sqrt(dt) * w(gen_meas_imu); - true_bias_gyro(2) += params.imu_noises.sigma_wb * std::sqrt(dt) * w(gen_meas_imu); - true_bias_accel(0) += params.imu_noises.sigma_ab * std::sqrt(dt) * w(gen_meas_imu); - true_bias_accel(1) += params.imu_noises.sigma_ab * std::sqrt(dt) * w(gen_meas_imu); - true_bias_accel(2) += params.imu_noises.sigma_ab * std::sqrt(dt) * w(gen_meas_imu); - - // Append the current true bias to our history - hist_true_bias_time.push_back(timestamp_last_imu); - hist_true_bias_gyro.push_back(true_bias_gyro); - hist_true_bias_accel.push_back(true_bias_accel); - // Return success return true; } @@ -348,7 +365,7 @@ bool Simulator::get_next_cam(double &time_cam, std::vector &camids, // Get the pose at the current timestep Eigen::Matrix3d R_GtoI; Eigen::Vector3d p_IinG; - bool success_pose = spline.get_pose(timestamp, R_GtoI, p_IinG); + bool success_pose = spline->get_pose(timestamp, R_GtoI, p_IinG); // We have finished generating measurements if (!success_pose) { diff --git a/ov_msckf/src/sim/Simulator.h b/ov_msckf/src/sim/Simulator.h index 65ef0a4ee..f5bb56217 100644 --- a/ov_msckf/src/sim/Simulator.h +++ b/ov_msckf/src/sim/Simulator.h @@ -30,15 +30,12 @@ #include #include -#include "cam/CamBase.h" -#include "cam/CamEqui.h" -#include "cam/CamRadtan.h" -#include "sim/BsplineSE3.h" -#include "utils/colors.h" -#include "utils/dataset_reader.h" - #include "core/VioManagerOptions.h" +namespace ov_core { +class BsplineSE3; +} // namespace ov_core + namespace ov_msckf { /** @@ -158,7 +155,7 @@ class Simulator { std::vector traj_data; /// Our b-spline trajectory - ov_core::BsplineSE3 spline; + std::shared_ptr spline; /// Our map of 3d features size_t id_map = 0; @@ -199,6 +196,7 @@ class Simulator { Eigen::Vector3d true_bias_gyro = Eigen::Vector3d::Zero(); // Our history of true biases + bool has_skipped_first_bias = false; std::vector hist_true_bias_time; std::vector hist_true_bias_accel; std::vector hist_true_bias_gyro; diff --git a/ov_msckf/src/state/Propagator.cpp b/ov_msckf/src/state/Propagator.cpp index 52f5730d3..22b572d21 100644 --- a/ov_msckf/src/state/Propagator.cpp +++ b/ov_msckf/src/state/Propagator.cpp @@ -21,6 +21,10 @@ #include "Propagator.h" +#include "state/State.h" +#include "state/StateHelper.h" +#include "utils/quat_ops.h" + using namespace ov_core; using namespace ov_type; using namespace ov_msckf; @@ -94,6 +98,7 @@ void Propagator::propagate_and_clone(std::shared_ptr state, double timest dt_summed += prop_data.at(i + 1).timestamp - prop_data.at(i).timestamp; } } + assert(std::abs((time1 - time0) - dt_summed) < 1e-4); // Last angular velocity (used for cloning when estimating time offset) Eigen::Matrix last_w = Eigen::Matrix::Zero(); @@ -208,7 +213,7 @@ bool Propagator::fast_state_propagate(std::shared_ptr state, double times Phi.block(6, 6, 3, 3) = quat_2_Rot(q_Gtoi); state_covariance = Phi * state_covariance * Phi.transpose(); covariance.block(0, 0, 9, 9) = state_covariance.block(0, 0, 9, 9); - double dt = prop_data.at(prop_data.size() - 1).timestamp + prop_data.at(prop_data.size() - 2).timestamp; + double dt = prop_data.at(prop_data.size() - 1).timestamp - prop_data.at(prop_data.size() - 2).timestamp; covariance.block(9, 9, 3, 3) = _noises.sigma_w_2 / dt * Eigen::Matrix3d::Identity(); return true; } @@ -237,8 +242,8 @@ std::vector Propagator::select_imu_readings(const std::vector< if (imu_data.at(i + 1).timestamp > time0 && imu_data.at(i).timestamp < time0) { ov_core::ImuData data = Propagator::interpolate_data(imu_data.at(i), imu_data.at(i + 1), time0); prop_data.push_back(data); - // PRINT_DEBUG("propagation #%d = CASE 1 = %.3f => %.3f\n", - // (int)i,data.timestamp-prop_data.at(0).timestamp,time0-prop_data.at(0).timestamp); + // PRINT_DEBUG("propagation #%d = CASE 1 = %.3f => %.3f\n", (int)i, data.timestamp - prop_data.at(0).timestamp, + // time0 - prop_data.at(0).timestamp); continue; } @@ -247,7 +252,7 @@ std::vector Propagator::select_imu_readings(const std::vector< // Then we should just append the whole measurement time to our propagation vector if (imu_data.at(i).timestamp >= time0 && imu_data.at(i + 1).timestamp <= time1) { prop_data.push_back(imu_data.at(i)); - // PRINT_DEBUG("propagation #%d = CASE 2 = %.3f\n",(int)i,imu_data.at(i).timestamp-prop_data.at(0).timestamp); + // PRINT_DEBUG("propagation #%d = CASE 2 = %.3f\n", (int)i, imu_data.at(i).timestamp - prop_data.at(0).timestamp); continue; } diff --git a/ov_msckf/src/state/Propagator.h b/ov_msckf/src/state/Propagator.h index e5794a61b..87b2e5f4f 100644 --- a/ov_msckf/src/state/Propagator.h +++ b/ov_msckf/src/state/Propagator.h @@ -22,15 +22,18 @@ #ifndef OV_MSCKF_STATE_PROPAGATOR_H #define OV_MSCKF_STATE_PROPAGATOR_H +#include #include -#include "state/StateHelper.h" +#include "utils/NoiseManager.h" + #include "utils/print.h" -#include "utils/quat_ops.h" #include "utils/sensor_data.h" namespace ov_msckf { +class State; + /** * @brief Performs the state covariance and mean propagation using imu measurements * @@ -41,44 +44,6 @@ namespace ov_msckf { class Propagator { public: - /** - * @brief Struct of our imu noise parameters - */ - struct NoiseManager { - - /// Gyroscope white noise (rad/s/sqrt(hz)) - double sigma_w = 1.6968e-04; - - /// Gyroscope white noise covariance - double sigma_w_2 = pow(1.6968e-04, 2); - - /// Gyroscope random walk (rad/s^2/sqrt(hz)) - double sigma_wb = 1.9393e-05; - - /// Gyroscope random walk covariance - double sigma_wb_2 = pow(1.9393e-05, 2); - - /// Accelerometer white noise (m/s^2/sqrt(hz)) - double sigma_a = 2.0000e-3; - - /// Accelerometer white noise covariance - double sigma_a_2 = pow(2.0000e-3, 2); - - /// Accelerometer random walk (m/s^3/sqrt(hz)) - double sigma_ab = 3.0000e-03; - - /// Accelerometer random walk covariance - double sigma_ab_2 = pow(3.0000e-03, 2); - - /// Nice print function of what parameters we have loaded - void print() { - PRINT_DEBUG(" - gyroscope_noise_density: %.6f\n", sigma_w); - PRINT_DEBUG(" - accelerometer_noise_density: %.5f\n", sigma_a); - PRINT_DEBUG(" - gyroscope_random_walk: %.7f\n", sigma_wb); - PRINT_DEBUG(" - accelerometer_random_walk: %.6f\n", sigma_ab); - } - }; - /** * @brief Default constructor * @param noises imu noise characteristics (continuous time) @@ -108,7 +73,7 @@ class Propagator { if (oldest_time != -1) { auto it0 = imu_data.begin(); while (it0 != imu_data.end()) { - if (message.timestamp < oldest_time) { + if (it0->timestamp < oldest_time - 0.10) { it0 = imu_data.erase(it0); } else { it0++; diff --git a/ov_msckf/src/state/State.cpp b/ov_msckf/src/state/State.cpp index f526775ab..24dde8d24 100644 --- a/ov_msckf/src/state/State.cpp +++ b/ov_msckf/src/state/State.cpp @@ -74,7 +74,7 @@ State::State(StateOptions &options) { } // Finally initialize our covariance to small value - _Cov = 1e-3 * Eigen::MatrixXd::Identity(current_id, current_id); + _Cov = std::pow(1e-3, 2) * Eigen::MatrixXd::Identity(current_id, current_id); // Finally, set some of our priors for our calibration parameters if (_options.do_calib_camera_timeoffset) { @@ -82,7 +82,7 @@ State::State(StateOptions &options) { } if (_options.do_calib_camera_pose) { for (int i = 0; i < _options.num_cameras; i++) { - _Cov.block(_calib_IMUtoCAM.at(i)->id(), _calib_IMUtoCAM.at(i)->id(), 3, 3) = std::pow(0.001, 2) * Eigen::MatrixXd::Identity(3, 3); + _Cov.block(_calib_IMUtoCAM.at(i)->id(), _calib_IMUtoCAM.at(i)->id(), 3, 3) = std::pow(0.005, 2) * Eigen::MatrixXd::Identity(3, 3); _Cov.block(_calib_IMUtoCAM.at(i)->id() + 3, _calib_IMUtoCAM.at(i)->id() + 3, 3, 3) = std::pow(0.01, 2) * Eigen::MatrixXd::Identity(3, 3); } diff --git a/ov_msckf/src/state/StateHelper.cpp b/ov_msckf/src/state/StateHelper.cpp index 4013245c2..d5da933d2 100644 --- a/ov_msckf/src/state/StateHelper.cpp +++ b/ov_msckf/src/state/StateHelper.cpp @@ -21,6 +21,14 @@ #include "StateHelper.h" +#include "state/State.h" + +#include "types/Landmark.h" +#include "utils/colors.h" +#include "utils/print.h" + +#include + using namespace ov_core; using namespace ov_type; using namespace ov_msckf; @@ -54,10 +62,10 @@ void StateHelper::EKFPropagation(std::shared_ptr state, const std::vector } // Assert that we have correct sizes - assert(size_order_NEW == Phi.rows()); - assert(size_order_OLD == Phi.cols()); - assert(size_order_NEW == Q.cols()); - assert(size_order_NEW == Q.rows()); + assert_r(size_order_NEW == Phi.rows()); + assert_r(size_order_OLD == Phi.cols()); + assert_r(size_order_NEW == Q.cols()); + assert_r(size_order_NEW == Q.rows()); // Get the location in small phi for each measuring variable int current_it = 0; @@ -100,7 +108,7 @@ void StateHelper::EKFPropagation(std::shared_ptr state, const std::vector found_neg = true; } } - assert(!found_neg); + assert_r(!found_neg); } void StateHelper::EKFUpdate(std::shared_ptr state, const std::vector> &H_order, const Eigen::MatrixXd &H, @@ -109,8 +117,8 @@ void StateHelper::EKFUpdate(std::shared_ptr state, const std::vector_Cov.rows(), res.rows()); // Get the location in small jacobian for each measuring variable @@ -167,7 +175,7 @@ void StateHelper::EKFUpdate(std::shared_ptr state, const std::vector state, std::shared_ptr_Cov.resize(Cov_new.rows(),Cov_new.cols()); state->_Cov = Cov_new; // state->Cov() = 0.5*(Cov_new+Cov_new.transpose()); - assert(state->_Cov.rows() == Cov_new.rows()); + assert_r(state->_Cov.rows() == Cov_new.rows()); // Now we keep the remaining variables and update their ordering // Note: DOES NOT SUPPORT MARGINALIZING SUBVARIABLES YET!!!!!!! @@ -391,8 +399,8 @@ bool StateHelper::initialize(std::shared_ptr state, std::shared_ptr // Check that we have isotropic noise (i.e. is diagonal and all the same value) // TODO: can we simplify this so it doesn't take as much time? - assert(R.rows() == R.cols()); - assert(R.rows() > 0); + assert_r(R.rows() == R.cols()); + assert_r(R.rows() > 0); for (int r = 0; r < R.rows(); r++) { for (int c = 0; c < R.cols(); c++) { if (r == c && R(0, 0) != R(r, c)) { @@ -412,7 +420,7 @@ bool StateHelper::initialize(std::shared_ptr state, std::shared_ptr // First we perform QR givens to seperate the system // The top will be a system that depends on the new state, while the bottom does not size_t new_var_size = new_variable->size(); - assert((int)new_var_size == H_L.cols()); + assert_r((int)new_var_size == H_L.cols()); Eigen::JacobiRotation tempHo_GR; for (int n = 0; n < H_L.cols(); ++n) { @@ -445,8 +453,8 @@ bool StateHelper::initialize(std::shared_ptr state, std::shared_ptr // Do mahalanobis distance testing Eigen::MatrixXd P_up = get_marginal_covariance(state, H_order); - assert(Rup.rows() == Hup.rows()); - assert(Hup.cols() == P_up.cols()); + assert_r(Rup.rows() == Hup.rows()); + assert_r(Hup.cols() == P_up.cols()); Eigen::MatrixXd S = Hup * P_up * Hup.transpose() + Rup; double chi2 = resup.dot(S.llt().solve(resup)); @@ -482,8 +490,8 @@ void StateHelper::initialize_invertible(std::shared_ptr state, std::share // Check that we have isotropic noise (i.e. is diagonal and all the same value) // TODO: can we simplify this so it doesn't take as much time? - assert(R.rows() == R.cols()); - assert(R.rows() > 0); + assert_r(R.rows() == R.cols()); + assert_r(R.rows() > 0); for (int r = 0; r < R.rows(); r++) { for (int c = 0; c < R.cols(); c++) { if (r == c && R(0, 0) != R(r, c)) { @@ -501,9 +509,9 @@ void StateHelper::initialize_invertible(std::shared_ptr state, std::share //========================================================== //========================================================== // Part of the Kalman Gain K = (P*H^T)*S^{-1} = M*S^{-1} - assert(res.rows() == R.rows()); - assert(H_L.rows() == res.rows()); - assert(H_L.rows() == H_R.rows()); + assert_r(res.rows() == R.rows()); + assert_r(H_L.rows() == res.rows()); + assert_r(H_L.rows() == H_R.rows()); Eigen::MatrixXd M_a = Eigen::MatrixXd::Zero(state->_Cov.rows(), res.rows()); // Get the location in small jacobian for each measuring variable @@ -539,8 +547,8 @@ void StateHelper::initialize_invertible(std::shared_ptr state, std::share M.triangularView() += R; // Covariance of the variable/landmark that will be initialized - assert(H_L.rows() == H_L.cols()); - assert(H_L.rows() == new_variable->size()); + assert_r(H_L.rows() == H_L.cols()); + assert_r(H_L.rows() == new_variable->size()); Eigen::MatrixXd H_Linv = H_L.inverse(); Eigen::MatrixXd P_LL = H_Linv * M.selfadjointView() * H_Linv.transpose(); @@ -595,9 +603,35 @@ void StateHelper::augment_clone(std::shared_ptr state, Eigen::Matrix_imu->vel(); // Augment covariance with time offset Jacobian + // TODO: replace this with a call to the EKFPropagate function instead.... state->_Cov.block(0, pose->id(), state->_Cov.rows(), 6) += state->_Cov.block(0, state->_calib_dt_CAMtoIMU->id(), state->_Cov.rows(), 1) * dnc_dt.transpose(); state->_Cov.block(pose->id(), 0, 6, state->_Cov.rows()) += dnc_dt * state->_Cov.block(state->_calib_dt_CAMtoIMU->id(), 0, 1, state->_Cov.rows()); } } + +void StateHelper::marginalize_old_clone(std::shared_ptr state) { + if ((int)state->_clones_IMU.size() > state->_options.max_clone_size) { + double marginal_time = state->margtimestep(); + assert_r(marginal_time != INFINITY); + StateHelper::marginalize(state, state->_clones_IMU.at(marginal_time)); + // Note that the marginalizer should have already deleted the clone + // Thus we just need to remove the pointer to it from our state + state->_clones_IMU.erase(marginal_time); + } +} + +void StateHelper::marginalize_slam(std::shared_ptr state) { + // Remove SLAM features that have their marginalization flag set + // We also check that we do not remove any aruoctag landmarks + auto it0 = state->_features_SLAM.begin(); + while (it0 != state->_features_SLAM.end()) { + if ((*it0).second->should_marg && (int)(*it0).first > 4 * state->_options.max_aruco_features) { + StateHelper::marginalize(state, (*it0).second); + it0 = state->_features_SLAM.erase(it0); + } else { + it0++; + } + } +} \ No newline at end of file diff --git a/ov_msckf/src/state/StateHelper.h b/ov_msckf/src/state/StateHelper.h index e5122ba38..f164a154b 100644 --- a/ov_msckf/src/state/StateHelper.h +++ b/ov_msckf/src/state/StateHelper.h @@ -22,15 +22,17 @@ #ifndef OV_MSCKF_STATE_HELPER_H #define OV_MSCKF_STATE_HELPER_H -#include "State.h" -#include "types/Landmark.h" -#include "utils/colors.h" -#include "utils/print.h" +#include +#include -#include +namespace ov_type { +class Type; +} // namespace ov_type namespace ov_msckf { +class State; + /** * @brief Helper which manipulates the State and its covariance. * @@ -218,34 +220,13 @@ class StateHelper { * * @param state Pointer to state */ - static void marginalize_old_clone(std::shared_ptr state) { - if ((int)state->_clones_IMU.size() > state->_options.max_clone_size) { - double marginal_time = state->margtimestep(); - assert(marginal_time != INFINITY); - StateHelper::marginalize(state, state->_clones_IMU.at(marginal_time)); - // Note that the marginalizer should have already deleted the clone - // Thus we just need to remove the pointer to it from our state - state->_clones_IMU.erase(marginal_time); - } - } + static void marginalize_old_clone(std::shared_ptr state); /** * @brief Marginalize bad SLAM features * @param state Pointer to state */ - static void marginalize_slam(std::shared_ptr state) { - // Remove SLAM features that have their marginalization flag set - // We also check that we do not remove any aruoctag landmarks - auto it0 = state->_features_SLAM.begin(); - while (it0 != state->_features_SLAM.end()) { - if ((*it0).second->should_marg && (int)(*it0).first > 4 * state->_options.max_aruco_features) { - StateHelper::marginalize(state, (*it0).second); - it0 = state->_features_SLAM.erase(it0); - } else { - it0++; - } - } - } + static void marginalize_slam(std::shared_ptr state); private: /** diff --git a/ov_msckf/src/update/UpdaterHelper.cpp b/ov_msckf/src/update/UpdaterHelper.cpp index 4282e4c24..66ce153a7 100644 --- a/ov_msckf/src/update/UpdaterHelper.cpp +++ b/ov_msckf/src/update/UpdaterHelper.cpp @@ -21,6 +21,10 @@ #include "UpdaterHelper.h" +#include "state/State.h" + +#include "utils/quat_ops.h" + using namespace ov_core; using namespace ov_type; using namespace ov_msckf; @@ -301,7 +305,7 @@ void UpdaterHelper::get_feature_jacobian_full(std::shared_ptr state, Upda // Assert that all the ones in our order are already in our local jacobian mapping for (auto &type : dpfg_dx_order) { - assert(map_hx.find(type) != map_hx.end()); + assert_r(map_hx.find(type) != map_hx.end()); } // Loop through each camera for this feature diff --git a/ov_msckf/src/update/UpdaterHelper.h b/ov_msckf/src/update/UpdaterHelper.h index f3406cc8e..2f1048964 100644 --- a/ov_msckf/src/update/UpdaterHelper.h +++ b/ov_msckf/src/update/UpdaterHelper.h @@ -23,16 +23,19 @@ #define OV_MSCKF_UPDATER_HELPER_H #include +#include +#include -#include "feat/Feature.h" -#include "state/State.h" -#include "state/StateOptions.h" #include "types/LandmarkRepresentation.h" -#include "utils/colors.h" -#include "utils/quat_ops.h" + +namespace ov_type { +class Type; +} // namespace ov_type namespace ov_msckf { +class State; + /** * @brief Class that has helper functions for our updaters. * diff --git a/ov_msckf/src/update/UpdaterMSCKF.cpp b/ov_msckf/src/update/UpdaterMSCKF.cpp index 3f942cfe8..25bee2a4a 100644 --- a/ov_msckf/src/update/UpdaterMSCKF.cpp +++ b/ov_msckf/src/update/UpdaterMSCKF.cpp @@ -21,10 +21,40 @@ #include "UpdaterMSCKF.h" +#include "UpdaterHelper.h" + +#include "feat/Feature.h" +#include "feat/FeatureInitializer.h" +#include "state/State.h" +#include "state/StateHelper.h" +#include "types/LandmarkRepresentation.h" +#include "utils/colors.h" +#include "utils/print.h" +#include "utils/quat_ops.h" + +#include +#include + using namespace ov_core; using namespace ov_type; using namespace ov_msckf; +UpdaterMSCKF::UpdaterMSCKF(UpdaterOptions &options, ov_core::FeatureInitializerOptions &feat_init_options) : _options(options) { + + // Save our raw pixel noise squared + _options.sigma_pix_sq = std::pow(_options.sigma_pix, 2); + + // Save our feature initializer + initializer_feat = std::shared_ptr(new ov_core::FeatureInitializer(feat_init_options)); + + // Initialize the chi squared test table with confidence level 0.95 + // https://github.com/KumarRobotics/msckf_vio/blob/050c50defa5a7fd9a04c1eed5687b405f02919b5/src/msckf_vio.cpp#L215-L221 + for (int i = 1; i < 500; i++) { + boost::math::chi_squared chi_squared_dist(i); + chi_squared_table[i] = boost::math::quantile(chi_squared_dist, 0.95); + } +} + void UpdaterMSCKF::update(std::shared_ptr state, std::vector> &feature_vec) { // Return if no features @@ -91,15 +121,15 @@ void UpdaterMSCKF::update(std::shared_ptr state, std::vectorconfig().triangulate_1d) { - success_tri = initializer_feat->single_triangulation_1d(it1->get(), clones_cam); + success_tri = initializer_feat->single_triangulation_1d(*it1, clones_cam); } else { - success_tri = initializer_feat->single_triangulation(it1->get(), clones_cam); + success_tri = initializer_feat->single_triangulation(*it1, clones_cam); } // Gauss-newton refine the feature bool success_refine = true; if (initializer_feat->config().refine_features) { - success_refine = initializer_feat->single_gaussnewton(it1->get(), clones_cam); + success_refine = initializer_feat->single_gaussnewton(*it1, clones_cam); } // Remove the feature if not a success diff --git a/ov_msckf/src/update/UpdaterMSCKF.h b/ov_msckf/src/update/UpdaterMSCKF.h index 83ac4c4c8..73ab02ffb 100644 --- a/ov_msckf/src/update/UpdaterMSCKF.h +++ b/ov_msckf/src/update/UpdaterMSCKF.h @@ -23,27 +23,21 @@ #define OV_MSCKF_UPDATER_MSCKF_H #include -#include -#include +#include -#include "feat/Feature.h" -#include "feat/FeatureInitializer.h" #include "feat/FeatureInitializerOptions.h" -#include "state/State.h" -#include "state/StateHelper.h" -#include "types/LandmarkRepresentation.h" -#include "utils/colors.h" -#include "utils/print.h" -#include "utils/quat_ops.h" -#include "UpdaterHelper.h" #include "UpdaterOptions.h" -#include -#include +namespace ov_core { +class Feature; +class FeatureInitializer; +} // namespace ov_core namespace ov_msckf { +class State; + /** * @brief Will compute the system for our sparse features and update the filter. * @@ -63,21 +57,7 @@ class UpdaterMSCKF { * @param options Updater options (include measurement noise value) * @param feat_init_options Feature initializer options */ - UpdaterMSCKF(UpdaterOptions &options, ov_core::FeatureInitializerOptions &feat_init_options) : _options(options) { - - // Save our raw pixel noise squared - _options.sigma_pix_sq = std::pow(_options.sigma_pix, 2); - - // Save our feature initializer - initializer_feat = std::unique_ptr(new ov_core::FeatureInitializer(feat_init_options)); - - // Initialize the chi squared test table with confidence level 0.95 - // https://github.com/KumarRobotics/msckf_vio/blob/050c50defa5a7fd9a04c1eed5687b405f02919b5/src/msckf_vio.cpp#L215-L221 - for (int i = 1; i < 500; i++) { - boost::math::chi_squared chi_squared_dist(i); - chi_squared_table[i] = boost::math::quantile(chi_squared_dist, 0.95); - } - } + UpdaterMSCKF(UpdaterOptions &options, ov_core::FeatureInitializerOptions &feat_init_options); /** * @brief Given tracked features, this will try to use them to update the state. @@ -92,7 +72,7 @@ class UpdaterMSCKF { UpdaterOptions _options; /// Feature initializer class object - std::unique_ptr initializer_feat; + std::shared_ptr initializer_feat; /// Chi squared 95th percentile table (lookup would be size of residual) std::map chi_squared_table; diff --git a/ov_msckf/src/update/UpdaterSLAM.cpp b/ov_msckf/src/update/UpdaterSLAM.cpp index 9b4c868b7..b9cfdfd75 100644 --- a/ov_msckf/src/update/UpdaterSLAM.cpp +++ b/ov_msckf/src/update/UpdaterSLAM.cpp @@ -21,10 +21,43 @@ #include "UpdaterSLAM.h" +#include "UpdaterHelper.h" + +#include "feat/Feature.h" +#include "feat/FeatureInitializer.h" +#include "state/State.h" +#include "state/StateHelper.h" +#include "types/Landmark.h" +#include "types/LandmarkRepresentation.h" +#include "utils/colors.h" +#include "utils/print.h" +#include "utils/quat_ops.h" + +#include +#include + using namespace ov_core; using namespace ov_type; using namespace ov_msckf; +UpdaterSLAM::UpdaterSLAM(UpdaterOptions &options_slam, UpdaterOptions &options_aruco, ov_core::FeatureInitializerOptions &feat_init_options) + : _options_slam(options_slam), _options_aruco(options_aruco) { + + // Save our raw pixel noise squared + _options_slam.sigma_pix_sq = std::pow(_options_slam.sigma_pix, 2); + _options_aruco.sigma_pix_sq = std::pow(_options_aruco.sigma_pix, 2); + + // Save our feature initializer + initializer_feat = std::shared_ptr(new ov_core::FeatureInitializer(feat_init_options)); + + // Initialize the chi squared test table with confidence level 0.95 + // https://github.com/KumarRobotics/msckf_vio/blob/050c50defa5a7fd9a04c1eed5687b405f02919b5/src/msckf_vio.cpp#L215-L221 + for (int i = 1; i < 500; i++) { + boost::math::chi_squared chi_squared_dist(i); + chi_squared_table[i] = boost::math::quantile(chi_squared_dist, 0.95); + } +} + void UpdaterSLAM::delayed_init(std::shared_ptr state, std::vector> &feature_vec) { // Return if no features @@ -91,15 +124,15 @@ void UpdaterSLAM::delayed_init(std::shared_ptr state, std::vectorconfig().triangulate_1d) { - success_tri = initializer_feat->single_triangulation_1d(it1->get(), clones_cam); + success_tri = initializer_feat->single_triangulation_1d(*it1, clones_cam); } else { - success_tri = initializer_feat->single_triangulation(it1->get(), clones_cam); + success_tri = initializer_feat->single_triangulation(*it1, clones_cam); } // Gauss-newton refine the feature bool success_refine = true; if (initializer_feat->config().refine_features) { - success_refine = initializer_feat->single_gaussnewton(it1->get(), clones_cam); + success_refine = initializer_feat->single_gaussnewton(*it1, clones_cam); } // Remove the feature if not a success @@ -375,9 +408,12 @@ void UpdaterSLAM::update(std::shared_ptr state, std::vector_options.max_aruco_features) ? _options_aruco.chi2_multipler : _options_slam.chi2_multipler; if (chi2 > chi2_multipler * chi2_check) { - if ((int)feat.featid < state->_options.max_aruco_features) + if ((int)feat.featid < state->_options.max_aruco_features) { PRINT_WARNING(YELLOW "[SLAM-UP]: rejecting aruco tag %d for chi2 thresh (%.3f > %.3f)\n" RESET, (int)feat.featid, chi2, chi2_multipler * chi2_check); + } else { + landmark->should_marg = true; + } (*it2)->to_delete = true; it2 = feature_vec.erase(it2); continue; diff --git a/ov_msckf/src/update/UpdaterSLAM.h b/ov_msckf/src/update/UpdaterSLAM.h index 7757684a4..1db0f1727 100644 --- a/ov_msckf/src/update/UpdaterSLAM.h +++ b/ov_msckf/src/update/UpdaterSLAM.h @@ -25,25 +25,22 @@ #include #include -#include "feat/Feature.h" -#include "feat/FeatureInitializer.h" #include "feat/FeatureInitializerOptions.h" -#include "state/State.h" -#include "state/StateHelper.h" -#include "types/Landmark.h" -#include "types/LandmarkRepresentation.h" -#include "utils/colors.h" -#include "utils/print.h" -#include "utils/quat_ops.h" - -#include "UpdaterHelper.h" + #include "UpdaterOptions.h" -#include -#include +namespace ov_core { +class Feature; +class FeatureInitializer; +} // namespace ov_core +namespace ov_type { +class Landmark; +} // namespace ov_type namespace ov_msckf { +class State; + /** * @brief Will compute the system for our sparse SLAM features and update the filter. * @@ -63,23 +60,7 @@ class UpdaterSLAM { * @param options_aruco Updater options (include measurement noise value) for ARUCO features * @param feat_init_options Feature initializer options */ - UpdaterSLAM(UpdaterOptions &options_slam, UpdaterOptions &options_aruco, ov_core::FeatureInitializerOptions &feat_init_options) - : _options_slam(options_slam), _options_aruco(options_aruco) { - - // Save our raw pixel noise squared - _options_slam.sigma_pix_sq = std::pow(_options_slam.sigma_pix, 2); - _options_aruco.sigma_pix_sq = std::pow(_options_aruco.sigma_pix, 2); - - // Save our feature initializer - initializer_feat = std::unique_ptr(new ov_core::FeatureInitializer(feat_init_options)); - - // Initialize the chi squared test table with confidence level 0.95 - // https://github.com/KumarRobotics/msckf_vio/blob/050c50defa5a7fd9a04c1eed5687b405f02919b5/src/msckf_vio.cpp#L215-L221 - for (int i = 1; i < 500; i++) { - boost::math::chi_squared chi_squared_dist(i); - chi_squared_table[i] = boost::math::quantile(chi_squared_dist, 0.95); - } - } + UpdaterSLAM(UpdaterOptions &options_slam, UpdaterOptions &options_aruco, ov_core::FeatureInitializerOptions &feat_init_options); /** * @brief Given tracked SLAM features, this will try to use them to update the state. @@ -123,7 +104,7 @@ class UpdaterSLAM { UpdaterOptions _options_aruco; /// Feature initializer class object - std::unique_ptr initializer_feat; + std::shared_ptr initializer_feat; /// Chi squared 95th percentile table (lookup would be size of residual) std::map chi_squared_table; diff --git a/ov_msckf/src/update/UpdaterZeroVelocity.cpp b/ov_msckf/src/update/UpdaterZeroVelocity.cpp index 659641116..f55cb40c3 100644 --- a/ov_msckf/src/update/UpdaterZeroVelocity.cpp +++ b/ov_msckf/src/update/UpdaterZeroVelocity.cpp @@ -21,10 +21,45 @@ #include "UpdaterZeroVelocity.h" +#include "feat/FeatureDatabase.h" +#include "feat/FeatureHelper.h" +#include "state/Propagator.h" +#include "state/State.h" +#include "state/StateHelper.h" +#include "utils/colors.h" +#include "utils/print.h" +#include "utils/quat_ops.h" + +#include +#include + using namespace ov_core; using namespace ov_type; using namespace ov_msckf; +UpdaterZeroVelocity::UpdaterZeroVelocity(UpdaterOptions &options, NoiseManager &noises, std::shared_ptr db, + std::shared_ptr prop, double gravity_mag, double zupt_max_velocity, + double zupt_noise_multiplier, double zupt_max_disparity) + : _options(options), _noises(noises), _db(db), _prop(prop), _zupt_max_velocity(zupt_max_velocity), + _zupt_noise_multiplier(zupt_noise_multiplier), _zupt_max_disparity(zupt_max_disparity) { + + // Gravity + _gravity << 0.0, 0.0, gravity_mag; + + // Save our raw pixel noise squared + _noises.sigma_w_2 = std::pow(_noises.sigma_w, 2); + _noises.sigma_a_2 = std::pow(_noises.sigma_a, 2); + _noises.sigma_wb_2 = std::pow(_noises.sigma_wb, 2); + _noises.sigma_ab_2 = std::pow(_noises.sigma_ab, 2); + + // Initialize the chi squared test table with confidence level 0.95 + // https://github.com/KumarRobotics/msckf_vio/blob/050c50defa5a7fd9a04c1eed5687b405f02919b5/src/msckf_vio.cpp#L215-L221 + for (int i = 1; i < 1000; i++) { + boost::math::chi_squared chi_squared_dist(i); + chi_squared_table[i] = boost::math::quantile(chi_squared_dist, 0.95); + } +} + bool UpdaterZeroVelocity::try_update(std::shared_ptr state, double timestamp) { // Return if we don't have any imu data yet diff --git a/ov_msckf/src/update/UpdaterZeroVelocity.h b/ov_msckf/src/update/UpdaterZeroVelocity.h index 78aa35136..a7087f6dd 100644 --- a/ov_msckf/src/update/UpdaterZeroVelocity.h +++ b/ov_msckf/src/update/UpdaterZeroVelocity.h @@ -22,24 +22,26 @@ #ifndef OV_MSCKF_UPDATER_ZEROVELOCITY_H #define OV_MSCKF_UPDATER_ZEROVELOCITY_H -#include "feat/FeatureDatabase.h" -#include "feat/FeatureHelper.h" -#include "state/Propagator.h" -#include "state/State.h" -#include "state/StateHelper.h" -#include "utils/colors.h" -#include "utils/print.h" -#include "utils/quat_ops.h" +#include + #include "utils/sensor_data.h" -#include "UpdaterHelper.h" #include "UpdaterOptions.h" +#include "utils/NoiseManager.h" -#include -#include +namespace ov_core { +class Feature; +class FeatureDatabase; +} // namespace ov_core +namespace ov_type { +class Landmark; +} // namespace ov_type namespace ov_msckf { +class State; +class Propagator; + /** * @brief Will try to *detect* and then update using zero velocity assumption. * @@ -63,28 +65,9 @@ class UpdaterZeroVelocity { * @param zupt_noise_multiplier Multiplier of our IMU noise matrix (default should be 1.0) * @param zupt_max_disparity Max disparity we should consider to do a update with */ - UpdaterZeroVelocity(UpdaterOptions &options, Propagator::NoiseManager &noises, std::shared_ptr db, + UpdaterZeroVelocity(UpdaterOptions &options, NoiseManager &noises, std::shared_ptr db, std::shared_ptr prop, double gravity_mag, double zupt_max_velocity, double zupt_noise_multiplier, - double zupt_max_disparity) - : _options(options), _noises(noises), _db(db), _prop(prop), _zupt_max_velocity(zupt_max_velocity), - _zupt_noise_multiplier(zupt_noise_multiplier), _zupt_max_disparity(zupt_max_disparity) { - - // Gravity - _gravity << 0.0, 0.0, gravity_mag; - - // Save our raw pixel noise squared - _noises.sigma_w_2 = std::pow(_noises.sigma_w, 2); - _noises.sigma_a_2 = std::pow(_noises.sigma_a, 2); - _noises.sigma_wb_2 = std::pow(_noises.sigma_wb, 2); - _noises.sigma_ab_2 = std::pow(_noises.sigma_ab, 2); - - // Initialize the chi squared test table with confidence level 0.95 - // https://github.com/KumarRobotics/msckf_vio/blob/050c50defa5a7fd9a04c1eed5687b405f02919b5/src/msckf_vio.cpp#L215-L221 - for (int i = 1; i < 1000; i++) { - boost::math::chi_squared chi_squared_dist(i); - chi_squared_table[i] = boost::math::quantile(chi_squared_dist, 0.95); - } - } + double zupt_max_disparity); /** * @brief Feed function for inertial data @@ -105,7 +88,7 @@ class UpdaterZeroVelocity { if (oldest_time != -1) { auto it0 = imu_data.begin(); while (it0 != imu_data.end()) { - if (message.timestamp < oldest_time) { + if (it0->timestamp < oldest_time - 0.10) { it0 = imu_data.erase(it0); } else { it0++; @@ -127,7 +110,7 @@ class UpdaterZeroVelocity { UpdaterOptions _options; /// Container for the imu noise values - Propagator::NoiseManager _noises; + NoiseManager _noises; /// Feature tracker database with all features in it std::shared_ptr _db; diff --git a/ov_msckf/src/utils/NoiseManager.h b/ov_msckf/src/utils/NoiseManager.h new file mode 100644 index 000000000..e22044b2f --- /dev/null +++ b/ov_msckf/src/utils/NoiseManager.h @@ -0,0 +1,71 @@ +/* + * OpenVINS: An Open Platform for Visual-Inertial Research + * Copyright (C) 2018-2022 Patrick Geneva + * Copyright (C) 2018-2022 Guoquan Huang + * Copyright (C) 2018-2022 OpenVINS Contributors + * Copyright (C) 2018-2019 Kevin Eckenhoff + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#ifndef OV_MSCKF_NOISEMANAGER_H +#define OV_MSCKF_NOISEMANAGER_H + +#include + +#include "utils/print.h" + +namespace ov_msckf { + +/** + * @brief Struct of our imu noise parameters + */ +struct NoiseManager { + + /// Gyroscope white noise (rad/s/sqrt(hz)) + double sigma_w = 1.6968e-04; + + /// Gyroscope white noise covariance + double sigma_w_2 = pow(1.6968e-04, 2); + + /// Gyroscope random walk (rad/s^2/sqrt(hz)) + double sigma_wb = 1.9393e-05; + + /// Gyroscope random walk covariance + double sigma_wb_2 = pow(1.9393e-05, 2); + + /// Accelerometer white noise (m/s^2/sqrt(hz)) + double sigma_a = 2.0000e-3; + + /// Accelerometer white noise covariance + double sigma_a_2 = pow(2.0000e-3, 2); + + /// Accelerometer random walk (m/s^3/sqrt(hz)) + double sigma_ab = 3.0000e-03; + + /// Accelerometer random walk covariance + double sigma_ab_2 = pow(3.0000e-03, 2); + + /// Nice print function of what parameters we have loaded + void print() { + PRINT_DEBUG(" - gyroscope_noise_density: %.6f\n", sigma_w); + PRINT_DEBUG(" - accelerometer_noise_density: %.5f\n", sigma_a); + PRINT_DEBUG(" - gyroscope_random_walk: %.7f\n", sigma_wb); + PRINT_DEBUG(" - accelerometer_random_walk: %.6f\n", sigma_ab); + } +}; + +} // namespace ov_msckf + +#endif // OV_MSCKF_NOISEMANAGER_H \ No newline at end of file