diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index d9e1d805..0e74910b 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -31,22 +31,7 @@ #include #include -namespace { -// TODO(all): Maybe try to merge these voxel uitls with VoxelHashMap implementation -using Voxel = Eigen::Vector3i; -struct VoxelHash { - size_t operator()(const Voxel &voxel) const { - const uint32_t *vec = reinterpret_cast(voxel.data()); - return ((1 << 20) - 1) & (vec[0] * 73856093 ^ vec[1] * 19349669 ^ vec[2] * 83492791); - } -}; - -Voxel PointToVoxel(const Eigen::Vector3d &point, 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))); -} -} // namespace +#include "VoxelUtils.hpp" namespace kiss_icp { std::vector VoxelDownsample(const std::vector &frame, diff --git a/cpp/kiss_icp/core/Registration.cpp b/cpp/kiss_icp/core/Registration.cpp index 137fa50c..b8acea10 100644 --- a/cpp/kiss_icp/core/Registration.cpp +++ b/cpp/kiss_icp/core/Registration.cpp @@ -36,6 +36,7 @@ #include #include "VoxelHashMap.hpp" +#include "VoxelUtils.hpp" namespace Eigen { using Matrix6d = Eigen::Matrix; @@ -54,7 +55,7 @@ void TransformPoints(const Sophus::SE3d &T, std::vector &points [&](const auto &point) { return T * point; }); } -using Voxel = kiss_icp::VoxelHashMap::Voxel; +using Voxel = kiss_icp::Voxel; std::vector GetAdjacentVoxels(const Voxel &voxel, int adjacent_voxels = 1) { std::vector voxel_neighborhood; for (int i = voxel.x() - adjacent_voxels; i < voxel.x() + adjacent_voxels + 1; ++i) { @@ -70,7 +71,7 @@ std::vector GetAdjacentVoxels(const Voxel &voxel, int adjacent_voxels = 1 std::tuple GetClosestNeighbor(const Eigen::Vector3d &point, const kiss_icp::VoxelHashMap &voxel_map) { // Convert the point to voxel coordinates - const auto &voxel = voxel_map.PointToVoxel(point); + const auto &voxel = kiss_icp::PointToVoxel(point, voxel_map.voxel_size_); // Get nearby voxels on the map const auto &query_voxels = GetAdjacentVoxels(voxel); // Extract the points contained within the neighborhood voxels diff --git a/cpp/kiss_icp/core/VoxelHashMap.cpp b/cpp/kiss_icp/core/VoxelHashMap.cpp index 6b3b061b..e6445ebd 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.cpp +++ b/cpp/kiss_icp/core/VoxelHashMap.cpp @@ -27,6 +27,8 @@ #include #include +#include "VoxelUtils.hpp" + namespace kiss_icp { std::vector VoxelHashMap::GetPoints(const std::vector &query_voxels) const { @@ -74,7 +76,7 @@ void VoxelHashMap::Update(const std::vector &points, const Soph void VoxelHashMap::AddPoints(const std::vector &points) { std::for_each(points.cbegin(), points.cend(), [&](const auto &point) { - auto voxel = PointToVoxel(point); + auto voxel = PointToVoxel(point, voxel_size_); auto search = map_.find(voxel); if (search != map_.end()) { auto &voxel_block = search.value(); diff --git a/cpp/kiss_icp/core/VoxelHashMap.hpp b/cpp/kiss_icp/core/VoxelHashMap.hpp index 8ffe9ed9..a49b7096 100644 --- a/cpp/kiss_icp/core/VoxelHashMap.hpp +++ b/cpp/kiss_icp/core/VoxelHashMap.hpp @@ -32,9 +32,10 @@ #include #include +#include "VoxelUtils.hpp" + namespace kiss_icp { struct VoxelHashMap { - using Voxel = Eigen::Vector3i; struct VoxelBlock { // buffer of points with a max limit of n_points std::vector points; @@ -43,12 +44,6 @@ struct VoxelHashMap { if (points.size() < static_cast(num_points_)) points.push_back(point); } }; - struct VoxelHash { - 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); - } - }; explicit VoxelHashMap(double voxel_size, double max_distance, int max_points_per_voxel) : voxel_size_(voxel_size), @@ -57,11 +52,6 @@ struct VoxelHashMap { inline void Clear() { map_.clear(); } inline bool Empty() const { return map_.empty(); } - inline Voxel PointToVoxel(const Eigen::Vector3d &point) const { - 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_))); - } void Update(const std::vector &points, const Eigen::Vector3d &origin); void Update(const std::vector &points, const Sophus::SE3d &pose); void AddPoints(const std::vector &points); diff --git a/cpp/kiss_icp/core/VoxelUtils.hpp b/cpp/kiss_icp/core/VoxelUtils.hpp new file mode 100644 index 00000000..331a2ce1 --- /dev/null +++ b/cpp/kiss_icp/core/VoxelUtils.hpp @@ -0,0 +1,45 @@ +// MIT License +// +// Copyright (c) 2024 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill +// Stachniss. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +#pragma once + +#include +#include + +namespace kiss_icp { + +using Voxel = Eigen::Vector3i; + +inline Voxel PointToVoxel(const Eigen::Vector3d &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))); +} + +struct VoxelHash { + 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); + } +}; +} // namespace kiss_icp