Skip to content

[AMCL] 50% initialisation (and reset) speed improvement #4941

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
SteveMacenski merged 2 commits into from
Apr 10, 2025
Merged

[AMCL] 50% initialisation (and reset) speed improvement #4941

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
1ca93c1
index lookup and comparator optimisation
Apr 10, 2025
2db9e44
lint
Apr 10, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
63 changes: 30 additions & 33 deletions nav2_amcl/src/map/map_cspace.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,15 +25,15 @@
#include "nav2_amcl/map/map.hpp"

/*
* @class CellData
* @struct CellData
* @brief Data about map cells
*/
class CellData
struct CellData
{
public:
map_t * map_;
unsigned int i_, j_;
unsigned int src_i_, src_j_;
float occ_dist;
};

/*
Expand Down Expand Up @@ -82,9 +82,7 @@ class CachedDistanceMap
*/
bool operator<(const CellData & a, const CellData & b)
{
return a.map_->cells[MAP_INDEX(
a.map_, a.i_,
a.j_)].occ_dist > a.map_->cells[MAP_INDEX(b.map_, b.i_, b.j_)].occ_dist;
return a.occ_dist > b.occ_dist;
}

/*
Expand Down Expand Up @@ -118,7 +116,9 @@ void enqueue(
CachedDistanceMap * cdm,
unsigned char * marked)
{
if (marked[MAP_INDEX(map, i, j)]) {
const int map_index = MAP_INDEX(map, i, j);

if (marked[map_index]) {
return;
}

Expand All @@ -130,18 +130,13 @@ void enqueue(
return;
}

map->cells[MAP_INDEX(map, i, j)].occ_dist = distance * map->scale;

CellData cell;
cell.map_ = map;
cell.i_ = i;
cell.j_ = j;
cell.src_i_ = src_i;
cell.src_j_ = src_j;
map->cells[map_index].occ_dist = distance * map->scale;

Q.push(cell);
Q.emplace(CellData{map, static_cast<unsigned int>(i), static_cast<unsigned int>(j),
static_cast<unsigned int>(src_i), static_cast<unsigned int>(src_j),
map->cells[map_index].occ_dist});

marked[MAP_INDEX(map, i, j)] = 1;
marked[map_index] = 1;
}

/*
Expand All @@ -164,16 +159,18 @@ void map_update_cspace(map_t * map, double max_occ_dist)
// Enqueue all the obstacle cells
CellData cell;
cell.map_ = map;
int loop_map_index;
for (int i = 0; i < map->size_x; i++) {
cell.src_i_ = cell.i_ = i;
for (int j = 0; j < map->size_y; j++) {
if (map->cells[MAP_INDEX(map, i, j)].occ_state == +1) {
map->cells[MAP_INDEX(map, i, j)].occ_dist = 0.0;
loop_map_index = MAP_INDEX(map, i, j);
if (map->cells[loop_map_index].occ_state == +1) {
map->cells[loop_map_index].occ_dist = 0.0;
cell.src_j_ = cell.j_ = j;
marked[MAP_INDEX(map, i, j)] = 1;
marked[loop_map_index] = 1;
Q.push(cell);
} else {
map->cells[MAP_INDEX(map, i, j)].occ_dist = max_occ_dist;
map->cells[loop_map_index].occ_dist = max_occ_dist;
}
}
}
Expand All @@ -182,27 +179,27 @@ void map_update_cspace(map_t * map, double max_occ_dist)
CellData current_cell = Q.top();
if (current_cell.i_ > 0) {
enqueue(
map, current_cell.i_ - 1, current_cell.j_,
current_cell.src_i_, current_cell.src_j_,
Q, cdm, marked);
map, current_cell.i_ - 1, current_cell.j_,
current_cell.src_i_, current_cell.src_j_,
Q, cdm, marked);
}
if (current_cell.j_ > 0) {
enqueue(
map, current_cell.i_, current_cell.j_ - 1,
current_cell.src_i_, current_cell.src_j_,
Q, cdm, marked);
map, current_cell.i_, current_cell.j_ - 1,
current_cell.src_i_, current_cell.src_j_,
Q, cdm, marked);
}
if (static_cast<int>(current_cell.i_) < map->size_x - 1) {
enqueue(
map, current_cell.i_ + 1, current_cell.j_,
current_cell.src_i_, current_cell.src_j_,
Q, cdm, marked);
map, current_cell.i_ + 1, current_cell.j_,
current_cell.src_i_, current_cell.src_j_,
Q, cdm, marked);
}
if (static_cast<int>(current_cell.j_) < map->size_y - 1) {
enqueue(
map, current_cell.i_, current_cell.j_ + 1,
current_cell.src_i_, current_cell.src_j_,
Q, cdm, marked);
map, current_cell.i_, current_cell.j_ + 1,
current_cell.src_i_, current_cell.src_j_,
Q, cdm, marked);
}

Q.pop();
Expand Down
Loading