Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add support for setting sensor parameters externally #9

Open
wants to merge 3 commits into
base: master
Choose a base branch
from
Open

Add support for setting sensor parameters externally #9

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
6973ad5
Add support for setting sensor parameters externally (gyroscope/accel…
alexmora-sr Oct 16, 2020
40f3153
Fixed an argument passing issue.
alexmora-sr Oct 16, 2020
f2caba9
Set package default sensor parameters in calibration script.
alexmora-sr Oct 16, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 12 additions & 1 deletion calib.sh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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=0.001745
accelerometer_noise=0.000588
lidar_noise=0.02
imu_rate=400.0

show_ui=true #false

bag_count=-1
Expand Down Expand Up @@ -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
44 changes: 32 additions & 12 deletions include/core/calibration.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;

Expand Down
30 changes: 19 additions & 11 deletions launch/licalib_gui.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,24 +7,32 @@
<arg name="scan4map" default="15" />
<arg name="lidar_model" default="VLP_16" />
<arg name="ndtResolution" default="0.5" /> <!-- 0.5 for indoor case and 1.0 for outdoor case -->

<arg name="time_offset_padding" default="0.015" />
<arg name="show_ui" default="false" />

<arg name="gyroscope_noise" default="0.001745" /> <!-- for sensor modeling -->
<arg name="accelerometer_noise" default="0.000588" />
<arg name="lidar_noise" default="0.02" />
<arg name="imu_rate" default="400.0" />

<node pkg="li_calib" type="li_calib_gui" name="li_calib_gui" output="screen">
<!-- <node pkg="li_calib" type="li_calib_gui" name="li_calib_gui" output="screen" clear_params="true" launch-prefix="gdb -ex run &#45;&#45;args">-->

<param name="topic_imu" type="string" value="$(arg topic_imu)" />
<param name="topic_lidar" type="string" value="/velodyne_packets" />
<param name="LidarModel" type="string" value="$(arg lidar_model)" />
<param name="path_bag" type="string" value="$(arg path_bag)" />
<param name="bag_start" type="double" value="$(arg bag_start)" />
<param name="bag_durr" type="double" value="$(arg bag_durr)" /> <!-- for data association -->
<param name="scan4map" type="double" value="$(arg scan4map)" />
<param name="ndtResolution" type="double" value="$(arg ndtResolution)" />
<param name="topic_imu" type="string" value="$(arg topic_imu)" />
<param name="topic_lidar" type="string" value="/velodyne_packets" />
<param name="LidarModel" type="string" value="$(arg lidar_model)" />
<param name="path_bag" type="string" value="$(arg path_bag)" />
<param name="bag_start" type="double" value="$(arg bag_start)" />
<param name="bag_durr" type="double" value="$(arg bag_durr)" /> <!-- for data association -->
<param name="scan4map" type="double" value="$(arg scan4map)" />
<param name="ndtResolution" type="double" value="$(arg ndtResolution)" />
<param name="gyroscope_noise" type="double" value="$(arg gyroscope_noise)" /> <!-- for sensor modeling -->
<param name="accelerometer_noise" type="double" value="$(arg accelerometer_noise)" />
<param name="lidar_noise" type="double" value="$(arg lidar_noise)" />
<param name="imu_rate" type="double" value="$(arg imu_rate)" />

<param name="time_offset_padding" type="double" value="$(arg time_offset_padding)" />
<param name="show_ui" type="bool" value="$(arg show_ui)" />
<param name="time_offset_padding" type="double" value="$(arg time_offset_padding)" />
<param name="show_ui" type="bool" value="$(arg show_ui)" />
</node>

</launch>
12 changes: 12 additions & 0 deletions src/ui/calib_helper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::string>("path_bag", bag_path_, "V1_01_easy.bag");
nh.param<std::string>("topic_imu", topic_imu_, "/imu0");
Expand All @@ -50,6 +52,10 @@ CalibrHelper::CalibrHelper(ros::NodeHandle& nh)
nh.param<double>("ndtResolution", ndt_resolution_, 0.5);
nh.param<double>("time_offset_padding", time_offset_padding, 0.015);
nh.param<double>("knot_distance", knot_distance, 0.02);
nh.param<double>("gyroscope_noise", gyroscope_noise, 1.745e-4);
nh.param<double>("accelerometer_noise", accelerometer_noise, 5.88e-4);
nh.param<double>("lidar_noise", lidar_noise, 0.02);
nh.param<double>("imu_rate", imu_rate, 400.0);

if (!createCacheFolder(bag_path_)) {
calib_step_ = Error;
Expand Down Expand Up @@ -79,6 +85,12 @@ CalibrHelper::CalibrHelper(ros::NodeHandle& nh)

traj_manager_ = std::make_shared<TrajectoryManager>(
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<ScanUndistortion>(
traj_manager_, dataset_reader_);
Expand Down