kubelvla
commented
3 months ago Hi,
For EKF-RIO, check the changes in my fork of their REVE as our point cloud formats from the radar(s) differ from the ones they used. I recall I have added one for the Hugin and one for the Oculli Eeagle.
I am also attaching the launch file and the calib files I used to run the EKF RIO. Check them out:
calib_oru.yaml:
## Topics
topic_imu: "/imu/data"
topic_baro_altimeter: "/sensor_platform/baro"
topic_radar_trigger: "/sensor_platform/radar_center/trigger"
topic_radar_scan: "/hugin_raf_1/radar_data"
#topic_ground_truth_pose: "/ground_truth/pose"
#topic_ground_truth_twist: "/ground_truth/twist"
#topic_ground_truth_twist_body: "/ground_truth/twist_body"
## Extrinsic calibration of body frame to radar frame
# translation of radar frame expressed in body frame
l_b_r_x: 0.200
l_b_r_y: 0.170
l_b_r_z: 0.080
# rotation radar frame to body frame
q_b_r_x: 0.699
q_b_r_y: 0.712
q_b_r_z: -0.047
q_b_r_w: -0.046
# length of each radar frame (="exposure time" of radar scan) in milliseconds
radar_frame_ms: 30.0
# nominal radar rate
radar_rate: 16and ekf_rio_oru.yaml:
# Default parameters for the demo datasets
# Documentation of the paramters can be found in the corresponding reconfigure files (see <package_name>/cfg/<package_name>/*.py)
# running rqt_reconfigure also provides tooltip text for the parameters and allows for online tuning
# General
frame_id: "odom"
# Publishers
publisher_rate: 30
pose_path_publisher_rate: 5
# Subscribers
topic_imu: "/imu/data"
topic_radar_scan: "/hugin_raf_1/radar_data"
run_without_radar_trigger: True
topic_baro_altimeter: "/sensor_platform/baro"
topic_radar_trigger: "/sensor_platform/radar/trigger"
# Rosbag mode --> used by rosbag node
republish_ground_truth: True
topic_ground_truth_pose: "/ground_truth/pose"
topic_ground_truth_twist: "/ground_truth/twist"
topic_ground_truth_twist_body: "/ground_truth/twist_body"
# KF Updates
altimeter_update: False
sigma_altimeter: 5.0
radar_update: True
# Radar Measurement Model
outlier_percentil_radar: 0.001
use_w: True
## radar ego velocity estimation
# filtering
min_dist: 0.25 # min distance of valid detection
max_dist: 100 # Max distance of valid detection
min_db: 5 # min SNR in [db]
elevation_thresh_deg: 60 # threshold for elevation [deg]
azimuth_thresh_deg: 60 # threshold fo azimuth [deg]
radar_velocity_correction_factor: 1.0 # Doppler velocity correction
filter_min_z: -100 # in -2 2 out
filter_max_z: 100
# zero velocity detection
thresh_zero_velocity: 0.05 # all inliers need to smaller than this value
allowed_outlier_percentage: 0.25 # outlier ratio (=percentage of detections which are allowed to be above thresh_zero_velocity)
sigma_zero_velocity_x: 0.025 # sigma v_r
sigma_zero_velocity_y: 0.025 # sigma_v_r
sigma_zero_velocity_z: 0.025 # sigma v_r
# result filtering
max_sigma_x: 0.2 # max estimated sigma to be considered an inlier (right)
max_sigma_y: 0.15 # (forward)
max_sigma_z: 0.2 # (up)
max_r_cond: 1.0e3 # max conditional number of LSQ Pseudo Inverse to ensure a stable result
use_cholesky_instead_of_bdcsvd: True # faster but less stable
# RANSAC parameters
use_ransac: True # turn on RANSAC LSQ
outlier_prob: 0.4 # worst case outlier probability
success_prob: 0.9999 # probability of successful determination of inliers
N_ransac_points: 3 # number of measurements used for the RANSAC solution
inlier_thresh: 0.15 # inlier threshold for inlier determination
# noise offset
sigma_offset_radar_x: 10.5 # offset added to estimated sigmas
sigma_offset_radar_y: 10.5
sigma_offset_radar_z: 10.5
# ODR refinement
use_odr: False # turn on odr refinement
min_speed_odr: 4.0 # min speed for ODR refinement
sigma_v_r: 0.125 # noise of v_r measurement used for the refinement
model_noise_offset_deg: 2.0 # min model noise
model_noise_scale_deg: 10.0 # scale model noise
# Initialization
T_init: 10
calib_gyro: true
g_n: 9.81
p_0_x: 0
p_0_y: 0
p_0_z: 0
v_0_x: 0
v_0_y: 0
v_0_z: 0
yaw_0_deg: -87.5201487
b_0_a_x: 0
b_0_a_y: 0
b_0_a_z: 0
b_0_w_x_deg: 0
b_0_w_y_deg: 0
b_0_w_z_deg: 0
b_0_alt: 0
# Initial Uncertainty
sigma_p: 0
sigma_v: 0
sigma_roll_pitch_deg: 0
sigma_yaw_deg: 0
sigma_b_a: 0.02
sigma_b_w_deg: 0.000003
sigma_b_alt: 0.1
sigma_l_b_r_x: 0.01
sigma_l_b_r_y: 0.01
sigma_l_b_r_z: 0.01
sigma_eul_b_r_roll_deg: 0.25
sigma_eul_b_r_pitch_deg: 0.25
sigma_eul_b_r_yaw_deg: 0.25
# Noise PSDs
noise_psd_a: 0.03 #0.03
noise_psd_w_deg: 0.6
noise_psd_b_a: 0.00001
noise_psd_b_w_deg: 0.00001
noise_psd_b_alt: 0.000001and finally the launch file:
<?xml version="1.0"?>
<!--This file is part of RIO - Radar Inertial Odometry and Radar based ego velocity estimation.-->
<!--@author Christopher Doer <christopher.doer@kit.edu>-->
<launch>
<param name="use_sim_time" value="True" type="bool"/>
<arg name="do_plot" default="False"/>
<arg name="enable_rviz" default="True"/>
<arg name="config" default="ekf_rio_oru"/>
<arg name="calibration" default="$(find ekf_rio)/config/calib_oru"/>
<arg name="filter_node_name" default="ekf_rio"/>
<arg name="log_level" default="Info"/>
<node name="$(arg filter_node_name)" pkg="ekf_rio" type="ros_node" output="screen">
<rosparam file="$(find ekf_rio)/config/$(arg config).yaml" command="load" ns=""/>
<rosparam file="$(arg calibration).yaml" command="load" ns=""/>
<param name="republish_ground_truth" value="false" type="bool"/>
</node>
<!-- <node name="evaluator" pkg="ekf_rio" type="pose_velocity_evaluator.py" output="screen" required="True"> -->
<!-- <rosparam file="$(arg calibration).yaml" command="load" ns=""/> -->
<!-- <param name="topic_pose" value="$(arg filter_node_name)/pose" type="string"/> -->
<!-- <param name="topic_twist" value="$(arg filter_node_name)/twist" type="string"/> -->
<!-- <param name="filter_name" value="ekf_rio" type="string"/> -->
<!-- <param name="do_plot" value="$(arg do_plot)" type="bool"/> -->
<!-- </node> -->
<node pkg="rosservice" type="rosservice" name="set_$(arg filter_node_name)_log_level"
args="call --wait /$(arg filter_node_name)/set_logger_level 'ros.$(arg filter_node_name)' '$(arg log_level)'"/>
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find ekf_rio)/config/ekf_rio.rviz" if="$(arg enable_rviz)"/>
</launch>
spearwin
Thank you for sharing the Hugin radar dataset, which seems to be a good fit for my application. I have been trying to run EKF-RIO with this dataset but am facing some difficulties. I have used the extrinsic parameters from the rosbag file, specifically the TF_hugin_to_baselink values, and the 90-degree rotation value between the base_link and the IMU.
I noticed that in the paper "Do we need scan-matching in radar odometry?", EKF-RIO was tested. Could you please share the EKF-RIO config file that is compatible with the Hugin radar dataset? Any help would be greatly appreciated.