From 0862b569bc6dda66ecc4d6551c09e85a58e34720 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Mon, 15 Mar 2021 22:26:54 +0100 Subject: [PATCH 1/5] Fix A* star update rule to take shortest, not longest evaluated branch --- .../global/skeleton/skeleton_a_star.cpp | 20 ++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/glocal_exploration/src/planning/global/skeleton/skeleton_a_star.cpp b/glocal_exploration/src/planning/global/skeleton/skeleton_a_star.cpp index 1e4ab09..204f293 100644 --- a/glocal_exploration/src/planning/global/skeleton/skeleton_a_star.cpp +++ b/glocal_exploration/src/planning/global/skeleton/skeleton_a_star.cpp @@ -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; open_set.insert(current_vertex_id); } @@ -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; @@ -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 { @@ -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; } } From 38b6783985c2e9d81e492be3767dfc60ceb9d617 Mon Sep 17 00:00:00 2001 From: Victor Reijgwart Date: Thu, 25 Mar 2021 11:50:58 +0100 Subject: [PATCH 2/5] Update the GLocal ArXiv link to the right paper --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 8e62550..55e81ed 100644 --- a/README.md +++ b/README.md @@ -21,7 +21,7 @@ If you find this package useful for your research, please consider citing our pa * **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*, 2021 \[IEEE | [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}}, From a2d0474a103dd239d1de5a995594fc748c5ad0a1 Mon Sep 17 00:00:00 2001 From: Lukas Schmid <36043993+Schmluk@users.noreply.github.com> Date: Mon, 29 Mar 2021 10:10:39 +0200 Subject: [PATCH 3/5] Update README.md --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 55e81ed..7e2b425 100644 --- a/README.md +++ b/README.md @@ -21,7 +21,7 @@ If you find this package useful for your research, please consider citing our pa * **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/2010.09859) | 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*, 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}}, @@ -31,7 +31,7 @@ If you find this package useful for your research, please consider citing our pa volume={?}, number={?}, pages={?}, - doi={?}, + doi={10.1109/LRA.2021.3068954}, month={?}, } ``` From af03498148445104d85169fb975e18dc95a41dc1 Mon Sep 17 00:00:00 2001 From: Lukas Schmid <36043993+Schmluk@users.noreply.github.com> Date: Wed, 14 Apr 2021 08:39:40 +0200 Subject: [PATCH 4/5] Update README.md --- README.md | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index 7e2b425..4da418e 100644 --- a/README.md +++ b/README.md @@ -19,8 +19,6 @@ 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](https://ieeexplore.ieee.org/document/9387110) | [ArXiv](https://arxiv.org/abs/2010.09859) | Video\] ```bibtex @ARTICLE{schmid2021glocal, @@ -28,11 +26,11 @@ If you find this package useful for your research, please consider citing our pa 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={?}, + volume={6}, + number={3}, + pages={4504-4511}, doi={10.1109/LRA.2021.3068954}, - month={?}, + month={July}, } ``` From 18d51100e5cc86b2f6b081bb1e85e20beb3c5a0e Mon Sep 17 00:00:00 2001 From: Lukas Schmid <36043993+Schmluk@users.noreply.github.com> Date: Wed, 14 Apr 2021 09:39:38 +0200 Subject: [PATCH 5/5] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 4da418e..3c29f85 100644 --- a/README.md +++ b/README.md @@ -19,7 +19,7 @@ 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: -* 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](https://ieeexplore.ieee.org/document/9387110) | [ArXiv](https://arxiv.org/abs/2010.09859) | 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}},