diff --git a/beluga/include/beluga/sensor.hpp b/beluga/include/beluga/sensor.hpp index 7458dd582..109322712 100644 --- a/beluga/include/beluga/sensor.hpp +++ b/beluga/include/beluga/sensor.hpp @@ -63,5 +63,6 @@ #include #include #include +#include #endif diff --git a/beluga/include/beluga/sensor/ndt_sensor_model.hpp b/beluga/include/beluga/sensor/ndt_sensor_model.hpp new file mode 100644 index 000000000..6a9ea4d2a --- /dev/null +++ b/beluga/include/beluga/sensor/ndt_sensor_model.hpp @@ -0,0 +1,238 @@ +// Copyright 2024 Ekumen, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "H5Cpp.h" + +namespace beluga { + +namespace detail { +/// Hash function for N dimensional Eigen::Vectors of int. +/// The code is from `hash_combine` function of the Boost library. See +/// http://www.boost.org/doc/libs/1_55_0/doc/html/hash/reference.html#boost.hash_combine . +template +struct CellHasher { + /// Hashes an integer N dimensional integer vector. + size_t operator()(const Eigen::Vector vector) const { + size_t seed = 0; + for (auto i = 0L; i < vector.size(); ++i) { + auto elem = *(vector.data() + i); + seed ^= std::hash()(elem) + 0x9e3779b9 + (seed << 6) + (seed >> 2); + } + return seed; + } +}; + +/// Fit a vector of points to an NDT cell, by computing its mean and covariance. +template +inline NDTCell fit_points(const std::vector>& points) { + static constexpr double kMinVariance = 1e-5; + Eigen::Map> points_view( + reinterpret_cast(points.data()), 2, static_cast(points.size())); + const Eigen::Vector mean = points_view.rowwise().mean(); + const Eigen::Matrix centered = points_view.colwise() - mean; + // Use sample covariance. + Eigen::Matrix cov = + (centered * centered.transpose()) / static_cast(points_view.cols() - 1); + cov(0, 0) = std::max(cov(0, 0), kMinVariance); + cov(1, 1) = std::max(cov(1, 1), kMinVariance); + if constexpr (NDim == 3) { + cov(2, 2) = std::max(cov(2, 2), kMinVariance); + } + return NDTCell{mean, cov}; +} + +/// Given a number of N dimensional points and a resolution, constructs a vector of NDT cells, by clusterizing the +/// points using 'resolution' and fitting a normal distribution to each of the resulting clusters if they contain a +/// minimum number of points in them. +template +inline std::vector> to_cells( + const std::vector>& points, + const double resolution) { + static constexpr int kMinPointsPerCell = 5; + + Eigen::Map> points_view( + reinterpret_cast(points.data()), NDim, static_cast(points.size())); + + std::vector> ret; + ret.reserve(static_cast(points_view.cols()) / kMinPointsPerCell); + + std::unordered_map, std::vector>, CellHasher> cell_grid; + for (const Eigen::Vector2d& col : points) { + cell_grid[(col / resolution).cast()].emplace_back(col); + } + + for (const auto& [cell, points_in_cell] : cell_grid) { + if (points_in_cell.size() < kMinPointsPerCell) { + continue; + } + ret.push_back(fit_points(points_in_cell)); + } + + return ret; +} + +} // namespace detail + +/// Parameters used to construct a NDTSensorModel instance. +struct NDTModelParam { + /// Likelihood for measurements that lie inside cells that are not present in the map. + double minimum_likelihood = 0; + /// Scaling parameter d1 in literature, used for scaling 2D likelihoods. + double d1 = 1.0; + /// Scaling parameter d2 in literature, used for scaling 2D likelihoods. + double d2 = 1.0; +}; + +/// NDT sensor model for range finders. +/** + * This class satisfies \ref SensorModelPage. + * + * \tparam SparseGridT Type representing a sparse NDT grid, as a specialization of 'beluga::SparseValueGrid'. + */ +template +class NDTSensorModel { + public: + static_assert(std::is_same_v, SparseGridT>); + /// NDT Cell type. + using ndt_cell_type = typename SparseGridT::mapped_type; + static_assert( + ndt_cell_type::num_dim == 2 || ndt_cell_type::num_dim == 3, + "NDT sensor model is only implemented for 2D or 3D problems."); + /// State type of a particle. + using state_type = std::conditional_t< + ndt_cell_type::num_dim == 2, + Sophus::SE2, + Sophus::SE3>; + /// Weight type of the particle. + using weight_type = double; + /// Measurement type of the sensor: a point cloud for the range finder. + using measurement_type = std::vector>; + /// Map representation type. + using map_type = SparseGridT; + /// Parameter type that the constructor uses to configure the NDT sensor model. + using param_type = NDTModelParam; + + /// Constructs a NDTSensorModel instance. + /** + * \param params Parameters to configure this instance. + * See beluga::NDTModelParam for details. + * \param cells_data Sparse grid representing an NDT map, used for calculating importance weights for the different + * particles. + */ + NDTSensorModel(param_type params, SparseGridT cells_data) + : params_{std::move(params)}, cells_data_{std::move(cells_data)} {} + + /// Returns a state weighting function conditioned on 2D / 3D lidar hits. + /** + * \param points 2D lidar hit points in the reference frame of particle states. + * \return a state weighting function satisfying \ref StateWeightingFunctionPage + * and borrowing a reference to this sensor model (and thus their lifetime are bound). + */ + [[nodiscard]] auto operator()(measurement_type&& points) const { + return [this, cells = detail::to_cells( + points, cells_data_.resolution())](const state_type& state) -> weight_type { + return std::transform_reduce( + cells.cbegin(), cells.cend(), 1.0, std::plus{}, + [this, state](const ndt_cell_type& ndt_cell) { return likelihood_at(state * ndt_cell); }); + }; + } + + /// Returns the L2 likelihood scaled by 'd1' and 'd2' set in the parameters for this instance for 'measurement, or + /// 'params_.min_likelihood' if the cell corresponding to 'measurement' doesn't exist in the map. + [[nodiscard]] double likelihood_at(const ndt_cell_type& measurement) const { + const auto maybe_cell = cells_data_.data_near(measurement.mean); + if (!maybe_cell.has_value()) { + return params_.minimum_likelihood; + } + return maybe_cell->likelihood_at(measurement, params_.d1, params_.d2); + } + + private: + const param_type params_; + const SparseGridT cells_data_; +}; + +namespace io { + +/// Loads a 2D NDT map representation from a hdf5 file, with the following datasets: +/// - "resolution": () resolution for the discrete grid (cells are resolution x +/// resolution m^2 squares). +/// - "cells": (NUM_CELLS, 2) that contains the cell coordinates. +/// - "means": (NUM_CELLS, 2) that contains the 2d mean of the normal distribution +/// of the cell. +/// - "covariances": (NUM_CELLS, 2, 2) Contains the covariance for each cell. +/// +/// \tparam NDTMapRepresentation A specialized SparseValueGrid (see sensor/data/sparse_value_grid.hpp), where +/// mapped_type == NDTCell2d, that will represent the NDT map as a mapping from 2D cells to NDTCells. +template +NDTMapRepresentationT load_from_hdf5_2d(const std::filesystem::path& path_to_hdf5_file) { + static_assert(std::is_same_v); + static_assert(std::is_same_v); + if (!std::filesystem::exists(path_to_hdf5_file) || std::filesystem::is_directory(path_to_hdf5_file)) { + std::stringstream ss; + ss << "Couldn't find a valid HDF5 file at " << path_to_hdf5_file; + throw std::invalid_argument(ss.str()); + } + + H5::H5File file(path_to_hdf5_file, H5F_ACC_RDONLY); + H5::DataSet means_dataset = file.openDataSet("means"); + H5::DataSet covariances_dataset = file.openDataSet("covariances"); + H5::DataSet resolution_dataset = file.openDataSet("resolution"); + H5::DataSet cells_dataset = file.openDataSet("cells"); + + std::array dims_out; + means_dataset.getSpace().getSimpleExtentDims(dims_out.data(), nullptr); + + Eigen::Matrix2Xd means_matrix(dims_out[1], dims_out[0]); + Eigen::Matrix2Xi cells_matrix(dims_out[1], dims_out[0]); + + means_dataset.read(means_matrix.data(), H5::PredType::NATIVE_DOUBLE); + cells_dataset.read(cells_matrix.data(), H5::PredType::NATIVE_INT); + + std::vector> covariances(dims_out[0]); + covariances_dataset.read(covariances.data(), H5::PredType::NATIVE_DOUBLE); + + double resolution; + + resolution_dataset.read(&resolution, H5::PredType::NATIVE_DOUBLE); + + typename NDTMapRepresentationT::map_type map{}; + + for (const auto& [cell, mean, covariance] : + ranges::zip_view(cells_matrix.colwise(), means_matrix.colwise(), covariances)) { + map[cell] = NDTCell2d{mean, covariance}; + } + return NDTMapRepresentationT{std::move(map), resolution}; +} + +} // namespace io + +} // namespace beluga diff --git a/beluga/test/beluga/CMakeLists.txt b/beluga/test/beluga/CMakeLists.txt index 77ec29bbb..203cb61a0 100644 --- a/beluga/test/beluga/CMakeLists.txt +++ b/beluga/test/beluga/CMakeLists.txt @@ -12,6 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. +file(COPY test_data DESTINATION ${CMAKE_CURRENT_BINARY_DIR}) + add_executable( test_beluga actions/test_assign.cpp @@ -39,6 +41,7 @@ add_executable( sensor/data/test_landmark_map.cpp sensor/data/test_laser_scan.cpp sensor/data/test_linear_grid.cpp + sensor/data/test_ndt_cell.cpp sensor/data/test_occupancy_grid.cpp sensor/data/test_regular_grid.cpp sensor/data/test_sparse_value_grid.cpp @@ -46,6 +49,7 @@ add_executable( sensor/test_bearing_sensor_model.cpp sensor/test_landmark_sensor_model.cpp sensor/test_likelihood_field_model.cpp + sensor/test_ndt_model.cpp test_primitives.cpp test_spatial_hash.cpp testing/test_sophus_matchers.cpp diff --git a/beluga/test/beluga/sensor/test_ndt_model.cpp b/beluga/test/beluga/sensor/test_ndt_model.cpp new file mode 100644 index 000000000..08883f852 --- /dev/null +++ b/beluga/test/beluga/sensor/test_ndt_model.cpp @@ -0,0 +1,146 @@ +// Copyright 2023 Ekumen, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include +#include +#include "beluga/sensor/data/sparse_value_grid.hpp" +namespace beluga { + +namespace { + +Eigen::Matrix2Xd get_diagonal_covariance(double x_var = 0.5, double y_var = 0.5) { + return Eigen::Vector2d{x_var, y_var}.asDiagonal(); +} +using sparse_grid_2d_t = SparseValueGrid>>; +} // namespace + +TEST(NDTSensorModelTests, CanConstruct) { + NDTSensorModel{{}, sparse_grid_2d_t{}}; +} + +TEST(NDTSensorModelTests, MinLikelihood) { + const double minimum_likelihood = -1.0; + + NDTModelParam param{minimum_likelihood}; + NDTSensorModel model{param, sparse_grid_2d_t{}}; + + for (const auto& val : { + Eigen::Vector2d{0.1, 0.1}, + Eigen::Vector2d{0.15, 0.15}, + Eigen::Vector2d{0.25, 0.25}, + Eigen::Vector2d{0.35, 0.35}, + Eigen::Vector2d{0.45, 0.45}, + Eigen::Vector2d{0.50, 0.50}, + Eigen::Vector2d{1.65, 0.65}, + Eigen::Vector2d{0.75, 0.75}, + + }) { + ASSERT_DOUBLE_EQ(model.likelihood_at({val, get_diagonal_covariance()}), minimum_likelihood); + } +} + +TEST(NDTSensorModelTests, Likelihoood) { + typename sparse_grid_2d_t::map_type map; + { + Eigen::Array cov; + // clang-format off + cov << 0.5, 0.0, + 0.0, 0.3; + // clang-format on + Eigen::Vector2d mean(0.5, 0.5); + map[Eigen::Vector2i(0, 0)] = NDTCell2d{mean, cov}; + } + { + Eigen::Array cov; + // clang-format off + cov << 0.5, 0.0, + 0.0, 0.5; + // clang-format on + Eigen::Vector2d mean(1.5, 1.5); + map[Eigen::Vector2i(1, 1)] = NDTCell2d{mean, cov}; + } + sparse_grid_2d_t grid{std::move(map), 1.0}; + + const double minimum_likelihood = -1.0; + NDTModelParam param{minimum_likelihood}; + NDTSensorModel model{param, std::move(grid)}; + + EXPECT_DOUBLE_EQ(model.likelihood_at({{0.5, 0.5}, get_diagonal_covariance()}), 1); + EXPECT_DOUBLE_EQ(model.likelihood_at({{0.8, 0.5}, get_diagonal_covariance()}), 0.95599748183309985); + EXPECT_DOUBLE_EQ(model.likelihood_at({{0.5, 0.8}, get_diagonal_covariance()}), 0.94530278065205942); + + EXPECT_DOUBLE_EQ(model.likelihood_at({{1.5, 1.5}, get_diagonal_covariance()}), 1); + EXPECT_DOUBLE_EQ(model.likelihood_at({{1.8, 1.5}, get_diagonal_covariance()}), 0.95599748183309985); + EXPECT_DOUBLE_EQ(model.likelihood_at({{1.5, 1.8}, get_diagonal_covariance()}), 0.95599748183309985); +} + +TEST(NDTSensorModelTests, FitPoints) { + { + std::vector meas{ + Eigen::Vector2d{0.1, 0.2}, Eigen::Vector2d{0.1, 0.2}, Eigen::Vector2d{0.1, 0.2}, + Eigen::Vector2d{0.1, 0.2}, Eigen::Vector2d{0.1, 0.2}, Eigen::Vector2d{0.1, 0.2}, + }; + auto cell = detail::fit_points<2>(meas); + ASSERT_TRUE(cell.mean.isApprox(Eigen::Vector2d{0.1, 0.2})); + // We introduce a minimum variance to avoid numeric errors down the line. + ASSERT_FALSE(cell.covariance.isZero()); + } + { + std::vector meas{ + Eigen::Vector2d{0.1, 0.2}, Eigen::Vector2d{0.1, 0.9}, Eigen::Vector2d{0.1, 0.2}, + Eigen::Vector2d{0.1, 0.9}, Eigen::Vector2d{0.1, 0.2}, Eigen::Vector2d{0.1, 0.2}, + }; + auto cell = detail::fit_points<2>(meas); + ASSERT_TRUE(cell.mean.isApprox(Eigen::Vector2d{0.1, 0.433333}, 1e-6)); + ASSERT_FALSE(cell.covariance.isZero()); + ASSERT_GT(cell.covariance(1, 1), cell.covariance(0, 0)); + } +} + +TEST(NDTSensorModelTests, SensorModel) { + double map_res = 0.5; + std::vector map_data{ + Eigen::Vector2d{0.1, 0.2}, Eigen::Vector2d{0.112, 0.22}, Eigen::Vector2d{0.15, 0.23}, + Eigen::Vector2d{0.1, 0.24}, Eigen::Vector2d{0.16, 0.25}, Eigen::Vector2d{0.1, 0.26}, + }; + auto cells = detail::to_cells<2UL>(map_data, map_res); + + typename sparse_grid_2d_t::map_type map_cells_data; + for (const auto& cell : cells) { + map_cells_data[(cell.mean.array() / map_res).cast()] = cell; + } + std::vector perfect_measurement = map_data; + NDTSensorModel model{{}, sparse_grid_2d_t{map_cells_data, map_res}}; + auto state_weighing_fn = model(std::move(perfect_measurement)); + // This is a perfect hit, so we should expect weight to be 1 + num_cells == 2. + ASSERT_DOUBLE_EQ(state_weighing_fn(Sophus::SE2d{}), 2); + + // Subtle miss, this value should be close to 1 but not quite. + ASSERT_GT(state_weighing_fn(Sophus::SE2d{Sophus::SO2d{}, Eigen::Vector2d{0.1, 0.1}}), 1.0); + + // This is a perfect miss, so we should expect weight to be 1. + ASSERT_DOUBLE_EQ(state_weighing_fn(Sophus::SE2d{Sophus::SO2d{}, Eigen::Vector2d{-10, -10}}), 1.0); +} + +TEST(NDTSensorModelTests, LoadFromHDF5) { + const auto ndt_map_representation = io::load_from_hdf5_2d("./test_data/turtlebot3_world.hdf5"); + ASSERT_EQ(ndt_map_representation.size(), 30UL); +} + +} // namespace beluga diff --git a/beluga/test/beluga/test_data/turtlebot3_world.hdf5 b/beluga/test/beluga/test_data/turtlebot3_world.hdf5 new file mode 100644 index 000000000..de69054d7 Binary files /dev/null and b/beluga/test/beluga/test_data/turtlebot3_world.hdf5 differ