From 0c0b81e7015580889aae817bdfb9118ad6cbb2ea Mon Sep 17 00:00:00 2001 From: Nicolas Lauzon Date: Thu, 2 Jan 2025 16:45:17 -0500 Subject: [PATCH 1/2] feat: voxel downsampling mapper module --- CMakeLists.txt | 17 ++--- norlab_icp_mapper/Mapper.cpp | 4 +- .../VoxelDownsamplingMapperModule.cpp | 68 +++++++++++++++++++ .../VoxelDownsamplingMapperModule.h | 56 +++++++++++++++ 4 files changed, 136 insertions(+), 9 deletions(-) create mode 100644 norlab_icp_mapper/MapperModules/VoxelDownsamplingMapperModule.cpp create mode 100644 norlab_icp_mapper/MapperModules/VoxelDownsamplingMapperModule.h diff --git a/CMakeLists.txt b/CMakeLists.txt index a9ba25b..e288e80 100644 --- a/CMakeLists.txt +++ b/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_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() \ No newline at end of file +endif() diff --git a/norlab_icp_mapper/Mapper.cpp b/norlab_icp_mapper/Mapper.cpp index 19e20eb..97a7501 100644 --- a/norlab_icp_mapper/Mapper.cpp +++ b/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 #include #include 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"]; diff --git a/norlab_icp_mapper/MapperModules/VoxelDownsamplingMapperModule.cpp b/norlab_icp_mapper/MapperModules/VoxelDownsamplingMapperModule.cpp new file mode 100644 index 0000000..2c03a5f --- /dev/null +++ b/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("voxelSize"); + pointsPerVoxel = PM::Parametrizable::get("pointsPerVoxel"); +} + +PointMatcher::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::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& voxel_points = search->second; + if (voxel_points.size() < pointsPerVoxel) { + voxel_points.emplace_back(i); + + points.setColFrom(insertedPointsCount, pointsToInsert, i); + insertedPointsCount++; + } + } else { + std::vector 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; +} diff --git a/norlab_icp_mapper/MapperModules/VoxelDownsamplingMapperModule.h b/norlab_icp_mapper/MapperModules/VoxelDownsamplingMapperModule.h new file mode 100644 index 0000000..1212cbd --- /dev/null +++ b/norlab_icp_mapper/MapperModules/VoxelDownsamplingMapperModule.h @@ -0,0 +1,56 @@ +#ifndef NORLAB_ICP_MAPPER_VOXELDOWNSAMPLINGMAPPERMODULE_H +#define NORLAB_ICP_MAPPER_VOXELDOWNSAMPLINGMAPPERMODULE_H + +#include +#include "MapperModule.h" +#include + +using Voxel = Eigen::Vector3i; +template <> +struct std::hash { + std::size_t operator()(const Voxel& voxel) const { + const uint32_t* vec = reinterpret_cast(voxel.data()); + return (vec[0] * 73856093 ^ vec[1] * 19349669 ^ vec[2] * 83492791); + } +}; + +class VoxelDownsamplingMapperModule : public MapperModule { + typedef PointMatcher 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}, + {"pointsPerVoxel", "The amount of points per voxel.", "3", "1", "9999999", P::Comp}, + }; + } + + 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(std::floor(point.x() / voxel_size)), static_cast(std::floor(point.y() / voxel_size)), + static_cast(std::floor(point.z() / voxel_size))); + } + + size_t pointsPerVoxel; + double voxelSize; + bool isHashMapBuilt; + std::unordered_map> hashMap; +}; + +#endif // NORLAB_ICP_MAPPER_VOXELDOWNSAMPLINGMAPPERMODULE_H From d4a49ddda34a30c8cbfda1300ac4c89eb843bef7 Mon Sep 17 00:00:00 2001 From: Nicolas Lauzon Date: Thu, 2 Jan 2025 17:07:12 -0500 Subject: [PATCH 2/2] Cited copied code --- norlab_icp_mapper/MapperModules/VoxelDownsamplingMapperModule.h | 1 + 1 file changed, 1 insertion(+) diff --git a/norlab_icp_mapper/MapperModules/VoxelDownsamplingMapperModule.h b/norlab_icp_mapper/MapperModules/VoxelDownsamplingMapperModule.h index 1212cbd..b4000df 100644 --- a/norlab_icp_mapper/MapperModules/VoxelDownsamplingMapperModule.h +++ b/norlab_icp_mapper/MapperModules/VoxelDownsamplingMapperModule.h @@ -5,6 +5,7 @@ #include "MapperModule.h" #include +// 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 {