search

dan11003 / tbv_slam_public

Radar SLAM in C++ / ROS
https://arxiv.org/abs/2301.04397
Other
81 stars 9 forks source link

How to run `./run_tbv_simple` demo without ground truth? #10

Closed MaxwellJung closed 4 months ago

MaxwellJung commented 5 months ago

Hi there,

We're a group of undergrads working on making an autonomous boat for our senior capstone project. We found your paper recently and are very interested in integrating it into our project, but we would like some help.

Currently, we have our own radar and ros2 nodes that publish the polar radar image to /Navtech/Polar topic. I tried bagging that topic, coverting the ros2 bag into ros1 using rosbags, renaming the ros1 bag to 2019-01-10-12-32-52-radar-oxford-10k.bag and following the demo as outlined in the readme. However, running the demo crashes immediately (something with rviz error) so I'm wondering if we're missing something in our custom bag file.

Note that we were able to successfully run the demo using the provided oxford dataset. Looking into the script, it looks like the provided bag file also records the ground truth odometry.

My question is: Is there a way to run the ./run_tbv_simple using just the polar images? I know the CFEAR_Radarodometry_code_public has an online vs offline mode so should we try running it online?

MaxwellJung commented 5 months ago

@dan11003

dan11003 commented 5 months ago

Hi Maxwell, Yes the Oxford and Mulran bags contain ground truth. I've previously tested it on the sequences Kvarntorp and Volvo which doesn't have ground truth included. What's output in the terminal? The online odometry node in the released CFEAR packages is not up to date.

Don't forget to set radar_ccw="true" and dataset="any name"

MaxwellJung commented 5 months ago

update: I resolved the rviz error and was able to run the demo from both this repo (tbv) and the other repo (cfear).

I'm pretty sure the demo is running as expected using our custom polar images, but I see "error, cloud empty" in the output. This makes me think that our custom radar images are too low quality or incomplete, so we're gonna try to get some better data tomorrow. Looks like it's a classic case of "garbage in, garbage out" since half of our radar images were corrupted due to software bug.

Here's the full terminal output after running ./run_tbv_simple.sh using our custom polar image bag renamed to "2019-01-10-12-32-52-radar-oxford-10k".

If you notice anything else in the output below, please let us know.

root@mint-ssd:~/tbv_ws/src/tbv_slam_public/tbv_slam/script/oxford# ./run_tbv_simple.sh
rviz: no process found
/BAG_LOCATION/oxford-eval-sequences/2019-01-10-12-32-52-radar-oxford-10k/radar/2019-01-10-12-32-52-radar-oxford-10k.bag
mkdir: cannot create directory ‘//BAG_LOCATION/TBV_Eval/oxford_tbv_model_8_2024-05-18_01-25’: File exists
mkdir: cannot create directory ‘//BAG_LOCATION/TBV_Eval/oxford_tbv_model_8_2024-05-18_01-25/job_0’: File exists
mkdir: cannot create directory ‘//BAG_LOCATION/TBV_Eval/oxford_tbv_model_8_2024-05-18_01-25/job_0/est’: File exists
mkdir: cannot create directory ‘//BAG_LOCATION/TBV_Eval/oxford_tbv_model_8_2024-05-18_01-25/job_0/gt’: File exists
|||||Estimating odometry with parameters: --range-res 0.0438 --sequence 2019-01-10-12-32-52-radar-oxford-10k --radar_ccw false --soft_constraint false --disable_compensate false --cost_type P2P --submap_scan_size 4 --registered_min_keyframe_dist 1.5 --res 3 --k_strongest 40 --bag_path /BAG_LOCATION/oxford-eval-sequences/2019-01-10-12-32-52-radar-oxford-10k/radar/2019-01-10-12-32-52-radar-oxford-10k.bag --est_directory /BAG_LOCATION/TBV_Eval/oxford/2019-01-10-12-32-52-radar-oxford-10k/est/ --gt_directory /BAG_LOCATION/TBV_Eval/oxford/2019-01-10-12-32-52-radar-oxford-10k/gt/ --training_directory /BAG_LOCATION/TBV_Eval/oxford/2019-01-10-12-32-52-radar-oxford-10k/training/ --radar_directory /BAG_LOCATION/TBV_Eval/oxford/2019-01-10-12-32-52-radar-oxford-10k/radar/ --job_nr 1 --z-min 60 --loss_type Huber --loss_limit 0.1  --weight_intensity true --cfear_method cfear-3 --weight_option 4 --dataset oxford --store_graph true --save_radar_img false --save_ROC false --covar_sampling False --disable_training true --experiment_name oxford_tbv_model_8 --input_directory //BAG_LOCATION/TBV_Eval --tbv_method TBV SLAM-6 --visualize true --N_CANDIDATES 1 --all_candidates false --model_threshold 0.9 --augment_sc true --odometry_coupled_closure true --raw_radar_scan false --model_features odom-bounds sc-sim alignment_quality --model_training_file_load tbv_model_8.txt  --load_trained_classifier true --speedup true --use_covariance_sampling_in_loop_closure false --output_directory //BAG_LOCATION/TBV_Eval/oxford_tbv_model_8_2024-05-18_01-25/job_0||||
... logging to /root/.ros/log/cb0d0510-14b2-11ef-8f2c-f426796c0e94/roslaunch-mint-ssd-3986.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

SLAM pars: 
DerivedMiniClosure - MiniClosure_enabled, 0
DerivedMiniClosure - max_d_close_, 15.000000
DerivedMiniClosure - max_d_travel_, 15.000000
DerivedMiniClosure - min_d_travel_, 25.000000
GTVicinity - GTVicinity_enabled, 0
GTVicinity - max_d_close_, 15.000000
GTVicinity - max_d_travel_, 4200.000000
GTVicinity - min_d_travel_, 40.000000
SC - NUM_CANDIDATES_FROM_TREE, 10
SC - N_CANDIDATES, 1
SC - PC_NUM_RING, 40
SC - PC_NUM_SECTORS, 120
SC - SC_DIST_THRES, 0.800000
SC - SEARCH_RATIO, 0.100000
SC - augment_sc, 1
SC - desc_divider, 1000.000000
SC - desc_function, sum
SC - desc_plot, 0
SC - interpolation, area
SC - no_point, 0.000000
SC - normalize, 0
SC - odom_sigma_error, 0.050000
SC - odometry_coupled_closure, 1
SC - prints, 1
SC - radar_threshold, 0.000000
Scan Context - N_aggregate, 1
Scan Context - SCClosure_enabled, 1
Scan Context - raw_scan_context, 0
Scan Context - use_peaks, 1
all_candidates, 0
data_dir, //BAG_LOCATION/TBV_Eval/oxford/2019-01-10-12-32-52-radar-oxford-10k/radar
idx_halt, 0
model_threshold, 0.900000
model_training_file_load, tbv_model_8.txt
odom_sigma_error, 0.050000
registration_disabled, 0
training_data_dir, /root/tbv_ws/src/tbv_slam_public/tbv_slam/model_parameters/
verification_disabled, 0
verify_via_odometry, 1

Eval pars: 
input_directory, //BAG_LOCATION/TBV_Eval
dataset, oxford
sequence, 2019-01-10-12-32-52-radar-oxford-10k
simple_graph path, //BAG_LOCATION/TBV_Eval/oxford/2019-01-10-12-32-52-radar-oxford-10k/est/simple_graph.sgh
eval_output dir, //BAG_LOCATION/TBV_Eval/oxford_tbv_model_8_2024-05-18_01-25/job_0
visualize, true
experiment name, oxford_tbv_model_8
method, TBV

Pose graph pars: 
disable_optimization, 0
est_output_dir, //BAG_LOCATION/TBV_Eval/oxford_tbv_model_8_2024-05-18_01-25/job_0/est
gt_output_dir, //BAG_LOCATION/TBV_Eval/oxford_tbv_model_8_2024-05-18_01-25/job_0/gt

Output directory: //BAG_LOCATION/TBV_Eval/oxford_tbv_model_8_2024-05-18_01-25/job_0
Odometry parameters:
range res, 0.0438
z min, 60
min distance, 2.5
max distance, 200
k strongest, 40
topic_filtered, /Navtech/Filtered
radar_frameid, sensor_est
dataset, oxford
filter type, kstrong
nb guard cells, 40
window size, 1
false alarm rate, 1
input_points_topic, /Navtech/Filtered
scan_registered_latest_topic, radar_registered
scan_registered_keyframe_topic, radar_registered_keyframe
odom_latest_topic, radar_odom
odom_keyframe_topic, radar_odom_keyframe
use raw pointcloud, false
submap keyframes, 4
resolution r,3
resample factor f, 1
min. sensor distance [m], 1.5
min. sensor rot. [deg], 5
use keyframe, true
use initial guess, true
radar reversed, false
disable registration, false
soft velocity constraint, false
compensate, true
cost type, P2P
loss type, Huber
loss limit, 0.100000
covar scale, 1.000000
regularization, 1.000000
weight intensity, true
publish_tf, true
store graph, true
Weight, 4
Use cost sampling for covariance, false
Save cost samples to a file, false
Cost-samples-file folder, /tmp/cfear_out
XY sampling range, 0.4
Yaw sampling range, 0.0043625
Cost samples per axis, 3
Sampled covariance scale, 4
odom_est_topic, 
gt_topic, 
est_output_dir, /BAG_LOCATION/TBV_Eval/oxford/2019-01-10-12-32-52-radar-oxford-10k/est/
gt_output_dir, /BAG_LOCATION/TBV_Eval/oxford/2019-01-10-12-32-52-radar-oxford-10k/gt/
sequence, 2019-01-10-12-32-52-radar-oxford-10k
job nr, 1
save pcd, 0
method, cfear-3
nr_frames, 0

Started optimization at rate :1
starting : ScanContextClosure 
3.8.10 (default, Nov 22 2023, 10:22:35) 
[GCC 9.4.0]
started roslaunch server http://mint-ssd:41105/

SUMMARY
========

PARAMETERS
 * /rosdistro: noetic
 * /rosversion: 1.16.0

NODES
  /
    rviz (rviz/rviz)

ROS_MASTER_URI=http://localhost:11311

process[rviz-1]: started with pid [4044]
3.8.10 (default, Nov 22 2023, 10:22:35) 
[GCC 9.4.0]
3.8.10 (default, Nov 22 2023, 10:22:35) 
[GCC 9.4.0]
3.8.10 (default, Nov 22 2023, 10:22:35) 
[GCC 9.4.0]
QStandardPaths: XDG_RUNTIME_DIR not set, defaulting to '/tmp/runtime-root'
callback disabled
3.8.10 (default, Nov 22 2023, 10:22:35) 
[GCC 9.4.0]
3.8.10 (default, Nov 22 2023, 10:22:35) 
[GCC 9.4.0]
3.8.10 (default, Nov 22 2023, 10:22:35) 
[GCC 9.4.0]
Loading bag from: /BAG_LOCATION/oxford-eval-sequences/2019-01-10-12-32-52-radar-oxford-10k/radar/2019-01-10-12-32-52-radar-oxford-10k.bag
error, cloud empty
gt dir /BAG_LOCATION/TBV_Eval/oxford/2019-01-10-12-32-52-radar-oxford-10k/gt
results dir /BAG_LOCATION/TBV_Eval/oxford/2019-01-10-12-32-52-radar-oxford-10k/est
patss: 01.txt
dir: /BAG_LOCATION/TBV_Eval/oxford/2019-01-10-12-32-52-radar-oxford-10k/est
load/BAG_LOCATION/TBV_Eval/oxford/2019-01-10-12-32-52-radar-oxford-10k/est/01.txt
load/BAG_LOCATION/TBV_Eval/oxford/2019-01-10-12-32-52-radar-oxford-10k/gt/01.txt
save: 130terms, step size: 10
Sequence-nr: 1
Translational error (%):  0.9554346695040388
Rotational error (deg/100m):  0.2195172573792138
ATE (m):  0.8158431432541323
RPE (m):  0.053233743721028716
RPE (deg):  0.034243508395772934
RPE dev (m):  0.04696601874453307
RPE rot dev(deg):  0.03981937582821803
bias_x 0.0010964305487378254
bias_y -0.013139898025973113
bias_theta 0.0
RMSE (m) 0.0709904105303526
QStandardPaths: XDG_RUNTIME_DIR not set, defaulting to '/tmp/runtime-root'
-------------------- For Copying ------------------------------
0.96
0.22
0.816
0.0532
0.0342
0.0470
0.0398
0.0011
-0.0131
0.0000
0.0710
No such csv file!
mkdir: cannot create directory ‘//BAG_LOCATION/TBV_Eval/oxford_tbv_model_8_2024-05-18_01-25/job_0/odom’: File exists
Looking for jobs under: {} //BAG_LOCATION/TBV_Eval/oxford_tbv_model_8_2024-05-18_01-25/
Job directory//BAG_LOCATION/TBV_Eval/oxford_tbv_model_8_2024-05-18_01-25/
Files to merge files: ['est/result.txt', 'pars.txt']
Save evaluation to  //BAG_LOCATION/TBV_Eval/oxford_tbv_model_8_2024-05-18_01-25//oxford_tbv_model_8_2024-05-18_01-25_slam_eval.csv
Looking for jobs under: {} //BAG_LOCATION/TBV_Eval/oxford_tbv_model_8_2024-05-18_01-25/
Job directory//BAG_LOCATION/TBV_Eval/oxford_tbv_model_8_2024-05-18_01-25/
Files to merge files: ['loop/result.txt', 'pars.txt']
Save evaluation to  //BAG_LOCATION/TBV_Eval/oxford_tbv_model_8_2024-05-18_01-25//oxford_tbv_model_8_2024-05-18_01-25_loops_eval.csv
Looking for jobs under: {} //BAG_LOCATION/TBV_Eval/oxford_tbv_model_8_2024-05-18_01-25/
Job directory//BAG_LOCATION/TBV_Eval/oxford_tbv_model_8_2024-05-18_01-25/
Files to merge files: ['odom/result.txt', 'odom/pars.txt']
Save evaluation to  //BAG_LOCATION/TBV_Eval/oxford_tbv_model_8_2024-05-18_01-25//oxford_tbv_model_8_2024-05-18_01-25_odometry_eval.csv
MaxwellJung commented 5 months ago

Is there a way to run tbv_slam_online node without supplying a bag file? My guess is that we have to modify this line in tbv_slam_online.cpp to subscribe to the Navtech/Polar topic instead of reading the messages from the bag.

Also just for clarification, is the definition of offline vs online flipped in tbv compared to CFEAR? I noticed that tbv_slam/src/tbv_slam_online.cpp looked suspciously similar to cfear_radarodometry/src/offline_odometry.cpp.