feat: voxel downsampling mapper module

CMakeLists.txt

@@ -6,7 +6,7 @@ include(CMakePackageConfigHelpers)
 
 project(norlab_icp_mapper)
 
-## Extract version from package.xml
+# # Extract version from package.xml
 file(READ "package.xml" PACKAGE_XML_CONTENT)
 string(REGEX MATCH "<version>([^<]+)</version>" VERSION_MATCH ${PACKAGE_XML_CONTENT})
 # Extract the matched version from the captured group
@@ -37,13 +37,12 @@ set(EXTERNAL_LIBS ${EXTERNAL_LIBS} ${libpointmatcher_LIBRARIES})
 message(STATUS "Looking for yaml-cpp on system...")
 find_package(yaml-cpp CONFIG REQUIRED)
 if(TARGET yaml-cpp::yaml-cpp)
-	set(YAML_CPP_LIBRARIES "yaml-cpp::yaml-cpp")
+    set(YAML_CPP_LIBRARIES "yaml-cpp::yaml-cpp")
 endif()
 if(YAML_CPP_LIBRARIES STREQUAL "")
-	set(YAML_CPP_LIBRARIES "yaml-cpp")
+    set(YAML_CPP_LIBRARIES "yaml-cpp")
 endif ()
 
-
 # norlab_icp_mapper target
 add_library(norlab_icp_mapper
         norlab_icp_mapper/Mapper.cpp
@@ -56,6 +55,7 @@ add_library(norlab_icp_mapper
         norlab_icp_mapper/MapperModules/PointDistanceMapperModule.cpp
         norlab_icp_mapper/MapperModules/DynamicPointsMapperModule.cpp
         norlab_icp_mapper/MapperModules/OctreeMapperModule.cpp
+        norlab_icp_mapper/MapperModules/VoxelDownsamplingMapperModule.cpp
         norlab_icp_mapper/MapperModules/MapperModule.cpp
         )
 target_include_directories(norlab_icp_mapper PUBLIC
@@ -95,6 +95,7 @@ install(FILES
         norlab_icp_mapper/MapperModules/DynamicPointsMapperModule.h
         norlab_icp_mapper/MapperModules/OctreeMapperModule.h
         norlab_icp_mapper/MapperModules/PointDistanceMapperModule.h
+        norlab_icp_mapper/MapperModules/VoxelDownsamplingMapperModule.h
         DESTINATION ${INSTALL_INCLUDE_DIR}/norlab_icp_mapper/MapperModules/)
 
 configure_file(norlab_icp_mapperConfig.cmake.in
@@ -103,11 +104,11 @@ configure_file(norlab_icp_mapperConfig.cmake.in
 
 # The same versioning file can be used for both cases
 write_basic_package_version_file(norlab_icp_mapperConfigVersion.cmake
-		COMPATIBILITY SameMajorVersion)
+  COMPATIBILITY SameMajorVersion)
 
 install(FILES
         "${PROJECT_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/norlab_icp_mapperConfig.cmake"
-	    "${PROJECT_BINARY_DIR}/norlab_icp_mapperConfigVersion.cmake"
+     "${PROJECT_BINARY_DIR}/norlab_icp_mapperConfigVersion.cmake"
         DESTINATION ${DEF_INSTALL_CMAKE_DIR}
         COMPONENT dev
         )
@@ -125,7 +126,7 @@ endif ()
 
 set(BUILD_EXAMPLE false CACHE BOOL "Set to true to build an example program")
 if(BUILD_EXAMPLE)
-	message(STATUS "Building example")
+    message(STATUS "Building example")
     add_executable(build_map_from_scans_and_trajectories examples/build_map_from_scans_and_trajectory.cpp)
     target_link_libraries(build_map_from_scans_and_trajectories norlab_icp_mapper)
-endif()
+endif()

norlab_icp_mapper/Mapper.cpp

@@ -2,11 +2,13 @@
 #include "MapperModules/PointDistanceMapperModule.h"
 #include "MapperModules/OctreeMapperModule.h"
 #include "MapperModules/DynamicPointsMapperModule.h"
+#include "MapperModules/VoxelDownsamplingMapperModule.h"
 #include <fstream>
 #include <chrono>
 #include <yaml-cpp/node/iterator.h>
 
 void norlab_icp_mapper::Mapper::fillRegistrar() {
+    ADD_TO_REGISTRAR(MapperModule, VoxelDownsamplingMapperModule, VoxelDownsamplingMapperModule);
     ADD_TO_REGISTRAR(MapperModule, PointDistanceMapperModule, PointDistanceMapperModule);
     ADD_TO_REGISTRAR(MapperModule, OctreeMapperModule, OctreeMapperModule);
     ADD_TO_REGISTRAR(MapperModule, DynamicPointsMapperModule, DynamicPointsMapperModule);
@@ -100,7 +102,7 @@ void norlab_icp_mapper::Mapper::loadYamlConfig(const std::string& configFilePath
     if (node["mapper"])
     {
         YAML::Node mapperNode = node["mapper"];
-        
+
         if(mapperNode["updateCondition"])
         {
             YAML::Node updateConditionNode = mapperNode["updateCondition"];

norlab_icp_mapper/MapperModules/VoxelDownsamplingMapperModule.cpp

@@ -0,0 +1,68 @@
+#include "VoxelDownsamplingMapperModule.h"
+
+VoxelDownsamplingMapperModule::VoxelDownsamplingMapperModule(const PM::Parameters& params)
+    : MapperModule("VoxelDownsamplingMapperModule", VoxelDownsamplingMapperModule::availableParameters(), params), isHashMapBuilt(false) {
+    voxelSize = PM::Parametrizable::get<double>("voxelSize");
+    pointsPerVoxel = PM::Parametrizable::get<std::size_t>("pointsPerVoxel");
+}
+
+PointMatcher<float>::DataPoints VoxelDownsamplingMapperModule::createMap(const PM::DataPoints& input, const PM::TransformationParameters& pose) {
+    DataPoints outputMap(input);
+    inPlaceCreateMap(outputMap, pose);
+    return outputMap;
+}
+
+void VoxelDownsamplingMapperModule::inPlaceCreateMap(PM::DataPoints& input, const PM::TransformationParameters& pose) {
+    DataPoints emptyMap = input.createSimilarEmpty();
+    inPlaceUpdateMap(emptyMap, input, pose);
+}
+
+PointMatcher<float>::DataPoints VoxelDownsamplingMapperModule::updateMap(const PM::DataPoints& input, const PM::DataPoints& map,
+                                                                         const PM::TransformationParameters& pose) {
+    DataPoints outputMap(map);
+    inPlaceUpdateMap(input, outputMap, pose);
+    return outputMap;
+}
+
+void VoxelDownsamplingMapperModule::inPlaceUpdateMap(const PM::DataPoints& input, PM::DataPoints& map, const PM::TransformationParameters& pose) {
+    if (!isHashMapBuilt) {
+        map = insertPointsInHashMap(map);
+        isHashMapBuilt = true;
+    } else {
+        DataPoints insertedPoints = insertPointsInHashMap(input);
+        map.concatenate(insertedPoints);
+    }
+}
+
+const VoxelDownsamplingMapperModule::DataPoints VoxelDownsamplingMapperModule::insertPointsInHashMap(const PM::DataPoints& pointsToInsert) {
+    int insertedPointsCount = 0;
+
+    DataPoints points = pointsToInsert.createSimilarEmpty();
+
+    for (int i = 0; i < pointsToInsert.getNbPoints(); ++i) {
+        Voxel voxel = PointToVoxel(pointsToInsert.features.col(i), voxelSize);
+        auto search = hashMap.find(voxel);
+
+        if (search != hashMap.end()) {
+            std::vector<Index>& voxel_points = search->second;
+            if (voxel_points.size() < pointsPerVoxel) {
+                voxel_points.emplace_back(i);
+
+                points.setColFrom(insertedPointsCount, pointsToInsert, i);
+                insertedPointsCount++;
+            }
+        } else {
+            std::vector<Index> voxel_points;
+            voxel_points.reserve(pointsPerVoxel);
+            voxel_points.emplace_back(i);
+            hashMap.insert({voxel, std::move(voxel_points)});
+
+            points.setColFrom(insertedPointsCount, pointsToInsert, i);
+            insertedPointsCount++;
+        }
+    }
+
+    points.conservativeResize(insertedPointsCount);
+
+    return points;
+}

norlab_icp_mapper/MapperModules/VoxelDownsamplingMapperModule.h

@@ -0,0 +1,57 @@
+#ifndef NORLAB_ICP_MAPPER_VOXELDOWNSAMPLINGMAPPERMODULE_H
+#define NORLAB_ICP_MAPPER_VOXELDOWNSAMPLINGMAPPERMODULE_H
+
+#include <cstddef>
+#include "MapperModule.h"
+#include <unordered_map>
+
+// got this hashing of eigen vector from kiss-icp : https://github.com/PRBonn/kiss-icp/blob/main/cpp/kiss_icp/core/VoxelUtils.hpp
+using Voxel = Eigen::Vector3i;
+template <>
+struct std::hash<Voxel> {
+    std::size_t operator()(const Voxel& voxel) const {
+        const uint32_t* vec = reinterpret_cast<const uint32_t*>(voxel.data());
+        return (vec[0] * 73856093 ^ vec[1] * 19349669 ^ vec[2] * 83492791);
+    }
+};
+
+class VoxelDownsamplingMapperModule : public MapperModule {
+    typedef PointMatcher<float> PM;
+    typedef typename PM::DataPoints DataPoints;
+    typedef PM::Parametrizable P;
+    typedef DataPoints::Index Index;
+
+   public:
+    inline static const std::string description() { return "Downsample the map using a voxel grid."; }
+
+    inline static const PM::ParametersDoc availableParameters() {
+        return {
+            {"voxelSize", "The size of the voxel.", "1.0", "0.0", "inf", P::Comp<float>},
+            {"pointsPerVoxel", "The amount of points per voxel.", "3", "1", "9999999", P::Comp<size_t>},
+        };
+    }
+
+    explicit VoxelDownsamplingMapperModule(const PM::Parameters& params = PM::Parameters());
+    ~VoxelDownsamplingMapperModule() override = default;
+
+    PM::DataPoints createMap(const PM::DataPoints& input, const PM::TransformationParameters& pose) override;
+    void inPlaceCreateMap(PM::DataPoints& input, const PM::TransformationParameters& pose) override;
+
+    DataPoints updateMap(const PM::DataPoints& input, const PM::DataPoints& map, const PM::TransformationParameters& pose) override;
+    void inPlaceUpdateMap(const PM::DataPoints& input, PM::DataPoints& map, const PM::TransformationParameters& pose) override;
+
+   private:
+    const DataPoints insertPointsInHashMap(const PM::DataPoints& pointsToInsert);
+
+    inline Voxel PointToVoxel(const Eigen::Vector3f& point, const double voxel_size) {
+        return Voxel(static_cast<int>(std::floor(point.x() / voxel_size)), static_cast<int>(std::floor(point.y() / voxel_size)),
+                     static_cast<int>(std::floor(point.z() / voxel_size)));
+    }
+
+    size_t pointsPerVoxel;
+    double voxelSize;
+    bool isHashMapBuilt;
+    std::unordered_map<Voxel, std::vector<Index>> hashMap;
+};
+
+#endif  // NORLAB_ICP_MAPPER_VOXELDOWNSAMPLINGMAPPERMODULE_H