From 913b3bbaf5eef6ba6f11a016f6de902b1c309ec5 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Thu, 22 Aug 2024 11:02:56 +0200 Subject: [PATCH] Add NDT-3D map class --- docs/source/Doxyfile | 2 +- docs/source/refs.bib | 11 + mola_metric_maps/CMakeLists.txt | 26 +- .../include/mola_metric_maps/NDT.h | 479 ++++++++++ mola_metric_maps/package.xml | 1 + mola_metric_maps/src/NDT.cpp | 854 ++++++++++++++++++ mola_metric_maps/src/register.cpp | 2 + mola_metric_maps/tests/CMakeLists.txt | 7 + .../tests/test-mola_metric_maps_ndt.cpp | 135 +++ 9 files changed, 1511 insertions(+), 6 deletions(-) create mode 100644 mola_metric_maps/include/mola_metric_maps/NDT.h create mode 100644 mola_metric_maps/src/NDT.cpp create mode 100644 mola_metric_maps/tests/test-mola_metric_maps_ndt.cpp diff --git a/docs/source/Doxyfile b/docs/source/Doxyfile index 7a38fd19..516c133c 100644 --- a/docs/source/Doxyfile +++ b/docs/source/Doxyfile @@ -704,7 +704,7 @@ LAYOUT_FILE = doxygen-layout.xml # LATEX_BIB_STYLE. To use this feature you need bibtex and perl available in the # search path. See also \cite for info how to create references. -CITE_BIB_FILES = +CITE_BIB_FILES = refs.bib #--------------------------------------------------------------------------- # Configuration options related to warning and progress messages diff --git a/docs/source/refs.bib b/docs/source/refs.bib index 3dd7165e..da0c4f7d 100644 --- a/docs/source/refs.bib +++ b/docs/source/refs.bib @@ -102,4 +102,15 @@ @article{bhandari2024minimal pages={02783649241235325}, year={2024}, publisher={SAGE Publications Sage UK: London, England} +} + +@article{magnusson2007scan, + title={Scan registration for autonomous mining vehicles using 3D-NDT}, + author={Magnusson, Martin and Lilienthal, Achim and Duckett, Tom}, + journal={Journal of Field Robotics}, + volume={24}, + number={10}, + pages={803--827}, + year={2007}, + publisher={Wiley Online Library} } \ No newline at end of file diff --git a/mola_metric_maps/CMakeLists.txt b/mola_metric_maps/CMakeLists.txt index 4a055c5b..1f73f5e7 100644 --- a/mola_metric_maps/CMakeLists.txt +++ b/mola_metric_maps/CMakeLists.txt @@ -21,15 +21,31 @@ project(mola_metric_maps LANGUAGES CXX) find_package(mola_common REQUIRED) # find CMake dependencies: -find_package(mrpt-maps) +find_package(mrpt-maps REQUIRED) +find_package(mp2p_icp_map REQUIRED) # 3rdparty deps: add_subdirectory(3rdparty) # ----------------------- # define lib: -file(GLOB_RECURSE LIB_SRCS src/*.cpp src/*.h) -file(GLOB_RECURSE LIB_PUBLIC_HDRS include/*.h) +set(LIB_SRCS + src/SparseVoxelPointCloud.cpp + src/register.cpp + src/NDT.cpp + src/HashedVoxelPointCloud.cpp + src/OccGrid.cpp + src/SparseTreesPointCloud.cpp +) +set(LIB_PUBLIC_HDRS + include/mola_metric_maps/OccGrid.h + include/mola_metric_maps/HashedVoxelPointCloud.h + include/mola_metric_maps/SparseTreesPointCloud.h + include/mola_metric_maps/SparseVoxelPointCloud.h + include/mola_metric_maps/index3d_t.h + include/mola_metric_maps/NDT.h + include/mola_metric_maps/FixedDenseGrid3D.h +) mola_add_library( TARGET ${PROJECT_NAME} @@ -37,10 +53,10 @@ mola_add_library( PUBLIC_LINK_LIBRARIES mrpt::maps tsl::robin_map -# PRIVATE_LINK_LIBRARIES -# mrpt::maps + mola::mp2p_icp_map CMAKE_DEPENDENCIES mrpt-maps + mp2p_icp_map robin_map ) diff --git a/mola_metric_maps/include/mola_metric_maps/NDT.h b/mola_metric_maps/include/mola_metric_maps/NDT.h new file mode 100644 index 00000000..bc75f66f --- /dev/null +++ b/mola_metric_maps/include/mola_metric_maps/NDT.h @@ -0,0 +1,479 @@ +/* ------------------------------------------------------------------------- + * 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 NDT.h + * @brief NDT 3D map representation + * @author Jose Luis Blanco Claraco + * @date Aug 22, 2024 + */ +#pragma once + +#include +#include // PointCloudEigen +//#include --> TODO: NN-planes +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +//#define HASHED_VOXEL_POINT_CLOUD_WITH_CACHED_ACCESS + +namespace mola +{ +/** 3D NDT maps: Voxels with points approximated by Gaussians. + * Implementation of \cite magnusson2007scan + */ +class NDT : public mrpt::maps::CMetricMap +// public mrpt::maps::NearestNeighborsCapable => TODO: NN-planes +{ + DEFINE_SERIALIZABLE(NDT, mola) + public: + using global_index3d_t = index3d_t; + + /** @name Indices and coordinates + * @{ */ + + constexpr static size_t MAX_POINTS_PER_VOXEL = 32; + + inline global_index3d_t coordToGlobalIdx( + const mrpt::math::TPoint3Df& pt) const + { + return global_index3d_t( + static_cast(pt.x * voxel_size_inv_), // + static_cast(pt.y * voxel_size_inv_), // + static_cast(pt.z * voxel_size_inv_)); + } + + /// returns the coordinate of the voxel "bottom" corner + inline mrpt::math::TPoint3Df globalIdxToCoord( + const global_index3d_t idx) const + { + return { + idx.cx * voxel_size_, // + idx.cy * voxel_size_, // + idx.cz * voxel_size_}; + } + + /** @} */ + + /** @name Basic API for construction and main parameters + * @{ */ + + /** + * @brief Constructor / default ctor + * @param voxel_size Voxel size [meters] + */ + NDT(float voxel_size = 5.00f); + + ~NDT(); + + /** Reset the main voxel parameters, and *clears* all current map contents + */ + void setVoxelProperties(float voxel_size); + + /** @} */ + + /** @name Data structures + * @{ */ + + struct point_vector_t + { + std::array xs, ys, zs; + }; + + struct VoxelData + { + public: + VoxelData() = default; + + struct PointSpan + { + PointSpan(const point_vector_t& points, uint32_t n) + : points_(points), n_(n) + { + } + + size_t size() const { return n_; } + bool empty() const { return n_ == 0; } + + mrpt::math::TPoint3Df operator[](int i) const + { + return {points_.xs[i], points_.ys[i], points_.zs[i]}; + } + + private: + const point_vector_t& points_; + const uint32_t n_; + }; + + auto points() const -> PointSpan + { + return PointSpan(points_, nPoints_); + } + + void insertPoint(const mrpt::math::TPoint3Df& p) + { + if (nPoints_ < MAX_POINTS_PER_VOXEL) + { + points_.xs[nPoints_] = p.x; + points_.ys[nPoints_] = p.y; + points_.zs[nPoints_] = p.z; + nPoints_++; + } + } + + /// Gets cached NDT for this voxel, or computes it right now. + /// If there are no points enough, nullopt is + const std::optional& ndt() const; + + bool has_ndt() const { return ndt_.has_value(); } + + // for serialization, do not use in normal use: + size_t size() const { return nPoints_; } + + private: + point_vector_t points_; + uint32_t nPoints_ = 0; + mutable std::optional ndt_; + }; + + using grids_map_t = + tsl::robin_map>; + + /** @} */ + + /** @name Data access API + * @{ */ + // clear(): available in base class + + /** returns the voxeldata by global index coordinates, creating it or not if + * not found depending on createIfNew. + * Returns nullptr if not found and createIfNew is false + * + * Function defined in the header file so compilers can optimize + * for literals "createIfNew" + */ + inline VoxelData* voxelByGlobalIdxs( + const global_index3d_t& idx, bool createIfNew) + { + // 1) Insert into decimation voxel map: + VoxelData* voxel = nullptr; + +#if defined(HASHED_VOXEL_POINT_CLOUD_WITH_CACHED_ACCESS) + for (int i = 0; i < CachedData::NUM_CACHED_IDXS; i++) + { + if (cached_.lastAccessVoxel[i] && cached_.lastAccessIdx[i] == idx) + { + // Cache hit: +#ifdef USE_DEBUG_PROFILER + mrpt::system::CTimeLoggerEntry tle( + profiler, "insertPoint.cache_hit"); +#endif + voxel = cached_.lastAccessVoxel[i]; + break; + } + } + + if (!voxel) + { +#endif +#ifdef USE_DEBUG_PROFILER + mrpt::system::CTimeLoggerEntry tle( + profiler, "insertPoint.cache_misss"); +#endif + + auto it = voxels_.find(idx); + if (it == voxels_.end()) + { + if (!createIfNew) + return nullptr; + else + voxel = &voxels_[idx]; // Create it + } + else + { + // Use the found grid + voxel = &it.value(); + } +#if defined(HASHED_VOXEL_POINT_CLOUD_WITH_CACHED_ACCESS) + // Add to cache: + cached_.lastAccessIdx[cached_.lastAccessNextWrite] = idx; + cached_.lastAccessVoxel[cached_.lastAccessNextWrite] = voxel; + cached_.lastAccessNextWrite++; + cached_.lastAccessNextWrite &= CachedData::NUM_CACHED_IDX_MASK; + } +#endif + return voxel; + } + + /// \overload (const version) + const VoxelData* voxelByGlobalIdxs( + const global_index3d_t& idx // + /*, bool createIfNew this must be false for const! */) const + { // reuse the non-const method: + return const_cast(this)->voxelByGlobalIdxs(idx, false); + } + + /** Get a voxeldata by (x,y,z) coordinates, **creating** the voxel if + * needed. */ + VoxelData* voxelByCoords(const mrpt::math::TPoint3Df& pt, bool createIfNew) + { + return voxelByGlobalIdxs(coordToGlobalIdx(pt), createIfNew); + } + + /// \overload const version. Returns nullptr if voxel does not exist + const VoxelData* voxelByCoords(const mrpt::math::TPoint3Df& pt) const + { + return const_cast(this)->voxelByCoords(pt, false); + } + + /** Insert one point into the dual voxel map */ + void insertPoint(const mrpt::math::TPoint3Df& pt); + + const grids_map_t& voxels() const { return voxels_; } + + /** Computes the bounding box of all the points, or (0,0 ,0,0, 0,0) if + * there are no points. Results are cached unless the map is somehow + * modified to avoid repeated calculations. + */ + mrpt::math::TBoundingBoxf boundingBox() const override; + + void visitAllPoints( + const std::function& f) const; + + void visitAllVoxels( + const std::function& f) + const; + + /** Save to a text file. Each line contains "X Y Z" point coordinates. + * Returns false if any error occured, true elsewere. + */ + bool saveToTextFile(const std::string& file) const; + + /** @} */ + + /** @name API of the NearestNeighborsCapable virtual interface + @{ */ + + /** @} */ + + /** @name Public virtual methods implementation for CMetricMap + * @{ */ + + /** Returns a short description of the map. */ + std::string asString() const override; + + void getVisualizationInto( + mrpt::opengl::CSetOfObjects& outObj) const override; + + /** Returns true if the map is empty */ + bool isEmpty() const override; + + /** This virtual method saves the map to a file "filNamePrefix"+< + * some_file_extension >, as an image or in any other applicable way (Notice + * that other methods to save the map may be implemented in classes + * implementing this virtual interface). */ + void saveMetricMapRepresentationToFile( + const std::string& filNamePrefix) const override; + + /// Returns a cached point cloud view of the hash map. + /// Not efficient at all. Only for MOLA->ROS2 bridge. + const mrpt::maps::CSimplePointsMap* getAsSimplePointsMap() const override; + + /** @} */ + + /** Options for insertObservation() + */ + struct TInsertionOptions : public mrpt::config::CLoadableOptions + { + TInsertionOptions() = default; + + void loadFromConfigFile( + const mrpt::config::CConfigFileBase& source, + const std::string& section) override; // See base docs + void dumpToTextStream( + std::ostream& out) const override; // See base docs + + void writeToStream(mrpt::serialization::CArchive& out) const; + void readFromStream(mrpt::serialization::CArchive& in); + + /** Maximum number of points per voxel. 0 means no limit. + */ + uint32_t max_points_per_voxel = 0; + + /** If !=0, remove the voxels farther (L1 distance) than this + * distance, in meters. */ + double remove_voxels_farther_than = .0; + + /** If !=0 skip the insertion of points that are closer than this + * distance to any other already in the voxel. */ + float min_distance_between_points = .0f; + }; + TInsertionOptions insertionOptions; + + /** Options used when evaluating "computeObservationLikelihood" in the + * derived classes. + * \sa CObservation::computeObservationLikelihood + */ + struct TLikelihoodOptions : public mrpt::config::CLoadableOptions + { + TLikelihoodOptions() = default; + + void loadFromConfigFile( + const mrpt::config::CConfigFileBase& source, + const std::string& section) override; // See base docs + void dumpToTextStream( + std::ostream& out) const override; // See base docs + + void writeToStream(mrpt::serialization::CArchive& out) const; + void readFromStream(mrpt::serialization::CArchive& in); + + /** Sigma (standard deviation, in meters) of the Gaussian observation + * model used to model the likelihood (default= 0.5 m) */ + double sigma_dist = 0.5; + + /** Maximum distance in meters to consider for the numerator divided by + * "sigma_dist", so that each point has a minimum (but very small) + * likelihood to avoid underflows (default=1.0 meters) */ + double max_corr_distance = 1.0; + + /** Speed up the likelihood computation by considering only one out of N + * rays (default=10) */ + uint32_t decimation = 10; + }; + TLikelihoodOptions likelihoodOptions; + + /** Rendering options, used in getAs3DObject() + */ + struct TRenderOptions : public mrpt::config::CLoadableOptions + { + void loadFromConfigFile( + const mrpt::config::CConfigFileBase& source, + const std::string& section) override; // See base docs + void dumpToTextStream( + std::ostream& out) const override; // See base docs + + /** Binary dump to stream - used in derived classes' serialization */ + void writeToStream(mrpt::serialization::CArchive& out) const; + /** Binary dump to stream - used in derived classes' serialization */ + void readFromStream(mrpt::serialization::CArchive& in); + + float point_size = 1.0f; + + /** Color of points. Superseded by colormap if the latter is set. */ + mrpt::img::TColorf color{.0f, .0f, 1.0f}; + + /** Colormap for points (index is "z" coordinates) */ + mrpt::img::TColormap colormap = mrpt::img::cmHOT; + + /** If colormap!=mrpt::img::cmNONE, use this coordinate + * as color index: 0=x 1=y 2=z + */ + uint8_t recolorizeByCoordinateIndex = 2; + }; + TRenderOptions renderOptions; + + public: + // Interface for use within a mrpt::maps::CMultiMetricMap: + MAP_DEFINITION_START(NDT) + float voxel_size = 1.00f; + + mola::NDT::TInsertionOptions insertionOpts; + mola::NDT::TLikelihoodOptions likelihoodOpts; + mola::NDT::TRenderOptions renderOpts; + MAP_DEFINITION_END(NDT) + + private: + float voxel_size_ = 1.00f; + + // Calculated from the above, in setVoxelProperties() + float voxel_size_inv_ = 1.0f / voxel_size_; + float voxel_size_sqr_ = voxel_size_ * voxel_size_; + mrpt::math::TPoint3Df voxelDiagonal_; + + /** Voxel map as a set of fixed-size grids */ + grids_map_t voxels_; + + struct CachedData + { + CachedData() = default; + + void reset() { *this = CachedData(); } + + mutable std::optional boundingBox_; + +#if defined(HASHED_VOXEL_POINT_CLOUD_WITH_CACHED_ACCESS) + // 2 bits seems to be the optimum for typical cases: + constexpr static int CBITS = 2; + constexpr static int NUM_CACHED_IDXS = 1 << CBITS; + constexpr static int NUM_CACHED_IDX_MASK = NUM_CACHED_IDXS - 1; + + int lastAccessNextWrite = 0; + global_index3d_t lastAccessIdx[NUM_CACHED_IDXS]; + VoxelData* lastAccessVoxel[NUM_CACHED_IDXS] = {nullptr}; +#endif + }; + + CachedData cached_; + + protected: + // See docs in base CMetricMap class: + void internal_clear() override; + + private: + // See docs in base CMetricMap class: + bool internal_insertObservation( + const mrpt::obs::CObservation& obs, + const std::optional& robotPose = + std::nullopt) override; + // See docs in base class + double internal_computeObservationLikelihood( + const mrpt::obs::CObservation& obs, + const mrpt::poses::CPose3D& takenFrom) const override; + + double internal_computeObservationLikelihoodPointCloud3D( + const mrpt::poses::CPose3D& pc_in_map, const float* xs, const float* ys, + const float* zs, const std::size_t num_pts) const; + + /** - (xs,ys,zs): Sensed point local coordinates in the robot frame. + * - pc_in_map: SE(3) pose of the robot in the map frame. + */ + void internal_insertPointCloud3D( + const mrpt::poses::CPose3D& pc_in_map, const float* xs, const float* ys, + const float* zs, const std::size_t num_pts); + + // See docs in base class + bool internal_canComputeObservationLikelihood( + const mrpt::obs::CObservation& obs) const override; + + /// Used for getAsSimplePointsMap only. + mutable mrpt::maps::CSimplePointsMap::Ptr cachedPoints_; +}; + +} // namespace mola diff --git a/mola_metric_maps/package.xml b/mola_metric_maps/package.xml index a395bea6..35a01ddc 100644 --- a/mola_metric_maps/package.xml +++ b/mola_metric_maps/package.xml @@ -17,6 +17,7 @@ mola_common mrpt_libmaps + mp2p_icp doxygen diff --git a/mola_metric_maps/src/NDT.cpp b/mola_metric_maps/src/NDT.cpp new file mode 100644 index 00000000..992037e9 --- /dev/null +++ b/mola_metric_maps/src/NDT.cpp @@ -0,0 +1,854 @@ +/* ------------------------------------------------------------------------- + * 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 NDT.cpp + * @brief NDT 3D map representation + * @author Jose Luis Blanco Claraco + * @date Aug 22, 2024 + */ + +/* An implementation of: + Scan registration for autonomous mining vehicles using 3D-NDT, + Magnusson, Martin and Lilienthal, Achim and Duckett, Tom, + Journal of Field Robotics, 2007. +*/ + +#include +#include // MRPT_LOAD_CONFIG_VAR +#include +#include +#include // confidenceIntervals() +#include +#include +#include +#include +#include +#include +#include // serialization +#include + +#include + +//#define USE_DEBUG_PROFILER + +#ifdef USE_DEBUG_PROFILER +#include +static mrpt::system::CTimeLogger profiler(true, "NDT"); +#endif + +using namespace mola; + +// =========== Begin of Map definition ============ +MAP_DEFINITION_REGISTER("mola::NDT,NDT", mola::NDT) + +NDT::TMapDefinition::TMapDefinition() = default; +void NDT::TMapDefinition::loadFromConfigFile_map_specific( + const mrpt::config::CConfigFileBase& s, const std::string& sectionPrefix) +{ + using namespace std::string_literals; + + // [+"_creationOpts"] + const std::string sSectCreation = sectionPrefix + "_creationOpts"s; + MRPT_LOAD_CONFIG_VAR(voxel_size, float, s, sSectCreation); + + ASSERT_(s.sectionExists(sectionPrefix + "_insertOpts"s)); + insertionOpts.loadFromConfigFile(s, sectionPrefix + "_insertOpts"s); + + if (s.sectionExists(sectionPrefix + "_likelihoodOpts"s)) + likelihoodOpts.loadFromConfigFile( + s, sectionPrefix + "_likelihoodOpts"s); + + if (s.sectionExists(sectionPrefix + "_renderOpts"s)) + renderOpts.loadFromConfigFile(s, sectionPrefix + "_renderOpts"s); +} + +void NDT::TMapDefinition::dumpToTextStream_map_specific(std::ostream& out) const +{ + LOADABLEOPTS_DUMP_VAR(voxel_size, float); + + insertionOpts.dumpToTextStream(out); + likelihoodOpts.dumpToTextStream(out); + renderOpts.dumpToTextStream(out); +} + +mrpt::maps::CMetricMap::Ptr NDT::internal_CreateFromMapDefinition( + const mrpt::maps::TMetricMapInitializer& _def) +{ + const NDT::TMapDefinition* def = + dynamic_cast(&_def); + ASSERT_(def); + auto obj = NDT::Create(def->voxel_size); + + obj->insertionOptions = def->insertionOpts; + obj->likelihoodOptions = def->likelihoodOpts; + obj->renderOptions = def->renderOpts; + + return obj; +} +// =========== End of Map definition Block ========= + +IMPLEMENTS_SERIALIZABLE(NDT, CMetricMap, mola) + +// ===================================== +// Serialization +// ===================================== + +uint8_t NDT::serializeGetVersion() const { return 0; } +void NDT::serializeTo(mrpt::serialization::CArchive& out) const +{ + // params: + out << voxel_size_; + insertionOptions.writeToStream(out); + likelihoodOptions.writeToStream(out); + renderOptions.writeToStream(out); + + // data: + out.WriteAs(voxels_.size()); + for (const auto& [idx, voxel] : voxels_) + { + out << idx.cx << idx.cy << idx.cz; + const auto& pts = voxel.points(); + const size_t N = pts.size(); + out.WriteAs(N); + for (size_t j = 0; j < N; j++) out << pts[j]; + } +} +void NDT::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) +{ + switch (version) + { + case 0: + { + // params: + in >> voxel_size_; + + // clear contents and compute computed fields: + this->setVoxelProperties(voxel_size_); + + insertionOptions.readFromStream(in); + likelihoodOptions.readFromStream(in); + renderOptions.readFromStream(in); + + // data: + const auto nGrids = in.ReadAs(); + for (uint32_t i = 0; i < nGrids; i++) + { + global_index3d_t idx; + in >> idx.cx >> idx.cy >> idx.cz; + + auto& grid = voxels_[idx]; + + const auto nPts = in.ReadAs(); + for (size_t j = 0; j < nPts; j++) + { + mrpt::math::TPoint3Df pt; + in >> pt; + grid.insertPoint(pt); + } + } + } + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; + + // cache reset: + cached_.reset(); +} + +// VoxelData + +// Ctor: +NDT::NDT(float voxel_size) { setVoxelProperties(voxel_size); } + +NDT::~NDT() = default; + +void NDT::setVoxelProperties(float voxel_size) +{ + voxel_size_ = voxel_size; + + // calculated fields: + voxel_size_inv_ = 1.0f / voxel_size_; + voxel_size_sqr_ = voxel_size_ * voxel_size_; + voxelDiagonal_ = mrpt::math::TPoint3Df(1.0f, 1.0f, 1.0f) * voxel_size_; + + // clear all: + NDT::internal_clear(); +} + +std::string NDT::asString() const +{ + return mrpt::format( + "NDT, resolution=%.03f bbox=%s", voxel_size_, + boundingBox().asString().c_str()); +} + +void NDT::getVisualizationInto(mrpt::opengl::CSetOfObjects& outObj) const +{ + MRPT_START + if (!genericMapParams.enableSaveAs3DObject) return; + + if (renderOptions.colormap == mrpt::img::cmNONE) + { + // Single color: + auto obj = mrpt::opengl::CPointCloud::Create(); + + const auto lambdaVisitPoints = [&obj](const mrpt::math::TPoint3Df& pt) + { obj->insertPoint(pt); }; + this->visitAllPoints(lambdaVisitPoints); + + obj->setColor(renderOptions.color); + obj->setPointSize(renderOptions.point_size); + obj->enableColorFromZ(false); + outObj.insert(obj); + } + else + { + auto obj = mrpt::opengl::CPointCloudColoured::Create(); + + auto bb = this->boundingBox(); + + // handle planar maps (avoids error in histogram below): + for (int i = 0; i < 3; i++) + if (bb.max[i] - bb.min[i] < 0.1f) bb.max[i] = bb.min[i] + 0.1f; + + // Use a histogram to discard outliers from the colormap extremes: + constexpr size_t nBins = 100; + // for x,y,z + std::array hists = { + mrpt::math::CHistogram(bb.min.x, bb.max.x, nBins), + mrpt::math::CHistogram(bb.min.y, bb.max.y, nBins), + mrpt::math::CHistogram(bb.min.z, bb.max.z, nBins)}; + + size_t nPoints = 0; + + const auto lambdaVisitPoints = + [&obj, &hists, &nPoints](const mrpt::math::TPoint3Df& pt) + { + // x y z R G B [A] + obj->insertPoint({pt.x, pt.y, pt.z, 0, 0, 0}); + for (int i = 0; i < 3; i++) hists[i].add(pt[i]); + nPoints++; + }; + + this->visitAllPoints(lambdaVisitPoints); + + obj->setPointSize(renderOptions.point_size); + + if (nPoints) + { + // Analyze the histograms and get confidence intervals: + std::vector coords; + std::vector hits; + + const int idx = renderOptions.recolorizeByCoordinateIndex; + ASSERT_(idx >= 0 && idx < 3); + + float min = .0, max = 1.f; + constexpr double confidenceInterval = 0.02; + + hists[idx].getHistogramNormalized(coords, hits); + mrpt::math::confidenceIntervalsFromHistogram( + coords, hits, min, max, confidenceInterval); + + obj->recolorizeByCoordinate( + min, max, renderOptions.recolorizeByCoordinateIndex, + renderOptions.colormap); + } + outObj.insert(obj); + } + MRPT_END +} + +void NDT::internal_clear() { voxels_.clear(); } + +bool NDT::internal_insertObservation( + const mrpt::obs::CObservation& obs, + const std::optional& robotPose) +{ + MRPT_START + + using namespace mrpt::obs; + + mrpt::poses::CPose2D robotPose2D; + mrpt::poses::CPose3D robotPose3D; + + if (robotPose) + { + robotPose2D = mrpt::poses::CPose2D(*robotPose); + robotPose3D = (*robotPose); + } + else + { + // Default values are (0,0,0) + } + + if (insertionOptions.remove_voxels_farther_than > 0) + { + const int distInGrid = static_cast(std::ceil( + insertionOptions.remove_voxels_farther_than * voxel_size_inv_)); + + const auto idxCurObs = + coordToGlobalIdx(robotPose3D.translation().cast()); + + for (auto it = voxels_.begin(); it != voxels_.end();) + { + // manhattan distance: + const int dist = mrpt::max3( + std::abs(it->first.cx - idxCurObs.cx), + std::abs(it->first.cy - idxCurObs.cy), + std::abs(it->first.cz - idxCurObs.cz)); + + if (dist > distInGrid) + it = voxels_.erase(it); + else + ++it; + } + } + + if (IS_CLASS(obs, CObservation2DRangeScan)) + { + /******************************************************************** + OBSERVATION TYPE: CObservation2DRangeScan + ********************************************************************/ + const auto& o = static_cast(obs); + + // Build (if not done before) the points map representation of this + // observation: + const auto* scanPoints = o.buildAuxPointsMap(); + + if (scanPoints->empty()) return 0; + + const auto& xs = scanPoints->getPointsBufferRef_x(); + const auto& ys = scanPoints->getPointsBufferRef_y(); + const auto& zs = scanPoints->getPointsBufferRef_z(); + + internal_insertPointCloud3D( + robotPose3D, xs.data(), ys.data(), zs.data(), xs.size()); + + return true; + } + else if (IS_CLASS(obs, CObservation3DRangeScan)) + { + /******************************************************************** + OBSERVATION TYPE: CObservation3DRangeScan + ********************************************************************/ + const auto& o = static_cast(obs); + + mrpt::obs::T3DPointsProjectionParams pp; + pp.takeIntoAccountSensorPoseOnRobot = true; + + // Empty point set, or load from XYZ in observation: + if (o.hasPoints3D) + { + for (size_t i = 0; i < o.points3D_x.size(); i++) + this->insertPoint(robotPose3D.composePoint( + {o.points3D_x[i], o.points3D_y[i], o.points3D_z[i]})); + + return true; + } + else if (o.hasRangeImage) + { + mrpt::maps::CSimplePointsMap pointMap; + const_cast(o).unprojectInto(pointMap, pp); + + const auto& xs = pointMap.getPointsBufferRef_x(); + const auto& ys = pointMap.getPointsBufferRef_y(); + const auto& zs = pointMap.getPointsBufferRef_z(); + + internal_insertPointCloud3D( + robotPose3D, xs.data(), ys.data(), zs.data(), xs.size()); + + return true; + } + else + return false; + } + else if (IS_CLASS(obs, CObservationVelodyneScan)) + { + /******************************************************************** + OBSERVATION TYPE: CObservationVelodyneScan + ********************************************************************/ + const auto& o = static_cast(obs); + + // Automatically generate pointcloud if needed: + if (!o.point_cloud.size()) + const_cast(o).generatePointCloud(); + + for (size_t i = 0; i < o.point_cloud.x.size(); i++) + { + insertPoint(robotPose3D.composePoint( + {o.point_cloud.x[i], o.point_cloud.y[i], o.point_cloud.z[i]})); + } + + return true; + } + else if (IS_CLASS(obs, CObservationPointCloud)) + { + const auto& o = static_cast(obs); + ASSERT_(o.pointcloud); + + const auto& xs = o.pointcloud->getPointsBufferRef_x(); + const auto& ys = o.pointcloud->getPointsBufferRef_y(); + const auto& zs = o.pointcloud->getPointsBufferRef_z(); + + for (size_t i = 0; i < xs.size(); i++) + { + insertPoint(robotPose3D.composePoint({xs[i], ys[i], zs[i]})); + } + + return true; + } + else + { + /******************************************************************** + OBSERVATION TYPE: Unknown + ********************************************************************/ + return false; + } + + MRPT_END +} + +double NDT::internal_computeObservationLikelihood( + const mrpt::obs::CObservation& obs, + const mrpt::poses::CPose3D& takenFrom) const +{ + using namespace mrpt::obs; + using namespace mrpt::poses; + using namespace mrpt::maps; + + if (isEmpty()) return 0; + + // This function depends on the observation type: + // ----------------------------------------------------- + if (IS_CLASS(obs, CObservation2DRangeScan)) + { + // Observation is a laser range scan: + // ------------------------------------------- + const auto& o = static_cast(obs); + + // Build (if not done before) the points map representation of this + // observation: + const auto* scanPoints = o.buildAuxPointsMap(); + + const size_t N = scanPoints->size(); + if (!N) return 0; + + const auto& xs = scanPoints->getPointsBufferRef_x(); + const auto& ys = scanPoints->getPointsBufferRef_y(); + const auto& zs = scanPoints->getPointsBufferRef_z(); + + return internal_computeObservationLikelihoodPointCloud3D( + takenFrom, xs.data(), ys.data(), zs.data(), N); + } + else if (IS_CLASS(obs, CObservationVelodyneScan)) + { + const auto& o = dynamic_cast(obs); + + // Automatically generate pointcloud if needed: + if (!o.point_cloud.size()) + const_cast(o).generatePointCloud(); + + const size_t N = o.point_cloud.size(); + if (!N) return 0; + + const CPose3D sensorAbsPose = takenFrom + o.sensorPose; + + const auto& xs = o.point_cloud.x; + const auto& ys = o.point_cloud.y; + const auto& zs = o.point_cloud.z; + + return internal_computeObservationLikelihoodPointCloud3D( + sensorAbsPose, xs.data(), ys.data(), zs.data(), N); + } + else if (IS_CLASS(obs, CObservationPointCloud)) + { + const auto& o = dynamic_cast(obs); + + const size_t N = o.pointcloud->size(); + if (!N) return 0; + + const CPose3D sensorAbsPose = takenFrom + o.sensorPose; + + auto xs = o.pointcloud->getPointsBufferRef_x(); + auto ys = o.pointcloud->getPointsBufferRef_y(); + auto zs = o.pointcloud->getPointsBufferRef_z(); + + return internal_computeObservationLikelihoodPointCloud3D( + sensorAbsPose, xs.data(), ys.data(), zs.data(), N); + } + + return .0; +} + +double NDT::internal_computeObservationLikelihoodPointCloud3D( + const mrpt::poses::CPose3D& pc_in_map, const float* xs, const float* ys, + const float* zs, const std::size_t num_pts) const +{ + MRPT_TRY_START + + ASSERT_GT_(likelihoodOptions.sigma_dist, .0); + + mrpt::math::TPoint3Df closest; + float closest_err; + uint64_t closest_id; + const float max_sqr_err = mrpt::square(likelihoodOptions.max_corr_distance); + double sumSqrDist = .0; + + std::size_t nPtsForAverage = 0; + for (std::size_t i = 0; i < num_pts; + i += likelihoodOptions.decimation, nPtsForAverage++) + { + // Transform the point from the scan reference to its global 3D + // position: + const auto gPt = pc_in_map.composePoint({xs[i], ys[i], zs[i]}); + + THROW_EXCEPTION("to-do"); +#if 0 + const bool found = + nn_single_search(gPt, closest, closest_err, closest_id); + if (!found) +#endif + continue; + + // Put a limit: + mrpt::keep_min(closest_err, max_sqr_err); + + sumSqrDist += static_cast(closest_err); + } + if (nPtsForAverage) sumSqrDist /= nPtsForAverage; + + // Log-likelihood: + return -sumSqrDist / likelihoodOptions.sigma_dist; + + MRPT_TRY_END +} + +bool NDT::internal_canComputeObservationLikelihood( + const mrpt::obs::CObservation& obs) const +{ + using namespace mrpt::obs; + + return IS_CLASS(obs, CObservation2DRangeScan) || + IS_CLASS(obs, CObservationVelodyneScan) || + IS_CLASS(obs, CObservationPointCloud); +} + +bool NDT::isEmpty() const +{ + // empty if no voxels exist: + return voxels_.empty(); +} + +void NDT::saveMetricMapRepresentationToFile( + const std::string& filNamePrefix) const +{ + using namespace std::string_literals; + + const auto fil = filNamePrefix + ".txt"s; + saveToTextFile(fil); +} + +bool NDT::saveToTextFile(const std::string& file) const +{ + FILE* f = mrpt::system::os::fopen(file.c_str(), "wt"); + if (!f) return false; + + const auto lambdaVisitPoints = [f](const mrpt::math::TPoint3Df& pt) + { mrpt::system::os::fprintf(f, "%f %f %f\n", pt.x, pt.y, pt.z); }; + + this->visitAllPoints(lambdaVisitPoints); + + mrpt::system::os::fclose(f); + return true; +} + +void NDT::insertPoint(const mrpt::math::TPoint3Df& pt) +{ + auto& v = *voxelByCoords(pt, true /*create if new*/); + + const auto nPreviousPoints = v.points().size(); + + if (insertionOptions.max_points_per_voxel != 0 && + nPreviousPoints >= insertionOptions.max_points_per_voxel) + return; // skip, voxel is full + + if (insertionOptions.min_distance_between_points > 0 && + nPreviousPoints != 0) + { + // Look for the closest existing point in the voxel: + std::optional curClosestDistSqr; + + const auto& pts = v.points(); + for (size_t i = 0; i < pts.size(); i++) + { + const float distSqr = pts[i].sqrDistanceTo(pt); + if (!curClosestDistSqr.has_value() || + distSqr < curClosestDistSqr.value()) + curClosestDistSqr = distSqr; + } + const float minDistSqr = + mrpt::square(insertionOptions.min_distance_between_points); + + // Skip if the point is too close to existing ones: + if (curClosestDistSqr.value() < minDistSqr) return; + } + + v.insertPoint(pt); + + // Also, update bbox: + if (!cached_.boundingBox_.has_value()) + cached_.boundingBox_.emplace(pt, pt); + else + cached_.boundingBox_->updateWithPoint(pt); +} + +mrpt::math::TBoundingBoxf NDT::boundingBox() const +{ + if (!cached_.boundingBox_) + { + cached_.boundingBox_.emplace(); + if (this->isEmpty()) + { + cached_.boundingBox_->min = {0, 0, 0}; + cached_.boundingBox_->max = {0, 0, 0}; + } + else + { + cached_.boundingBox_ = + mrpt::math::TBoundingBoxf::PlusMinusInfinity(); + + auto lambdaForEachVoxel = + [this](const global_index3d_t& idxs, const VoxelData&) + { + const mrpt::math::TPoint3Df voxelCorner = + globalIdxToCoord(idxs); + + cached_.boundingBox_->updateWithPoint(voxelCorner); + cached_.boundingBox_->updateWithPoint( + voxelCorner + voxelDiagonal_); + }; + + this->visitAllVoxels(lambdaForEachVoxel); + } + } + + return cached_.boundingBox_.value(); +} + +void NDT::visitAllPoints( + const std::function& f) const +{ + for (const auto& [idx, v] : voxels_) + { + const auto& pts = v.points(); + for (size_t i = 0; i < pts.size(); i++) f(pts[i]); + } +} + +void NDT::visitAllVoxels( + const std::function& f) + const +{ + for (const auto& [idx, v] : voxels_) f(idx, v); +} + +// ========== Option structures ========== +void NDT::TInsertionOptions::writeToStream( + mrpt::serialization::CArchive& out) const +{ + const int8_t version = 0; + out << version; + out << max_points_per_voxel << remove_voxels_farther_than + << min_distance_between_points; +} + +void NDT::TInsertionOptions::readFromStream(mrpt::serialization::CArchive& in) +{ + int8_t version; + in >> version; + switch (version) + { + case 0: + { + in >> max_points_per_voxel >> remove_voxels_farther_than >> + min_distance_between_points; + } + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + } +} + +void NDT::TLikelihoodOptions::writeToStream( + mrpt::serialization::CArchive& out) const +{ + const int8_t version = 0; + out << version; + out << sigma_dist << max_corr_distance << decimation; +} + +void NDT::TLikelihoodOptions::readFromStream(mrpt::serialization::CArchive& in) +{ + int8_t version; + in >> version; + switch (version) + { + case 0: + { + in >> sigma_dist >> max_corr_distance >> decimation; + } + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + } +} + +void NDT::TRenderOptions::writeToStream( + mrpt::serialization::CArchive& out) const +{ + const int8_t version = 0; + out << version; + out << point_size << color << int8_t(colormap) + << recolorizeByCoordinateIndex; +} + +void NDT::TRenderOptions::readFromStream(mrpt::serialization::CArchive& in) +{ + int8_t version; + in >> version; + switch (version) + { + case 0: + { + in >> point_size; + in >> this->color; + in.ReadAsAndCastTo(this->colormap); + in >> recolorizeByCoordinateIndex; + } + break; + default: + MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + } +} + +void NDT::TInsertionOptions::dumpToTextStream(std::ostream& out) const +{ + out << "\n------ [NDT::TInsertionOptions] ------- " + "\n\n"; + + LOADABLEOPTS_DUMP_VAR(max_points_per_voxel, int); + LOADABLEOPTS_DUMP_VAR(remove_voxels_farther_than, double); + LOADABLEOPTS_DUMP_VAR(min_distance_between_points, float) +} + +void NDT::TLikelihoodOptions::dumpToTextStream(std::ostream& out) const +{ + out << "\n------ [NDT::TLikelihoodOptions] ------- " + "\n\n"; + + LOADABLEOPTS_DUMP_VAR(sigma_dist, double); + LOADABLEOPTS_DUMP_VAR(max_corr_distance, double); + LOADABLEOPTS_DUMP_VAR(decimation, int); +} + +void NDT::TRenderOptions::dumpToTextStream(std::ostream& out) const +{ + out << "\n------ [NDT::TRenderOptions] ------- \n\n"; + + LOADABLEOPTS_DUMP_VAR(point_size, float); + LOADABLEOPTS_DUMP_VAR(color.R, float); + LOADABLEOPTS_DUMP_VAR(color.G, float); + LOADABLEOPTS_DUMP_VAR(color.B, float); + LOADABLEOPTS_DUMP_VAR(colormap, int); + LOADABLEOPTS_DUMP_VAR(recolorizeByCoordinateIndex, int); +} + +void NDT::TInsertionOptions::loadFromConfigFile( + const mrpt::config::CConfigFileBase& c, const std::string& s) +{ + MRPT_LOAD_CONFIG_VAR(max_points_per_voxel, int, c, s); + MRPT_LOAD_CONFIG_VAR(remove_voxels_farther_than, double, c, s); + MRPT_LOAD_CONFIG_VAR(min_distance_between_points, float, c, s); +} + +void NDT::TLikelihoodOptions::loadFromConfigFile( + const mrpt::config::CConfigFileBase& c, const std::string& s) +{ + MRPT_LOAD_CONFIG_VAR(sigma_dist, double, c, s); + MRPT_LOAD_CONFIG_VAR(max_corr_distance, double, c, s); + MRPT_LOAD_CONFIG_VAR(decimation, int, c, s); +} + +void NDT::TRenderOptions::loadFromConfigFile( + const mrpt::config::CConfigFileBase& c, const std::string& s) +{ + MRPT_LOAD_CONFIG_VAR(point_size, float, c, s); + MRPT_LOAD_CONFIG_VAR(color.R, float, c, s); + MRPT_LOAD_CONFIG_VAR(color.G, float, c, s); + MRPT_LOAD_CONFIG_VAR(color.B, float, c, s); + colormap = c.read_enum(s, "colormap", this->colormap); + MRPT_LOAD_CONFIG_VAR(recolorizeByCoordinateIndex, int, c, s); +} + +void NDT::internal_insertPointCloud3D( + const mrpt::poses::CPose3D& pc_in_map, const float* xs, const float* ys, + const float* zs, const std::size_t num_pts) +{ + MRPT_TRY_START + + voxels_.reserve(voxels_.size() + num_pts); + + for (std::size_t i = 0; i < num_pts; i++) + { + // Transform the point from the scan reference to its global 3D + // position: + const auto gPt = pc_in_map.composePoint({xs[i], ys[i], zs[i]}); + insertPoint(gPt); + } + + MRPT_TRY_END +} + +const mrpt::maps::CSimplePointsMap* NDT::getAsSimplePointsMap() const +{ + if (!cachedPoints_) cachedPoints_ = mrpt::maps::CSimplePointsMap::Create(); + + cachedPoints_->clear(); + + this->visitAllPoints([this](const mrpt::math::TPoint3Df& p) + { cachedPoints_->insertPointFast(p.x, p.y, p.z); }); + + return cachedPoints_.get(); +} + +const std::optional& NDT::VoxelData::ndt() const +{ + if (has_ndt() || size() < 3) return ndt_; + + ndt_.emplace(); + *ndt_ = mp2p_icp::estimate_points_eigen( + points_.xs.data(), points_.ys.data(), points_.zs.data(), std::nullopt, + size()); + + return ndt_; +} diff --git a/mola_metric_maps/src/register.cpp b/mola_metric_maps/src/register.cpp index 6bb769b2..30e12c57 100644 --- a/mola_metric_maps/src/register.cpp +++ b/mola_metric_maps/src/register.cpp @@ -30,6 +30,7 @@ */ #include +#include #include #include #include @@ -47,4 +48,5 @@ MRPT_INITIALIZER(do_register_mola_metric_maps) registerClass(CLASS_ID(mola::SparseVoxelPointCloud)); registerClass(CLASS_ID(mola::SparseTreesPointCloud)); registerClass(CLASS_ID(mola::HashedVoxelPointCloud)); + registerClass(CLASS_ID(mola::NDT)); } diff --git a/mola_metric_maps/tests/CMakeLists.txt b/mola_metric_maps/tests/CMakeLists.txt index 742b47de..6e40c12b 100644 --- a/mola_metric_maps/tests/CMakeLists.txt +++ b/mola_metric_maps/tests/CMakeLists.txt @@ -20,6 +20,13 @@ mola_add_test( mola_metric_maps ) +mola_add_test( + TARGET test-mola_metric_maps_ndt + SOURCES test-mola_metric_maps_ndt.cpp + LINK_LIBRARIES + mola_metric_maps +) + mola_add_test( TARGET test-mola_metric_maps_serialization SOURCES test-serialization.cpp diff --git a/mola_metric_maps/tests/test-mola_metric_maps_ndt.cpp b/mola_metric_maps/tests/test-mola_metric_maps_ndt.cpp new file mode 100644 index 00000000..eef3a93a --- /dev/null +++ b/mola_metric_maps/tests/test-mola_metric_maps_ndt.cpp @@ -0,0 +1,135 @@ +/* ------------------------------------------------------------------------- + * 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-mola_metric_maps_ndt.cpp + * @brief Test the 3D NDT map class + * @author Jose Luis Blanco Claraco + * @date Aug 22, 2024 + */ + +#include +#include +#include +#include + +#include + +void test_voxelmap_basic_ops() +{ + mola::NDT map; + ASSERT_(map.isEmpty()); + + map.insertPoint({1.0f, 2.0f, 3.0f}); + ASSERT_(!map.isEmpty()); +} + +void test_voxelmap_insert_2d_scan() +{ + mola::NDT map(0.2 /*voxel size*/); + + mrpt::obs::CObservation2DRangeScan scan2D; + mrpt::obs::stock_observations::example2DRangeScan(scan2D); + + map.insertObservation(scan2D); +#if 0 + mrpt::opengl::Scene scene; + map.renderOptions.point_size = 5.0f; + scene.insert(map.getVisualization()); + scene.saveToFile("sparsevoxelmap_scan2d.3Dscene"); +#endif + + { + size_t nPts = 0; + + const auto lambdaVisitPoints = [&nPts](const mrpt::math::TPoint3Df&) + { nPts++; }; + + map.visitAllPoints(lambdaVisitPoints); + + ASSERT_EQUAL_(nPts, 267UL); + } + + // test NN search: + mrpt::maps::CSimplePointsMap refPts; + map.visitAllPoints([&](const mrpt::math::TPoint3Df& pt) + { refPts.insertPoint(pt); }); + + mrpt::maps::CSimplePointsMap queryPts; + queryPts.insertObservation( + scan2D, mrpt::poses::CPose3D::FromXYZYawPitchRoll( + -0.05, -0.01, -0.01, 0, 0, 0)); + +// TODO: Update for new NN API! +#if 0 + for (size_t knn = 2; knn < 3; knn++) + { + for (size_t i = 0; i < queryPts.size(); i++) + { + mrpt::math::TPoint3D query; + queryPts.getPoint(i, query); + + std::vector results; + std::vector dists_sqr; + std::vector IDs; + map.nn_multiple_search(query, knn, results, dists_sqr, IDs); + + std::vector gt_results; + std::vector gt_dists_sqr; + std::vector gt_IDs; + refPts.nn_multiple_search( + query, knn, gt_results, gt_dists_sqr, gt_IDs); + +#if 0 + for (size_t k = 0; k < results.size(); k++) + { + std::cout << "k: " << k << "/" << knn << " query: " << query + << " nn: " << results.at(k) << " (" + << std::sqrt(dists_sqr.at(k)) << ") " + << " gt: " << gt_results.at(k) << " (" + << std::sqrt(gt_dists_sqr.at(k)) << ")" << std::endl; + } +#endif + for (size_t k = 0; k < results.size(); k++) + { + ASSERT_NEAR_(results[k].x, gt_results[k].x, 1e-3f); + ASSERT_NEAR_(results[k].y, gt_results[k].y, 1e-3f); + ASSERT_NEAR_(results[k].z, gt_results[k].z, 1e-3f); + + ASSERT_NEAR_(dists_sqr[k], gt_dists_sqr[k], 1e-3f); + } + } + } +#endif +} + +int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) +{ + try + { + test_voxelmap_basic_ops(); + test_voxelmap_insert_2d_scan(); + } + catch (std::exception& e) + { + std::cerr << e.what() << "\n"; + return 1; + } +}