add Run it yourself section with example

leggedrobotics · Jun 13, 2024 · b7ac941 · b7ac941
1 parent 689a9c6
commit b7ac941
Show file tree

Hide file tree

Showing 13 changed files with 91 additions and 13 deletions.
diff --git a/README.md b/README.md
@@ -52,10 +52,32 @@ roslaunch rms rms_nodelet.launch NS:=<NAMESPACE> points_in:=<POINTS IN TOPIC> 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 [email protected]: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:=<PATH TO ROSBAG> 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
@@ -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
diff --git a/example/fig/APE_comparison.png b/example/fig/APE_comparison.png
diff --git a/example/fig/APE_vanilla.png b/example/fig/APE_vanilla.png
diff --git a/example/fig/RPE_comparison.png b/example/fig/RPE_comparison.png
diff --git a/example/fig/plot_RMS.png b/example/fig/plot_RMS.png
diff --git a/example/fig/plot_vanilla.png b/example/fig/plot_vanilla.png
diff --git a/example/fig/sampling_example.png b/example/fig/sampling_example.png
diff --git a/example/sejong01.md 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
@@ -2,11 +2,12 @@
 
   <arg name="NS" default="$(optenv UAV_NAME)" />
 
-  <arg name="standalone"        default="true" />
-  <arg name="debug"             default="false" />
-  <arg name="custom_config"     default="" />
-  <arg name="node_name"         default="rms_nodelet" />
-  <arg name="name_postfix"      default="" />
+  <arg name="standalone"     default="true" />
+  <arg name="debug"          default="false" />
+  <arg name="custom_config"  default="" />
+  <arg name="node_name"      default="rms_nodelet" />
+  <arg name="name_postfix"   default="" />
+  <arg name="verbose"        default="false" />
 
   <!-- Inputs -->
   <arg name="points_in" default="/$(arg NS)/os_cloud_nodelet/points_processed" />
@@ -37,6 +38,8 @@
 
       <rosparam file="$(find rms)/config/default.yaml" command="load" />
       <rosparam if="$(eval not arg('custom_config') == '')" file="$(arg custom_config)" command="load" />
+
+      <param name="verbose" value="$(arg verbose)" />
 
       <!-- Subscribers -->
       <remap from="~points_in" to="$(arg points_in)" />

diff --git 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<int>());
+    const auto pt_eig = Eigen::Vector3f(pt.x, pt.y, pt.z);
+    const auto voxel = t_voxel((pt_eig / voxel_size).cast<int>());
 
     // voxel occupied: erase this point's index
     if (grid.contains(voxel)) {

diff --git 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<bool>("verbose", _verbose);
   _rms = std::make_unique<RMS>(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);