Skip to content

Commit 46172d7

Browse files
tonynajjartonynajjar
authored
New inflation layer with optional OpenMP acceleration (ros-navigation#5933)
* OpenMP inflation layer Signed-off-by: Tony Najjar <[email protected]> * restart tests Signed-off-by: Tony Najjar <[email protected]> * disable openmp Signed-off-by: Tony Najjar <[email protected]> * enable flag Signed-off-by: Tony Najjar <[email protected]> * limit thread number Signed-off-by: Tony Najjar <[email protected]> * enable openmp for testing Signed-off-by: Tony Najjar <[email protected]> * check docker cores Signed-off-by: Tony Najjar <[email protected]> * set OMP_NUM_THREADS to 4 Signed-off-by: Tony Najjar <[email protected]> * dynamic num_thread Signed-off-by: Tony Najjar <[email protected]> * remove openmp from packages.xml Signed-off-by: Tony Najjar <[email protected]> * turn on by default Signed-off-by: Tony Najjar <[email protected]> * hardcode COST_LUT_PRECISION Signed-off-by: Tony Najjar <[email protected]> * test out big test Signed-off-by: Tony Najjar <[email protected]> * add back legacy layer Signed-off-by: Tony Najjar <[email protected]> * set to 100 Signed-off-by: Tony Najjar <[email protected]> * fix legacy inflation layer Signed-off-by: Tony Najjar <[email protected]> * Revert "fix legacy inflation layer" This reverts commit 21fa149. * off by default Signed-off-by: Tony Najjar <[email protected]> * base class approach Signed-off-by: Tony Najjar <[email protected]> * simplified interface Signed-off-by: Tony Najjar <[email protected]> * add test coverage Signed-off-by: Tony Najjar <[email protected]> * remove cpp file Signed-off-by: Tony Najjar <[email protected]> * Fix broken build Signed-off-by: Tony Najjar <[email protected]> * Add tests for legacy inflation layer Signed-off-by: Tony Najjar <[email protected]> * Enable OpenMP support and add legacy inflation layer test configuration Signed-off-by: Tony Najjar <[email protected]> * lint * lint * remove -DENABLE_OPENMP=ON Signed-off-by: Tony Najjar <[email protected]> --------- Signed-off-by: Tony Najjar <[email protected]>
1 parent 105b109 commit 46172d7

21 files changed

Lines changed: 3073 additions & 443 deletions

‎.circleci/config.yml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -499,6 +499,7 @@ _environments:
499499
RCUTILS_LOGGING_USE_STDOUT: "0"
500500
DEBIAN_FRONTEND: "noninteractive"
501501
PYTHONUNBUFFERED: "1"
502+
OMP_NUM_THREADS: "4"
502503

503504
executors:
504505
release_exec:

‎nav2_costmap_2d/CMakeLists.txt‎

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,13 @@ find_package(nav2_ros_common REQUIRED)
2727
find_package(tf2_sensor_msgs REQUIRED)
2828
find_package(visualization_msgs REQUIRED)
2929
find_package(point_cloud_transport REQUIRED)
30+
find_package(Eigen3 REQUIRED)
31+
32+
option(ENABLE_OPENMP "Enable OpenMP parallelization" OFF)
33+
34+
if(ENABLE_OPENMP)
35+
find_package(OpenMP REQUIRED)
36+
endif()
3037

3138
nav2_package()
3239

@@ -66,6 +73,7 @@ target_link_libraries(nav2_costmap_2d_core PUBLIC
6673

6774
add_library(layers SHARED
6875
plugins/inflation_layer.cpp
76+
plugins/legacy_inflation_layer.cpp
6977
plugins/static_layer.cpp
7078
plugins/obstacle_layer.cpp
7179
src/observation_buffer.cpp
@@ -90,7 +98,19 @@ target_link_libraries(layers PUBLIC
9098
tf2::tf2
9199
nav2_ros_common::nav2_ros_common
92100
point_cloud_transport::point_cloud_transport
101+
Eigen3::Eigen
93102
)
103+
104+
# Link OpenMP if enabled
105+
if(ENABLE_OPENMP)
106+
message(STATUS "OpenMP ENABLED for nav2_costmap_2d layers (${OpenMP_CXX_VERSION})")
107+
target_link_libraries(layers PRIVATE OpenMP::OpenMP_CXX)
108+
target_compile_options(layers PRIVATE -fopenmp)
109+
else()
110+
message(STATUS "OpenMP DISABLED for nav2_costmap_2d layers")
111+
target_compile_options(layers PRIVATE -Wno-unknown-pragmas)
112+
endif()
113+
94114
target_link_libraries(layers PRIVATE
95115
pluginlib::pluginlib
96116
)
@@ -226,5 +246,8 @@ ament_export_dependencies(
226246
nav2_ros_common
227247
point_cloud_transport
228248
)
249+
250+
ament_export_dependencies(Eigen3)
251+
229252
pluginlib_export_plugin_description_file(nav2_costmap_2d costmap_plugins.xml)
230253
ament_package()

‎nav2_costmap_2d/costmap_plugins.xml‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,10 @@
11
<class_libraries>
22
<library path="layers">
33
<class type="nav2_costmap_2d::InflationLayer" base_class_type="nav2_costmap_2d::Layer">
4-
<description>Inflates obstacles to speed collision checking and to make robot prefer to stay away from obstacles.</description>
4+
<description>OpenMP-accelerated inflation layer using Felzenszwalb-Huttenlocher distance transform for improved performance.</description>
5+
</class>
6+
<class type="nav2_costmap_2d::LegacyInflationLayer" base_class_type="nav2_costmap_2d::Layer">
7+
<description>Legacy inflation layer using traditional CellData-based algorithm (original Navigation2 implementation).</description>
58
</class>
69
<class type="nav2_costmap_2d::ObstacleLayer" base_class_type="nav2_costmap_2d::Layer">
710
<description>Listens to laser scan and point cloud messages and marks and clears grid cells.</description>

‎nav2_costmap_2d/include/nav2_costmap_2d/distance_transform.hpp‎

Lines changed: 179 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,179 @@
1+
// Copyright (c) 2026, Dexory (Tony Najjar)
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef NAV2_COSTMAP_2D__DISTANCE_TRANSFORM_HPP_
16+
#define NAV2_COSTMAP_2D__DISTANCE_TRANSFORM_HPP_
17+
18+
#include <limits>
19+
#include <vector>
20+
#ifdef _OPENMP
21+
#include <omp.h>
22+
#endif
23+
#include <Eigen/Core>
24+
25+
26+
namespace nav2_costmap_2d
27+
{
28+
29+
/// Row-major float matrix type for efficient row-wise access
30+
using MatrixXfRM = Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
31+
32+
/**
33+
* @class DistanceTransform
34+
* @brief Efficient Euclidean distance transform using the Felzenszwalb-Huttenlocher algorithm.
35+
*
36+
* This class provides a standalone implementation of the linear-time (O(n)) distance transform
37+
* algorithm based on the lower envelope of parabolas method. It can be used for various
38+
* applications including costmap inflation, obstacle detection, and path planning.
39+
*
40+
* The algorithm computes exact squared Euclidean distances in two separable 1D passes
41+
* (rows and columns), making it highly efficient for large images/grids.
42+
*
43+
* Reference: Distance Transforms of Sampled Functions
44+
* P. Felzenszwalb and D. Huttenlocher
45+
* Theory of Computing, Vol. 8, No. 19, September 2012
46+
*/
47+
class DistanceTransform
48+
{
49+
public:
50+
/// Infinity constant for distance transform
51+
static constexpr float DT_INF = std::numeric_limits<float>::max();
52+
53+
/**
54+
* @brief Perform 1D distance transform using lower envelope of parabolas.
55+
*
56+
* This is the core Felzenszwalb-Huttenlocher algorithm for computing squared
57+
* Euclidean distances in 1D. It uses a lower envelope of parabolas to achieve
58+
* linear time complexity.
59+
*
60+
* @param f Input array of squared distances (typically 0 for obstacles, INF for free space)
61+
* @param d Output array for transformed squared distances (same size as f)
62+
* @param n Length of the arrays
63+
* @param v Buffer for parabola indices (size n)
64+
* @param z Buffer for parabola boundaries (size n+1)
65+
*/
66+
static void distanceTransform1D(
67+
const float * f, float * d, int n,
68+
int * v, float * z)
69+
{
70+
if (!f || !d || !v || !z || n <= 0) {
71+
return;
72+
}
73+
74+
int k = 0;
75+
v[0] = 0;
76+
z[0] = -DT_INF;
77+
z[1] = DT_INF;
78+
79+
for (int q = 1; q < n; q++) {
80+
// Use integer arithmetic for squared values to avoid precision loss
81+
// Only convert to float for the division operation
82+
float s = (f[q] - f[v[k]] + static_cast<float>(q * q - v[k] * v[k])) /
83+
(2.0f * static_cast<float>(q - v[k]));
84+
while (s <= z[k]) {
85+
k--;
86+
s = (f[q] - f[v[k]] + static_cast<float>(q * q - v[k] * v[k])) /
87+
(2.0f * static_cast<float>(q - v[k]));
88+
}
89+
k++;
90+
v[k] = q;
91+
z[k] = s;
92+
z[k + 1] = DT_INF;
93+
}
94+
95+
k = 0;
96+
for (int q = 0; q < n; q++) {
97+
while (z[k + 1] < static_cast<float>(q)) {
98+
k++;
99+
}
100+
const int diff = q - v[k];
101+
d[q] = static_cast<float>(diff * diff) + f[v[k]];
102+
}
103+
}
104+
105+
/**
106+
* @brief Perform 2D Euclidean distance transform using separable passes.
107+
*
108+
* This method applies the 1D distance transform separately along columns and rows,
109+
* exploiting the separability property of the Euclidean metric. The result is an
110+
* exact Euclidean distance map computed in O(width × height) time.
111+
*
112+
* The algorithm is parallelized using OpenMP when available for improved performance
113+
* on multi-core systems.
114+
*
115+
* @param img Input/output matrix (modified in place). Input values should be 0 for
116+
* obstacles and DT_INF for free space. Output will contain Euclidean distances.
117+
* @param height Number of rows in the matrix
118+
* @param width Number of columns in the matrix
119+
*/
120+
static void distanceTransform2D(MatrixXfRM & img, int height, int width)
121+
{
122+
// Column pass (parallelizable)
123+
#ifdef _OPENMP
124+
#pragma omp parallel for schedule(dynamic, 16)
125+
#endif
126+
for (int x = 0; x < width; x++) {
127+
// Thread-local buffers
128+
std::vector<float> f(height);
129+
std::vector<float> d(height);
130+
std::vector<int> v(height);
131+
std::vector<float> z(height + 1);
132+
133+
// Extract column
134+
for (int y = 0; y < height; y++) {
135+
f[y] = img(y, x);
136+
}
137+
138+
// 1D transform
139+
distanceTransform1D(f.data(), d.data(), height, v.data(), z.data());
140+
141+
// Write back
142+
for (int y = 0; y < height; y++) {
143+
img(y, x) = d[y];
144+
}
145+
}
146+
147+
// Row pass (parallelizable)
148+
#ifdef _OPENMP
149+
#pragma omp parallel for schedule(dynamic, 16)
150+
#endif
151+
for (int y = 0; y < height; y++) {
152+
// Thread-local buffers
153+
std::vector<float> f(width);
154+
std::vector<float> d(width);
155+
std::vector<int> v(width);
156+
std::vector<float> z(width + 1);
157+
158+
// Extract row (already contiguous in row-major)
159+
for (int x = 0; x < width; x++) {
160+
f[x] = img(y, x);
161+
}
162+
163+
// 1D transform
164+
distanceTransform1D(f.data(), d.data(), width, v.data(), z.data());
165+
166+
// Write back
167+
for (int x = 0; x < width; x++) {
168+
img(y, x) = d[x];
169+
}
170+
}
171+
172+
// Square root to get Euclidean distance (not squared distance)
173+
img = img.cwiseSqrt();
174+
}
175+
};
176+
177+
} // namespace nav2_costmap_2d
178+
179+
#endif // NAV2_COSTMAP_2D__DISTANCE_TRANSFORM_HPP_

0 commit comments

Comments
 (0)