add code, how to, and acknowledgment

.gitignore

@@ -0,0 +1,2 @@
+*.swo
+*.swp

CMakeLists.txt

@@ -0,0 +1,100 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(rms)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  nodelet
+  sensor_msgs
+  pcl_ros
+  mrs_lib
+)
+
+# TSL Robin Map for voxelization
+add_subdirectory(thirdparty/robin-map)
+
+# Point Cloud Library
+find_package(PCL REQUIRED)
+
+# Eigen
+find_package(Eigen3 REQUIRED)
+set(Eigen_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS})
+set(Eigen_LIBRARIES ${Eigen_LIBRARIES})
+
+set(LIBRARIES
+  RMS RMSNodelet
+)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES ${LIBRARIES}
+  CATKIN_DEPENDS roscpp nodelet sensor_msgs pcl_ros mrs_lib
+  DEPENDS Eigen
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${PCL_INCLUDE_DIRS}
+  ${Eigen_INCLUDE_DIRS}
+)
+
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+
+add_library(RMS
+  src/histogram.cpp
+  src/rms.cpp
+)
+
+add_dependencies(RMS
+  ${${PROJECT_NAME}_EXPORTED_TARGETS}
+  ${catkin_EXPORTED_TARGETS}
+)
+
+target_link_libraries(RMS
+  ${catkin_LIBRARIES}
+  ${PCL_LIBRARIES}
+  ${Eigen_INCLUDE_DIRS}
+  tsl::robin_map
+)
+
+add_library(RMSNodelet
+  src/histogram.cpp
+  src/rms.cpp
+  src/rms_nodelet.cpp
+)
+
+add_dependencies(RMSNodelet
+  ${${PROJECT_NAME}_EXPORTED_TARGETS}
+  ${catkin_EXPORTED_TARGETS}
+)
+
+target_link_libraries(RMSNodelet
+  ${catkin_LIBRARIES}
+  ${PCL_LIBRARIES}
+  ${Eigen_INCLUDE_DIRS}
+  tsl::robin_map
+)
+
+## --------------------------------------------------------------
+## |                           Install                          |
+## --------------------------------------------------------------
+
+install(TARGETS ${LIBRARIES}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+)
+
+install(DIRECTORY launch config
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+install(DIRECTORY ./
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  FILES_MATCHING PATTERN "*.xml"
+)

README.md

@@ -2,36 +2,78 @@
 
 [![RMS](./fig/snapshot.jpg)](https://www.youtube.com/watch?v=Y9ZlRrX1UBY)
 
-  * novel method for sampling large 3D LiDAR point clouds
-  * replacement for voxelization
-    * pipelines using **RMS** are faster (lower latency) and more accurate (less drift)
+  * quick sampling of 3D LiDAR point clouds
+    * pipelines using **RMS** are fast (low latency) and accurate
   * designed for **real-time LiDAR-based** 6-DoF odometry/SLAM pipelines
     * both point-based (ICP-like) and feature-based (LOAM-like) methods 
-    * potentially improving most L, LI, LVI, LV pipelines  
-  * **single parameter only -> depends on the SLAM pipeline (and not the environment!)**
+  * **single parameter only: [lambda](https://github.com/ctu-mrs/RMS/blob/master/config/default.yaml)**
+    * depends on the SLAM pipeline (and not the environment!)
     * tuned just once given your pipeline
   * deterministic (no data for learning needed) 
-  * when is it (probably) not going to perform well:
+  * when is it not going to perform well (most probably):
     * the data have large orientation changes between two consecutive frames (tens of degrees)
-    * under heavy noise (such as dust clouds)
+    * under heavy noise
 
-#### Code & How to
-The code will be made available upon acceptance.
+## Paper
+Published in IEEE RA-L --- [pdf](https://arxiv.org/pdf/2312.07337.pdf).
 
-#### Paper
-Submitted to IEEE RA-L on December 1, 2023.
-Preprint available at [arXiv](https://arxiv.org/pdf/2312.07337.pdf).
+## Code & How to
 
-#### How to cite
+### Installation
+1) Install prerequisities (`mrs_lib`, `PCL`):
+```bash
+  curl https://ctu-mrs.github.io/ppa-stable/add_ppa.sh | bash
+  apt-get install ros-noetic-mrs-lib ros-noetic-pcl-ros
 ```
-@article{petracek2023rms,
-  title   = {{RMS: Redundancy-Minimizing Point Cloud Sampling for Real-Time Pose Estimation in Degenerated Environments}}, 
-  author  = {Petracek, Pavel and Alexis, Kostas and Saska, Martin}, 
-  year    = {2023},
-  journal = {arXiv:2312.07337},
-  note    = {Submitted to IEEE RA-L on December 1, 2023}
+2) Clone and build via `catkin`
+```bash
+  cd <ROS1_WORKSPACE>/src
+  git clone [email protected]:ctu-mrs/RMS.git
+  catkin build
+```
+
+### How to use
+1) Launch as nodelet:
+```bash
+roslaunch rms rms_nodelet.launch NS:=<NAMESPACE> points_in:=<POINTS IN TOPIC> points_out:=<POINTS OUT TOPIC>
+```
+2) Use as library in your code:
+  - add `rms` among dependencies in `CMakeLists.txt` and `package.xml` and include the `<rms/rms.h>` header file
+  - basic usage: `<rms/rms.h>` and use as in [src/rms_nodelet.cpp](https://github.com/ctu-mrs/RMS/blob/master/src/rms_nodelet.cpp) example
+    ```cpp
+    #include <rms/rms.h>
+    ...
+    // Initialize
+    ros::NodeHandle nh;
+    mrs_lib::ParamLoader param_loader(nh, "RMS");
+    RMS rms = RMS(param_loader);
+    ...
+    // Use
+    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
+[WORK IN PROGRESS] To be added in coming days.
+
+## How to cite
+```tex
+@article{petracek2024rms,
+  author  = {Petracek, Pavel and Alexis, Kostas and Saska, Martin},
+  title   = {{RMS: Redundancy-Minimizing Point Cloud Sampling for Real-Time Pose Estimation}},
+  journal = {IEEE Robotics and Automation Letters},
+  year    = {2024},
+  volume  = {9},
+  number  = {6},
+  pages   = {5230--5237},
+  doi     = {10.1109/LRA.2024.3389820}
 }
 ```
 
-#### Acknowledgment
-To be added upon acceptance.
+## Acknowledgment
+This work was supported
+- by CTU grant no. SGS23/177/OHK3/3T/13,
+- by the Czech Science Foundation under research project No. 23-06162M,
+- by the European Union under the project Robotics and advanced industrial production (reg. no. CZ.02.01.01/00/22_008/0004590), and
+- by the Research Council of Norway Award NO-321435.

config/default.yaml

@@ -0,0 +1,6 @@
+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

include/rms/histogram.h

@@ -0,0 +1,58 @@
+#pragma once
+
+#include <vector>
+#include <iostream>
+#include <cmath>
+#include <algorithm>
+#include <set>
+
+namespace rms
+{
+
+// key: point index, value: GFH norm
+using t_gfh = std::pair<size_t, float>;
+using t_indices = std::vector<size_t>;
+
+/* //{ class Histogram1D */
+
+class Histogram1D {
+
+public:
+  Histogram1D(const size_t no_bins, const float min_value, const float max_value, const std::vector<t_gfh> &gfh);
+
+  float  getBinWidth();
+  size_t getBinCount();
+
+  void selectByUniformnessMaximization();
+  void selectByUniformnessMaximization(float &entropy);
+
+  void selectByNormMaximization();
+  void selectByNormMaximization(float &entropy);
+
+  t_indices getSelectedIndices();
+
+private:
+  size_t _no_bins;
+  size_t _point_count_remaining;
+  size_t _point_count_selected;
+
+  size_t _bin_sel_ptr;
+
+  float _bin_width;
+  float _val_min;
+  float _val_max;
+
+  // vector (size: number of bins) of vectors of pairs (first: index in original cloud; second: value) sorted in descending order by pair value
+  std::vector<std::vector<t_gfh>> _counts_remaining;
+  std::vector<std::vector<t_gfh>> _counts_selected;
+
+  void construct(const std::vector<t_gfh> &gfh);
+
+  size_t             getBin(const float point);
+  std::vector<float> estimatePDF();
+  float              computeEntropy(const std::vector<float> &pdf);
+};
+
+//}
+
+}  // namespace rms

include/rms/rms.h

@@ -0,0 +1,61 @@
+#include <ros/ros.h>
+
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/kdtree/kdtree_flann.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/point_cloud2_iterator.h>
+
+#include <mrs_lib/param_loader.h>
+#include <mrs_lib/scope_timer.h>
+
+#include <rms/histogram.h>
+
+namespace rms
+{
+
+/* //{ class RMS */
+
+using t_points  = pcl::PointCloud<pcl::PointXYZ>::Ptr;
+using t_voxel   = Eigen::Vector3i;
+
+class RMS {
+
+  // | ---------------------- Voxelization ---------------------- |
+  struct VoxelHash
+  {
+    size_t operator()(const t_voxel& voxel) const {
+      const std::uint32_t* vec = reinterpret_cast<const uint32_t*>(voxel.data());
+      return ((1 << 20) - 1) & (vec[0] * 73856093 ^ vec[1] * 19349663 ^ vec[2] * 83492791);
+    }
+  };
+
+  // | ----------------------- Public API ----------------------- |
+public:
+  RMS(mrs_lib::ParamLoader& param_loader);
+  void sample(sensor_msgs::PointCloud2::Ptr& msg_inout);
+
+  // | --------------- ROS and conversion methods --------------- |
+private:
+  void fromROSMsg(const sensor_msgs::PointCloud2::Ptr cloud, t_points& pcl_cloud);
+  void extractByIndices(sensor_msgs::PointCloud2::Ptr& msg_inout, t_indices& indices);
+
+  // | ---------------- RMS variables and methods --------------- |
+private:
+  size_t _K            = 10;
+  float  _lambda       = 1.0f;
+  float  _voxel_input  = -1.0f;
+  float  _voxel_output = -1.0f;
+
+  void voxelizeIndices(const float voxel_size, const t_points pc_in, t_indices& indices_inout, const bool check_NaNs = true);
+
+  std::vector<t_gfh> computeGFH(const t_points points, const t_indices& indices);
+  t_indices          sampleByGFH(const t_points points, const std::vector<t_gfh>& gfh, const size_t K, const float lambda);
+};
+
+//}
+
+}  // namespace rms

launch/rms_nodelet.launch

@@ -0,0 +1,51 @@
+<launch>
+
+  <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="" />
+
+  <!-- Inputs -->
+  <arg name="points_in" default="/$(arg NS)/os_cloud_nodelet/points_processed" />
+
+  <!-- Outputs -->
+  <arg name="points_out" default="$(arg points_in)_filtered" />
+
+  <!-- Debug? -->
+  <arg     if="$(arg debug)" name="launch_prefix" value="valgrind --tool=callgrind" />
+  <arg unless="$(arg debug)" name="launch_prefix" value="" />
+
+  <!-- Standalone? -->
+  <arg if="$(eval arg('standalone') or arg('debug'))" name="nodelet" value="standalone" />
+  <arg if="$(eval arg('standalone') or arg('debug'))" name="nodelet_manager" value="" />
+
+  <!-- Nodeleted? -->
+  <arg name="nodelet_manager_name" default="$(arg NS)_rms_nodelet_manager" />
+  <arg if="$(eval not arg('standalone') and not arg('debug'))" name="nodelet" value="load" />
+  <arg if="$(eval not arg('standalone') and not arg('debug'))" name="nodelet_manager" value="$(arg nodelet_manager_name)" />
+
+  <!-- Postfix? -->
+  <arg     if="$(eval arg('name_postfix') == '')" name="nodelet_name" value="$(arg node_name)" />
+  <arg unless="$(eval arg('name_postfix') == '')" name="nodelet_name" value="$(arg node_name)_$(arg name_postfix)" />
+
+  <group ns="$(arg NS)">
+
+    <node pkg="nodelet" type="nodelet" name="$(arg nodelet_name)" args="$(arg nodelet) rms/RMSNodelet $(arg nodelet_manager)" output="screen" launch-prefix="$(arg launch_prefix)">
+
+      <rosparam file="$(find rms)/config/default.yaml" command="load" />
+      <rosparam if="$(eval not arg('custom_config') == '')" file="$(arg custom_config)" command="load" />
+
+      <!-- Subscribers -->
+      <remap from="~points_in" to="$(arg points_in)" />
+
+      <!-- Publishers -->
+      <remap from="~points_out" to="$(arg points_out)" />
+
+    </node>
+
+  </group>
+
+</launch>