Skip to content

Commit 43ea703

Browse files
orlando21laraXuRobotics
orlando21lara
authored and
XuRobotics
committed
Optimized MPL::MapUtil
1 parent 6d22452 commit 43ea703

File tree

3 files changed

+58
-47
lines changed

3 files changed

+58
-47
lines changed

‎autonomy_core/map_plan/mpl/include/mpl_collision/map_util.h‎

Lines changed: 41 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,7 @@
22

33
#include "mpl_basis/data_type.h"
44
#include <planning_ros_msgs/VoxelMap.h>
5+
#include <vector>
56

67
namespace MPL {
78

@@ -104,12 +105,51 @@ typedef MapUtil<2> OccMapUtil;
104105
typedef MapUtil<3> VoxelMapUtil;
105106

106107
template <int Dim>
107-
int MapUtil<Dim>::getIndex(const Veci<Dim> &pn) {
108+
inline int MapUtil<Dim>::getIndex(const Veci<Dim> &pn) {
108109
if constexpr (Dim == 3) {
109110
return pn(0) + dim_(0) * pn(1) + dim_(0) * dim_(1) * pn(2);
110111
} else {
111112
return pn(0) + dim_(0) * pn(1);
112113
}
113114
}
114115

116+
template <int Dim>
117+
inline bool MapUtil<Dim>::isOutside(const Veci<Dim> &pn) {
118+
if constexpr (Dim == 3) {
119+
return (pn(0) < 0 || pn(0) >= dim_(0) ||
120+
pn(1) < 0 || pn(1) >= dim_(1) ||
121+
pn(2) < 0 || pn(2) >= dim_(2));
122+
} else {
123+
return (pn(0) < 0 || pn(0) >= dim_(0) ||
124+
pn(1) < 0 || pn(1) >= dim_(1));
125+
}
126+
}
127+
128+
template <int Dim>
129+
inline bool MapUtil<Dim>::isFree(const Veci<Dim> &pn) {
130+
// First check if the voxel is within the bounds of the map and if it isn't,
131+
// return false. This is accomplished by taking advantage of short-circuit
132+
// evaluation. An if-else statement could also be used here but it isn't
133+
// to avoid issues with branch predictions and inline.
134+
return (!isOutside(pn) && isFree(getIndex(pn)));
135+
}
136+
137+
template <int Dim>
138+
inline bool MapUtil<Dim>::isOccupied(const Veci<Dim> &pn) {
139+
// First check if the voxel is within the bounds of the map and if it isn't,
140+
// return false. This is accomplished by taking advantage of short-circuit
141+
// evaluation. An if-else statement could also be used here but it isn't
142+
// to avoid issues with branch predictions and inline.
143+
return (!isOutside(pn) && isOccupied(getIndex(pn)));
144+
}
145+
146+
template <int Dim>
147+
inline bool MapUtil<Dim>::isUnknown(const Veci<Dim> &pn) {
148+
// First check if the voxel is within the bounds of the map and if it isn't,
149+
// return false. This is accomplished by taking advantage of short-circuit
150+
// evaluation. An if-else statement could also be used here but it isn't
151+
// to avoid issues with branch predictions and inline.
152+
return (!isOutside(pn) && isUnknown(getIndex(pn)));
153+
}
154+
115155
} // namespace MPL

‎autonomy_core/map_plan/mpl/src/map_util.cpp‎

Lines changed: 12 additions & 41 deletions
Original file line numberDiff line numberDiff line change
@@ -4,35 +4,6 @@
44

55
namespace MPL {
66

7-
template <int Dim>
8-
bool MapUtil<Dim>::isOutside(const Veci<Dim> &pn) {
9-
for (int i = 0; i < Dim; i++)
10-
if (pn(i) < 0 || pn(i) >= dim_(i)) return true;
11-
return false;
12-
}
13-
14-
template <int Dim>
15-
bool MapUtil<Dim>::isFree(const Veci<Dim> &pn) {
16-
if (isOutside(pn))
17-
return false;
18-
else
19-
return isFree(getIndex(pn));
20-
}
21-
22-
template <int Dim>
23-
bool MapUtil<Dim>::isOccupied(const Veci<Dim> &pn) {
24-
if (isOutside(pn))
25-
return false;
26-
else
27-
return isOccupied(getIndex(pn));
28-
}
29-
30-
template <int Dim>
31-
bool MapUtil<Dim>::isUnknown(const Veci<Dim> &pn) {
32-
if (isOutside(pn)) return false;
33-
return isUnknown(getIndex(pn));
34-
}
35-
367
template <int Dim>
378
void MapUtil<Dim>::setMap(const Vecf<Dim> &ori, const Veci<Dim> &dim,
389
const Tmap &map, decimal_t res) {
@@ -93,16 +64,16 @@ vec_Vecf<Dim> MapUtil<Dim>::getCloud() {
9364
vec_Vecf<Dim> cloud;
9465
Veci<Dim> n;
9566
if constexpr (Dim == 3) {
96-
for (n(0) = 0; n(0) < dim_(0); n(0)++) {
67+
for (n(2) = 0; n(2) < dim_(2); n(2)++) {
9768
for (n(1) = 0; n(1) < dim_(1); n(1)++) {
98-
for (n(2) = 0; n(2) < dim_(2); n(2)++) {
69+
for (n(0) = 0; n(0) < dim_(0); n(0)++) {
9970
if (isOccupied(getIndex(n))) cloud.push_back(intToFloat(n));
10071
}
10172
}
10273
}
10374
} else {
104-
for (n(0) = 0; n(0) < dim_(0); n(0)++) {
105-
for (n(1) = 0; n(1) < dim_(1); n(1)++) {
75+
for (n(1) = 0; n(1) < dim_(1); n(1)++) {
76+
for (n(0) = 0; n(0) < dim_(0); n(0)++) {
10677
if (isOccupied(getIndex(n))) cloud.push_back(intToFloat(n));
10778
}
10879
}
@@ -116,16 +87,16 @@ vec_Vecf<Dim> MapUtil<Dim>::getFreeCloud() {
11687
Veci<Dim> n;
11788

11889
if constexpr (Dim == 3) {
119-
for (n(0) = 0; n(0) < dim_(0); n(0)++) {
90+
for (n(2) = 0; n(2) < dim_(2); n(2)++) {
12091
for (n(1) = 0; n(1) < dim_(1); n(1)++) {
121-
for (n(2) = 0; n(2) < dim_(2); n(2)++) {
92+
for (n(0) = 0; n(0) < dim_(0); n(0)++) {
12293
if (isFree(getIndex(n))) cloud.push_back(intToFloat(n));
12394
}
12495
}
12596
}
12697
} else {
127-
for (n(0) = 0; n(0) < dim_(0); n(0)++) {
128-
for (n(1) = 0; n(1) < dim_(1); n(1)++) {
98+
for (n(1) = 0; n(1) < dim_(1); n(1)++) {
99+
for (n(0) = 0; n(0) < dim_(0); n(0)++) {
129100
if (isFree(getIndex(n))) cloud.push_back(intToFloat(n));
130101
}
131102
}
@@ -139,16 +110,16 @@ vec_Vecf<Dim> MapUtil<Dim>::getUnknownCloud() {
139110
Veci<Dim> n;
140111

141112
if constexpr (Dim == 3) {
142-
for (n(0) = 0; n(0) < dim_(0); n(0)++) {
113+
for (n(2) = 0; n(2) < dim_(2); n(2)++) {
143114
for (n(1) = 0; n(1) < dim_(1); n(1)++) {
144-
for (n(2) = 0; n(2) < dim_(2); n(2)++) {
115+
for (n(0) = 0; n(0) < dim_(0); n(0)++) {
145116
if (isUnknown(getIndex(n))) cloud.push_back(intToFloat(n));
146117
}
147118
}
148119
}
149120
} else {
150-
for (n(0) = 0; n(0) < dim_(0); n(0)++) {
151-
for (n(1) = 0; n(1) < dim_(1); n(1)++) {
121+
for (n(1) = 0; n(1) < dim_(1); n(1)++) {
122+
for (n(0) = 0; n(0) < dim_(0); n(0)++) {
152123
if (isUnknown(getIndex(n))) cloud.push_back(intToFloat(n));
153124
}
154125
}

‎autonomy_core/map_plan/mpl/test/benchmark_map_util.cpp‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -69,7 +69,7 @@ BENCHMARK_DEFINE_F(BenchmarkMapUtil, BM_isOutside)(benchmark::State& state) {
6969
// being optimized out
7070
int counter = 0;
7171

72-
for (auto &voxel: test_voxels_) {
72+
for (auto &voxel : test_voxels_) {
7373
bool it_is = mapper_.isOutside(voxel);
7474

7575
if (it_is) {
@@ -89,7 +89,7 @@ BENCHMARK_DEFINE_F(BenchmarkMapUtil, BM_isFree)(benchmark::State& state) {
8989
// being optimized out
9090
int counter = 0;
9191

92-
for (auto &voxel: test_voxels_) {
92+
for (auto &voxel : test_voxels_) {
9393
bool it_is = mapper_.isFree(voxel);
9494

9595
if (it_is) {
@@ -109,7 +109,7 @@ BENCHMARK_DEFINE_F(BenchmarkMapUtil, BM_isOccupied)(benchmark::State& state) {
109109
// being optimized out
110110
int counter = 0;
111111

112-
for (auto &voxel: test_voxels_) {
112+
for (auto &voxel : test_voxels_) {
113113
bool it_is = mapper_.isOccupied(voxel);
114114

115115
if (it_is) {
@@ -129,7 +129,7 @@ BENCHMARK_DEFINE_F(BenchmarkMapUtil, BM_isUnknown)(benchmark::State& state) {
129129
// being optimized out
130130
int counter = 0;
131131

132-
for (auto &voxel: test_voxels_) {
132+
for (auto &voxel : test_voxels_) {
133133
bool it_is = mapper_.isUnknown(voxel);
134134

135135
if (it_is) {
@@ -187,7 +187,7 @@ BENCHMARK_DEFINE_F(BenchmarkMapUtil, BM_rayTrace)(benchmark::State& state) {
187187
for (auto &start : start_positions) {
188188
for (auto &end : end_positions) {
189189
vec_Vec3i voxels = mapper_.rayTrace(start, end);
190-
if (voxels.size() == -1 ) {
190+
if (voxels.size() == -1) {
191191
std::cout << "This message should not have been printed" << std::endl;
192192
}
193193
}

0 commit comments

Comments
 (0)