iris-ua/iris_lama

LaMa - A Localization and Mapping library

LaMa - A Localization and Mapping library.

https://github.com/iris-ua/iris_lama

Developed and maintained by Eurico Pedrosa, University of Aveiro (C) 2019.

Overview

LaMa is a C++11 software library for robotic localization and mapping developed at the Intelligent Robotics and Systems (IRIS) Laboratory from the University of Aveiro - Portugal. It includes a framework for 3D volumetric grids (for mapping), a localization algorithm based on scan matching and two SLAM solution (an Online SLAM and a Particle Filter SLAM).

The main feature is efficiency. Low computational effort and low memory usage whenever possible. The minimum viable computer to run our localization and SLAM solutions is a Raspberry Pi 3 Model B+.

Build

To build LaMa, clone it from GitHub and use CMake to build.

$ git clone https://github.com/iris-ua/iris_lama
$ cd iris_lama
$ mkdir build
$ cd build
$ cmake ..

Its only dependency is Eigen3. Note: LaMa does not provide any executable. For an example on how to use it, please take a look at our integration with ROS.

Integration with ROS

The source code contains package.xml so that it can be used as a library from external ros packages. We provide ROS nodes to run the localization and the two SLAM solutions. Please go to iris_lama_ros for more information.

Sparse-Dense Mapping (SDM)

Sparse-Dense Mapping (SDM) is a framework for efficient implementation of 3D volumetric grids. Its divides space into small dense patches addressable by a sparse data-structure. To improve memory usage each individual patch can be compressed during live operations using lossless data compression (currently lz4 and Zstandard) with low overhead. It can be a replacement for OctoMap.

Currently it has the following grid maps implemented:

• Distance Map: It provides the distance to the closest occupied cells in the map. We provide the DynamicDistanceMap which is an implementation of the dynamic Euclidean map proposed by:

  B. Lau, C. Sprunk, and W. Burgard Efficient Grid-Based Spatial Representations for Robot Navigation in Dynamic Environments Robotics and Autonomous Systems, 61 (10), 2013, pp. 1116-1130, Elsevier

• Occupancy Map: The most common representation of the environment used in robotics. Three (3) variants of the occupancy map are provided: a SimpleOccupancyMap where each cell has a tri-state: free, occupied or unknown: a ProbabilisticOccupancyMap that encodes the occupancy probability of each cell with logods; and a FrequencyOccupancyMap that tracks the number of times a beam hits or traverses (miss) a cell and calculates a hit/miss ratio.

For more information about SDM please read

Eurico Pedrosa, Artur Pereira, Nuno Lau
A Sparse-Dense Approach for Efficient Grid Mapping
2018 IEEE International Conference on Autonomous Robot Systems and Competitions (ICARSC)

Localization based on Scan Matching

We provide a fast scan matching approach to mobile robot localization supported by a continuous likelihood field. It can be used to provide accurate localization for robots equipped with a laser and a not so good odometry. Nevertheless, a good odometry is always recommended.

Eurico Pedrosa, Artur Pereira, Nuno Lau
Efficient Localization Based on Scan Matching with a Continuous Likelihood Field
2017 IEEE International Conference on Autonomous Robot Systems and Competitions (ICARSC)

Online SLAM

For environments without considerable loops this solution can be accurate and very efficient. It can run in real time even on a low-spec computer (we have it running on a turtlebot with a raspberry pi 3B+). It uses our localization algorithm combined with a dynamic likelihood field to incrementally build an occupancy map.

For more information please read

Eurico Pedrosa, Artur Pereira, Nuno Lau
A Non-Linear Least Squares Approach to SLAM using a Dynamic Likelihood Field
Journal of Intelligent & Robotic Systems 93 (3-4), 519-532

Multi-threaded Particle Filter SLAM

This Particle Filter SLAM is a RBPF SLAM like GMapping and it is the extension of the Online SLAM solution to multiple particles with multi-thread support. Our solution is capable of parallelizing both the localization and mapping processes. It uses a thread-pool to manage the number of working threads.

Even without multi-threading, our solutions is a lightweight competitor against the heavyweight GMapping.

For more information please read

Eurico Pedrosa, Artur Pereira, Nuno Lau
Fast Grid SLAM Based on Particle Filter with Scan Matching and Multithreading
2020 IEEE International Conference on Autonomous Robot Systems and Competitions (ICARSC), Ponta Delgada, Portugal, 2020, pp. 194-199, doi: 10.1109/ICARSC49921.2020.9096191.

Graph SLAM

A really fast graph based solution.

iris_lama's Issues

Release plans

Hi, are there any plans to release iris_lama and iris_lama_ros for ROS Kinetic, Melodic, Noetic?

cmake Install support

Hi,

Can you please add CMake support for installation?

so it will be possible to do:

$ git clone https://github.com/iris-ua/iris_lama
$ cd iris_lama
$ mkdir build
$ cd build
$ cmake ..
$ make
$ sudo make install

Thanks!

Last upgrade of Sophus broke compilation in Ubuntu 16.04

Hi,

I don't know if other Ubuntu versions are affected, but I can not compile anymore.

This seems to be related to the version of Eigen, but maybe it is something else...

The error message is very long, this is just the beginning of it that may give you some hints:

In file included from /home/df/ws_lama/src/iris_lama/include/lama/sophus/se2.hpp:7:0,
                 from /home/df/ws_lama/src/iris_lama/include/lama/lie.h:36,
                 from /home/df/ws_lama/src/iris_lama/include/lama/pose2d.h:39,
                 from /home/df/ws_lama/src/iris_lama/src/pose2d.cpp:34:
/home/df/ws_lama/src/iris_lama/include/lama/sophus/so2.hpp:106:40: error: ‘ScalarBinaryOpTraits’ in namespace ‘Eigen’ does not name a template type
   using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<
                                        ^
/home/df/ws_lama/src/iris_lama/include/lama/sophus/so2.hpp:110:26: error: ‘ReturnScalar’ was not declared in this scope
   using SO2Product = SO2<ReturnScalar<OtherDerived>>;
                          ^
/home/df/ws_lama/src/iris_lama/include/lama/sophus/so2.hpp:110:39: error: template argument 1 is invalid
   using SO2Product = SO2<ReturnScalar<OtherDerived>>;
                                       ^
/home/df/ws_lama/src/iris_lama/include/lama/sophus/so2.hpp:113:32: error: ‘ReturnScalar’ was not declared in this scope
   using PointProduct = Vector2<ReturnScalar<PointDerived>>;
                                ^
/home/df/ws_lama/src/iris_lama/include/lama/sophus/so2.hpp:113:45: error: template argument 1 is invalid
   using PointProduct = Vector2<ReturnScalar<PointDerived>>;
                                             ^
/home/df/ws_lama/src/iris_lama/include/lama/sophus/so2.hpp:116:43: error: ‘ReturnScalar’ was not declared in this scope
   using HomogeneousPointProduct = Vector3<ReturnScalar<HPointDerived>>;
                                           ^
/home/df/ws_lama/src/iris_lama/include/lama/sophus/so2.hpp:116:56: error: template argument 1 is invalid
   using HomogeneousPointProduct = Vector3<ReturnScalar<HPointDerived>>;
                                                        ^
/home/df/ws_lama/src/iris_lama/include/lama/sophus/so2.hpp:214:15: error: ‘SO2Product’ does not name a type
   SOPHUS_FUNC SO2Product<OtherDerived> operator*(
               ^
/home/df/ws_lama/src/iris_lama/include/lama/sophus/so2.hpp:249:15: error: ‘PointProduct’ does not name a type
   SOPHUS_FUNC PointProduct<PointDerived> operator*(

[Discussion] Mapping with Moving Obstacles

Hi,

I did some reading on distance function mapping and localization methods and I wondered if this has a problem with moving obstacles for feature/scan/(surface by your code?) matching.

A talk I was at at IROS explaining visually how their technique works showed that the features generated by free space are very sensitive to changes, which makes sense. So if you have something moving in free space while mapping, will that trip this up? For example people walking around while mapping.

If this is a problem, would it make sense the problem is worse in narrow passage ways than in larger open spaces?

I appreciate how clean and clear your code is, thank you for that. This was clearly built with love and it shows.

Use LZ4 library from ROS when available

This is related to the problem reported here: iris-ua/iris_lama_ros#2

There might be other solutions, of course

[Question] experience with ProbabilisticOccupancyMap

Hi,

I noticed that the class FrequencyOccupancyMap seems to be preferred over the ProbabilisticOccupancyMap.

I have taken a look at the underlying algorithm and seems like the latter mimics the behavior of Octomap, somehow.

I wonder what your experience and recommendations are, when using one over the other.

[Discussion] Poor scalability by thread number in PF

This is just a brainstorming, not really an "issue". You don't need to "solve" it, it is just an open discussion between nerds :)

I noticed that the scalability of the PF Slam is quite poor with the number of threads.

For instance, moving from 4 threads to 8 increase performance only by 50%. Note that the profiler still say that we are using 100% of 8 CPU!

I do know that there isn't such a thing as perfect scalability, but in this case I think there "might" be a bottleneck somewhere.

I inspected the code and I couldn't find any mutex or potential false sharing, but of course I haven't done an exhaustive search.

Missing tf_conversions dependencies

Hello,m while building lapa from a clean install I got this error:
Errors << iris_lama_ros:cmake /home/ros_ws/logs/iris_lama_ros/build.cmake.000.log
CMake Error at /opt/ros/melodic/share/catkin/cmake/catkinConfig.cmake:83 (find_package):
Could not find a package configuration file provided by "tf_conversions"
with any of the following names:

tf_conversionsConfig.cmake
tf_conversions-config.cmake

I think it is solved by addind tf_conversions to the build depends, if this is correct, I can PR the mod

Use the scan intensities to detect glass walls

Hey,

I wonder if you have tackled this already or have some ideas on how to do it. In the company I work in, we consider adding this feature to the algorithm. Is it something that you would be interested in?

FrequencyMap: is this correct or a bug?

I was looking at this code:

bool lama::FrequencyOccupancyMap::setOccupied(const Vector2ui& coordinates)
{
    // ...
    bool occupied = prob(*cell) > occ_thresh;
    cell->occupied++;
    cell->visited++;
    // ...
}

Shouldn't it be instead:

    bool occupied = prob(*cell) > occ_thresh;
    if( occupied ) cell->occupied++;
    cell->visited++;
}

Shared library

Any reason why iris-lama is a static library (https://github.com/iris-ua/iris_lama/blob/master/src/CMakeLists.txt#L54) and no shared library?

AFAIK, catkin_package depend is not possible with this library. Why is it different then for example octomap?

Run in Ubuntu 18

How do I run it in Ubuntu and How do I configure the localization for different camera settings?

Compilation in Kinetic

Hi,

I am trying to compile in Kinetic. A couple of changes to make it easier for everybody:

• lower minimum CMAKE requirement to 3.5
• add set(CMAKE_CXX_STANDARD 11) to CMakeLists.txt iris_lama_ros.

First steps toward Slam3D

Hi,

as we started discussed in the ROS related repository, it would be cool to explore the evolution to 3D, using the foundation we have already.

I started getting familiar with Slam2D and I guess that the first tiny step would be to implement the MatchSurface3D class.

I am not an expert, but as far as I understand, software live loam_velodyne and Lego_LOAM extract features (planar and corners), whilst Cartographer has a Ceres based scan matching.

cartographer-project/cartographer#1539

I guess that the latter approach is more similar to MatchSurface2D, but the former has been proved to be a very effective heuristic.

Any thought?

did LaMA suite for mmwave radar sensor?

DynamicDistanceMap

The code is very clear and worship, but I simulate Dynamic_distance_map separately and found that when removeObstacle, there is a minor problem, the related removal cell sqdist is not cleared, maybe this should be the case here;

`void lama::DynamicDistanceMap::raise(const Vector3ui& location, distance_t& current)
{
Vector3ui newloc;
const int numOfNeighbor = is_3d ? 26 : 8;
for (int i = 0; i < numOfNeighbor; ++i){

    Vector3l d(deltas_[i][0], deltas_[i][1], deltas_[i][2]);

    newloc = (location.cast<int64_t>() + d).cast<uint32_t>();
    distance_t* neighbor = (distance_t*) get(newloc);
    if (neighbor == 0 or neighbor->is_queued)
        continue;

    if (not neighbor->valid_obstacle)
        continue;

    const distance_t* obstacle = (distance_t*) get(neighbor->obstacle);
    if (obstacle == 0)
        continue;

    // verify if the closest point is no longer an obstacle.
    if (not obstacle->sqdist == 0){
        raise_.push({neighbor->sqdist, newloc});

        neighbor->obstacle       = current.obstacle;
        neighbor->valid_obstacle = false;
        neighbor->is_queued      = true;
    }else if(not neighbor->is_queued){
        lower_.push({neighbor->sqdist, newloc});

        neighbor->is_queued = true;
    }

}// end for

current.is_queued = false;

}
// verify if the closest point is no longer an obstacle.
if (not obstacle->sqdist == 0){
raise_.push({neighbor->sqdist, newloc});
neighbor->sqdist = max_sqdist_;
neighbor->obstacle = current.obstacle;
neighbor->valid_obstacle = false;
neighbor->is_queued = true;
}else if(not neighbor->is_queued){
lower_.push({neighbor->sqdist, newloc});

        neighbor->is_queued = true;
    }

`

Noetic release

Seems to build nicely under Ubuntu 20.04 / Noetic, can we get a release?

Consider providing PDFs for the papers

I believe you have the right to make the papers mentioned here available so that interested users don't have to pay the IEEE tax in order to read them and understand your system better. I would like to request that you upload them to this or another public repo and link them in your readme.

Thank you :).

catkin build error Object library target "minisam" may not link to anything.

Errors << iris_lama:cmake /home/jason/agv_ws/logs/iris_lama/build.cmake.000.log
CMake Error at /home///src/iris_lama-master/vendor/minisam/CMakeLists.txt:28 (target_link_libraries):
Object library target "minisam" may not link to anything.

carmen to rosbag

Hi,
Thank you for such a great piece of software. I wish to do some tests with the carmen log files and was wondering if you could provide a way to convert the carmen(.clf) data logs into rosbag (.bag) format. Can you help me with this and provide any instructions (command line) to do so.

Thanks
Alex

possible nullptr deference in iris_lama/src/nlls/solver.cpp

Hi. I'm doing some test and found loc2d_ros process crash because of SEGV.
Here is the stderr debug output.
I'm appreciated you could help me to debug.

AddressSanitizer:DEADLYSIGNAL
=================================================================
==228409==ERROR: AddressSanitizer: SEGV on unknown address 0x000000000000 (pc 0x00000078705e bp 0x7fff6353e8a0 sp 0x7fff6353e870 T0)
==228409==The signal is caused by a READ memory access.
==228409==Hint: address points to the zero page.
    #0 0x78705e in Eigen::internal::enable_if<(NumTraits<double>::IsSigned) || (NumTraits<double>::IsComplex), Eigen::NumTraits<double>::Real>::type Eigen::numext::abs<double>(double const&) /usr/include/eigen3/Eigen/src/Core/MathFunctions.h:1051:14
    #1 0x78705e in Eigen::internal::scalar_abs_op<double>::operator()(double const&) const /usr/include/eigen3/Eigen/src/Core/functors/UnaryFunctors.h:44:103
    #2 0x786ff8 in Eigen::internal::unary_evaluator<Eigen::CwiseUnaryOp<Eigen::internal::scalar_abs_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> const>, Eigen::internal::IndexBased, double>::coeff(long) const /usr/include/eigen3/Eigen/src/Core/CoreEvaluators.h:549:12
    #3 0x7bfafe in Eigen::internal::redux_evaluator<Eigen::CwiseUnaryOp<Eigen::internal::scalar_abs_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> const> >::coeff(long) const /usr/include/eigen3/Eigen/src/Core/Redux.h:369:24
    #4 0x7bf6d3 in Eigen::internal::redux_impl<Eigen::internal::scalar_max_op<double, double>, Eigen::internal::redux_evaluator<Eigen::CwiseUnaryOp<Eigen::internal::scalar_abs_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> const> >, 3, 0>::run(Eigen::internal::redux_evaluator<Eigen::CwiseUnaryOp<Eigen::internal::scalar_abs_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> const> > const&, Eigen::internal::scalar_max_op<double, double> const&) /usr/include/eigen3/Eigen/src/Core/Redux.h:258:17
    #5 0x7beb25 in double Eigen::DenseBase<Eigen::CwiseUnaryOp<Eigen::internal::scalar_abs_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> const> >::redux<Eigen::internal::scalar_max_op<double, double> >(Eigen::internal::scalar_max_op<double, double> const&) const /usr/include/eigen3/Eigen/src/Core/Redux.h:418:10
    #6 0x7ba987 in Eigen::DenseBase<Eigen::CwiseUnaryOp<Eigen::internal::scalar_abs_op<double>, Eigen::Matrix<double, -1, -1, 0, -1, -1> const> >::maxCoeff() const /usr/include/eigen3/Eigen/src/Core/Redux.h:438:20
    #7 0x7b79d9 in Eigen::JacobiSVD<Eigen::Matrix<double, -1, -1, 0, -1, -1>, 2>::compute(Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, unsigned int) /usr/include/eigen3/Eigen/src/SVD/JacobiSVD.h:676:40
    #8 0x725069 in Eigen::JacobiSVD<Eigen::Matrix<double, -1, -1, 0, -1, -1>, 2>::JacobiSVD(Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, unsigned int) /usr/include/eigen3/Eigen/src/SVD/JacobiSVD.h:548:7
    #9 0x721808 in lama::Solver::calculateCovariance(Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, Eigen::Matrix<double, -1, -1, 0, -1, -1>*) const /home/r1/ros2_clang_navigation/src/iris_lama/src/nlls/solver.cpp:144:36
    #10 0x7207ad in lama::Solver::solve(lama::Problem&, Eigen::Matrix<double, -1, -1, 0, -1, -1>*) /home/r1/ros2_clang_navigation/src/iris_lama/src/nlls/solver.cpp:115:9
    #11 0x72235b in lama::Solve(lama::Solver::Options const&, lama::Problem&, Eigen::Matrix<double, -1, -1, 0, -1, -1>*) /home/r1/ros2_clang_navigation/src/iris_lama/src/nlls/solver.cpp:158:12
    #12 0x8d77dc in lama::Loc2D::update(std::shared_ptr<lama::PointCloudXYZ> const&, lama::Pose2D const&, double, bool) /home/r1/ros2_clang_navigation/src/iris_lama/src/loc2d.cpp:149:5
    #13 0x4f7fc2 in lama::Loc2DROS::onLaserScan(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>) /home/r1/ros2_clang_navigation/src/iris_lama_ros/src/loc2d_ros.cpp:216:17
    #14 0x6cc142 in void std::__invoke_impl<void, void (lama::Loc2DROS::*&)(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>), lama::Loc2DROS*&, std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&>(std::__invoke_memfun_deref, void (lama::Loc2DROS::*&)(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>), lama::Loc2DROS*&, std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:73:14
    #15 0x6cbe4a in std::__invoke_result<void (lama::Loc2DROS::*&)(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>), lama::Loc2DROS*&, std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&>::type std::__invoke<void (lama::Loc2DROS::*&)(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>), lama::Loc2DROS*&, std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&>(void (lama::Loc2DROS::*&)(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>), lama::Loc2DROS*&, std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:95:14
    #16 0x6cbcfb in void std::_Bind<void (lama::Loc2DROS::* (lama::Loc2DROS*, std::_Placeholder<1>))(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>)>::__call<void, std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&, 0ul, 1ul>(std::tuple<std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&>&&, std::_Index_tuple<0ul, 1ul>) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/functional:400:11
    #17 0x6cbb06 in void std::_Bind<void (lama::Loc2DROS::* (lama::Loc2DROS*, std::_Placeholder<1>))(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>)>::operator()<std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&, void>(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/functional:482:17
    #18 0x6cb62d in std::_Function_handler<void (std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&), std::_Bind<void (lama::Loc2DROS::* (lama::Loc2DROS*, std::_Placeholder<1>))(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>)> >::_M_invoke(std::_Any_data const&, std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:300:2
    #19 0x6ca7cd in std::function<void (std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&)>::operator()(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&) const /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:688:14
    #20 0x6ca0dd in std::_Function_handler<void (std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>), std::function<void (std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&)> >::_M_invoke(std::_Any_data const&, std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:300:2
    #21 0x6925d5 in std::function<void (std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>)>::operator()(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>) const /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:688:14
    #22 0x6cacf8 in message_filters::CallbackHelper1T<std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&, sensor_msgs::msg::LaserScan_<std::allocator<void> > >::call(message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&, bool) /opt/ros/foxy/include/message_filters/signal1.h:74:5
    #23 0x663e7d in message_filters::Signal1<sensor_msgs::msg::LaserScan_<std::allocator<void> > >::call(message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&) /opt/ros/foxy/include/message_filters/signal1.h:117:15
    #24 0x663b68 in message_filters::SimpleFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> > >::signalMessage(message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&) /opt/ros/foxy/include/message_filters/simple_filter.h:133:13
    #25 0x6c2325 in tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::messageReady(message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&) /opt/ros/foxy/include/tf2_ros/message_filter.h:643:13
    #26 0x6b6c68 in tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::transformReadyCallback(std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long) /opt/ros/foxy/include/tf2_ros/message_filter.h:537:7
    #27 0x6c4b42 in void std::__invoke_impl<void, void (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::*&)(std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long), tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>*&, std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long&>(std::__invoke_memfun_deref, void (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::*&)(std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long), tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>*&, std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:73:14
    #28 0x6c47e7 in std::__invoke_result<void (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::*&)(std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long), tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>*&, std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long&>::type std::__invoke<void (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::*&)(std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long), tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>*&, std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long&>(void (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::*&)(std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long), tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>*&, std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:95:14
    #29 0x6c4662 in void std::_Bind<void (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::* (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>*, std::_Placeholder<1>, unsigned long))(std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long)>::__call<void, std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, 0ul, 1ul, 2ul>(std::tuple<std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&>&&, std::_Index_tuple<0ul, 1ul, 2ul>) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/functional:400:11
    #30 0x6c43d6 in void std::_Bind<void (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::* (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>*, std::_Placeholder<1>, unsigned long))(std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long)>::operator()<std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, void>(std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/functional:482:17
    #31 0x6c3a3d in std::_Function_handler<void (std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&), std::_Bind<void (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::* (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>*, std::_Placeholder<1>, unsigned long))(std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&, unsigned long)> >::_M_invoke(std::_Any_data const&, std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:300:2
    #32 0x7f38c69ab79e in std::function<void (std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&)>::operator()(std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&) const /usr/include/c++/9/bits/std_function.h:688:14
    #33 0x7f38c69ab79e in tf2_ros::Buffer::waitForTransform(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::chrono::time_point<std::chrono::_V2::system_clock, std::chrono::duration<long, std::ratio<1l, 1000000000l> > > const&, std::chrono::duration<long, std::ratio<1l, 1000000000l> > const&, std::function<void (std::shared_future<geometry_msgs::msg::TransformStamped_<std::allocator<void> > > const&)>) obj-x86_64-linux-gnu/./src/buffer.cpp:272:13
    #34 0x6b1a43 in tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::add(message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&) /opt/ros/foxy/include/tf2_ros/message_filter.h:402:15
    #35 0x6a3b18 in tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::incomingMessage(message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&) /opt/ros/foxy/include/tf2_ros/message_filter.h:552:5
    #36 0x6a94db in void std::__invoke_impl<void, void (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::*&)(message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&), tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>*&, message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&>(std::__invoke_memfun_deref, void (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::*&)(message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&), tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>*&, message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:73:14
    #37 0x6a925a in std::__invoke_result<void (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::*&)(message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&), tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>*&, message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&>::type std::__invoke<void (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::*&)(message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&), tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>*&, message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&>(void (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::*&)(message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&), tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>*&, message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/invoke.h:95:14
    #38 0x6a910b in void std::_Bind<void (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::* (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>*, std::_Placeholder<1>))(message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&)>::__call<void, message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&, 0ul, 1ul>(std::tuple<message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&>&&, std::_Index_tuple<0ul, 1ul>) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/functional:400:11
    #39 0x6a8f16 in void std::_Bind<void (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::* (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>*, std::_Placeholder<1>))(message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&)>::operator()<message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&, void>(message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/functional:482:17
    #40 0x6a876d in std::_Function_handler<void (message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&), std::_Bind<void (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::* (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>*, std::_Placeholder<1>))(message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&)> >::_M_invoke(std::_Any_data const&, message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:300:2
    #41 0x6a581d in std::function<void (message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&)>::operator()(message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&) const /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:688:14
    #42 0x6a52e0 in message_filters::CallbackHelper1T<message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&, sensor_msgs::msg::LaserScan_<std::allocator<void> > >::call(message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&, bool) /opt/ros/foxy/include/message_filters/signal1.h:74:5
    #43 0x663e7d in message_filters::Signal1<sensor_msgs::msg::LaserScan_<std::allocator<void> > >::call(message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&) /opt/ros/foxy/include/message_filters/signal1.h:117:15
    #44 0x663b68 in message_filters::SimpleFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> > >::signalMessage(message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&) /opt/ros/foxy/include/message_filters/simple_filter.h:133:13
    #45 0x663585 in message_filters::Subscriber<sensor_msgs::msg::LaserScan_<std::allocator<void> > >::cb(message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&) /opt/ros/foxy/include/message_filters/subscriber.h:235:11
    #46 0x6632f7 in message_filters::Subscriber<sensor_msgs::msg::LaserScan_<std::allocator<void> > >::subscribe(rclcpp::Node*, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, rmw_qos_profile_t)::'lambda'(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>)::operator()(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>) const /opt/ros/foxy/include/message_filters/subscriber.h:174:24
    #47 0x662ebe in std::_Function_handler<void (std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>), message_filters::Subscriber<sensor_msgs::msg::LaserScan_<std::allocator<void> > >::subscribe(rclcpp::Node*, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, rmw_qos_profile_t)::'lambda'(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>)>::_M_invoke(std::_Any_data const&, std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>&&) /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:300:2
    #48 0x6925d5 in std::function<void (std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>)>::operator()(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > const>) const /usr/bin/../lib/gcc/x86_64-linux-gnu/9/../../../../include/c++/9/bits/std_function.h:688:14
    #49 0x6954f8 in rclcpp::AnySubscriptionCallback<sensor_msgs::msg::LaserScan_<std::allocator<void> >, std::allocator<void> >::dispatch(std::shared_ptr<sensor_msgs::msg::LaserScan_<std::allocator<void> > >, rclcpp::MessageInfo const&) /opt/ros/foxy/include/rclcpp/any_subscription_callback.hpp:167:7
    #50 0x6705f9 in rclcpp::Subscription<sensor_msgs::msg::LaserScan_<std::allocator<void> >, std::allocator<void>, rclcpp::message_memory_strategy::MessageMemoryStrategy<sensor_msgs::msg::LaserScan_<std::allocator<void> >, std::allocator<void> > >::handle_message(std::shared_ptr<void>&, rclcpp::MessageInfo const&) /opt/ros/foxy/include/rclcpp/subscription.hpp:275:19
    #51 0x7f38c676802b  (/opt/ros/foxy/lib/librclcpp.so+0xd702b)
    #52 0x7f38c67688ea in rclcpp::Executor::execute_subscription(std::shared_ptr<rclcpp::SubscriptionBase>) (/opt/ros/foxy/lib/librclcpp.so+0xd78ea)
    #53 0x7f38c67690a4 in rclcpp::Executor::execute_any_executable(rclcpp::AnyExecutable&) (/opt/ros/foxy/lib/librclcpp.so+0xd80a4)
    #54 0x7f38c676da4b in rclcpp::executors::SingleThreadedExecutor::spin() (/opt/ros/foxy/lib/librclcpp.so+0xdca4b)
    #55 0x7f38c676b4e7 in rclcpp::spin(std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface>) (/opt/ros/foxy/lib/librclcpp.so+0xda4e7)
    #56 0x7f38c676b78f in rclcpp::spin(std::shared_ptr<rclcpp::Node>) (/opt/ros/foxy/lib/librclcpp.so+0xda78f)
    #57 0x4ffc55 in main /home/r1/ros2_clang_navigation/src/iris_lama_ros/src/loc2d_ros.cpp:393:5
    #58 0x7f38c5d120b2 in __libc_start_main /build/glibc-eX1tMB/glibc-2.31/csu/../csu/libc-start.c:308:16
    #59 0x43885d in _start (/home/r1/ros2_clang_navigation/build/iris_lama_ros2/src/loc2d_ros+0x43885d)

consider using std::priority_queue and remove PriorityQueue

I was curious so I took a look at your implementation of PriorityQueue.

I did some benchmark and std::priority_queue< std::pair<int,T>> is actually faster (double as much).

Therefore, I believe it would be better to remove it, since the STL implementation is better and well supported.
You can see the results of the benchmark and the code here: http://quick-bench.com/SzpCCMx3aqGrAp-gjpuL6XvXEOg

Local minimum

Hi @eupedrosa
the distance map use squaredNorm even use the robust kernel is easy to local minimum ,why not use norm , I think some times that will improve the robust,you can try it.