diff --git a/README.md b/README.md index 67f620d..24bfc36 100644 --- a/README.md +++ b/README.md @@ -52,10 +52,32 @@ roslaunch rms rms_nodelet.launch NS:= points_in:= po sensor_msgs::PointCloud2::Ptr msg = ...; rms->sample(msg); // 'msg' now contains sampled data ``` - - **example**: `RMSNodelet` implemented in [src/rms_nodelet.cpp](https://github.com/ctu-mrs/RMS/blob/master/src/rms_nodelet.cpp) + - **example usage**: `RMSNodelet` implemented in [src/rms_nodelet.cpp](https://github.com/ctu-mrs/RMS/blob/master/src/rms_nodelet.cpp) -### Example -[WORK IN PROGRESS] To be added in coming days. +### Run it yourself +To complement the in-paper experiments, we offer comparison on the [MulRan](https://sites.google.com/view/mulran-pr/dataset) dataset by plugging its 3D LiDAR (Ouster OS1-64) data to the [KISS-ICP](https://github.com/PRBonn/kiss-icp) odometry. +For **ROS Noetic**, you may follow this workflow: + +1) Click [here](todo) to download the `Sejong01` sequence rosbag. +2) Install RMS (see `Installation` above). +3) Clone, compile, and source our [KISS-ICP fork](https://github.com/petrapa6/kiss-icp) (minor changes made for ROS Noetic and launching). +```bash +cd ~/ROS1_WORKSPACE/src +git clone git@github.com:petrapa6/kiss-icp.git +cd kiss_icp +git checkout noetic +catkin build --this +source ~/ROS1_WORKSPACE/devel/setup.sh +``` +4) Launch as: +```bash + roslaunch kiss_icp odometry.launch bagfile:= topic:=/mulran/velo/pointclouds use_RMS:=[true | false] +``` + +[**Results for the `Sejong01` experiment here.**](https://github.com/ctu-mrs/RMS/blob/master/example/sejong01.md). +APE of the experiment (voxelization in blue, RMS in orange): + +![ape rms](./example/fig/APE_comparison.png) ## How to cite ```tex diff --git a/config/default.yaml b/config/default.yaml index 5774f10..ba9485c 100644 --- a/config/default.yaml +++ b/config/default.yaml @@ -2,5 +2,5 @@ lambda: 0.004 # <0, 1> | Alg 1: L5 | tune for your estimation pipeline K: 10 # number of bins | Alg 1: L4 | should not need to be changed voxelization: - input: 0.2 # (0, inf) (m) | applied before RMS | Alg 1: L3 | lower: higher timing, higher: lower timing - output: 0.2 # <0, inf) (m) | applied after RMS + input: 0.4 # (0, inf) (m) | applied before RMS | Alg 1: L3 | lower: higher timing, higher: lower timing + output: 0.4 # <0, inf) (m) | applied after RMS (only when greater than 'voxelization/input') diff --git a/example/fig/APE_RMS.png b/example/fig/APE_RMS.png new file mode 100644 index 0000000..91951d3 Binary files /dev/null and b/example/fig/APE_RMS.png differ diff --git a/example/fig/APE_comparison.png b/example/fig/APE_comparison.png new file mode 100644 index 0000000..b031b40 Binary files /dev/null and b/example/fig/APE_comparison.png differ diff --git a/example/fig/APE_vanilla.png b/example/fig/APE_vanilla.png new file mode 100644 index 0000000..ac199e2 Binary files /dev/null and b/example/fig/APE_vanilla.png differ diff --git a/example/fig/RPE_comparison.png b/example/fig/RPE_comparison.png new file mode 100644 index 0000000..9966807 Binary files /dev/null and b/example/fig/RPE_comparison.png differ diff --git a/example/fig/plot_RMS.png b/example/fig/plot_RMS.png new file mode 100644 index 0000000..af0124d Binary files /dev/null and b/example/fig/plot_RMS.png differ diff --git a/example/fig/plot_vanilla.png b/example/fig/plot_vanilla.png new file mode 100644 index 0000000..0babbff Binary files /dev/null and b/example/fig/plot_vanilla.png differ diff --git a/example/fig/sampling_example.png b/example/fig/sampling_example.png new file mode 100644 index 0000000..8f3dceb Binary files /dev/null and b/example/fig/sampling_example.png differ diff --git a/example/sejong01.md b/example/sejong01.md new file mode 100644 index 0000000..0e46efb --- /dev/null +++ b/example/sejong01.md @@ -0,0 +1,49 @@ +## MulRan: Sejong01 + +Experiment summary: +- Robot: car +- Sensor: Ouster OS1-64 +- Odometry: KISS-ICP +- Trajectory length: 23 km +- Duration: 47 min +- Mean speed: 12.4 m/s +- Max speed: 20.3 m/s +- Results below comptued on: AMD Ryzen 7 PRO 4750U + +Graphs' legend from top to bottom: + +- timing (better: higher) +- compression (better: higher) +- per-point information in the optimization (better: higher) +- \# of ICP iterations till convergence (better: lower) + +### Pure KISS-ICP +Results: Unstable with a lot of redundancy in the data. + +![data vanilla](./fig/plot_vanilla.png) + +### KISS-ICP preceded by RMS +Results: +Low and stable timing+compression, which effectively lowers the \# of ICP iterations (registration time lowered 12 times on average!). +High level of information preserved. + +![data rms](./fig/plot_RMS.png) + +### APE and RPE comparison +$${\color{blue}\text{Blue}$$: pure KISS-ICP, $${\color{orange}\text{orange}$$: KISS-ICP preceded by RMS. +Results: Higher drift reduction by RMS. + +| Sampling method | RMSE (m) | mean (m) | median (m) | max (m) | +| Voxelization | 7100 | 6511 | 7815 | 9581 | +| RMS | 2230 | 1953 | 2352 | 3292 | + +![ape rms](./fig/APE_comparison.png) + +| Sampling method | RMSE (m) | mean (m) | median (m) | max (m) | +| Voxelization | 17.9 | 4.3 | 2.7 | 1311.6 | +| RMS | 3.1 | 2.9 | 2.6 | 21.6 | + +![rpe rms](./fig/RPE_comparison.png) + +### Example sampling of a single frame +![frame](./fig/sampling_example.png) diff --git a/launch/rms_nodelet.launch b/launch/rms_nodelet.launch index 33e1254..4f35e6b 100644 --- a/launch/rms_nodelet.launch +++ b/launch/rms_nodelet.launch @@ -2,11 +2,12 @@ - - - - - + + + + + + @@ -37,6 +38,8 @@ + + diff --git a/src/rms.cpp b/src/rms.cpp index dbf86f5..f4690e2 100644 --- a/src/rms.cpp +++ b/src/rms.cpp @@ -195,7 +195,8 @@ void RMS::voxelizeIndices(const float voxel_size, const t_points pc_in, t_indice continue; } - const auto voxel = t_voxel((Eigen::Vector3f(pt.x, pt.y, pt.z) / voxel_size).cast()); + const auto pt_eig = Eigen::Vector3f(pt.x, pt.y, pt.z); + const auto voxel = t_voxel((pt_eig / voxel_size).cast()); // voxel occupied: erase this point's index if (grid.contains(voxel)) { diff --git a/src/rms_nodelet.cpp b/src/rms_nodelet.cpp index 2b93892..467c218 100644 --- a/src/rms_nodelet.cpp +++ b/src/rms_nodelet.cpp @@ -16,6 +16,7 @@ class RMSNodelet : public nodelet::Nodelet { private: bool _is_initialized = false; + bool _verbose = false; ros::Subscriber _sub_points; void callbackPointCloud(const sensor_msgs::PointCloud2::ConstPtr &msg); @@ -42,6 +43,7 @@ void RMSNodelet::onInit() { // Load parameters and setup objects mrs_lib::ParamLoader param_loader = mrs_lib::ParamLoader(nh, "RMSNodelet"); + param_loader.loadParam("verbose", _verbose); _rms = std::make_unique(param_loader); if (!param_loader.loadedSuccessfully()) { @@ -79,8 +81,9 @@ void RMSNodelet::callbackPointCloud(const sensor_msgs::PointCloud2::ConstPtr &ms const size_t size_after = msg_out->width * msg_out->height; _frame_no++; _t_total += runtime; - NODELET_INFO("[RMSNodelet] RMS sampling -> %ld/%ld pts | compression: %.2f %s | timing: %.2f ms (avg: %0.2f ms)", size_after, size_before, - 100.0f - 100.0f * (float(size_after) / float(size_before)), "%", runtime, _t_total / float(_frame_no)); + + NODELET_INFO_COND(_verbose, "[RMSNodelet] RMS sampling -> %ld/%ld pts | compression: %.2f %s | timing: %.2f ms (avg: %0.2f ms)", size_after, size_before, + 100.0f - 100.0f * (float(size_after) / float(size_before)), "%", runtime, _t_total / float(_frame_no)); // Publish publishCloud(_pub_points, msg_out);