diff --git a/config/rpng_sim/kalibr_imucam_chain.yaml b/config/rpng_sim/kalibr_imucam_chain.yaml index 7012144b5..ec98602e4 100644 --- a/config/rpng_sim/kalibr_imucam_chain.yaml +++ b/config/rpng_sim/kalibr_imucam_chain.yaml @@ -25,4 +25,30 @@ cam1: distortion_model: radtan intrinsics: [457.587, 456.134, 379.999, 255.238] #fu, fv, cu, cv resolution: [752, 480] - rostopic: /cam1/image_raw \ No newline at end of file + rostopic: /cam1/image_raw +cam2: + T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI + - [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975] + - [0.999557249008, 0.0149672133247, 0.025715529948, 0.124676986768] + - [-0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [1] + camera_model: pinhole + distortion_coeffs: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05] + distortion_model: radtan + intrinsics: [458.654, 457.296, 367.215, 248.375] #fu, fv, cu, cv + resolution: [752, 480] + rostopic: /cam2/image_raw +cam3: + T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI + - [0.0125552670891, -0.999755099723, 0.0182237714554, -0.0198435579556] + - [0.999598781151, 0.0130119051815, 0.0251588363115, 0.2253689425024] + - [-0.0253898008918, 0.0179005838253, 0.999517347078, 0.00786212447038] + - [0.0, 0.0, 0.0, 1.0] + cam_overlaps: [0] + camera_model: pinhole + distortion_coeffs: [-0.28368365,0.07451284,-0.00010473,-3.55590700e-05] + distortion_model: radtan + intrinsics: [457.587, 456.134, 379.999, 255.238] #fu, fv, cu, cv + resolution: [752, 480] + rostopic: /cam3/image_raw \ No newline at end of file diff --git a/ov_init/src/utils/helper.h b/ov_init/src/utils/helper.h index f14e37d1b..d2c3f03c2 100644 --- a/ov_init/src/utils/helper.h +++ b/ov_init/src/utils/helper.h @@ -137,16 +137,15 @@ class InitializerHelper { */ static void gram_schmidt(const Eigen::Vector3d &gravity_inI, Eigen::Matrix3d &R_GtoI) { - // Now gram schmidt to find rot for grav - // https://en.wikipedia.org/wiki/Gram%E2%80%93Schmidt_process - // Get z axis, which alines with -g (z_in_G=0,0,1) + // This will find an orthogonal vector to gravity which is our local z-axis + // We need to ensure we normalize after each one such that we obtain unit vectors 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)) { + 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); @@ -159,6 +158,7 @@ class InitializerHelper { } // Original method + // https://en.wikipedia.org/wiki/Gram%E2%80%93Schmidt_process // 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; diff --git a/ov_msckf/launch/simulation.launch b/ov_msckf/launch/simulation.launch index 0a11c1b96..753785ff3 100644 --- a/ov_msckf/launch/simulation.launch +++ b/ov_msckf/launch/simulation.launch @@ -17,6 +17,10 @@ + + + + @@ -57,6 +61,9 @@ + + + @@ -77,6 +84,7 @@ + diff --git a/ov_msckf/scripts/run_ros_eth.sh b/ov_msckf/scripts/run_ros_eth.sh index ef9ea0e17..91cd27878 100755 --- a/ov_msckf/scripts/run_ros_eth.sh +++ b/ov_msckf/scripts/run_ros_eth.sh @@ -46,19 +46,19 @@ bagstarttimes=( "5" # 18 ) - # location to save log files into -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.2" - +save_path1="/home/patrick/github/pubs_data/pgeneva/2023_openvins_reproduce/exp_euroc/algorithms" +save_path2="/home/patrick/github/pubs_data/pgeneva/2023_openvins_reproduce/exp_euroc/timings" +bag_path="/home/patrick/datasets/euroc_mav/" +ov_ver="2.6.3" #============================================================= #============================================================= #============================================================= +big_start_time="$(date -u +%s)" + # Loop through all datasets for i in "${!bagnames[@]}"; do # Loop through all modes @@ -118,3 +118,12 @@ done done + + +# print out the time elapsed +big_end_time="$(date -u +%s)" +big_elapsed="$(($big_end_time-$big_start_time))" +echo "BASH: script took $big_elapsed seconds in total!!"; + + + diff --git a/ov_msckf/scripts/run_ros_kaist.sh b/ov_msckf/scripts/run_ros_kaist.sh index 011012979..bb92f00a5 100755 --- a/ov_msckf/scripts/run_ros_kaist.sh +++ b/ov_msckf/scripts/run_ros_kaist.sh @@ -35,17 +35,18 @@ bagstarttimes=( ) # 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.2" - +save_path1="/home/patrick/github/pubs_data/pgeneva/2023_openvins_reproduce/exp_kaist/algorithms" +save_path2="/home/patrick/github/pubs_data/pgeneva/2023_openvins_reproduce/exp_kaist/timings" +bag_path="/home/patrick/datasets/euroc_mav/" +ov_ver="2.6.3" #============================================================= #============================================================= #============================================================= +big_start_time="$(date -u +%s)" + # Loop through all datasets for i in "${!bagnames[@]}"; do # Loop through all modes @@ -105,3 +106,12 @@ done done + +# print out the time elapsed +big_end_time="$(date -u +%s)" +big_elapsed="$(($big_end_time-$big_start_time))" +echo "BASH: script took $big_elapsed seconds in total!!"; + + + + diff --git a/ov_msckf/scripts/run_ros_kaistvio.sh b/ov_msckf/scripts/run_ros_kaistvio.sh deleted file mode 100755 index a0e77fa05..000000000 --- a/ov_msckf/scripts/run_ros_kaistvio.sh +++ /dev/null @@ -1,119 +0,0 @@ -#!/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" # has scale issues, need to debug (imu intrinsics?)!! -# "binocular" # has scale issues, need to debug (imu intrinsics?)!! - "stereo" -) - -# dataset locations -bagnames=( - "circle" - "circle_fast" - "circle_head" - "infinite" - "infinite_fast" - "infinite_head" - "rotation" - "rotation_fast" - "square" - "square_fast" - "square_head" -) - -# how far we should start into the dataset -# this can be used to skip the initial sections -bagstarttimes=( - "0" - "0" - "0" - "0" - "0" - "0" - "0" - "0" - "0" - "0" - "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.2" - - - -#============================================================= -#============================================================= -#============================================================= - -# 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_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))" -echo "BASH: ${modes[h]} - ${bagnames[i]} - run $j took $elapsed seconds"; - -done - - -done -done - - diff --git a/ov_msckf/scripts/run_ros_tumvi.sh b/ov_msckf/scripts/run_ros_tumvi.sh index fd4390f2c..76e15bf5f 100755 --- a/ov_msckf/scripts/run_ros_tumvi.sh +++ b/ov_msckf/scripts/run_ros_tumvi.sh @@ -37,17 +37,18 @@ bagstarttimes=( ) # location to save log files into -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.2" - +save_path1="/home/patrick/github/pubs_data/pgeneva/2023_openvins_reproduce/exp_tumvi/algorithms" +save_path2="/home/patrick/github/pubs_data/pgeneva/2023_openvins_reproduce/exp_tumvi/timings" +bag_path="/home/patrick/datasets/tum_vi/" +ov_ver="2.6.3" #============================================================= #============================================================= #============================================================= +big_start_time="$(date -u +%s)" + # Loop through all datasets for i in "${!bagnames[@]}"; do # Loop through all modes @@ -107,3 +108,10 @@ done done + +# print out the time elapsed +big_end_time="$(date -u +%s)" +big_elapsed="$(($big_end_time-$big_start_time))" +echo "BASH: script took $big_elapsed seconds in total!!"; + + diff --git a/ov_msckf/scripts/run_ros_uzhfpv.sh b/ov_msckf/scripts/run_ros_uzhfpv.sh index bc224d697..1b08f4abb 100755 --- a/ov_msckf/scripts/run_ros_uzhfpv.sh +++ b/ov_msckf/scripts/run_ros_uzhfpv.sh @@ -80,17 +80,18 @@ bagstarttimes=( ) # location to save log files into -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.2" - +save_path1="/home/patrick/github/pubs_data/pgeneva/2023_openvins_reproduce/exp_uzhfpv/algorithms" +save_path2="/home/patrick/github/pubs_data/pgeneva/2023_openvins_reproduce/exp_uzhfpv/timings" +bag_path="/home/patrick/datasets/euroc_mav/" +ov_ver="2.6.3" #============================================================= #============================================================= #============================================================= +big_start_time="$(date -u +%s)" + # Loop through all datasets for i in "${!bagnames[@]}"; do # Loop through all modes @@ -150,3 +151,10 @@ done done + +# print out the time elapsed +big_end_time="$(date -u +%s)" +big_elapsed="$(($big_end_time-$big_start_time))" +echo "BASH: script took $big_elapsed seconds in total!!"; + + diff --git a/ov_msckf/scripts/run_sim_calib.sh b/ov_msckf/scripts/run_sim_calib.sh index 3848c1d97..40a7d72ce 100755 --- a/ov_msckf/scripts/run_sim_calib.sh +++ b/ov_msckf/scripts/run_sim_calib.sh @@ -10,7 +10,7 @@ source ${SCRIPT_DIR}/../../../../devel/setup.bash # datasets datasets=( - "udel_gore" + "udel_gore" # "udel_arl" # "udel_gore_zupt" # "tum_corridor1_512_16_okvis" @@ -29,13 +29,18 @@ sim_do_perturbation=( ) # location to save log files into -save_path_est="/home/chuchu/test_ov/openvins_pra/sim_calib/algorithms" -save_path_gt="/home/chuchu/test_ov/openvins_pra/sim_calib/truths" +save_path1="/home/patrick/github/pubs_data/pgeneva/2023_openvins_reproduce/sim_calib/algorithms" +save_path2="/home/patrick/github/pubs_data/pgeneva/2023_openvins_reproduce/sim_calib/timings" +save_path3="/home/patrick/github/pubs_data/pgeneva/2023_openvins_reproduce/sim_calib/truths" + + #============================================================= # Start the Monte Carlo Simulations #============================================================= +big_start_time="$(date -u +%s)" + # Loop through the datasets for h in "${!datasets[@]}"; do # Loop through if we want to calibrate @@ -43,27 +48,45 @@ for m in "${!sim_do_calibration[@]}"; do # Loop through if we want to perturb for n in "${!sim_do_perturbation[@]}"; do # Monte Carlo runs for this dataset -for j in {00..02}; do +for j in {00..49}; 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" +# SKIP: no calib and perturbing +if [ "${sim_do_calibration[m]}" == "false" ] && [ "${sim_do_perturbation[n]}" == "true" ] +then + continue +fi + + +folder="VIO" +if [ "${sim_do_calibration[m]}" == "true" ] +then + folder="${folder}_CALIB" +fi +if [ "${sim_do_perturbation[n]}" == "true" ] +then + folder="${folder}_PERTURBED" +fi +filename_est="$save_path1/${folder}/${datasets[h]}/estimate_$j.txt" +filename_gt="$save_path3/${datasets[h]}.txt" # launch the simulation script start_time="$(date -u +%s)" roslaunch ov_msckf simulation.launch \ + verbosity:="WARNING" \ seed:="$((10#$j + 1))" \ dataset:="${datasets[h]}.txt" \ + max_cameras:="1" \ + fej:="true" \ sim_do_calibration:="${sim_do_calibration[m]}" \ sim_do_perturbation:="${sim_do_perturbation[n]}" \ 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[m]} - calib_${sim_do_calibration[m]} - perturb_${sim_do_perturbation[n]} - run $j took $elapsed seconds"; +echo "BASH: ${datasets[h]} - ${folder} - run $j took $elapsed seconds"; done @@ -71,3 +94,11 @@ done done done + + +# print out the time elapsed +big_end_time="$(date -u +%s)" +big_elapsed="$(($big_end_time-$big_start_time))" +echo "BASH: script took $big_elapsed seconds in total!!"; + + diff --git a/ov_msckf/scripts/run_sim_cams.sh b/ov_msckf/scripts/run_sim_cams.sh index 3b5bb8ca3..132d02dc4 100755 --- a/ov_msckf/scripts/run_sim_cams.sh +++ b/ov_msckf/scripts/run_sim_cams.sh @@ -10,12 +10,18 @@ source ${SCRIPT_DIR}/../../../../devel/setup.bash # datasets datasets=( - "udel_gore" + "udel_gore" # "udel_arl" # "udel_gore_zupt" # "tum_corridor1_512_16_okvis" ) +# what modes to use +use_cross_cam=( + "false" + "true" +) + # number of cameras cameras=( "1" @@ -25,31 +31,51 @@ cameras=( ) # location to save log files into -save_path_est="/home/cc/test/openvins_pra/sim_cam/algorithms" -save_path_gt="/home/cc/test/openvins_pra/sim_cam/truths" +save_path1="/home/patrick/github/pubs_data/pgeneva/2023_openvins_reproduce/sim_cams/algorithms" +save_path2="/home/patrick/github/pubs_data/pgeneva/2023_openvins_reproduce/sim_cams/timings" +save_path3="/home/patrick/github/pubs_data/pgeneva/2023_openvins_reproduce/sim_cams/truths" #============================================================= # Start Monte-Carlo Simulations #============================================================= + +big_start_time="$(date -u +%s)" + # Loop through datasets for h in "${!datasets[@]}"; do # Loop through number of cameras we want to use +for n in "${!use_cross_cam[@]}"; do for i in "${!cameras[@]}"; do # Monte Carlo runs for this dataset -for j in {00..02}; do +for j in {00..49}; do # start timing start_time="$(date -u +%s)" +# skip the case were we are trying to do cross sensor +if [ "${cameras[i]}" == "1" ] && [ "${use_cross_cam[n]}" == "true" ] +then + continue +fi + # our filename -filename_est="$save_path_est/ov_v23_cam${cameras[i]}/${datasets[h]}/estimate_$j.txt" -filename_gt="$save_path_gt/${datasets[h]}.txt" +temp="" +if [ "${use_cross_cam[n]}" == "true" ] +then + temp="_cross" +fi +folder="cam${cameras[i]}${temp}" +filename_est="$save_path1/${folder}/${datasets[h]}/estimate_$j.txt" +filename_gt="$save_path3/${datasets[h]}.txt" # launch the simulation script roslaunch ov_msckf simulation.launch \ + verbosity:="WARNING" \ seed:="$((10#$j + 1))" \ - max_cameras:="${cameras[i]}" \ dataset:="${datasets[h]}.txt" \ + use_stereo:="${use_cross_cam[n]}" \ + max_cameras:="${cameras[i]}" \ + fej:="true" \ dosave_pose:="true" \ path_est:="$filename_est" \ path_gt:="$filename_gt" &> /dev/null @@ -57,10 +83,19 @@ roslaunch ov_msckf simulation.launch \ # print out the time elapsed end_time="$(date -u +%s)" elapsed="$(($end_time-$start_time))" -echo "BASH: ${datasets[h]} - ${cameras[i]} - run $j took $elapsed seconds"; +echo "BASH: ${datasets[h]} - ${folder} - run $j took $elapsed seconds"; done done done +done + + + +# print out the time elapsed +big_end_time="$(date -u +%s)" +big_elapsed="$(($big_end_time-$big_start_time))" +echo "BASH: script took $big_elapsed seconds in total!!"; + diff --git a/ov_msckf/scripts/run_sim_rep.sh b/ov_msckf/scripts/run_sim_featrep.sh similarity index 50% rename from ov_msckf/scripts/run_sim_rep.sh rename to ov_msckf/scripts/run_sim_featrep.sh index 0827b46c1..b97c81533 100755 --- a/ov_msckf/scripts/run_sim_rep.sh +++ b/ov_msckf/scripts/run_sim_featrep.sh @@ -10,76 +10,105 @@ source ${SCRIPT_DIR}/../../../../devel/setup.bash # datasets datasets=( -# "udel_gore" - "udel_arl" + "udel_gore" +# "udel_arl" # "udel_gore_zupt" # "tum_corridor1_512_16_okvis" ) # estimator configurations usefej=( - "false" "true" +# "false" ) # feature representations representations=( "GLOBAL_3D" - "GLOBAL_FULL_INVERSE_DEPTH" +# "GLOBAL_FULL_INVERSE_DEPTH" "ANCHORED_3D" "ANCHORED_FULL_INVERSE_DEPTH" "ANCHORED_MSCKF_INVERSE_DEPTH" - "ANCHORED_INVERSE_DEPTH_SINGLE" +# "ANCHORED_INVERSE_DEPTH_SINGLE" +) + +# extra configuration (feature scene depth) +configs=( + "03m_" + "06m_" + "10m_" +) +configs_params=( + "feat_dist_min:=2.0 feat_dist_max:=4.0" + "feat_dist_min:=5.0 feat_dist_max:=7.0" + "feat_dist_min:=9.0 feat_dist_max:=11.0" ) # location to save log files into -save_path_est="/home/cc/test/openvins_pra/sim_representations/algorithms" -save_path_gt="/home/cc/test/openvins_pra/sim_representations/truths" +save_path1="/home/patrick/github/pubs_data/pgeneva/2023_openvins_reproduce/sim_featrep/algorithms" +save_path2="/home/patrick/github/pubs_data/pgeneva/2023_openvins_reproduce/sim_featrep/timings" +save_path3="/home/patrick/github/pubs_data/pgeneva/2023_openvins_reproduce/sim_featrep/truths" #============================================================= # Start Monte-Carlo Simulations #============================================================= +big_start_time="$(date -u +%s)" + # Loop through datasets -for m in "${!datasets[@]}"; do +for h in "${!datasets[@]}"; do # Loop through if use fej or not -for h in "${!usefej[@]}"; do +for m in "${!usefej[@]}"; do # Loop through all representations +for k in "${!configs[@]}"; do for i in "${!representations[@]}"; do # Monte Carlo runs for this dataset -for j in {00..09}; do +for j in {00..49}; do # start timing start_time="$(date -u +%s)" # filename change if we are using fej -if [ "${usefej[h]}" == "true" ] +temp="" +if [ "${usefej[m]}" == "true" ] then - temp="_FEJ" -else - temp="" + temp="FEJ_" fi -filename_est="$save_path_est/${representations[i]}$temp/${datasets[m]}/estimate_$j.txt" -filename_gt="$save_path_gt/${datasets[m]}.txt" +folder="${temp}${configs[k]}${representations[i]}" +filename_est="$save_path1/$folder/${datasets[h]}/estimate_$j.txt" +filename_gt="$save_path3/${datasets[h]}.txt" # launch the simulation script roslaunch ov_msckf simulation.launch \ + verbosity:="WARNING" \ seed:="$((10#$j + 1))" \ - dataset:="${datasets[m]}.txt" \ - fej:="${usefej[h]}" \ + dataset:="${datasets[h]}.txt" \ + max_cameras:="1" \ + fej:="${usefej[m]}" \ feat_rep:="${representations[i]}" \ dosave_pose:="true" \ path_est:="$filename_est" \ - path_gt:="$filename_gt" &> /dev/null + path_gt:="$filename_gt" \ + ${configs_params[k]} &> /dev/null # print out the time elapsed end_time="$(date -u +%s)" elapsed="$(($end_time-$start_time))" -echo "BASH: ${datasets[m]} - ${usefej[h]} - ${representations[i]} - run $j took $elapsed seconds"; +echo "BASH: ${datasets[h]} - ${folder} - run $j took $elapsed seconds"; done done done done +done + + + +# print out the time elapsed +big_end_time="$(date -u +%s)" +big_elapsed="$(($big_end_time-$big_start_time))" +echo "BASH: script took $big_elapsed seconds in total!!"; + + diff --git a/ov_msckf/src/core/VioManager.cpp b/ov_msckf/src/core/VioManager.cpp index 8bc04cdfc..0fc67b2f0 100644 --- a/ov_msckf/src/core/VioManager.cpp +++ b/ov_msckf/src/core/VioManager.cpp @@ -215,6 +215,7 @@ void VioManager::feed_measurement_simulation(double timestamp, const std::vector assert(state->_timestamp == timestamp); propagator->clean_old_imu_measurements(timestamp + state->_calib_dt_CAMtoIMU->value()(0) - 0.10); updaterZUPT->clean_old_imu_measurements(timestamp + state->_calib_dt_CAMtoIMU->value()(0) - 0.10); + propagator->invalidate_cache(); return; } } @@ -287,6 +288,7 @@ void VioManager::track_image_and_update(const ov_core::CameraData &message_const assert(state->_timestamp == message.timestamp); propagator->clean_old_imu_measurements(message.timestamp + state->_calib_dt_CAMtoIMU->value()(0) - 0.10); updaterZUPT->clean_old_imu_measurements(message.timestamp + state->_calib_dt_CAMtoIMU->value()(0) - 0.10); + propagator->invalidate_cache(); return; } } @@ -509,6 +511,7 @@ void VioManager::do_feature_propagate_update(const ov_core::CameraData &message) if ((int)featsup_MSCKF.size() > state->_options.max_msckf_in_update) featsup_MSCKF.erase(featsup_MSCKF.begin(), featsup_MSCKF.end() - state->_options.max_msckf_in_update); updaterMSCKF->update(state, featsup_MSCKF); + propagator->invalidate_cache(); rT4 = boost::posix_time::microsec_clock::local_time(); // Perform SLAM delay init and update @@ -525,6 +528,7 @@ void VioManager::do_feature_propagate_update(const ov_core::CameraData &message) // Do the update updaterSLAM->update(state, featsup_TEMP); feats_slam_UPDATE_TEMP.insert(feats_slam_UPDATE_TEMP.end(), featsup_TEMP.begin(), featsup_TEMP.end()); + propagator->invalidate_cache(); } feats_slam_UPDATE = feats_slam_UPDATE_TEMP; rT5 = boost::posix_time::microsec_clock::local_time(); diff --git a/ov_msckf/src/ros/ROS1Visualizer.cpp b/ov_msckf/src/ros/ROS1Visualizer.cpp index 90f84d225..a665bd357 100644 --- a/ov_msckf/src/ros/ROS1Visualizer.cpp +++ b/ov_msckf/src/ros/ROS1Visualizer.cpp @@ -252,6 +252,33 @@ void ROS1Visualizer::visualize_odometry(double timestamp) { if (!_app->get_propagator()->fast_state_propagate(state, timestamp, state_plus, cov_plus)) return; + // // Get the simulated groundtruth so we can evaulate the error in respect to it + // // NOTE: we get the true time in the IMU clock frame + // if (_sim != nullptr) { + // Eigen::Matrix state_gt; + // if (_sim->get_state(timestamp, state_gt)) { + // // Difference between positions + // double dx = state_plus(4, 0) - state_gt(5, 0); + // double dy = state_plus(5, 0) - state_gt(6, 0); + // double dz = state_plus(6, 0) - state_gt(7, 0); + // double err_pos = std::sqrt(dx * dx + dy * dy + dz * dz); + // // Quaternion error + // Eigen::Matrix quat_gt, quat_st, quat_diff; + // quat_gt << state_gt(1, 0), state_gt(2, 0), state_gt(3, 0), state_gt(4, 0); + // quat_st << state_plus(0, 0), state_plus(1, 0), state_plus(2, 0), state_plus(3, 0); + // quat_diff = quat_multiply(quat_st, Inv(quat_gt)); + // double err_ori = (180 / M_PI) * 2 * quat_diff.block(0, 0, 3, 1).norm(); + // // Calculate NEES values + // Eigen::Vector3d quat_diff_vec = quat_diff.block(0, 0, 3, 1); + // Eigen::Vector3d cov_vec = cov_plus.block(0, 0, 3, 3).inverse() * 2 * quat_diff.block(0, 0, 3, 1); + // double ori_nees = 2 * quat_diff_vec.dot(cov_vec); + // Eigen::Vector3d errpos = state_plus.block(4, 0, 3, 1) - state_gt.block(5, 0, 3, 1); + // double pos_nees = errpos.transpose() * cov_plus.block(3, 3, 3, 3).inverse() * errpos; + // PRINT_INFO(REDPURPLE "error to gt => %.3f, %.3f (deg,m) | nees => %.1f, %.1f (ori,pos) \n" RESET, err_ori, err_pos, ori_nees, + // pos_nees); + // } + // } + // Publish our odometry message if requested if (pub_odomimu.getNumSubscribers() != 0) { diff --git a/ov_msckf/src/ros/ROSVisualizerHelper.cpp b/ov_msckf/src/ros/ROSVisualizerHelper.cpp index f4078412a..0fd835a71 100644 --- a/ov_msckf/src/ros/ROSVisualizerHelper.cpp +++ b/ov_msckf/src/ros/ROSVisualizerHelper.cpp @@ -38,7 +38,7 @@ sensor_msgs::PointCloud2 ROSVisualizerHelper::get_ros_pointcloud(const std::vect sensor_msgs::PointCloud2 cloud; cloud.header.frame_id = "global"; cloud.header.stamp = ros::Time::now(); - cloud.width = 3 * feats.size(); + cloud.width = feats.size(); cloud.height = 1; cloud.is_bigendian = false; cloud.is_dense = false; // there may be invalid points @@ -46,7 +46,7 @@ sensor_msgs::PointCloud2 ROSVisualizerHelper::get_ros_pointcloud(const std::vect // Setup pointcloud fields sensor_msgs::PointCloud2Modifier modifier(cloud); modifier.setPointCloud2FieldsByString(1, "xyz"); - modifier.resize(3 * feats.size()); + modifier.resize(feats.size()); // Iterators sensor_msgs::PointCloud2Iterator out_x(cloud, "x"); @@ -96,7 +96,7 @@ sensor_msgs::msg::PointCloud2 ROSVisualizerHelper::get_ros_pointcloud(std::share sensor_msgs::msg::PointCloud2 cloud; cloud.header.frame_id = "global"; cloud.header.stamp = node->now(); - cloud.width = 3 * feats.size(); + cloud.width = feats.size(); cloud.height = 1; cloud.is_bigendian = false; cloud.is_dense = false; // there may be invalid points @@ -104,7 +104,7 @@ sensor_msgs::msg::PointCloud2 ROSVisualizerHelper::get_ros_pointcloud(std::share // Setup pointcloud fields sensor_msgs::PointCloud2Modifier modifier(cloud); modifier.setPointCloud2FieldsByString(1, "xyz"); - modifier.resize(3 * feats.size()); + modifier.resize(feats.size()); // Iterators sensor_msgs::PointCloud2Iterator out_x(cloud, "x"); diff --git a/ov_msckf/src/run_simulation.cpp b/ov_msckf/src/run_simulation.cpp index ef8a3194e..7b5ee4efc 100644 --- a/ov_msckf/src/run_simulation.cpp +++ b/ov_msckf/src/run_simulation.cpp @@ -155,8 +155,7 @@ int main(int argc, char **argv) { if (hasimu) { sys->feed_measurement_imu(message_imu); #if ROS_AVAILABLE == 1 || ROS_AVAILABLE == 2 - // TODO: fix this, can be slow at high frequency... - // viz->visualize_odometry(message_imu.timestamp - sim->get_true_parameters().calib_camimu_dt); + viz->visualize_odometry(message_imu.timestamp); #endif } @@ -167,9 +166,6 @@ int main(int argc, char **argv) { bool hascam = sim->get_next_cam(time_cam, camids, feats); if (hascam) { if (buffer_timecam != -1) { -#if ROS_AVAILABLE == 1 || ROS_AVAILABLE == 2 - viz->visualize_odometry(buffer_timecam); -#endif sys->feed_measurement_simulation(buffer_timecam, buffer_camids, buffer_feats); #if ROS_AVAILABLE == 1 || ROS_AVAILABLE == 2 viz->visualize(); diff --git a/ov_msckf/src/state/Propagator.cpp b/ov_msckf/src/state/Propagator.cpp index b6e76cb71..4a0ac187a 100644 --- a/ov_msckf/src/state/Propagator.cpp +++ b/ov_msckf/src/state/Propagator.cpp @@ -124,14 +124,17 @@ bool Propagator::fast_state_propagate(std::shared_ptr state, double times Eigen::Matrix &covariance) { // First we will store the current calibration / estimates of the state - double state_time = state->_timestamp; - Eigen::MatrixXd state_est = state->_imu->value(); - Eigen::MatrixXd state_covariance = StateHelper::get_marginal_covariance(state, {state->_imu}); - double t_off = state->_calib_dt_CAMtoIMU->value()(0); + if (!cache_imu_valid) { + cache_state_time = state->_timestamp; + cache_state_est = state->_imu->value(); + cache_state_covariance = StateHelper::get_marginal_covariance(state, {state->_imu}); + cache_t_off = state->_calib_dt_CAMtoIMU->value()(0); + cache_imu_valid = true; + } // First lets construct an IMU vector of measurements we need - double time0 = state_time + t_off; - double time1 = timestamp + t_off; + double time0 = cache_state_time + cache_t_off; + double time1 = timestamp + cache_t_off; std::vector prop_data; { std::lock_guard lck(imu_data_mtx); @@ -141,8 +144,8 @@ bool Propagator::fast_state_propagate(std::shared_ptr state, double times return false; // Biases - Eigen::Vector3d bias_g = state_est.block(10, 0, 3, 1); - Eigen::Vector3d bias_a = state_est.block(13, 0, 3, 1); + Eigen::Vector3d bias_g = cache_state_est.block(10, 0, 3, 1); + Eigen::Vector3d bias_a = cache_state_est.block(13, 0, 3, 1); // Loop through all IMU messages, and use them to move the state forward in time // This uses the zero'th order quat, and then constant acceleration discrete @@ -152,13 +155,12 @@ bool Propagator::fast_state_propagate(std::shared_ptr state, double times double dt = prop_data.at(i + 1).timestamp - prop_data.at(i).timestamp; Eigen::Vector3d w_hat = 0.5 * (prop_data.at(i + 1).wm + prop_data.at(i).wm) - bias_g; Eigen::Vector3d a_hat = 0.5 * (prop_data.at(i + 1).am + prop_data.at(i).am) - bias_a; - Eigen::Matrix3d R_Gtoi = quat_2_Rot(state_est.block(0, 0, 4, 1)); - Eigen::Vector3d v_iinG = state_est.block(7, 0, 3, 1); - Eigen::Vector3d p_iinG = state_est.block(4, 0, 3, 1); + Eigen::Matrix3d R_Gtoi = quat_2_Rot(cache_state_est.block(0, 0, 4, 1)); + Eigen::Vector3d v_iinG = cache_state_est.block(7, 0, 3, 1); + Eigen::Vector3d p_iinG = cache_state_est.block(4, 0, 3, 1); // State transition and noise matrix Eigen::Matrix F = Eigen::Matrix::Zero(); - Eigen::Matrix Qd = Eigen::Matrix::Zero(); F.block(0, 0, 3, 3) = exp_so3(-w_hat * dt); F.block(0, 9, 3, 3).noalias() = -exp_so3(-w_hat * dt) * Jr_so3(-w_hat * dt) * dt; F.block(9, 9, 3, 3).setIdentity(); @@ -180,6 +182,7 @@ bool Propagator::fast_state_propagate(std::shared_ptr state, double times // Construct our discrete noise covariance matrix // Note that we need to convert our continuous time noises to discrete // Equations (129) amd (130) of Trawny tech report + Eigen::Matrix Qd = Eigen::Matrix::Zero(); Eigen::Matrix Qc = Eigen::Matrix::Zero(); Qc.block(0, 0, 3, 3) = _noises.sigma_w_2 / dt * Eigen::Matrix3d::Identity(); Qc.block(3, 3, 3, 3) = _noises.sigma_a_2 / dt * Eigen::Matrix3d::Identity(); @@ -187,18 +190,23 @@ bool Propagator::fast_state_propagate(std::shared_ptr state, double times Qc.block(9, 9, 3, 3) = _noises.sigma_ab_2 * dt * Eigen::Matrix3d::Identity(); Qd = G * Qc * G.transpose(); Qd = 0.5 * (Qd + Qd.transpose()); - state_covariance = F * state_covariance * F.transpose() + Qd; + cache_state_covariance = F * cache_state_covariance * F.transpose() + Qd; // Propagate the mean forward - state_est.block(0, 0, 4, 1) = rot_2_quat(exp_so3(-w_hat * dt) * R_Gtoi); - state_est.block(4, 0, 3, 1) = p_iinG + v_iinG * dt + 0.5 * R_Gtoi.transpose() * a_hat * dt * dt - 0.5 * _gravity * dt * dt; - state_est.block(7, 0, 3, 1) = v_iinG + R_Gtoi.transpose() * a_hat * dt - _gravity * dt; + cache_state_est.block(0, 0, 4, 1) = rot_2_quat(exp_so3(-w_hat * dt) * R_Gtoi); + cache_state_est.block(4, 0, 3, 1) = p_iinG + v_iinG * dt + 0.5 * R_Gtoi.transpose() * a_hat * dt * dt - 0.5 * _gravity * dt * dt; + cache_state_est.block(7, 0, 3, 1) = v_iinG + R_Gtoi.transpose() * a_hat * dt - _gravity * dt; } + // Move the time forward + // This time will now be in the IMU clock, so reset the toff to zero + cache_state_time = time1; + cache_t_off = 0.0; + // Now record what the predicted state should be - Eigen::Vector4d q_Gtoi = state_est.block(0, 0, 4, 1); - Eigen::Vector3d v_iinG = state_est.block(7, 0, 3, 1); - Eigen::Vector3d p_iinG = state_est.block(4, 0, 3, 1); + Eigen::Vector4d q_Gtoi = cache_state_est.block(0, 0, 4, 1); + Eigen::Vector3d v_iinG = cache_state_est.block(7, 0, 3, 1); + Eigen::Vector3d p_iinG = cache_state_est.block(4, 0, 3, 1); state_plus.setZero(); state_plus.block(0, 0, 4, 1) = q_Gtoi; state_plus.block(4, 0, 3, 1) = p_iinG; @@ -211,8 +219,8 @@ bool Propagator::fast_state_propagate(std::shared_ptr state, double times covariance.setZero(); Eigen::Matrix Phi = Eigen::Matrix::Identity(); 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); + Eigen::MatrixXd covariance_tmp = Phi * cache_state_covariance * Phi.transpose(); + covariance.block(0, 0, 9, 9) = covariance_tmp.block(0, 0, 9, 9); 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; @@ -274,19 +282,20 @@ std::vector Propagator::select_imu_readings(const std::vector< } else if (imu_data.at(i).timestamp > time1) { ov_core::ImuData data = interpolate_data(imu_data.at(i - 1), imu_data.at(i), time1); prop_data.push_back(data); - // PRINT_DEBUG("propagation #%d = CASE 3.1 = %.3f => %.3f\n", - // (int)i,imu_data.at(i).timestamp-prop_data.at(0).timestamp,imu_data.at(i).timestamp-time0); + // PRINT_DEBUG("propagation #%d = CASE 3.1 = %.3f => %.3f\n", (int)i, imu_data.at(i).timestamp - prop_data.at(0).timestamp, + // imu_data.at(i).timestamp - time0); } else { prop_data.push_back(imu_data.at(i)); - // PRINT_DEBUG("propagation #%d = CASE 3.2 = %.3f => %.3f\n", - // (int)i,imu_data.at(i).timestamp-prop_data.at(0).timestamp,imu_data.at(i).timestamp-time0); + // PRINT_DEBUG("propagation #%d = CASE 3.2 = %.3f => %.3f\n", (int)i, imu_data.at(i).timestamp - prop_data.at(0).timestamp, + // imu_data.at(i).timestamp - time0); } // If the added IMU message doesn't end exactly at the camera time // Then we need to add another one that is right at the ending time if (prop_data.at(prop_data.size() - 1).timestamp != time1) { ov_core::ImuData data = interpolate_data(imu_data.at(i), imu_data.at(i + 1), time1); prop_data.push_back(data); - // PRINT_DEBUG("propagation #%d = CASE 3.3 = %.3f => %.3f\n", (int)i,data.timestamp-prop_data.at(0).timestamp,data.timestamp-time0); + // PRINT_DEBUG("propagation #%d = CASE 3.3 = %.3f => %.3f\n", (int)i, data.timestamp - prop_data.at(0).timestamp, + // data.timestamp - time0); } break; } @@ -302,15 +311,23 @@ std::vector Propagator::select_imu_readings(const std::vector< return prop_data; } - // If we did not reach the whole integration period (i.e., the last inertial measurement we have is smaller then the time we want to - // reach) Then we should just "stretch" the last measurement to be the whole period (case 3 in the above loop) - // if(time1-imu_data.at(imu_data.size()-1).timestamp > 1e-3) { - // PRINT_DEBUG(YELLOW "Propagator::select_imu_readings(): Missing inertial measurements to propagate with (%.6f sec missing). - // IMU-CAMERA are likely messed up!!!\n" RESET, (time1-imu_data.at(imu_data.size()-1).timestamp)); return prop_data; - //} + // If we did not reach the whole integration period + // (i.e., the last inertial measurement we have is smaller then the time we want to reach) + // Then we should just "stretch" the last measurement to be the whole period + // TODO: this really isn't that good of logic, we should fix this so the above logic is exact! + if (prop_data.at(prop_data.size() - 1).timestamp != time1) { + if (warn) + PRINT_DEBUG(YELLOW "Propagator::select_imu_readings(): Missing inertial measurements to propagate with (%f sec missing)!\n" RESET, + (time1 - imu_data.at(imu_data.size() - 1).timestamp)); + ov_core::ImuData data = interpolate_data(imu_data.at(imu_data.size() - 2), imu_data.at(imu_data.size() - 1), time1); + prop_data.push_back(data); + // PRINT_DEBUG("propagation #%d = CASE 3.4 = %.3f => %.3f\n", (int)(imu_data.size() - 2), data.timestamp - prop_data.at(0).timestamp, + // data.timestamp - time0); + } - // Loop through and ensure we do not have an zero dt values + // Loop through and ensure we do not have any zero dt values // This would cause the noise covariance to be Infinity + // TODO: we should actually fix this by properly implementing this function and doing unit tests on it... for (size_t i = 0; i < prop_data.size() - 1; i++) { if (std::abs(prop_data.at(i + 1).timestamp - prop_data.at(i).timestamp) < 1e-12) { if (warn) diff --git a/ov_msckf/src/state/Propagator.h b/ov_msckf/src/state/Propagator.h index f989e8273..e63062992 100644 --- a/ov_msckf/src/state/Propagator.h +++ b/ov_msckf/src/state/Propagator.h @@ -22,6 +22,7 @@ #ifndef OV_MSCKF_STATE_PROPAGATOR_H #define OV_MSCKF_STATE_PROPAGATOR_H +#include #include #include @@ -49,7 +50,7 @@ class Propagator { * @param noises imu noise characteristics (continuous time) * @param gravity_mag Global gravity magnitude of the system (normally 9.81) */ - Propagator(NoiseManager noises, double gravity_mag) : _noises(noises) { + Propagator(NoiseManager noises, double gravity_mag) : _noises(noises), cache_imu_valid(false) { _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); @@ -91,6 +92,13 @@ class Propagator { } } + /** + * @brief Will invalidate the cache used for fast propagation + */ + void invalidate_cache() { + cache_imu_valid = false; + } + /** * @brief Propagate state up to given timestamp and then clone * @@ -160,9 +168,6 @@ class Propagator { } protected: - /// Estimate for time offset at last propagation time - double last_prop_time_offset = 0.0; - bool have_last_prop_time_offset = false; /** * @brief Propagates the state forward using the imu data and computes the noise covariance and state-transition @@ -251,6 +256,17 @@ class Propagator { /// Gravity vector Eigen::Vector3d _gravity; + + // Estimate for time offset at last propagation time + double last_prop_time_offset = 0.0; + bool have_last_prop_time_offset = false; + + // Cache of the last fast propagated state + std::atomic cache_imu_valid; + double cache_state_time; + Eigen::MatrixXd cache_state_est; + Eigen::MatrixXd cache_state_covariance; + double cache_t_off; }; } // namespace ov_msckf