From 6973ad5589520b9fa1768a3b759382fda3c5f0ac Mon Sep 17 00:00:00 2001 From: Alejandro Date: Fri, 16 Oct 2020 11:45:59 +0200 Subject: [PATCH 1/3] Add support for setting sensor parameters externally (gyroscope/accelerometer noise density, LiDAR noise, IMU rate). Compare them to their default value to update the optimization weights if necessary. --- calib.sh | 13 ++++++++++- include/core/calibration.hpp | 44 ++++++++++++++++++++++++++---------- launch/licalib_gui.launch | 20 +++++++++------- src/ui/calib_helper.cpp | 12 ++++++++++ 4 files changed, 68 insertions(+), 21 deletions(-) diff --git a/calib.sh b/calib.sh index e9cbabd..293d923 100755 --- a/calib.sh +++ b/calib.sh @@ -24,11 +24,18 @@ imu_topic_name=( #"/imu3/data_sync" ) +# For data association. bag_start=1 bag_durr=30 scan4map=15 timeOffsetPadding=0.015 +# For sensor modeling. +gyroscope_noise=8.72665e-5 +accelerometer_noise=1.96133e-4 +lidar_noise=0.03 +imu_rate=250.0 + show_ui=true #false bag_count=-1 @@ -58,6 +65,10 @@ for i in "${!sync_bag_name[@]}"; do lidar_model:="VLP_16" \ time_offset_padding:="${timeOffsetPadding}"\ ndtResolution:="${ndtResolution}" \ - show_ui:="${show_ui}" + show_ui:="${show_ui}" \ + gyroscope_noise:="${gyroscope_noise}" \ + accelerometer_noise:="${accelerometer_noise}" \ + lidar_noise:="${lidar_noise}" \ + imu_rate:="${imu_rate}" done done diff --git a/include/core/calibration.hpp b/include/core/calibration.hpp index 01d0d7f..144a027 100755 --- a/include/core/calibration.hpp +++ b/include/core/calibration.hpp @@ -45,18 +45,6 @@ class CalibParamManager gyro_bias(Eigen::Vector3d(0,0,0)), acce_bias(Eigen::Vector3d(0,0,0)) { - double gyroscope_noise_density = 1.745e-4; - double accelerometer_noise_density = 5.88e-4; - double imu_rate = 400.0; - double lidar_noise = 0.02; - - double gyro_discrete = gyroscope_noise_density * std::sqrt(imu_rate); - double acce_discrete = accelerometer_noise_density * std::sqrt(imu_rate); - - global_opt_gyro_weight = 1.0 / std::pow(gyro_discrete, 2); // 8.21e4 - global_opt_acce_weight = 1.0 / std::pow(acce_discrete, 2); // 7.23e3 - global_opt_lidar_weight = 1.0 / std::pow(lidar_noise, 2); // 2.5e3 - // fine-tuned parameter global_opt_gyro_weight = 28.0; global_opt_acce_weight = 18.5; @@ -87,6 +75,29 @@ class CalibParamManager acce_bias = ab; } + void set_opt_weights(const double gyroscope_noise, + const double accelerometer_noise, + const double lidar_noise, + const double imu_rate) { + double gyro_discrete = gyroscope_noise * std::sqrt(imu_rate); + double acce_discrete = accelerometer_noise * std::sqrt(imu_rate); + + global_opt_gyro_weight = 1.0 / std::pow(gyro_discrete, 2); + global_opt_acce_weight = 1.0 / std::pow(acce_discrete, 2); + global_opt_lidar_weight = 1.0 / std::pow(lidar_noise, 2); + } + + bool areSensorParamsDefault(const double gyroscope_noise, + const double accelerometer_noise, + const double lidar_noise, + const double imu_rate, + const double tol = 1e-6) { + return( (fabs(default_gyro_noise_density_ - gyroscope_noise) < tol) && + (fabs(default_accel_noise_density_ - accelerometer_noise) < tol) && + (fabs(default_lidar_noise_ - lidar_noise) < tol) && + (fabs(default_imu_rate_ - imu_rate) < tol) ); + } + void showStates() const { Eigen::Vector3d euler_LtoI = q_LtoI.toRotationMatrix().eulerAngles(0,1,2); euler_LtoI = euler_LtoI * 180 / M_PI; @@ -134,6 +145,15 @@ class CalibParamManager Eigen::Vector3d acce_bias; + ///default sensor parameters + double default_gyro_noise_density_ = 1.745e-4; + + double default_accel_noise_density_ = 5.88e-4; + + double default_lidar_noise_ = 0.02; + + double default_imu_rate_ = 400.0; + ///weight double global_opt_gyro_weight; diff --git a/launch/licalib_gui.launch b/launch/licalib_gui.launch index f57f3ae..ceacc60 100644 --- a/launch/licalib_gui.launch +++ b/launch/licalib_gui.launch @@ -14,14 +14,18 @@ - - - - - - - - + + + + + + + + + + + + diff --git a/src/ui/calib_helper.cpp b/src/ui/calib_helper.cpp index 365b6cd..0b24410 100755 --- a/src/ui/calib_helper.cpp +++ b/src/ui/calib_helper.cpp @@ -40,6 +40,8 @@ CalibrHelper::CalibrHelper(ros::NodeHandle& nh) double scan4map; double knot_distance; double time_offset_padding; + double gyroscope_noise, accelerometer_noise, lidar_noise; + double imu_rate; nh.param("path_bag", bag_path_, "V1_01_easy.bag"); nh.param("topic_imu", topic_imu_, "/imu0"); @@ -50,6 +52,10 @@ CalibrHelper::CalibrHelper(ros::NodeHandle& nh) nh.param("ndtResolution", ndt_resolution_, 0.5); nh.param("time_offset_padding", time_offset_padding, 0.015); nh.param("knot_distance", knot_distance, 0.02); + nh.param("gyroscope_noise", gyroscope_noise, 1.745e-4); + nh.param("accelerometer_noise", accelerometer_noise, 5.88e-4); + nh.param("lidar_noise", lidar_noise, 0.02); + nh.param("imu_rate", imu_rate, 400.0); if (!createCacheFolder(bag_path_)) { calib_step_ = Error; @@ -79,6 +85,12 @@ CalibrHelper::CalibrHelper(ros::NodeHandle& nh) traj_manager_ = std::make_shared( map_time_, end_time, knot_distance, time_offset_padding); + /// if sensor parameters are not the default, update optimization weights + if (!traj_manager_->getCalibParamManager()->areSensorParamsDefault( + gyroscope_noise, accelerometer_noise, lidar_noise, imu_rate)) { + traj_manager_->getCalibParamManager()->set_opt_weights( + gyroscope_noise, accelerometer_noise, lidar_noise, imu_rate); + } scan_undistortion_ = std::make_shared( traj_manager_, dataset_reader_); From 40f31539b0a93e9f3214334fd5f5cbcdf797e33a Mon Sep 17 00:00:00 2001 From: Alejandro Date: Fri, 16 Oct 2020 13:03:40 +0200 Subject: [PATCH 2/3] Fixed an argument passing issue. --- launch/licalib_gui.launch | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/launch/licalib_gui.launch b/launch/licalib_gui.launch index ceacc60..9bed03e 100644 --- a/launch/licalib_gui.launch +++ b/launch/licalib_gui.launch @@ -7,10 +7,14 @@ - + + + + + @@ -22,13 +26,13 @@ - - - - + + + + - - + + From f2caba91b83ec3053adffe8bf5b6a7c334b52237 Mon Sep 17 00:00:00 2001 From: Alejandro Date: Fri, 16 Oct 2020 13:09:38 +0200 Subject: [PATCH 3/3] Set package default sensor parameters in calibration script. --- calib.sh | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/calib.sh b/calib.sh index 293d923..dcf97ea 100755 --- a/calib.sh +++ b/calib.sh @@ -31,10 +31,10 @@ scan4map=15 timeOffsetPadding=0.015 # For sensor modeling. -gyroscope_noise=8.72665e-5 -accelerometer_noise=1.96133e-4 -lidar_noise=0.03 -imu_rate=250.0 +gyroscope_noise=0.001745 +accelerometer_noise=0.000588 +lidar_noise=0.02 +imu_rate=400.0 show_ui=true #false