Skip to content

Fix A* star selection logic #22

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

Open
wants to merge 11 commits into
base: feature/clang_format_reorder_includes
Choose a base branch
from
Open

Fix A* star selection logic #22

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
11 commits
Select commit Hold shift + click to select a range
0862b56
Fix A* star update rule to take shortest, not longest evaluated branch
victorreijgwart Mar 15, 2021
e08c457
Merge branch 'feature/clang_format_reorder_includes' into fix/a_star_…
victorreijgwart Mar 17, 2021
f9a8f72
Merge pull request #21 from ethz-asl/feature/clang_format_reorder_inc…
victorreijgwart Mar 18, 2021
38b6783
Update the GLocal ArXiv link to the right paper
victorreijgwart Mar 25, 2021
71a5e07
Merge pull request #24 from ethz-asl/fix/correct_glocal_arxiv_link
Schmluk Mar 25, 2021
a2d0474
Update README.md
Schmluk Mar 29, 2021
095a63b
Merge pull request #25 from ethz-asl/feature/add_source_pt1
Schmluk Mar 29, 2021
af03498
Update README.md
Schmluk Apr 14, 2021
18d5110
Update README.md
Schmluk Apr 14, 2021
3783213
Merge pull request #26 from ethz-asl/feature/cite_readme
Schmluk Apr 15, 2021
d3e7525
Merge remote-tracking branch 'origin/master' into fix/a_star_update_rule
May 20, 2021
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
14 changes: 6 additions & 8 deletions README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,20 +19,18 @@ Different modules of GLocal (left) and GLocal in action for large scale explorat
# Papers
If you find this package useful for your research, please consider citing our paper:

* **Note:** Our paper was accepted for publication in IEEE RA-L, the information below will be updated upon publication. To read the paper please refer to ArXiv at the moment.

* Lukas Schmid, Victor Reijgwart, Lionel Ott, Juan Nieto, Roland Siegwart, and Cesar Cadena, "**A Unified Approach for Autonomous Volumetric Exploration of Large Scale Environments under Severe Odometry Drift**", in *IEEE Robotics and Automation Letters*, 2021 \[IEEE | [ArXiv](https://arxiv.org/abs/1909.09548) | Video\]
* Lukas Schmid, Victor Reijgwart, Lionel Ott, Juan Nieto, Roland Siegwart, and Cesar Cadena, "**A Unified Approach for Autonomous Volumetric Exploration of Large Scale Environments under Severe Odometry Drift**", in *IEEE Robotics and Automation Letters*, vol. 6, no. 3, pp. 4504-4511, July 2021 \[[IEEE](https://ieeexplore.ieee.org/document/9387110) | [ArXiv](https://arxiv.org/abs/2010.09859) | Video\]
```bibtex
@ARTICLE{schmid2021glocal,
author={L. {Schmid} and V. {Reijgwart} and L. {Ott} and J. {Nieto} and R. {Siegwart} and C. {Cadena}},
journal={IEEE Robotics and Automation Letters},
title={A Unified Approach for Autonomous Volumetric Exploration of Large Scale Environments under Severe Odometry Drift},
year={2021},
volume={?},
number={?},
pages={?},
doi={?},
month={?},
volume={6},
number={3},
pages={4504-4511},
doi={10.1109/LRA.2021.3068954},
month={July},
}
```

Expand Down
20 changes: 11 additions & 9 deletions glocal_exploration/src/planning/global/skeleton/skeleton_a_star.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -202,10 +202,10 @@ bool SkeletonAStar::getPathBetweenVertices(

const voxblox::Point t_odom_current_vertex =
current_submap.getPose() * current_vertex.point;
const FloatingPoint h_score = (goal_point - t_odom_current_vertex).norm();
g_score_map[current_vertex_id] =
(t_odom_current_vertex - start_point).norm();
f_score_map[current_vertex_id] =
(goal_point - t_odom_current_vertex).norm();
f_score_map[current_vertex_id] = h_score;
Copy link
Contributor

@Schmluk Schmluk Mar 18, 2021

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is this intentionally called h_score and then assigned to the f_score_map? I'm not too familiar with the terminology but might lead to confusions.

Copy link
Member Author

@victorreijgwart victorreijgwart Mar 18, 2021 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Strictly speaking, the update equation here should be f[neighbor] = g[neighbor] + h[neighbor]. The code above only initializes the starting vertices and will result in all of them being expanded before any regular vertex, so it won't affect the result. But I agree it's confusing and there's no advantage of not using the standard update equation.
In general, this whole getPathBetweenVertices(...) method grew organically and got quite messy. I'll do some refactoring.

open_set.insert(current_vertex_id);
}

Expand Down Expand Up @@ -264,7 +264,7 @@ bool SkeletonAStar::getPathBetweenVertices(
g_score_map[current_vertex_id] +
(goal_point - current_vertex.point).norm();
if (g_score_map.count(kGoalVertexId) == 0 ||
g_score_map[kGoalVertexId] < tentative_g_score) {
tentative_g_score < g_score_map[kGoalVertexId]) {
g_score_map[kGoalVertexId] = tentative_g_score;
f_score_map[kGoalVertexId] = tentative_g_score;
parent_map[kGoalVertexId] = current_vertex_id;
Expand Down Expand Up @@ -333,11 +333,12 @@ bool SkeletonAStar::getPathBetweenVertices(
g_score_map[current_vertex_id] +
(t_odom_nearby_vertex - t_odom_current_vertex).norm();
if (g_score_map.count(nearby_vertex_global_id) == 0 ||
g_score_map[nearby_vertex_global_id] < tentative_g_score) {
tentative_g_score < g_score_map[nearby_vertex_global_id]) {
const FloatingPoint h_score =
(goal_point - t_odom_nearby_vertex).norm();
g_score_map[nearby_vertex_global_id] = tentative_g_score;
f_score_map[nearby_vertex_global_id] =
tentative_g_score +
(goal_point - t_odom_nearby_vertex).norm();
tentative_g_score + h_score;
parent_map[nearby_vertex_global_id] = current_vertex_id;
}
} else {
Expand Down Expand Up @@ -389,10 +390,11 @@ bool SkeletonAStar::getPathBetweenVertices(
// NOTE: Since the vertex and its neighbor are already in the same
// (submap) frame, we can directly compute their distance above
if (g_score_map.count(neighbor_vertex_id) == 0 ||
g_score_map[neighbor_vertex_id] < tentative_g_score) {
tentative_g_score < g_score_map[neighbor_vertex_id]) {
const FloatingPoint h_score =
(goal_point - t_odom_neighbor_vertex).norm();
g_score_map[neighbor_vertex_id] = tentative_g_score;
f_score_map[neighbor_vertex_id] =
tentative_g_score + (goal_point - t_odom_neighbor_vertex).norm();
f_score_map[neighbor_vertex_id] = tentative_g_score + h_score;
parent_map[neighbor_vertex_id] = current_vertex_id;
}
}
Expand Down