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;
+ }
+}