Skip to content

Commit dc0fcc8

Browse files
petrapa6petrapa6
committed
add code, how to, and acknowledgment
1 parent 94c23f7 commit dc0fcc8

31 files changed

+10014
-21
lines changed

‎.gitignore

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,2 @@
1+
*.swo
2+
*.swp

‎CMakeLists.txt

Lines changed: 100 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,100 @@
1+
cmake_minimum_required(VERSION 2.8.3)
2+
project(rms)
3+
4+
set(CMAKE_CXX_STANDARD 17)
5+
set(CMAKE_CXX_STANDARD_REQUIRED ON)
6+
set(CMAKE_CXX_EXTENSIONS OFF)
7+
8+
find_package(catkin REQUIRED COMPONENTS
9+
roscpp
10+
nodelet
11+
sensor_msgs
12+
pcl_ros
13+
mrs_lib
14+
)
15+
16+
# TSL Robin Map for voxelization
17+
add_subdirectory(thirdparty/robin-map)
18+
19+
# Point Cloud Library
20+
find_package(PCL REQUIRED)
21+
22+
# Eigen
23+
find_package(Eigen3 REQUIRED)
24+
set(Eigen_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS})
25+
set(Eigen_LIBRARIES ${Eigen_LIBRARIES})
26+
27+
set(LIBRARIES
28+
RMS RMSNodelet
29+
)
30+
31+
catkin_package(
32+
INCLUDE_DIRS include
33+
LIBRARIES ${LIBRARIES}
34+
CATKIN_DEPENDS roscpp nodelet sensor_msgs pcl_ros mrs_lib
35+
DEPENDS Eigen
36+
)
37+
38+
include_directories(
39+
include
40+
${catkin_INCLUDE_DIRS}
41+
${PCL_INCLUDE_DIRS}
42+
${Eigen_INCLUDE_DIRS}
43+
)
44+
45+
link_directories(${PCL_LIBRARY_DIRS})
46+
add_definitions(${PCL_DEFINITIONS})
47+
48+
add_library(RMS
49+
src/histogram.cpp
50+
src/rms.cpp
51+
)
52+
53+
add_dependencies(RMS
54+
${${PROJECT_NAME}_EXPORTED_TARGETS}
55+
${catkin_EXPORTED_TARGETS}
56+
)
57+
58+
target_link_libraries(RMS
59+
${catkin_LIBRARIES}
60+
${PCL_LIBRARIES}
61+
${Eigen_INCLUDE_DIRS}
62+
tsl::robin_map
63+
)
64+
65+
add_library(RMSNodelet
66+
src/histogram.cpp
67+
src/rms.cpp
68+
src/rms_nodelet.cpp
69+
)
70+
71+
add_dependencies(RMSNodelet
72+
${${PROJECT_NAME}_EXPORTED_TARGETS}
73+
${catkin_EXPORTED_TARGETS}
74+
)
75+
76+
target_link_libraries(RMSNodelet
77+
${catkin_LIBRARIES}
78+
${PCL_LIBRARIES}
79+
${Eigen_INCLUDE_DIRS}
80+
tsl::robin_map
81+
)
82+
83+
## --------------------------------------------------------------
84+
## | Install |
85+
## --------------------------------------------------------------
86+
87+
install(TARGETS ${LIBRARIES}
88+
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
89+
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
90+
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
91+
)
92+
93+
install(DIRECTORY launch config
94+
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
95+
)
96+
97+
install(DIRECTORY ./
98+
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
99+
FILES_MATCHING PATTERN "*.xml"
100+
)

‎README.md

Lines changed: 63 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -2,36 +2,78 @@
22

33
[![RMS](./fig/snapshot.jpg)](https://www.youtube.com/watch?v=Y9ZlRrX1UBY)
44

5-
* novel method for sampling large 3D LiDAR point clouds
6-
* replacement for voxelization
7-
* pipelines using **RMS** are faster (lower latency) and more accurate (less drift)
5+
* quick sampling of 3D LiDAR point clouds
6+
* pipelines using **RMS** are fast (low latency) and accurate
87
* designed for **real-time LiDAR-based** 6-DoF odometry/SLAM pipelines
98
* both point-based (ICP-like) and feature-based (LOAM-like) methods
10-
* potentially improving most L, LI, LVI, LV pipelines
11-
* **single parameter only -> depends on the SLAM pipeline (and not the environment!)**
9+
* **single parameter only: [lambda](https://github.com/ctu-mrs/RMS/blob/master/config/default.yaml)**
10+
* depends on the SLAM pipeline (and not the environment!)
1211
* tuned just once given your pipeline
1312
* deterministic (no data for learning needed)
14-
* when is it (probably) not going to perform well:
13+
* when is it not going to perform well (most probably):
1514
* the data have large orientation changes between two consecutive frames (tens of degrees)
16-
* under heavy noise (such as dust clouds)
15+
* under heavy noise
1716

18-
#### Code & How to
19-
The code will be made available upon acceptance.
17+
## Paper
18+
Published in IEEE RA-L --- [pdf](https://arxiv.org/pdf/2312.07337.pdf).
2019

21-
#### Paper
22-
Submitted to IEEE RA-L on December 1, 2023.
23-
Preprint available at [arXiv](https://arxiv.org/pdf/2312.07337.pdf).
20+
## Code & How to
2421

25-
#### How to cite
22+
### Installation
23+
1) Install prerequisities (`mrs_lib`, `PCL`):
24+
```bash
25+
curl https://ctu-mrs.github.io/ppa-stable/add_ppa.sh | bash
26+
apt-get install ros-noetic-mrs-lib ros-noetic-pcl-ros
2627
```
27-
@article{petracek2023rms,
28-
title = {{RMS: Redundancy-Minimizing Point Cloud Sampling for Real-Time Pose Estimation in Degenerated Environments}},
29-
author = {Petracek, Pavel and Alexis, Kostas and Saska, Martin},
30-
year = {2023},
31-
journal = {arXiv:2312.07337},
32-
note = {Submitted to IEEE RA-L on December 1, 2023}
28+
2) Clone and build via `catkin`
29+
```bash
30+
cd <ROS1_WORKSPACE>/src
31+
git clone [email protected]:ctu-mrs/RMS.git
32+
catkin build
33+
```
34+
35+
### How to use
36+
1) Launch as nodelet:
37+
```bash
38+
roslaunch rms rms_nodelet.launch NS:=<NAMESPACE> points_in:=<POINTS IN TOPIC> points_out:=<POINTS OUT TOPIC>
39+
```
40+
2) Use as library in your code:
41+
- add `rms` among dependencies in `CMakeLists.txt` and `package.xml` and include the `<rms/rms.h>` header file
42+
- 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
43+
```cpp
44+
#include <rms/rms.h>
45+
...
46+
// Initialize
47+
ros::NodeHandle nh;
48+
mrs_lib::ParamLoader param_loader(nh, "RMS");
49+
RMS rms = RMS(param_loader);
50+
...
51+
// Use
52+
sensor_msgs::PointCloud2::Ptr msg = ...;
53+
rms->sample(msg); // 'msg' now contains sampled data
54+
```
55+
- **example**: `RMSNodelet` implemented in [src/rms_nodelet.cpp](https://github.com/ctu-mrs/RMS/blob/master/src/rms_nodelet.cpp)
56+
57+
### Example
58+
[WORK IN PROGRESS] To be added in coming days.
59+
60+
## How to cite
61+
```tex
62+
@article{petracek2024rms,
63+
author = {Petracek, Pavel and Alexis, Kostas and Saska, Martin},
64+
title = {{RMS: Redundancy-Minimizing Point Cloud Sampling for Real-Time Pose Estimation}},
65+
journal = {IEEE Robotics and Automation Letters},
66+
year = {2024},
67+
volume = {9},
68+
number = {6},
69+
pages = {5230--5237},
70+
doi = {10.1109/LRA.2024.3389820}
3371
}
3472
```
3573

36-
#### Acknowledgment
37-
To be added upon acceptance.
74+
## Acknowledgment
75+
This work was supported
76+
- by CTU grant no. SGS23/177/OHK3/3T/13,
77+
- by the Czech Science Foundation under research project No. 23-06162M,
78+
- by the European Union under the project Robotics and advanced industrial production (reg. no. CZ.02.01.01/00/22_008/0004590), and
79+
- by the Research Council of Norway Award NO-321435.

‎config/default.yaml

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
lambda: 0.004 # <0, 1> | Alg 1: L5 | tune for your estimation pipeline
2+
K: 10 # number of bins | Alg 1: L4 | should not need to be changed
3+
4+
voxelization:
5+
input: 0.2 # (0, inf) (m) | applied before RMS | Alg 1: L3 | lower: higher timing, higher: lower timing
6+
output: 0.2 # <0, inf) (m) | applied after RMS

‎include/rms/histogram.h

Lines changed: 58 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,58 @@
1+
#pragma once
2+
3+
#include <vector>
4+
#include <iostream>
5+
#include <cmath>
6+
#include <algorithm>
7+
#include <set>
8+
9+
namespace rms
10+
{
11+
12+
// key: point index, value: GFH norm
13+
using t_gfh = std::pair<size_t, float>;
14+
using t_indices = std::vector<size_t>;
15+
16+
/* //{ class Histogram1D */
17+
18+
class Histogram1D {
19+
20+
public:
21+
Histogram1D(const size_t no_bins, const float min_value, const float max_value, const std::vector<t_gfh> &gfh);
22+
23+
float getBinWidth();
24+
size_t getBinCount();
25+
26+
void selectByUniformnessMaximization();
27+
void selectByUniformnessMaximization(float &entropy);
28+
29+
void selectByNormMaximization();
30+
void selectByNormMaximization(float &entropy);
31+
32+
t_indices getSelectedIndices();
33+
34+
private:
35+
size_t _no_bins;
36+
size_t _point_count_remaining;
37+
size_t _point_count_selected;
38+
39+
size_t _bin_sel_ptr;
40+
41+
float _bin_width;
42+
float _val_min;
43+
float _val_max;
44+
45+
// vector (size: number of bins) of vectors of pairs (first: index in original cloud; second: value) sorted in descending order by pair value
46+
std::vector<std::vector<t_gfh>> _counts_remaining;
47+
std::vector<std::vector<t_gfh>> _counts_selected;
48+
49+
void construct(const std::vector<t_gfh> &gfh);
50+
51+
size_t getBin(const float point);
52+
std::vector<float> estimatePDF();
53+
float computeEntropy(const std::vector<float> &pdf);
54+
};
55+
56+
//}
57+
58+
} // namespace rms

‎include/rms/rms.h

Lines changed: 61 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,61 @@
1+
#include <ros/ros.h>
2+
3+
#include <pcl/point_types.h>
4+
#include <pcl/point_cloud.h>
5+
#include <pcl/kdtree/kdtree_flann.h>
6+
#include <pcl/filters/voxel_grid.h>
7+
#include <pcl_conversions/pcl_conversions.h>
8+
9+
#include <sensor_msgs/PointCloud2.h>
10+
#include <sensor_msgs/point_cloud2_iterator.h>
11+
12+
#include <mrs_lib/param_loader.h>
13+
#include <mrs_lib/scope_timer.h>
14+
15+
#include <rms/histogram.h>
16+
17+
namespace rms
18+
{
19+
20+
/* //{ class RMS */
21+
22+
using t_points = pcl::PointCloud<pcl::PointXYZ>::Ptr;
23+
using t_voxel = Eigen::Vector3i;
24+
25+
class RMS {
26+
27+
// | ---------------------- Voxelization ---------------------- |
28+
struct VoxelHash
29+
{
30+
size_t operator()(const t_voxel& voxel) const {
31+
const std::uint32_t* vec = reinterpret_cast<const uint32_t*>(voxel.data());
32+
return ((1 << 20) - 1) & (vec[0] * 73856093 ^ vec[1] * 19349663 ^ vec[2] * 83492791);
33+
}
34+
};
35+
36+
// | ----------------------- Public API ----------------------- |
37+
public:
38+
RMS(mrs_lib::ParamLoader& param_loader);
39+
void sample(sensor_msgs::PointCloud2::Ptr& msg_inout);
40+
41+
// | --------------- ROS and conversion methods --------------- |
42+
private:
43+
void fromROSMsg(const sensor_msgs::PointCloud2::Ptr cloud, t_points& pcl_cloud);
44+
void extractByIndices(sensor_msgs::PointCloud2::Ptr& msg_inout, t_indices& indices);
45+
46+
// | ---------------- RMS variables and methods --------------- |
47+
private:
48+
size_t _K = 10;
49+
float _lambda = 1.0f;
50+
float _voxel_input = -1.0f;
51+
float _voxel_output = -1.0f;
52+
53+
void voxelizeIndices(const float voxel_size, const t_points pc_in, t_indices& indices_inout, const bool check_NaNs = true);
54+
55+
std::vector<t_gfh> computeGFH(const t_points points, const t_indices& indices);
56+
t_indices sampleByGFH(const t_points points, const std::vector<t_gfh>& gfh, const size_t K, const float lambda);
57+
};
58+
59+
//}
60+
61+
} // namespace rms

‎launch/rms_nodelet.launch

Lines changed: 51 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,51 @@
1+
<launch>
2+
3+
<arg name="NS" default="$(optenv UAV_NAME)" />
4+
5+
<arg name="standalone" default="true" />
6+
<arg name="debug" default="false" />
7+
<arg name="custom_config" default="" />
8+
<arg name="node_name" default="rms_nodelet" />
9+
<arg name="name_postfix" default="" />
10+
11+
<!-- Inputs -->
12+
<arg name="points_in" default="/$(arg NS)/os_cloud_nodelet/points_processed" />
13+
14+
<!-- Outputs -->
15+
<arg name="points_out" default="$(arg points_in)_filtered" />
16+
17+
<!-- Debug? -->
18+
<arg if="$(arg debug)" name="launch_prefix" value="valgrind --tool=callgrind" />
19+
<arg unless="$(arg debug)" name="launch_prefix" value="" />
20+
21+
<!-- Standalone? -->
22+
<arg if="$(eval arg('standalone') or arg('debug'))" name="nodelet" value="standalone" />
23+
<arg if="$(eval arg('standalone') or arg('debug'))" name="nodelet_manager" value="" />
24+
25+
<!-- Nodeleted? -->
26+
<arg name="nodelet_manager_name" default="$(arg NS)_rms_nodelet_manager" />
27+
<arg if="$(eval not arg('standalone') and not arg('debug'))" name="nodelet" value="load" />
28+
<arg if="$(eval not arg('standalone') and not arg('debug'))" name="nodelet_manager" value="$(arg nodelet_manager_name)" />
29+
30+
<!-- Postfix? -->
31+
<arg if="$(eval arg('name_postfix') == '')" name="nodelet_name" value="$(arg node_name)" />
32+
<arg unless="$(eval arg('name_postfix') == '')" name="nodelet_name" value="$(arg node_name)_$(arg name_postfix)" />
33+
34+
<group ns="$(arg NS)">
35+
36+
<node pkg="nodelet" type="nodelet" name="$(arg nodelet_name)" args="$(arg nodelet) rms/RMSNodelet $(arg nodelet_manager)" output="screen" launch-prefix="$(arg launch_prefix)">
37+
38+
<rosparam file="$(find rms)/config/default.yaml" command="load" />
39+
<rosparam if="$(eval not arg('custom_config') == '')" file="$(arg custom_config)" command="load" />
40+
41+
<!-- Subscribers -->
42+
<remap from="~points_in" to="$(arg points_in)" />
43+
44+
<!-- Publishers -->
45+
<remap from="~points_out" to="$(arg points_out)" />
46+
47+
</node>
48+
49+
</group>
50+
51+
</launch>

0 commit comments

Comments
 (0)