Skip to content

Commit

Permalink
Clean up NDT base implementation and examples
Browse files Browse the repository at this point in the history 
Signed-off-by: Nahuel Espinosa <[email protected]>
  • Loading branch information
@nahueespinosa
nahueespinosa committed Oct 27, 2024
1 parent 08e682f commit ade0b90
Show file tree
Hide file tree
Showing 17 changed files with 241 additions and 643 deletions.
1 change: 1 addition & 0 deletions beluga/include/beluga/algorithm.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
* \brief Includes all beluga algorithms.
*/

#include <beluga/algorithm/amcl_core.hpp>
#include <beluga/algorithm/cluster_based_estimation.hpp>
#include <beluga/algorithm/distance_map.hpp>
#include <beluga/algorithm/effective_sample_size.hpp>
Expand Down
95 changes: 28 additions & 67 deletions beluga/include/beluga/algorithm/amcl_core.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,12 +21,25 @@
#include <utility>
#include <vector>

#include <beluga/beluga.hpp>

#include <range/v3/range/concepts.hpp>
#include <range/v3/view/any_view.hpp>
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/take_exactly.hpp>
#include "beluga/policies/on_motion.hpp"

#include <beluga/actions/assign.hpp>
#include <beluga/actions/normalize.hpp>
#include <beluga/actions/propagate.hpp>
#include <beluga/actions/reweight.hpp>
#include <beluga/algorithm/estimation.hpp>
#include <beluga/algorithm/spatial_hash.hpp>
#include <beluga/containers/circular_array.hpp>
#include <beluga/containers/tuple_vector.hpp>
#include <beluga/policies/every_n.hpp>
#include <beluga/policies/on_effective_size_drop.hpp>
#include <beluga/policies/on_motion.hpp>
#include <beluga/primitives.hpp>
#include <beluga/random/multivariate_normal_distribution.hpp>
#include <beluga/views/random_intersperse.hpp>
#include <beluga/views/sample.hpp>
#include <beluga/views/take_while_kld.hpp>

namespace beluga {

Expand All @@ -44,10 +57,6 @@ struct AmclParams {
std::size_t min_particles = 500UL;
/// Maximum number of particles in the filter at any point in time.
std::size_t max_particles = 2000UL;
/// Used as part of the recovery mechanism when considering how many random particles to insert.
double alpha_slow = 0.001;
/// Used as part of the recovery mechanism when considering how many random particles to insert.
double alpha_fast = 0.1;
/// Used as part of the kld sampling mechanism.
double kld_epsilon = 0.05;
/// Used as part of the kld sampling mechanism.
Expand All @@ -58,66 +67,40 @@ struct AmclParams {
/**
* \tparam MotionModel Class representing a motion model. Must satisfy \ref MotionModelPage.
* \tparam SensorModel Class representing a sensor model. Must satisfy \ref SensorModelPage.
* \tparam RandomStateGenerator A callable able to produce random states, optionally based on the current particles
* state.
* Class 'T' is a valid RandomStateGenerator if given 't' a possibly const instance of 'T' any of the following
* conditions are true:
* - t can be called without arguments returning an instance of 'SensorModel::state_type'
* representing a random state.
* - t can be called with `const beluga::TupleVector<ParticleType>&> returning a callable
* that can be called without arguments and return an instance of 'SensorModel::state_type'.
* \tparam WeightT Type to represent a weight of a particle.
* \tparam ParticleType Full particle type, containing state, weight and possibly
* other information .
* \tparam ExecutionPolicy Execution policy for particles processing.
*/
template <
class MotionModel,
class SensorModel,
class RandomStateGenerator,
typename WeightT = beluga::Weight,
class ParticleType = std::tuple<typename SensorModel::state_type, WeightT>,
class ExecutionPolicy = std::execution::sequenced_policy>
class Particle = std::tuple<typename SensorModel::state_type, beluga::Weight>>
class Amcl {
static_assert(
std::is_same_v<ExecutionPolicy, std::execution::parallel_policy> or
std::is_same_v<ExecutionPolicy, std::execution::sequenced_policy>);

using particle_type = ParticleType;
using particle_type = Particle;
using measurement_type = typename SensorModel::measurement_type;
using state_type = typename SensorModel::state_type;
using map_type = typename SensorModel::map_type;
using spatial_hasher_type = spatial_hash<state_type>;
using random_state_generator_type = RandomStateGenerator;
using estimation_type = std::invoke_result_t<beluga::detail::estimate_fn, std::vector<state_type>>;

public:
/// Construct a AMCL instance.
/**
* \param motion_model Motion model instance.
* \param sensor_model Sensor model Instance.
* \param random_state_generator A callable able to produce random states, optionally based on the current particles
* state.
* \param spatial_hasher A spatial hasher instance capable of computing a hash out of a particle state.
* \param params Parameters for AMCL implementation.
* \param execution_policy Policy to use when processing particles.
*/
Amcl(
MotionModel motion_model,
SensorModel sensor_model,
RandomStateGenerator random_state_generator,
spatial_hasher_type spatial_hasher,
const AmclParams& params = AmclParams{},
ExecutionPolicy execution_policy = std::execution::seq)
const AmclParams& params = AmclParams{})
: params_{params},
motion_model_{std::move(motion_model)},
sensor_model_{std::move(sensor_model)},
execution_policy_{std::move(execution_policy)},
spatial_hasher_{std::move(spatial_hasher)},
random_probability_estimator_{params_.alpha_slow, params_.alpha_fast},
update_policy_{beluga::policies::on_motion<state_type>(params_.update_min_d, params_.update_min_a)},
resample_policy_{beluga::policies::every_n(params_.resample_interval)},
random_state_generator_(std::move(random_state_generator)) {
resample_policy_{beluga::policies::every_n(params_.resample_interval)} {
if (params_.selective_resampling) {
resample_policy_ = resample_policy_ && beluga::policies::on_effective_size_drop;
}
Expand All @@ -141,9 +124,9 @@ class Amcl {
* \tparam CovarianceT type representing a covariance, compliant with state_type.
* \throw std::runtime_error If the provided covariance is invalid.
*/
template <class CovarianceT>
void initialize(state_type pose, CovarianceT covariance) {
initialize(beluga::MultivariateNormalDistribution{pose, covariance});
template <class Covariance>
void initialize(state_type pose, Covariance covariance) {
initialize(beluga::MultivariateNormalDistribution{std::move(pose), std::move(covariance)});
}

/// Update the map used for localization.
Expand Down Expand Up @@ -171,22 +154,12 @@ class Amcl {
return std::nullopt;
}

particles_ |= beluga::actions::propagate(
execution_policy_, motion_model_(control_action_window_ << std::move(control_action))) | //
beluga::actions::reweight(execution_policy_, sensor_model_(std::move(measurement))) | //
beluga::actions::normalize(execution_policy_);

const double random_state_probability = random_probability_estimator_(particles_);
particles_ |= beluga::actions::propagate(motion_model_(control_action_window_ << std::move(control_action))) | //
beluga::actions::reweight(sensor_model_(std::move(measurement))) | //
beluga::actions::normalize;

if (resample_policy_(particles_)) {
auto random_state = ranges::compose(beluga::make_from_state<particle_type>, get_random_state_generator());

if (random_state_probability > 0.0) {
random_probability_estimator_.reset();
}

particles_ |= beluga::views::sample |
beluga::views::random_intersperse(std::move(random_state), random_state_probability) |
beluga::views::take_while_kld(
spatial_hasher_, //
params_.min_particles, //
Expand All @@ -204,29 +177,17 @@ class Amcl {
void force_update() { force_update_ = true; }

private:
/// Gets a callable that will produce a random state.
[[nodiscard]] decltype(auto) get_random_state_generator() const {
if constexpr (std::is_invocable_v<random_state_generator_type>) {
return random_state_generator_;
} else {
return random_state_generator_(particles_);
}
}
beluga::TupleVector<particle_type> particles_;

AmclParams params_;

MotionModel motion_model_;
SensorModel sensor_model_;
ExecutionPolicy execution_policy_;

spatial_hasher_type spatial_hasher_;
beluga::ThrunRecoveryProbabilityEstimator random_probability_estimator_;
beluga::any_policy<state_type> update_policy_;
beluga::any_policy<decltype(particles_)> resample_policy_;

random_state_generator_type random_state_generator_;

beluga::RollingWindow<state_type, 2> control_action_window_;

bool force_update_{true};
Expand Down
22 changes: 11 additions & 11 deletions beluga/include/beluga/sensor/data/ndt_cell.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@ namespace beluga {

/// Representation for a cell of a N dimensional NDT cell.
template <int NDim, typename Scalar = double>
struct NDTCell {
struct NdtCell {
static_assert(std::is_floating_point_v<Scalar>, "Scalar template parameter should be a floating point.");
/// Number of dimensions of the cell's translation.
static constexpr int num_dim = NDim;
Expand All @@ -46,44 +46,44 @@ struct NDTCell {

/// Get the L2 likelihood at measurement, scaled by d1 and d2. It assumes the measurement is pre-transformed
/// into the same frame as this cell instance.
[[nodiscard]] double likelihood_at(const NDTCell& measurement, double d1 = 1.0, double d2 = 1.0) const {
[[nodiscard]] double likelihood_at(const NdtCell& measurement, double d1 = 1.0, double d2 = 1.0) const {
const Eigen::Vector<Scalar, NDim> error = measurement.mean - mean;
const double rhs =
std::exp((-d2 / 2.0) * error.transpose() * (measurement.covariance + covariance).inverse() * error);
return d1 * rhs;
}

/// Ostream overload mostly for debugging purposes.
friend std::ostream& operator<<(std::ostream& os, const NDTCell& cell) {
friend std::ostream& operator<<(std::ostream& os, const NdtCell& cell) {
os << "Mean \n" << cell.mean.transpose() << " \n\nCovariance: \n" << cell.covariance;
return os;
}

/// Transform the normal distribution according to tf, both mean and covariance.
friend NDTCell operator*(const Sophus::SE2<scalar_type>& tf, const NDTCell& ndt_cell) {
friend NdtCell operator*(const Sophus::SE2<scalar_type>& tf, const NdtCell& ndt_cell) {
static_assert(num_dim == 2, "Cannot transform a non 2D NDT Cell with a SE2 transform.");
const Eigen::Vector2d uij = tf * ndt_cell.mean;
const Eigen::Matrix2Xd cov = tf.so2().matrix() * ndt_cell.covariance * tf.so2().matrix().transpose();
return NDTCell{uij, cov};
return NdtCell{uij, cov};
}

/// Transform the normal distribution according to tf, both mean and covariance.
friend NDTCell operator*(const Sophus::SE3<scalar_type>& tf, const NDTCell& ndt_cell) {
friend NdtCell operator*(const Sophus::SE3<scalar_type>& tf, const NdtCell& ndt_cell) {
static_assert(num_dim == 3, "Cannot transform a non 3D NDT Cell with a SE3 transform.");
const Eigen::Vector3d uij = tf * ndt_cell.mean;
const Eigen::Matrix3Xd cov = tf.so3().matrix() * ndt_cell.covariance * tf.so3().matrix().transpose();
return NDTCell{uij, cov};
return NdtCell{uij, cov};
}
};

/// Convenience alias for a 2D NDT cell with double representation.
using NDTCell2d = NDTCell<2, double>;
using NdtCell2d = NdtCell<2, double>;
/// Convenience alias for a 2D NDT cell with float representation.
using NDTCell2f = NDTCell<2, float>;
using NdtCell2f = NdtCell<2, float>;
/// Convenience alias for a 3D NDT cell with double representation.
using NDTCell3d = NDTCell<3, double>;
using NdtCell3d = NdtCell<3, double>;
/// Convenience alias for a 3D NDT cell with float representation.
using NDTCell3f = NDTCell<3, float>;
using NdtCell3f = NdtCell<3, float>;

} // namespace beluga

Expand Down
50 changes: 25 additions & 25 deletions beluga/include/beluga/sensor/ndt_sensor_model.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,7 @@ struct CellHasher {

/// Fit a vector of points to an NDT cell, by computing its mean and covariance.
template <int NDim, typename Scalar = double>
inline NDTCell<NDim, Scalar> fit_points(const std::vector<Eigen::Vector<Scalar, NDim>>& points) {
inline NdtCell<NDim, Scalar> fit_points(const std::vector<Eigen::Vector<Scalar, NDim>>& points) {
static constexpr double kMinVariance = 1e-5;
Eigen::Map<const Eigen::Matrix<Scalar, NDim, Eigen::Dynamic>> points_view(
reinterpret_cast<const Scalar*>(points.data()), NDim, static_cast<int64_t>(points.size()));
Expand All @@ -71,22 +71,22 @@ inline NDTCell<NDim, Scalar> fit_points(const std::vector<Eigen::Vector<Scalar,
if constexpr (NDim == 3) {
cov(2, 2) = std::max(cov(2, 2), kMinVariance);
}
return NDTCell<NDim, Scalar>{mean, cov};
return NdtCell<NDim, Scalar>{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 <int NDim, typename Scalar = double>
inline std::vector<NDTCell<NDim, Scalar>> to_cells(
inline std::vector<NdtCell<NDim, Scalar>> to_cells(
const std::vector<Eigen::Vector<Scalar, NDim>>& points,
const double resolution) {
static constexpr int kMinPointsPerCell = 5;

const Eigen::Map<const Eigen::Matrix<Scalar, NDim, Eigen::Dynamic>> points_view(
reinterpret_cast<const Scalar*>(points.data()), NDim, static_cast<int64_t>(points.size()));

std::vector<NDTCell<NDim, Scalar>> ret;
std::vector<NdtCell<NDim, Scalar>> ret;
ret.reserve(static_cast<size_t>(points_view.cols()) / kMinPointsPerCell);

std::unordered_map<Eigen::Vector<int, NDim>, std::vector<Eigen::Vector<Scalar, NDim>>, CellHasher<NDim>> cell_grid;
Expand Down Expand Up @@ -142,9 +142,9 @@ get_default_neighbors_kernel() {

} // namespace detail

/// Parameters used to construct a NDTSensorModel instance.
/// Parameters used to construct a NdtSensorModel instance.
template <int NDim>
struct NDTModelParam {
struct NdtModelParam {
static_assert(NDim == 2 or NDim == 3, "Only 2D or 3D is supported for the NDT sensor model.");
/// Likelihood for measurements that lie inside cells that are not present in the map.
double minimum_likelihood = 0;
Expand All @@ -158,18 +158,18 @@ struct NDTModelParam {
};

/// Convenience alias for a 2d parameters struct for the NDT sensor model.
using NDTModelParam2d = NDTModelParam<2>;
using NdtModelParam2d = NdtModelParam<2>;

/// Convenience alias for a 3d parameters struct for the NDT sensor model.
using NDTModelParam3d = NDTModelParam<3>;
using NdtModelParam3d = NdtModelParam<3>;
/// 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 <typename SparseGridT>
class NDTSensorModel {
class NdtSensorModel {
public:
/// NDT Cell type.
using ndt_cell_type = typename SparseGridT::mapped_type;
Expand All @@ -189,16 +189,16 @@ class NDTSensorModel {
/// Map representation type.
using map_type = SparseGridT;
/// Parameter type that the constructor uses to configure the NDT sensor model.
using param_type = NDTModelParam<ndt_cell_type::num_dim>;
using param_type = NdtModelParam<ndt_cell_type::num_dim>;

/// Constructs a NDTSensorModel instance.
/// Constructs a NdtSensorModel instance.
/**
* \param params Parameters to configure this instance.
* See beluga::NDTModelParam for details.
* 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)
NdtSensorModel(param_type params, SparseGridT cells_data)
: params_{std::move(params)}, cells_data_{std::move(cells_data)} {
assert(params_.minimum_likelihood >= 0);
}
Expand Down Expand Up @@ -247,18 +247,18 @@ namespace io {
/// of the cell.
/// - "covariances": (NUM_CELLS, 2 / 3, 2 / 3) Contains the covariance for each cell.
///
/// \tparam NDTMapRepresentation A specialized SparseValueGrid (see sensor/data/sparse_value_grid.hpp), where
/// mapped_type == NDTCell2d / NDTCell3d, that will represent the NDT map as a mapping from 2D / 3D cells to NDTCells.
template <typename NDTMapRepresentationT>
NDTMapRepresentationT load_from_hdf5(const std::filesystem::path& path_to_hdf5_file) {
/// \tparam NdtMap A specialized SparseValueGrid (see sensor/data/sparse_value_grid.hpp), where
/// mapped_type == NdtCell2d / NdtCell3d, that will represent the NDT map as a mapping from 2D / 3D cells to NdtCells.
template <typename NdtMap>
NdtMap load_from_hdf5(const std::filesystem::path& path_to_hdf5_file) {
static_assert(
std::is_same_v<typename NDTMapRepresentationT::mapped_type, NDTCell2d> or
std::is_same_v<typename NDTMapRepresentationT::mapped_type, NDTCell3d>);
std::is_same_v<typename NdtMap::mapped_type, NdtCell2d> or
std::is_same_v<typename NdtMap::mapped_type, NdtCell3d>);
static_assert(
std::is_same_v<typename NDTMapRepresentationT::key_type, Eigen::Vector2i> or
std::is_same_v<typename NDTMapRepresentationT::key_type, Eigen::Vector3i>);
std::is_same_v<typename NdtMap::key_type, Eigen::Vector2i> or
std::is_same_v<typename NdtMap::key_type, Eigen::Vector3i>);

constexpr int kNumDim = NDTMapRepresentationT::key_type::RowsAtCompileTime;
constexpr int kNumDim = NdtMap::key_type::RowsAtCompileTime;
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;
Expand Down Expand Up @@ -293,15 +293,15 @@ NDTMapRepresentationT load_from_hdf5(const std::filesystem::path& path_to_hdf5_f

resolution_dataset.read(&resolution, H5::PredType::NATIVE_DOUBLE);

typename NDTMapRepresentationT::map_type map{};
typename NdtMap::map_type map{};

// Note: Ranges::zip_view doesn't seem to work in old Eigen.
for (size_t i = 0; i < covariances.size(); ++i) {
map[cells_matrix.col(static_cast<Eigen::Index>(i))] =
NDTCell<kNumDim, double>{means_matrix.col(static_cast<Eigen::Index>(i)), covariances.at(i)};
NdtCell<kNumDim, double>{means_matrix.col(static_cast<Eigen::Index>(i)), covariances.at(i)};
}

return NDTMapRepresentationT{std::move(map), resolution};
return NdtMap{std::move(map), resolution};
}

} // namespace io
Expand Down
Loading

0 comments on commit ade0b90

Please sign in to comment.