diff --git a/mola_relocalization/CMakeLists.txt b/mola_relocalization/CMakeLists.txt index dcadbf99..e9751e3c 100644 --- a/mola_relocalization/CMakeLists.txt +++ b/mola_relocalization/CMakeLists.txt @@ -31,7 +31,6 @@ mola_add_library( TARGET ${PROJECT_NAME} SOURCES src/find_best_poses_se2.cpp - src/AlignBEV.cpp src/RelocalizationICP_SE2.cpp src/RelocalizationLikelihood_SE2.cpp include/mola_relocalization/relocalization.h diff --git a/mola_relocalization/README.md b/mola_relocalization/README.md index ea5901c5..41790222 100644 --- a/mola_relocalization/README.md +++ b/mola_relocalization/README.md @@ -3,7 +3,29 @@ C++ library with algorithms for relocalization, global localization, or pose est Note that particle filtering is implemented in its own repository under [mrpt_navigation](https://github.com/mrpt-ros-pkg/mrpt_navigation). -Example result for the `mola::Relocalization_SE2` method (from a unit test, see [the code](https://github.com/MOLAorg/mola/blob/develop/mola_relocalization/tests/test-relocalization-se2-kitti.cpp) for details): +## Method #1: mola::RelocalizationICP_SE2 + +Takes a global and a local metric map, a SE(2) ROI, and tries to match +the local map in the global map by running ICP from all initial guesses +defined by a regular SE(2) lattice, returning the result as a SE(3) hashed +lattice. + +This method is based on [mp2p_icp ICP pipelines](https://docs.mola-slam.org/latest/module-mp2p-icp.html). + +## Method #2: mola::RelocalizationLikelihood_SE2 + +Takes a global metric map, an observation, and a SE(2) ROI, and evaluates +the likelihood of the observation in a regular SE(2) lattice. + +This is based on mrpt maps observationLikelihood() evaluation, so the main +parameters that determine the way likelihood is computed must be defined +on before hand within each of the metric map layers of the input reference +map. +At present this algorithm is useful for these sensor/map types: +- observations: pointclouds/2d_scan, reference_map: 2D gridmap +- observations: pointclouds, reference_map: pointclouds + +Example result for the `mola::RelocalizationLikelihood_SE2` method (from a unit test, see [the code](https://github.com/MOLAorg/mola/blob/develop/mola_relocalization/tests/test-relocalization-se2-kitti.cpp) for details): ![mola_relocalize_figs](https://github.com/MOLAorg/mola/assets/5497818/6622739f-95ca-4e39-a770-d5f15c01adb3) diff --git a/mola_relocalization/include/mola_relocalization/relocalization.h b/mola_relocalization/include/mola_relocalization/relocalization.h index dccf3a3a..41b8a6ac 100644 --- a/mola_relocalization/include/mola_relocalization/relocalization.h +++ b/mola_relocalization/include/mola_relocalization/relocalization.h @@ -147,37 +147,6 @@ struct RelocalizationICP_SE2 static Output run(const Input& in); }; -/** Takes a global and a local point cloud maps, and builds a point cloud - * density "grid map image" from a bird-eye-view (BEV), then applies the - * grid-map align algorithm in mrpt::maps. - * - * \ingroup mola_relocalization_grp - */ -struct AlignBEV -{ - struct Input - { - mp2p_icp::metric_map_t reference_map; - std::vector ref_map_layers; //!< empty=all - mp2p_icp::metric_map_t local_map; - std::vector local_map_layers; //!< empty=all - - double resolution_xy = 0.40; - - Input() = default; - }; - - struct Output - { - mola::HashedSetSE3 found_poses; - double time_cost = .0; //!< [s] - - Output() = default; - }; - - static Output run(const Input& in); -}; - /** Finds the SE(2) poses with the top given percentile likelihood, and returns * them sorted by likelihood (higher values are better matches). * diff --git a/mola_relocalization/src/AlignBEV.cpp b/mola_relocalization/src/AlignBEV.cpp deleted file mode 100644 index a4ede3f5..00000000 --- a/mola_relocalization/src/AlignBEV.cpp +++ /dev/null @@ -1,171 +0,0 @@ -/* ------------------------------------------------------------------------- - * A Modular Optimization framework for Localization and mApping (MOLA) - * - * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria - * Licensed under the GNU GPL v3 for non-commercial applications. - * - * This file is part of MOLA. - * MOLA is free software: you can redistribute it and/or modify it under the - * terms of the GNU General Public License as published by the Free Software - * Foundation, either version 3 of the License, or (at your option) any later - * version. - * - * MOLA is distributed in the hope that it will be useful, but WITHOUT ANY - * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR - * A PARTICULAR PURPOSE. See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along with - * MOLA. If not, see . - * ------------------------------------------------------------------------- */ - -#include -#include -#include -#include -#include - -#include - -namespace -{ - -class DensityGridMap -{ - public: - DensityGridMap( - double resolution, uint8_t inner_bits = 2, uint8_t leaf_bits = 3) - : grid_(resolution, inner_bits, leaf_bits) - { - } - - void insert_point_cloud(const mrpt::maps::CPointsMap& p) - { - const auto& xs = p.getPointsBufferRef_x(); - const auto& ys = p.getPointsBufferRef_y(); - const size_t N = xs.size(); - - for (size_t i = 0; i < N; i++) - { - const auto pt = mrpt::math::TPoint3Df(xs[i], ys[i], .0f); - - auto* cell = accessor_.value( - Bonxai::PosToCoord({pt.x, pt.y, pt.z}, grid_.inv_resolution), - true /*create*/); - cell->pointCount++; - - mrpt::keep_max(maxDensity_, cell->pointCount); - - if (!bbox_) - { - bbox_.emplace(); - bbox_ = mrpt::math::TBoundingBoxf(pt, pt); - } - else - bbox_->updateWithPoint(pt); - } - } - - mrpt::maps::COccupancyGridMap2D::Ptr get_as_occupancy_grid() - { - ASSERT_(bbox_.has_value()); - ASSERT_(maxDensity_ > 0); - - auto og = mrpt::maps::COccupancyGridMap2D::Create(); - og->setSize( - bbox_->min.x, bbox_->max.x, bbox_->min.y, bbox_->max.y, - grid_.resolution, .01f); - - const float nPts_inv = 1.0f / maxDensity_; - - grid_.forEachCell( - [&](Node& v, const Bonxai::CoordT& c) - { - const auto pt = Bonxai::CoordToPos(c, grid_.resolution); - - const float density = v.pointCount * nPts_inv; - og->setPos(pt.x, pt.y, density); - }); - - return og; - } - - private: - struct Node - { - uint32_t pointCount = 0; - }; - - Bonxai::VoxelGrid grid_; - Bonxai::VoxelGrid::Accessor accessor_{grid_}; - uint32_t maxDensity_ = 1; - std::optional bbox_; -}; - -} // namespace - -mola::AlignBEV::Output mola::AlignBEV::run(const Input& in) -{ - mola::AlignBEV::Output result; - - const double t0 = mrpt::Clock::nowDouble(); - - ASSERT_(!in.reference_map.layers.empty()); - ASSERT_(!in.local_map.layers.empty()); - - auto lambdaProcessMap = [](DensityGridMap& g, - const mp2p_icp::metric_map_t& m, - const std::vector& inLayers) - { - std::vector layers = inLayers; - if (layers.empty()) - for (const auto& [l, ml] : m.layers) layers.push_back(l); - - for (const auto& layer : layers) - { - auto pts = std::dynamic_pointer_cast( - m.layers.at(layer)); - if (!pts) continue; - g.insert_point_cloud(*pts); - } - }; - - // Build density maps: - DensityGridMap gGrid(in.resolution_xy); - DensityGridMap lGrid(in.resolution_xy); - - lambdaProcessMap(gGrid, in.reference_map, in.ref_map_layers); - lambdaProcessMap(lGrid, in.local_map, in.local_map_layers); - - const auto gOccGrid = gGrid.get_as_occupancy_grid(); - const auto lOccGrid = lGrid.get_as_occupancy_grid(); - -#if 1 - gOccGrid->saveAsBitmapFile("og_reference.png"); - lOccGrid->saveAsBitmapFile("og_local.png"); - -#endif - - mrpt::slam::CGridMapAligner ga; - ga.options.dumpToTextStream(std::cout); - - ga.options.feature_detector_options.featsType = mrpt::vision::featORB; - ga.options.feature_descriptor = mrpt::vision::descAny; - ga.options.ransac_minSetSizeRatio = 0.02; - ga.options.feature_detector_options.ORBOptions.n_levels = 1; - - ga.options.threshold_max = 100; // for ORB - ga.options.threshold_delta = 20; - - ga.options.methodSelection = mrpt::slam::CGridMapAligner::amModifiedRANSAC; - - ga.options.debug_save_map_pairs = true; - - auto resPose = ga.Align(gOccGrid.get(), lOccGrid.get(), {}); - - std::cout << "res: " << resPose->getMeanVal() << "\n"; - - // end - result.time_cost = mrpt::Clock::nowDouble() - t0; - - return result; -} diff --git a/mola_relocalization/tests/CMakeLists.txt b/mola_relocalization/tests/CMakeLists.txt index 09f9cb24..6a4abca8 100644 --- a/mola_relocalization/tests/CMakeLists.txt +++ b/mola_relocalization/tests/CMakeLists.txt @@ -14,10 +14,3 @@ mola_add_test( target_compile_definitions(test-relocalization-se2-kitti PRIVATE TEST_DATASETS_ROOT=\"${TEST_DATASETS_ROOT}\") endif() - -mola_add_test( - TARGET test-relocalization-align-bev - SOURCES test-relocalization-align-bev.cpp - LINK_LIBRARIES - mola::mola_relocalization -) diff --git a/mola_relocalization/tests/test-relocalization-align-bev.cpp b/mola_relocalization/tests/test-relocalization-align-bev.cpp deleted file mode 100644 index 707a87c1..00000000 --- a/mola_relocalization/tests/test-relocalization-align-bev.cpp +++ /dev/null @@ -1,64 +0,0 @@ -/* ------------------------------------------------------------------------- - * A Modular Optimization framework for Localization and mApping (MOLA) - * - * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria - * Licensed under the GNU GPL v3 for non-commercial applications. - * - * This file is part of MOLA. - * MOLA is free software: you can redistribute it and/or modify it under the - * terms of the GNU General Public License as published by the Free Software - * Foundation, either version 3 of the License, or (at your option) any later - * version. - * - * MOLA is distributed in the hope that it will be useful, but WITHOUT ANY - * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR - * A PARTICULAR PURPOSE. See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License along with - * MOLA. If not, see . - * ------------------------------------------------------------------------- */ - -/** - * @file test-relocalization-align-bev.cpp - * @brief Unit tests for mola_relocalization - * @author Jose Luis Blanco Claraco - * @date Apr 23, 2024 - */ - -#include -#include - -static void test1(const std::string& refMap, const std::string& localMap) -{ - mp2p_icp::metric_map_t rMap, lMap; - - bool gLoadOk = rMap.load_from_file(refMap); - ASSERT_(gLoadOk); - - bool lLoadOk = lMap.load_from_file(localMap); - ASSERT_(lLoadOk); - - mola::AlignBEV::Input in; - - in.reference_map = rMap; - in.local_map = lMap; - - const auto out = mola::AlignBEV::run(in); -} - -int main(int argc, char** argv) -{ - try - { - ASSERT_(argc == 3); - - test1(argv[1], argv[2]); - - std::cout << "Test successful." << std::endl; - } - catch (std::exception& e) - { - std::cerr << e.what() << std::endl; - return 1; - } -}