amcl.hpp

Go to the documentation of this file.

// 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.
 
#ifndef BELUGA_ROS_AMCL_HPP
#define BELUGA_ROS_AMCL_HPP
 
#include <optional>
#include <utility>
#include <variant>
 
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/take_exactly.hpp>
 
#include <sophus/se2.hpp>
 
#include <beluga/algorithm/spatial_hash.hpp>
#include <beluga/algorithm/thrun_recovery_probability_estimator.hpp>
#include <beluga/containers.hpp>
#include <beluga/motion.hpp>
#include <beluga/policies.hpp>
#include <beluga/random.hpp>
#include <beluga/sensor.hpp>
#include <beluga/sensor/data/value_grid.hpp>
#include <beluga/sensor/primitives.hpp>
#include <beluga/views/sample.hpp>
 
#include <beluga_ros/laser_scan.hpp>
#include <beluga_ros/occupancy_grid.hpp>
#include <beluga_ros/sparse_point_cloud.hpp>
 
namespace beluga_ros {
 
struct AmclParams {
  double update_min_d = 0.25;
 
  double update_min_a = 0.2;
 
  std::size_t resample_interval = 1UL;
 
  bool selective_resampling = false;
 
  std::size_t min_particles = 500UL;
 
  std::size_t max_particles = 2000UL;
 
  double alpha_slow = 0.001;
 
  double alpha_fast = 0.1;
 
  double kld_epsilon = 0.05;
 
  double kld_z = 3.0;
 
  double spatial_resolution_x = 0.5;
 
  double spatial_resolution_y = 0.5;
 
  double spatial_resolution_theta = 10 * Sophus::Constants<double>::pi() / 180;
};
 
class Amcl {
 public:
  using particle_type = std::tuple<Sophus::SE2d, beluga::Weight>;
 
  using motion_model_variant = std::variant<
      beluga::DifferentialDriveModel2d,   //
      beluga::OmnidirectionalDriveModel,  //
      beluga::StationaryModel>;
 
  using sensor_model_variant = std::variant<
      beluga::LikelihoodFieldModel<beluga_ros::OccupancyGrid>,      //
      beluga::LikelihoodFieldProbModel<beluga_ros::OccupancyGrid>,  //
      beluga::BeamSensorModel<beluga_ros::OccupancyGrid>>;
 
  using execution_policy_variant = std::variant<std::execution::sequenced_policy, std::execution::parallel_policy>;
 
 
  Amcl(
      beluga_ros::OccupancyGrid map,
      motion_model_variant motion_model,
      sensor_model_variant sensor_model,
      const AmclParams& params,
      execution_policy_variant execution_policy);
 
  [[nodiscard]] const auto& particles() const { return particles_; }
 
  [[nodiscard]] const auto& likelihood_field() const {
    std::optional<std::reference_wrapper<const beluga::ValueGrid2<float>>> result;
 
    std::visit(
        [&result](const auto& sensor_model) {
          using T = std::decay_t<decltype(sensor_model)>;
          if constexpr (beluga::has_likelihood_field_v<T>) {
            result = std::cref(sensor_model.likelihood_field());
          }
        },
        sensor_model_);
 
    if (!result) {
      throw std::runtime_error("The current sensor model does not support likelihood field");
    }
 
    return result->get();
  }
 
  [[nodiscard]] auto likelihood_field_origin() const {
    std::optional<Sophus::SE2<double>> result;
 
    std::visit(
        [&result](const auto& sensor_model) {
          using T = std::decay_t<decltype(sensor_model)>;
          if constexpr (beluga::has_likelihood_field_v<T>) {
            result = sensor_model.likelihood_field_origin();
          }
        },
        sensor_model_);
 
    if (!result) {
      throw std::runtime_error("The current sensor model does not support likelihood field");
    }
 
    return *result;  // return by value
  }
 
  [[nodiscard]] bool has_likelihood_field() const {
    return std::visit(
        [](const auto& sensor_model) {
          using T = std::decay_t<decltype(sensor_model)>;
          return beluga::has_likelihood_field_v<T>;
        },
        sensor_model_);
  }
 
  template <class Distribution>
  void initialize(Distribution distribution) {
    particles_ = beluga::views::sample(std::move(distribution)) |                    //
                 ranges::views::transform(beluga::make_from_state<particle_type>) |  //
                 ranges::views::take_exactly(params_.max_particles) |                //
                 ranges::to<beluga::TupleVector>;
    force_update_ = true;
  }
 
 
  void initialize(Sophus::SE2d pose, Sophus::Matrix3d covariance) {
    initialize(beluga::MultivariateNormalDistribution{pose, covariance});
  }
 
  void initialize_from_map() { initialize(std::ref(map_distribution_)); }
 
  void update_map(beluga_ros::OccupancyGrid map);
 
 
  auto update(Sophus::SE2d base_pose_in_odom, beluga_ros::LaserScan laser_scan)
      -> std::optional<std::pair<Sophus::SE2d, Sophus::Matrix3d>>;
 
 
  auto update(Sophus::SE2d base_pose_in_odom, beluga_ros::SparsePointCloud3f point_cloud)
      -> std::optional<std::pair<Sophus::SE2d, Sophus::Matrix3d>>;
 
 
  auto update(Sophus::SE2d base_pose_in_odom, std::vector<std::pair<double, double>>&& measurement)
      -> std::optional<std::pair<Sophus::SE2d, Sophus::Matrix3d>>;
 
  void force_update() { force_update_ = true; }
 
 private:
  beluga::TupleVector<particle_type> particles_;
 
  AmclParams params_;
  beluga::MultivariateUniformDistribution<Sophus::SE2d, beluga_ros::OccupancyGrid> map_distribution_;
  motion_model_variant motion_model_;
  sensor_model_variant sensor_model_;
  execution_policy_variant execution_policy_;
 
  beluga::spatial_hash<Sophus::SE2d> spatial_hasher_;
  beluga::ThrunRecoveryProbabilityEstimator random_probability_estimator_;
  beluga::any_policy<Sophus::SE2d> update_policy_;
  beluga::any_policy<decltype(particles_)> resample_policy_;
 
  beluga::RollingWindow<Sophus::SE2d, 2> control_action_window_;
 
  bool force_update_{true};
};
 
}  // namespace beluga_ros
 
#endif  // BELUGA_ROS_AMCL_HPP