diff --git a/cpp/map_closures/DensityMap.cpp b/cpp/map_closures/DensityMap.cpp index 9d6c70d..4bf7a32 100644 --- a/cpp/map_closures/DensityMap.cpp +++ b/cpp/map_closures/DensityMap.cpp @@ -55,12 +55,11 @@ DensityMap GenerateDensityMap(const std::vector &pcd, Eigen::Array2i lower_bound_coordinates = Eigen::Array2i::Constant(max_int); Eigen::Array2i upper_bound_coordinates = Eigen::Array2i::Constant(min_int); + const auto R2x3 = T_ground.block<2, 3>(0, 0); + const auto t2x1 = T_ground.block<2, 1>(0, 3); + const auto inv_resolution = 1.0 / density_map_resolution; auto Discretize2D = [&](const Eigen::Vector3d &p) -> Eigen::Array2i { - return ((T_ground.block<2, 3>(0, 0) * p + T_ground.block<2, 1>(0, 3)) / - density_map_resolution) - .array() - .floor() - .cast(); + return ((R2x3 * p + t2x1) * inv_resolution).array().floor().cast(); }; std::vector pixels(pcd.size()); std::transform(pcd.cbegin(), pcd.cend(), pixels.begin(), [&](const Eigen::Vector3d &point) { @@ -74,20 +73,23 @@ DensityMap GenerateDensityMap(const std::vector &pcd, const int n_cols = rows_and_columns.y() + 1; cv::Mat counting_grid(n_rows, n_cols, CV_64FC1, 0.0); - std::for_each(pixels.cbegin(), pixels.cend(), [&](const Eigen::Array2i &pixel) { + for (const Eigen::Array2i &pixel : pixels) { const Eigen::Array2i px = pixel - lower_bound_coordinates; double &count = counting_grid.at(px.x(), px.y()); count += 1.0; max_points = std::max(max_points, count); min_points = std::min(min_points, count); - }); + } DensityMap density_map(n_rows, n_cols, density_map_resolution, lower_bound_coordinates); - counting_grid.forEach([&](const double count, const int pos[]) { - double density = (count - min_points) / (max_points - min_points); - density = density > density_threshold ? density : 0.0; - density_map(pos[0], pos[1]) = static_cast(255 * density); - }); + for (int y = 0, x = 0; y < n_rows; ++y) { + for (x = 0; x < n_cols; ++x) { + const double count = counting_grid.at(y, x); + double density = (count - min_points) / (max_points - min_points); + density = density > density_threshold ? density : 0.0; + density_map(y, x) = static_cast(255 * density); + } + } return density_map; } diff --git a/cpp/map_closures/GroundAlign.cpp b/cpp/map_closures/GroundAlign.cpp index 2d94ccd..1a63369 100644 --- a/cpp/map_closures/GroundAlign.cpp +++ b/cpp/map_closures/GroundAlign.cpp @@ -50,8 +50,11 @@ struct PixelHash { }; void TransformPoints(const Sophus::SE3d &T, Vector3dVector &pointcloud) { - std::transform(pointcloud.cbegin(), pointcloud.cend(), pointcloud.begin(), - [&](const Eigen::Vector3d &point) { return T * point; }); + const Eigen::Matrix3d R = T.rotationMatrix(); + const Eigen::Vector3d t = T.translation(); + for (auto &point : pointcloud) { + point = R * point + t; + } } struct VoxelMeanAndNormal { @@ -69,8 +72,8 @@ std::pair SampleGroundPoints(const Vector3dVector std::unordered_map lowest_voxel_hash_map; lowest_voxel_hash_map.reserve(voxel_means.size()); for (size_t index = 0; index < voxel_means.size(); ++index) { - const Eigen::Vector3d mean = voxel_means[index]; - const Eigen::Vector3d normal = voxel_normals[index]; + const Eigen::Vector3d &mean = voxel_means[index]; + const Eigen::Vector3d &normal = voxel_normals[index]; const Eigen::Vector2i pixel = PointToPixel(mean); const auto it = lowest_voxel_hash_map.find(pixel); @@ -89,13 +92,11 @@ std::pair SampleGroundPoints(const Vector3dVector return {Vector3dVector(), Sophus::SE3d()}; } - const Eigen::Matrix3d normals_covariance_matrix = - std::transform_reduce(low_lying_voxels.cbegin(), low_lying_voxels.cend(), - Eigen::Matrix3d().setZero(), std::plus(), - [&](const VoxelMeanAndNormal &voxel) { - return voxel.normal * voxel.normal.transpose(); - }) / - static_cast(low_lying_voxels.size() - 1); + Eigen::Matrix3d normals_covariance_matrix = Eigen::Matrix3d::Zero(); + for (const VoxelMeanAndNormal &voxel : low_lying_voxels) { + normals_covariance_matrix += voxel.normal * voxel.normal.transpose(); + } + normals_covariance_matrix /= (static_cast(low_lying_voxels.size()) - 1); const Eigen::SelfAdjointEigenSolver eigensolver(normals_covariance_matrix); Eigen::Vector3d largest_eigenvector = eigensolver.eigenvectors().col(2); @@ -112,26 +113,25 @@ std::pair SampleGroundPoints(const Vector3dVector Eigen::Vector3d ground_centroid(0.0, 0.0, 0.0); Vector3dVector ground_samples; ground_samples.reserve(low_lying_voxels.size()); - std::for_each( - low_lying_voxels.cbegin(), low_lying_voxels.cend(), [&](const VoxelMeanAndNormal &voxel) { - if (std::abs(voxel.normal.dot(largest_eigenvector)) > normal_filter_threshold) { - ground_centroid += voxel.mean; - ground_samples.emplace_back(voxel.mean); - } - }); + for (const VoxelMeanAndNormal &voxel : low_lying_voxels) { + if (std::abs(voxel.normal.dot(largest_eigenvector)) > normal_filter_threshold) { + ground_centroid += voxel.mean; + ground_samples.emplace_back(voxel.mean); + } + } if (ground_samples.empty()) { return {Vector3dVector(), Sophus::SE3d()}; } - ground_centroid /= static_cast(ground_samples.size()); + ground_centroid /= static_cast(ground_samples.size()); const double z_shift = R.row(2) * ground_centroid; return {std::move(ground_samples), Sophus::SE3d(R, Eigen::Vector3d(0.0, 0.0, -1.0 * z_shift))}; } LinearSystem BuildLinearSystem(const Vector3dVector &points) { auto compute_jacobian_and_residual = - [](const Eigen::Vector3d &point) -> std::pair, double> { - return {Eigen::Matrix(1.0, point.y(), -point.x()), point.z()}; + [](const Eigen::Vector3d &point) -> std::pair { + return {Eigen::RowVector3d(1.0, point.y(), -point.x()), point.z()}; }; auto sum_linear_systems = [](LinearSystem a, const LinearSystem &b) -> LinearSystem { @@ -144,7 +144,7 @@ LinearSystem BuildLinearSystem(const Vector3dVector &points) { std::transform_reduce(points.cbegin(), points.cend(), LinearSystem(Eigen::Matrix3d::Zero(), Eigen::Vector3d::Zero()), sum_linear_systems, [&](const Eigen::Vector3d &point) { - const auto [J, residual] = compute_jacobian_and_residual(point); + const auto &[J, residual] = compute_jacobian_and_residual(point); const double w = std::exp(-1.0 * residual * residual); return LinearSystem(J.transpose() * w * J, // JTJ J.transpose() * w * residual); // JTr @@ -157,10 +157,11 @@ namespace map_closures { Eigen::Matrix4d AlignToLocalGround(const Vector3dVector &pointcloud, const double resolution) { VoxelMap voxel_map(resolution, 100.0); voxel_map.AddPoints(pointcloud); - const auto [voxel_means, voxel_normals] = voxel_map.PerVoxelMeanAndNormal(); + const auto &[voxel_means, voxel_normals] = voxel_map.PerVoxelMeanAndNormal(); auto [ground_samples, T] = SampleGroundPoints(voxel_means, voxel_normals); if (ground_samples.empty()) return Eigen::Matrix4d::Identity(); + TransformPoints(T, ground_samples); for (int iters = 0; iters < max_iterations; iters++) { const auto [H, b] = BuildLinearSystem(ground_samples); @@ -179,6 +180,7 @@ Eigen::Matrix4d AlignToLocalGround(const Vector3dVector &voxel_means, const Vector3dVector &voxel_normals) { auto [ground_samples, T] = SampleGroundPoints(voxel_means, voxel_normals); if (ground_samples.empty()) return Eigen::Matrix4d::Identity(); + TransformPoints(T, ground_samples); for (int iters = 0; iters < max_iterations; iters++) { const auto [H, b] = BuildLinearSystem(ground_samples); diff --git a/cpp/map_closures/MapClosures.cpp b/cpp/map_closures/MapClosures.cpp index 71a77e6..0be963a 100644 --- a/cpp/map_closures/MapClosures.cpp +++ b/cpp/map_closures/MapClosures.cpp @@ -23,7 +23,6 @@ #include "MapClosures.hpp" #include -#include #include #include #include @@ -72,28 +71,27 @@ void MapClosures::MatchAndAddToDatabase(const int id, DensityMap density_map = GenerateDensityMap(local_map, T_ground, config_.density_map_resolution, config_.density_threshold); cv::Mat orb_descriptors; - std::vector orb_keypoints; - orb_keypoints.reserve(nfeatures); - orb_extractor_->detectAndCompute(density_map.grid, cv::noArray(), orb_keypoints, + orb_keypoints_.clear(); + orb_keypoints_.reserve(nfeatures); + orb_extractor_->detectAndCompute(density_map.grid, cv::noArray(), orb_keypoints_, orb_descriptors); - std::vector> self_matches; - self_matches.reserve(orb_keypoints.size()); - self_matcher_.knnMatch(orb_descriptors, orb_descriptors, self_matches, 2); + self_matches_.clear(); + self_matches_.reserve(orb_keypoints_.size()); + self_matcher_.knnMatch(orb_descriptors, orb_descriptors, self_matches_, 2); std::vector hbst_matchable; hbst_matchable.reserve(orb_descriptors.rows); - std::for_each( - self_matches.cbegin(), self_matches.cend(), [&](const std::vector &self_match) { - if (self_match[1].distance > self_similarity_threshold) { - const int index_descriptor = self_match[0].queryIdx; - cv::KeyPoint keypoint = orb_keypoints[index_descriptor]; - keypoint.pt.x = keypoint.pt.x + static_cast(density_map.lower_bound.y()); - keypoint.pt.y = keypoint.pt.y + static_cast(density_map.lower_bound.x()); - hbst_matchable.emplace_back( - new Matchable(keypoint, orb_descriptors.row(index_descriptor), id)); - } - }); + for (const auto &self_match : self_matches_) { + if (self_match[1].distance > self_similarity_threshold) { + const int index_descriptor = self_match[0].queryIdx; + cv::KeyPoint keypoint = orb_keypoints_[index_descriptor]; + keypoint.pt.x = keypoint.pt.x + static_cast(density_map.lower_bound.y()); + keypoint.pt.y = keypoint.pt.y + static_cast(density_map.lower_bound.x()); + hbst_matchable.emplace_back( + new Matchable(keypoint, orb_descriptors.row(index_descriptor), id)); + } + } hbst_binary_tree_->matchAndAdd(hbst_matchable, descriptor_matches_, config_.hamming_distance_threshold, @@ -110,29 +108,29 @@ void MapClosures::MatchAndAddToDatabase(const int id, Eigen::Matrix4d T_ground = AlignToLocalGround(voxel_means, voxel_normals); DensityMap density_map = GenerateDensityMap(local_map, T_ground, config_.density_map_resolution, config_.density_threshold); + + orb_keypoints_.clear(); + orb_keypoints_.reserve(nfeatures); cv::Mat orb_descriptors; - std::vector orb_keypoints; - orb_keypoints.reserve(nfeatures); - orb_extractor_->detectAndCompute(density_map.grid, cv::noArray(), orb_keypoints, + orb_extractor_->detectAndCompute(density_map.grid, cv::noArray(), orb_keypoints_, orb_descriptors); - std::vector> self_matches; - self_matches.reserve(orb_keypoints.size()); - self_matcher_.knnMatch(orb_descriptors, orb_descriptors, self_matches, 2); + self_matches_.clear(); + self_matches_.reserve(orb_keypoints_.size()); + self_matcher_.knnMatch(orb_descriptors, orb_descriptors, self_matches_, 2); std::vector hbst_matchable; hbst_matchable.reserve(orb_descriptors.rows); - std::for_each( - self_matches.cbegin(), self_matches.cend(), [&](const std::vector &self_match) { - if (self_match[1].distance > self_similarity_threshold) { - const int index_descriptor = self_match[0].queryIdx; - cv::KeyPoint keypoint = orb_keypoints[index_descriptor]; - keypoint.pt.x = keypoint.pt.x + static_cast(density_map.lower_bound.y()); - keypoint.pt.y = keypoint.pt.y + static_cast(density_map.lower_bound.x()); - hbst_matchable.emplace_back( - new Matchable(keypoint, orb_descriptors.row(index_descriptor), id)); - } - }); + for (const auto &self_match : self_matches_) { + if (self_match[1].distance > self_similarity_threshold) { + const int index_descriptor = self_match[0].queryIdx; + cv::KeyPoint keypoint = orb_keypoints_[index_descriptor]; + keypoint.pt.x = keypoint.pt.x + static_cast(density_map.lower_bound.y()); + keypoint.pt.y = keypoint.pt.y + static_cast(density_map.lower_bound.x()); + hbst_matchable.emplace_back( + new Matchable(keypoint, orb_descriptors.row(index_descriptor), id)); + } + } hbst_binary_tree_->matchAndAdd(hbst_matchable, descriptor_matches_, config_.hamming_distance_threshold, @@ -147,28 +145,27 @@ void MapClosures::Match(const std::vector &local_map) { DensityMap density_map = GenerateDensityMap(local_map, T_ground, config_.density_map_resolution, config_.density_threshold); cv::Mat orb_descriptors; - std::vector orb_keypoints; - orb_keypoints.reserve(nfeatures); - orb_extractor_->detectAndCompute(density_map.grid, cv::noArray(), orb_keypoints, + orb_keypoints_.clear(); + orb_keypoints_.reserve(nfeatures); + orb_extractor_->detectAndCompute(density_map.grid, cv::noArray(), orb_keypoints_, orb_descriptors); - std::vector> self_matches; - self_matches.reserve(orb_keypoints.size()); - self_matcher_.knnMatch(orb_descriptors, orb_descriptors, self_matches, 2); + self_matches_.clear(); + self_matches_.reserve(orb_keypoints_.size()); + self_matcher_.knnMatch(orb_descriptors, orb_descriptors, self_matches_, 2); std::vector hbst_matchable; hbst_matchable.reserve(orb_descriptors.rows); - std::for_each( - self_matches.cbegin(), self_matches.cend(), [&](const std::vector &self_match) { - if (self_match[1].distance > self_similarity_threshold) { - const int index_descriptor = self_match[0].queryIdx; - cv::KeyPoint keypoint = orb_keypoints[index_descriptor]; - keypoint.pt.x = keypoint.pt.x + static_cast(density_map.lower_bound.y()); - keypoint.pt.y = keypoint.pt.y + static_cast(density_map.lower_bound.x()); - hbst_matchable.emplace_back( - new Matchable(keypoint, orb_descriptors.row(index_descriptor))); - } - }); + for (const auto &self_match : self_matches_) { + if (self_match[1].distance > self_similarity_threshold) { + const int index_descriptor = self_match[0].queryIdx; + cv::KeyPoint keypoint = orb_keypoints_[index_descriptor]; + keypoint.pt.x = keypoint.pt.x + static_cast(density_map.lower_bound.y()); + keypoint.pt.y = keypoint.pt.y + static_cast(density_map.lower_bound.x()); + hbst_matchable.emplace_back( + new Matchable(keypoint, orb_descriptors.row(index_descriptor))); + } + } hbst_binary_tree_->match(hbst_matchable, descriptor_matches_, config_.hamming_distance_threshold); } @@ -180,28 +177,27 @@ void MapClosures::Match(const std::vector &local_map, DensityMap density_map = GenerateDensityMap(local_map, T_ground, config_.density_map_resolution, config_.density_threshold); cv::Mat orb_descriptors; - std::vector orb_keypoints; - orb_keypoints.reserve(nfeatures); - orb_extractor_->detectAndCompute(density_map.grid, cv::noArray(), orb_keypoints, + orb_keypoints_.clear(); + orb_keypoints_.reserve(nfeatures); + orb_extractor_->detectAndCompute(density_map.grid, cv::noArray(), orb_keypoints_, orb_descriptors); - std::vector> self_matches; - self_matches.reserve(orb_keypoints.size()); - self_matcher_.knnMatch(orb_descriptors, orb_descriptors, self_matches, 2); + self_matches_.clear(); + self_matches_.reserve(orb_keypoints_.size()); + self_matcher_.knnMatch(orb_descriptors, orb_descriptors, self_matches_, 2); std::vector hbst_matchable; hbst_matchable.reserve(orb_descriptors.rows); - std::for_each( - self_matches.cbegin(), self_matches.cend(), [&](const std::vector &self_match) { - if (self_match[1].distance > self_similarity_threshold) { - const int index_descriptor = self_match[0].queryIdx; - cv::KeyPoint keypoint = orb_keypoints[index_descriptor]; - keypoint.pt.x = keypoint.pt.x + static_cast(density_map.lower_bound.y()); - keypoint.pt.y = keypoint.pt.y + static_cast(density_map.lower_bound.x()); - hbst_matchable.emplace_back( - new Matchable(keypoint, orb_descriptors.row(index_descriptor))); - } - }); + for (const auto &self_match : self_matches_) { + if (self_match[1].distance > self_similarity_threshold) { + const int index_descriptor = self_match[0].queryIdx; + cv::KeyPoint keypoint = orb_keypoints_[index_descriptor]; + keypoint.pt.x = keypoint.pt.x + static_cast(density_map.lower_bound.y()); + keypoint.pt.y = keypoint.pt.y + static_cast(density_map.lower_bound.x()); + hbst_matchable.emplace_back( + new Matchable(keypoint, orb_descriptors.row(index_descriptor))); + } + } hbst_binary_tree_->match(hbst_matchable, descriptor_matches_, config_.hamming_distance_threshold); } diff --git a/cpp/map_closures/MapClosures.hpp b/cpp/map_closures/MapClosures.hpp index e1bfd33..dc1cb78 100644 --- a/cpp/map_closures/MapClosures.hpp +++ b/cpp/map_closures/MapClosures.hpp @@ -61,7 +61,7 @@ class MapClosures { ClosureCandidate GetBestClosure(const int query_id, const std::vector &local_map) { - std::vector closures = GetTopKClosures(query_id, local_map, 1); + const std::vector closures = GetTopKClosures(query_id, local_map, 1); if (closures.empty()) { return ClosureCandidate(); } @@ -71,7 +71,7 @@ class MapClosures { const std::vector &local_map, const std::vector &voxel_means, const std::vector &voxel_normals) { - std::vector closures = + const std::vector closures = GetTopKClosures(query_id, local_map, voxel_means, voxel_normals, 1); if (closures.empty()) { return ClosureCandidate(); @@ -129,6 +129,9 @@ class MapClosures { std::unordered_map ground_alignments_; std::unique_ptr hbst_binary_tree_ = std::make_unique(); cv::Ptr orb_extractor_; + + std::vector orb_keypoints_; + std::vector> self_matches_; cv::BFMatcher self_matcher_ = cv::BFMatcher(cv::NORM_HAMMING); }; } // namespace map_closures diff --git a/cpp/map_closures/VoxelMap.cpp b/cpp/map_closures/VoxelMap.cpp index d63f7a9..9ac2e74 100644 --- a/cpp/map_closures/VoxelMap.cpp +++ b/cpp/map_closures/VoxelMap.cpp @@ -37,6 +37,13 @@ inline Eigen::Vector3i ToVoxelCoordinates(const Eigen::Vector3d &point, const do static_cast(std::floor(point.z() / voxel_size))); } +inline Eigen::Vector3d VoxelCenter(const Eigen::Vector3i &voxel, const double voxel_size) { + return Eigen::Vector3d(static_cast(voxel.x()) * voxel_size, + static_cast(voxel.y()) * voxel_size, + static_cast(voxel.z()) * voxel_size) + + Eigen::Vector3d::Constant(voxel_size * 0.5); +} + static constexpr unsigned int min_points_for_covariance_computation = 10; std::tuple ComputeMeanAndNormal( @@ -72,8 +79,8 @@ void VoxelBlock::emplace_back(const Eigen::Vector3d &p) { VoxelMap::VoxelMap(const double voxel_size, const double max_distance) : voxel_size_(voxel_size), - map_resolution_(voxel_size / - (std::sqrt(static_cast(max_points_per_normal_computation)))), + map_resolution2_(voxel_size * voxel_size / + static_cast(max_points_per_normal_computation)), max_distance_(max_distance) {} void VoxelMap::IntegrateFrame(const Vector3dVector &points, const Eigen::Matrix4d &pose) { @@ -86,7 +93,7 @@ void VoxelMap::IntegrateFrame(const Vector3dVector &points, const Eigen::Matrix4 } void VoxelMap::AddPoints(const Vector3dVector &points) { - std::for_each(points.cbegin(), points.cend(), [&](const Eigen::Vector3d &point) { + for (const auto &point : points) { const Voxel voxel = ToVoxelCoordinates(point, voxel_size_); const auto [it, inserted] = map_.try_emplace(voxel, VoxelBlock()); if (!inserted) { @@ -94,23 +101,23 @@ void VoxelMap::AddPoints(const Vector3dVector &points) { if (voxel_block.size() == max_points_per_normal_computation || std::any_of(voxel_block.cbegin(), voxel_block.cend(), [&](const Eigen::Vector3d &voxel_point) { - return (voxel_point - point).norm() < map_resolution_; + return (voxel_point - point).squaredNorm() < map_resolution2_; })) { - return; + continue; } } it->second.emplace_back(point); - }); + } } Vector3dVector VoxelMap::Pointcloud() const { Vector3dVector points; points.reserve(map_.size() * max_points_per_normal_computation); - std::for_each(map_.cbegin(), map_.cend(), [&](const auto &map_element) { - const VoxelBlock &voxel_block = map_element.second; - std::for_each(voxel_block.cbegin(), voxel_block.cend(), - [&](const Eigen::Vector3d &p) { points.emplace_back(p); }); - }); + for (const auto &[voxel, voxel_block] : map_) { + for (auto it = voxel_block.cbegin(); it != voxel_block.cend(); ++it) { + points.emplace_back(*it); + } + } return points; } @@ -119,26 +126,22 @@ std::tuple VoxelMap::PerVoxelMeanAndNormal() con voxel_means.reserve(map_.size()); Vector3dVector voxel_normals; voxel_normals.reserve(map_.size()); - std::for_each(map_.cbegin(), map_.cend(), [&](const auto &map_element) { - const VoxelBlock &voxel_block = map_element.second; + for (const auto &[_, voxel_block] : map_) { if (voxel_block.size() >= min_points_for_covariance_computation) { - auto [mean, normal] = ComputeMeanAndNormal(voxel_block); + const auto &[mean, normal] = ComputeMeanAndNormal(voxel_block); voxel_means.emplace_back(mean); voxel_normals.emplace_back(normal); } - }); + } return {std::move(voxel_means), std::move(voxel_normals)}; } void VoxelMap::RemovePointsFarFromLocation(const Eigen::Vector3d &origin) { - const double max_distance2 = max_distance_ * max_distance_; + const auto max_distance2 = max_distance_ * max_distance_; for (auto it = map_.begin(); it != map_.end();) { - const auto &[voxel, voxel_points] = *it; - if ((voxel_points.front() - origin).squaredNorm() >= (max_distance2)) { - it = map_.erase(it); - } else { - ++it; - } + it = (VoxelCenter(it->first, voxel_size_) - origin).squaredNorm() >= max_distance2 + ? map_.erase(it) + : std::next(it); } } } // namespace map_closures diff --git a/cpp/map_closures/VoxelMap.hpp b/cpp/map_closures/VoxelMap.hpp index 229bda4..a5de7ed 100644 --- a/cpp/map_closures/VoxelMap.hpp +++ b/cpp/map_closures/VoxelMap.hpp @@ -28,14 +28,6 @@ #include #include -template <> -struct std::hash { - std::size_t operator()(const Eigen::Vector3i &voxel) const { - const uint32_t *vec = reinterpret_cast(voxel.data()); - return (vec[0] * 73856093 ^ vec[1] * 19349669 ^ vec[2] * 83492791); - } -}; - // Same default as Open3d constexpr unsigned int max_points_per_normal_computation = 20; @@ -43,6 +35,13 @@ namespace map_closures { using Voxel = Eigen::Vector3i; using Vector3dVector = std::vector; +struct VoxelHash { + std::size_t operator()(const Eigen::Vector3i &voxel) const { + const uint32_t *vec = reinterpret_cast(voxel.data()); + return (vec[0] * 73856093 ^ vec[1] * 19349669 ^ vec[2] * 83492791); + } +}; + struct VoxelBlock { void emplace_back(const Eigen::Vector3d &point); size_t size() const { return num_points; } @@ -68,8 +67,8 @@ struct VoxelMap { void RemovePointsFarFromLocation(const Eigen::Vector3d &origin); double voxel_size_; - double map_resolution_; + double map_resolution2_; double max_distance_; - std::unordered_map map_; + std::unordered_map map_; }; } // namespace map_closures