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 28, 2024
1 parent 08e682f commit 1b9f23c
Show file tree
Hide file tree
Showing 17 changed files with 247 additions and 647 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
Loading

0 comments on commit 1b9f23c

Please sign in to comment.