Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Clean up NDT implementation and examples #451

Closed
wants to merge 2 commits into from
Closed

Clean up NDT implementation and examples #451

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
0d05297
Clean up NDT base implementation and examples
nahueespinosa Oct 27, 2024
e536e50
Remove support for Nav2 differential model name
nahueespinosa Oct 29, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions beluga/include/beluga/algorithm.hpp
View file
Open in desktop
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
Open in desktop
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 {
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@nahueespinosa meta: there is redundancy between beluga::Amcl and beluga_ros::Amcl. I recall we did not get as far as to deduplicate them, but that'd be the logical path. In which case, beluga_ros::Amcl is doing a lot more than it should, and with this change, beluga::Amcl would be doing less. Why remove the execution policy? Why remove (as opposed to generalize, if applicable) state jitter to avoid distribution collapse?

Copy link
Member Author

@nahueespinosa nahueespinosa Oct 28, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't think that's the kind of generalization that leads to maintainable code.

State jitter does not apply to NDT models. IIUC, the 2D case was sampling from the particle set, which has no effect; and the 3D case was adding zero-initialized states, which is simply wrong. We never created a map distribution compatible with the NDT map representation.

In any case, I think we should treat the NDT examples as a nice proof of concept, and avoid adding unnecessary features.

If we do want those features (e.g. configurable execution policies), we should do more thinking before generalizing things.

Copy link
Member Author

@nahueespinosa nahueespinosa Oct 29, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Also, having this amount of std::visit calls and templates of templates makes it really hard to play with explicit template instantiation. I really don't think that we should keep things as they are today.

Copy link
Member Author

@nahueespinosa nahueespinosa Oct 29, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The range pipeline refactor was meant to provide a way to leave behind (overly complicated) class templates and the mixin pattern. We are kind of going back to that if we try to provide an Amcl class that's general enough for any use case.

There are other approaches to composition that I think are worth exploring. I reckon I haven't had enough time to do so this year.

Copy link
Collaborator

@hidmic hidmic Oct 29, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

There's a lot to unpack there.

State jitter does not apply to NDT models.

But it does for AMCL, at least for this instantiation of it. Whether NDT filters should use this AMCL variant or a different MCL variant is a completely valid yet separate discussion IMO.

having this amount of std::visit calls and templates of templates makes it really hard to play with explicit template instantiation

That is a very good point. We ended up using std::visit to enable runtime configurations and to keep execution paths transparent to the compiler. We can push it to the edges, but I don't think we'll get to remove it. Can we?

If we do want those features (e.g. configurable execution policies), we should do more thinking before generalizing things.

I really don't think that we should keep things as they are today.

We are kind of going back to that if we try to provide an Amcl class that's general enough for any use case.

There are other approaches to composition that I think are worth exploring.

Hmm, all of that is valid, but that alternative design that is better than this doesn't exist yet. I give you that these AMCL classes are far from complete solutions, but walking back on them with nothing to show for in exchange seems like a regression to me.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

But it does for AMCL, at least for this instantiation of it. Whether NDT filters should use this AMCL variant or a different MCL variant is a completely valid yet separate discussion IMO.

On second read, I didn't remove state jitter to avoid distribution collapse. That's taken care of by the motion model. I removed the recovery strategy based on injecting random particles from the map (which none of the NDT variants were actually doing).

"Adaptive" in AMCL originally stands for the fact that the sample size can change based on the KLD metric. The recovery strategy was not mentioned in the original paper, it comes from Prob Rob.

Can we?

Yes, with carefully implemented type erasure and benchmarking. I don't know how much benefit we get from inlining the sensor and motion models, we should measure that. I've also seen really interesting techniques to make type-erasure really efficient. The only drawback is not being able to inline, which might be a big deal in some scenarios. We should investigate.

Range pipelines on the other hand will suffer greatly without inlining. But that's not what we are trying to customize at runtime.

Hmm, all of that is valid, but that alternative design that is better than this doesn't exist yet.

Fair point.

I give you that these AMCL classes are far from complete solutions, but walking back on them with nothing to show for in exchange seems like a regression to me.

I understand your point, but it seems to me that we took a lot of shortcuts to get to this point and this is only one small step back. I'm not removing vital features, the two classes still implement (a version of) AMCL, and this unblocks experimentation to reduce memory usage at build time.

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I removed the recovery strategy based on injecting random particles from the map

FWIW that still counts as roughening to cope with (particle depletion and) distribution collapse. Artificially increasing the variance of other distributions helps too but it is a different approach.

I've also seen really interesting techniques to make type-erasure really efficient. The only drawback is not being able to inline, which might be a big deal in some scenarios.

Hmm, the only path I see that avoids explicit combinatorial explosion (as std::visit already explodes but we don't see it) is type erasing within loops. Performance will take a hit, though I guess we could argue that you can't have it both ways: it's either performance or flexibility. I'm OK with that so long as top performance is still achievable.

it seems to me that we took a lot of shortcuts to get to this point

That's true.

this unblocks experimentation to reduce memory usage at build time.

Hmm, what's preventing explicit instantiation of the beluga::Amcl class in its current form? 🤔

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
Open in desktop
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 {
hidmic marked this conversation as resolved.
Show resolved Hide resolved
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
Loading