From 6c28b9fd983d78e798d65f0123f6c40c4e5a1386 Mon Sep 17 00:00:00 2001 From: Ramon Wijnands Date: Thu, 2 Dec 2021 17:37:44 +0100 Subject: [PATCH] docs: Add docstrings for most classes --- ruvu_mcl/README.md | 22 +++++- ruvu_mcl/src/adaptive/adaptive_method.hpp | 12 ++++ ruvu_mcl/src/adaptive/fixed.hpp | 3 + ruvu_mcl/src/adaptive/kld_sampling.hpp | 5 ++ ruvu_mcl/src/adaptive/split_and_merge.hpp | 13 ++++ ruvu_mcl/src/adaptive/utils.hpp | 5 ++ ruvu_mcl/src/cloud_publisher.hpp | 3 + ruvu_mcl/src/main.cpp | 3 + ruvu_mcl/src/map.hpp | 68 +++++++++++++++++-- ruvu_mcl/src/mcl.hpp | 8 ++- ruvu_mcl/src/mcl_ros.hpp | 8 ++- .../differential_motion_model.hpp | 21 ++++-- ruvu_mcl/src/motion_models/motion_model.hpp | 11 ++- ruvu_mcl/src/node.hpp | 3 + ruvu_mcl/src/offline.cpp | 3 + ruvu_mcl/src/offline/bag_buffer.hpp | 3 + ruvu_mcl/src/offline/bag_player.hpp | 27 +++++--- ruvu_mcl/src/particle_filter.hpp | 40 +++++++++++ ruvu_mcl/src/resamplers/low_variance.cpp | 3 +- ruvu_mcl/src/resamplers/low_variance.hpp | 11 ++- ruvu_mcl/src/resamplers/resampler.hpp | 14 ++-- ruvu_mcl/src/rng.hpp | 14 ++++ ruvu_mcl/src/sensor_models/beam_model.hpp | 9 +-- .../sensor_models/gaussian_landmark_model.cpp | 21 +++++- .../sensor_models/gaussian_landmark_model.hpp | 9 +-- ruvu_mcl/src/sensor_models/landmark.hpp | 46 ++++++++++++- .../landmark_likelihood_field_model.hpp | 9 +-- ruvu_mcl/src/sensor_models/laser.hpp | 55 +++++++++++++-- .../sensor_models/likelihood_field_model.hpp | 9 +-- 29 files changed, 388 insertions(+), 70 deletions(-) diff --git a/ruvu_mcl/README.md b/ruvu_mcl/README.md index a3150ec..99aee6c 100644 --- a/ruvu_mcl/README.md +++ b/ruvu_mcl/README.md @@ -51,8 +51,28 @@ See also [BENCHMARK.md](./BENCHMARK.md) for a guide on how to compare different ## Design -The design of the MCL is split into a core library (mcl) and a ROS wrapper. This makes it easy in the future to migrate to ROS2 or other platforms. +The design of the MCL is split into a core library (ruvu_mcl::Mcl) and a ROS wrapper (ruvu_mcl::MclRos). This makes it easy in the future to migrate to ROS2 or other platforms. +### Implemented motion models +- ruvu_mcl::DifferentialMotionModel + +### Implemented laser sensor models +- ruvu_mcl::BeamModel +- ruvu_mcl::LikelihoodFieldModel (default) + +### Implemented landmark sensor models +- ruvu_mcl::GaussianLandmarkModel (default) +- ruvu_mcl::LandmarkLikelihoodFieldModel + +### Implemented adaptive methods +- ruvu_mcl::Fixed (default) +- ruvu_mcl::KLDSampling +- ruvu_mcl::SplitAndMerge + +### Implemented resamplers +- ruvu_mcl::LowVariance + +### Class diagram ```plantuml class Node { tf2_ros::MessageFilter diff --git a/ruvu_mcl/src/adaptive/adaptive_method.hpp b/ruvu_mcl/src/adaptive/adaptive_method.hpp index 7f62787..a81a642 100644 --- a/ruvu_mcl/src/adaptive/adaptive_method.hpp +++ b/ruvu_mcl/src/adaptive/adaptive_method.hpp @@ -7,16 +7,28 @@ namespace ruvu_mcl // forward declare class ParticleFilter; +/** + * @brief Adaptive methods changes the efficiency of filtering by adapting the number of particles. + */ class AdaptiveMethod { public: virtual ~AdaptiveMethod() = default; + /** + * @brief Hook that runs before the odometry update + */ virtual void after_odometry_update(ParticleFilter * pf) const {} + + /** + * @brief Hook that runs after the sensor update + */ virtual void after_sensor_update(ParticleFilter * pf) const {} /** * @brief Calculate how many particles are wanted + * + * This number will be used as a target when resampling * @return Number of particles */ virtual int calc_needed_particles(const ParticleFilter & pf) const = 0; diff --git a/ruvu_mcl/src/adaptive/fixed.hpp b/ruvu_mcl/src/adaptive/fixed.hpp index aef1ddd..311b55b 100644 --- a/ruvu_mcl/src/adaptive/fixed.hpp +++ b/ruvu_mcl/src/adaptive/fixed.hpp @@ -11,6 +11,9 @@ namespace ruvu_mcl class ParticleFilter; struct Config; +/** + * @brief Use a fixed amount of particles (not adaptive) + */ class Fixed : public AdaptiveMethod { public: diff --git a/ruvu_mcl/src/adaptive/kld_sampling.hpp b/ruvu_mcl/src/adaptive/kld_sampling.hpp index 18045e2..7dc4399 100644 --- a/ruvu_mcl/src/adaptive/kld_sampling.hpp +++ b/ruvu_mcl/src/adaptive/kld_sampling.hpp @@ -10,6 +10,11 @@ namespace ruvu_mcl struct KLDSamplingConfig; class ParticleFilter; +/** + * @brief Use KLD sampling to adapt the number of particles + * + * From the paper: KLD-Sampling: Adaptive Particle Filters + */ class KLDSampling : public AdaptiveMethod { public: diff --git a/ruvu_mcl/src/adaptive/split_and_merge.hpp b/ruvu_mcl/src/adaptive/split_and_merge.hpp index 5d16126..7777ea6 100644 --- a/ruvu_mcl/src/adaptive/split_and_merge.hpp +++ b/ruvu_mcl/src/adaptive/split_and_merge.hpp @@ -9,6 +9,12 @@ namespace ruvu_mcl // forward declare class ParticleFilter; +/** + * @brief Use split & merge to adapt the number of particles + * + * From the paper: Monte Carlo localization for mobile robot using adaptive particle merging and + * splitting technique + */ class SplitAndMerge : public AdaptiveMethod { public: @@ -18,7 +24,14 @@ class SplitAndMerge : public AdaptiveMethod void after_sensor_update(ParticleFilter * pf) const override { split_particles(pf); } int calc_needed_particles(const ParticleFilter & pf) const override; + /** + * @brief Merge particles that are close together + */ void merge_particles(ParticleFilter * pf) const; + + /** + * @brief Split particles that have a high weight + */ void split_particles(ParticleFilter * pf) const; private: diff --git a/ruvu_mcl/src/adaptive/utils.hpp b/ruvu_mcl/src/adaptive/utils.hpp index a442193..885af83 100644 --- a/ruvu_mcl/src/adaptive/utils.hpp +++ b/ruvu_mcl/src/adaptive/utils.hpp @@ -7,6 +7,11 @@ namespace ruvu_mcl { using Key = std::tuple; +/** + * @brief Hash function for Keys + * + * This class can be used as hash function in unordered_map or unordered_set. + */ struct KeyHasher { std::size_t operator()(const Key & key) const noexcept; diff --git a/ruvu_mcl/src/cloud_publisher.hpp b/ruvu_mcl/src/cloud_publisher.hpp index 40e5529..d9d79e9 100644 --- a/ruvu_mcl/src/cloud_publisher.hpp +++ b/ruvu_mcl/src/cloud_publisher.hpp @@ -20,6 +20,9 @@ namespace ruvu_mcl { class Particle; +/** + * @brief Publishes a vector of particles as a visualization_msgs::Marker + */ class CloudPublisher { public: diff --git a/ruvu_mcl/src/main.cpp b/ruvu_mcl/src/main.cpp index 3605cea..16dc884 100644 --- a/ruvu_mcl/src/main.cpp +++ b/ruvu_mcl/src/main.cpp @@ -9,6 +9,9 @@ using ruvu_mcl::Node; constexpr auto name = "main"; +/** +* @brief Main ros node of the ruvu_mcl package +*/ int main(int argc, char ** argv) { ros::init(argc, argv, "mcl"); diff --git a/ruvu_mcl/src/map.hpp b/ruvu_mcl/src/map.hpp index 254d91f..4ebd933 100644 --- a/ruvu_mcl/src/map.hpp +++ b/ruvu_mcl/src/map.hpp @@ -15,51 +15,109 @@ ROS_DECLARE_MESSAGE(OccupancyGrid) namespace ruvu_mcl { +/** + * @brief Base class for 2-D grid maps + */ struct Map { + /** + * @brief The origin of the map + */ tf2::Transform origin; - // Map scale (m/cell) + /** + * @brief Map scale (m/cell) + */ double scale; + /** + * @brief Construct a Map from a nav_msgs::OccupancyGrid + */ explicit Map(const nav_msgs::OccupancyGrid & msg); + + /** + * @brief Convert a Map to a nav_msgs::OccupancyGrid + */ explicit operator nav_msgs::OccupancyGrid() const; + /** + * @brief Convert world coordinates to map indices + */ std::pair world2map(const tf2::Vector3 & v) const; }; +/** + * @brief A 2-D grid map, in which each cell has a occupance state + */ struct OccupancyMap : Map { - // Occupancy state (-1 = free, 0 = unknown, +1 = occ) + /** + * @brief A cell contains the occupancy state (-1 = free, 0 = unknown, +1 = occ) + */ using CellType = int8_t; - // rows = x, cols = y using CellsType = Eigen::Matrix; CellsType cells; + /** + * @brief Construct a Map from a nav_msgs::OccupancyGrid + */ explicit OccupancyMap(const nav_msgs::OccupancyGrid & msg); + /** + * @brief Check if the cells in the line between v1 and v2 are occupied + * @param v1 from + * @param v2 to + * @return if the range is occupied + */ double calc_range(const tf2::Vector3 & v1, const tf2::Vector3 & v2) const; + + /** + * @brief Check if the cells in the line between x0,y0 and x1,y1 are occupied + * @return if the range is occupied + */ double calc_range(Eigen::Index x0, Eigen::Index y0, Eigen::Index x1, Eigen::Index y1) const; + + /** + * @brief Check if the indices are in bounds of the map + */ bool is_valid(Eigen::Index i, Eigen::Index j) const; }; +/** + * @brief A 2-D grid map, in which each cell has the distance to the nearest obstacle + */ struct DistanceMap : Map { - // distance to the nearest obstacle + /** + * @brief A cell contains the distance to the nearest obstacle + */ using CellType = double; - // rows = x, cols = y using CellsType = Eigen::Matrix; CellsType cells; ros::Publisher debug_pub_; + /** + * @brief Construct a DistanceMap from a nav_msgs::OccupancyGrid + */ explicit DistanceMap(const nav_msgs::OccupancyGrid & msg); + + /** + * @brief Convert a DistanceMap to a nav_msgs::OccupancyGrid + */ explicit operator nav_msgs::OccupancyGrid() const; + /** + * @brief Get the distance to the closest obstacle from a given point + */ double closest_obstacle(const tf2::Vector3 & v) const; + + /** + * @brief Check if the indices are in bounds of the map + */ bool is_valid(Eigen::Index i, Eigen::Index j) const; // TODO(Ramon): move to baseclass }; } // namespace ruvu_mcl diff --git a/ruvu_mcl/src/mcl.hpp b/ruvu_mcl/src/mcl.hpp index 0be5181..834e138 100644 --- a/ruvu_mcl/src/mcl.hpp +++ b/ruvu_mcl/src/mcl.hpp @@ -43,11 +43,17 @@ class ParticleFilter; class Resampler; class Rng; +/** + * @brief Main class for monte-carlo localization. + * + * This class takes in measurments and outputs the position estimate of the robot. It can be + * configured with different sensor and motion models. + */ class Mcl { public: Mcl(); - Mcl(std::uint_fast32_t seed); + Mcl(uint_fast32_t seed); ~Mcl(); // to handle forward declares void configure(const Config & config); diff --git a/ruvu_mcl/src/mcl_ros.hpp b/ruvu_mcl/src/mcl_ros.hpp index c61c17a..5aae653 100644 --- a/ruvu_mcl/src/mcl_ros.hpp +++ b/ruvu_mcl/src/mcl_ros.hpp @@ -38,6 +38,12 @@ namespace ruvu_mcl { class AMCLConfig; +/** + * @brief ROS wrapper for the Mcl class + * + * This class is a ROS wrapper for the Mcl class. It receives ROS messages, converts then and sends + * then trough. It also publishes the output of Mcl to /tf and ~pose. + */ class MclRos { public: @@ -46,7 +52,7 @@ class MclRos const std::shared_ptr & buffer); MclRos( ros::NodeHandle nh, ros::NodeHandle private_nh, - const std::shared_ptr & buffer, std::uint_fast32_t seed); + const std::shared_ptr & buffer, uint_fast32_t seed); ~MclRos(); // to handle forward declares void configure(const ruvu_mcl::AMCLConfig & config); diff --git a/ruvu_mcl/src/motion_models/differential_motion_model.hpp b/ruvu_mcl/src/motion_models/differential_motion_model.hpp index cc68234..36658a1 100644 --- a/ruvu_mcl/src/motion_models/differential_motion_model.hpp +++ b/ruvu_mcl/src/motion_models/differential_motion_model.hpp @@ -13,11 +13,14 @@ namespace ruvu_mcl // forward declare class Rng; +/** + * @brief Motion model for a differential drive + */ class DifferentialMotionModel : public MotionModel { public: /** - * @brief A diff drive constructor + * @brief Construct a differental drive motion model * @param alpha1 error parameters, see documentation * @param alpha2 error parameters, see documentation * @param alpha3 error parameters, see documentation @@ -26,16 +29,22 @@ class DifferentialMotionModel : public MotionModel */ DifferentialMotionModel( double alpha1, double alpha2, double alpha3, double alpha4, const std::shared_ptr & rng); - DifferentialMotionModel( - const DifferentialMotionModelConfig & config, const std::shared_ptr & rng); /** - * @brief Update on new odometry data - * @param pf The particle filter to update - * @param delta change in pose in odometry update + * @brief Construct a differental drive motion model + * @param config parameters + * @param rng random number generator */ + DifferentialMotionModel( + const DifferentialMotionModelConfig & config, const std::shared_ptr & rng); + void odometry_update(ParticleFilter * pf, const tf2::Transform & delta) override; + /** + * @brief Compute {delta_rot1, delta_trans, delta_rot2} from a displacement + * @param delta displacement + * @return {delta_rot1, delta_trans, delta_rot2} + */ static std::array calculate_deltas(const tf2::Transform & delta); private: diff --git a/ruvu_mcl/src/motion_models/motion_model.hpp b/ruvu_mcl/src/motion_models/motion_model.hpp index c79ab85..fb5bc12 100644 --- a/ruvu_mcl/src/motion_models/motion_model.hpp +++ b/ruvu_mcl/src/motion_models/motion_model.hpp @@ -12,13 +12,22 @@ namespace ruvu_mcl { class ParticleFilter; +/** + * @brief Base class for all motion models + * + * Base class for all motion models. A motion model tries to accurately model the specific types of + * uncertainty that exist in robot actuation. + * + * Motion models take odometry measurements as input and move the particles according to a specific + * probabalistic distribution. See also chapter 5 of Probabilistc Robotics. + */ class MotionModel { public: virtual ~MotionModel(); /** - * @brief Update on new odometry data + * @brief Update particles based on new odometry data * @param pf The particle filter to update * @param delta change in pose in odometry update */ diff --git a/ruvu_mcl/src/node.hpp b/ruvu_mcl/src/node.hpp index b81770b..15ac5a4 100644 --- a/ruvu_mcl/src/node.hpp +++ b/ruvu_mcl/src/node.hpp @@ -26,6 +26,9 @@ ROS_DECLARE_MESSAGE(LaserScan) namespace ruvu_mcl { +/** + * @brief Main ros node of the ruvu_mcl package + */ class Node { public: diff --git a/ruvu_mcl/src/offline.cpp b/ruvu_mcl/src/offline.cpp index 9a7fe08..487184a 100644 --- a/ruvu_mcl/src/offline.cpp +++ b/ruvu_mcl/src/offline.cpp @@ -25,6 +25,9 @@ using ruvu_mcl::MclRos; constexpr auto name = "offline"; +/** + * @brief Executable for offline playback of the particle filter on ros bagfiles + */ int main(int argc, char ** argv) { if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) { diff --git a/ruvu_mcl/src/offline/bag_buffer.hpp b/ruvu_mcl/src/offline/bag_buffer.hpp index 1ce7fc0..94151eb 100644 --- a/ruvu_mcl/src/offline/bag_buffer.hpp +++ b/ruvu_mcl/src/offline/bag_buffer.hpp @@ -11,6 +11,9 @@ class Bag; namespace ruvu_mcl { +/** + * @brief Load all tf messages from a rosbag into a tf2_ros::Buffer + */ class BagBuffer : public tf2_ros::Buffer { public: diff --git a/ruvu_mcl/src/offline/bag_player.hpp b/ruvu_mcl/src/offline/bag_player.hpp index 5d13ed9..6d21140 100644 --- a/ruvu_mcl/src/offline/bag_player.hpp +++ b/ruvu_mcl/src/offline/bag_player.hpp @@ -45,24 +45,35 @@ using BagCallbackT = std::function &)>; using BagCallback = std::function; -/* A class for playing back bag files at an API level. It supports - relatime, as well as accelerated and slowed playback. */ +/** + * @brief A class for playing back bag files + * + * It supports relatime, as well as accelerated and slowed playback. + */ class BagPlayer { public: - /* Constructor expecting the filename of a bag */ + /** + * @brief Constructor expecting the filename of a bag + */ explicit BagPlayer(const std::string & filename); - /* Register a callback for a specific topic and type */ + /** + * @brief Register a callback for a specific topic and type + */ template void register_callback(const std::string & topic, BagCallbackT cb); - /* Set the speed to playback. 1.0 is the default. - * 2.0 would be twice as fast, 0.5 is half realtime. */ + /** + * @brief Set the speed to playback. + * + * 1.0 is the default. 2.0 would be twice as fast, 0.5 is half realtime. + */ void set_playback_speed(double scale); - /* Start playback of the bag file using the parameters previously - set */ + /** + * @brief Start playback of the bag file using the parameters previously set + */ void start_play(); // The bag file interface loaded in the constructor. diff --git a/ruvu_mcl/src/particle_filter.hpp b/ruvu_mcl/src/particle_filter.hpp index b811d5c..d297557 100644 --- a/ruvu_mcl/src/particle_filter.hpp +++ b/ruvu_mcl/src/particle_filter.hpp @@ -9,6 +9,9 @@ namespace ruvu_mcl { +/** + * @brief This represents a pose in free space with uncertainty. + */ struct PoseWithCovariance { PoseWithCovariance(const tf2::Transform & pose, const std::array & covariance) @@ -19,23 +22,60 @@ struct PoseWithCovariance std::array covariance; }; +/** + * @brief A single position estimate + */ class Particle { public: + /** + * @brief The position estimate + */ tf2::Transform pose; + + /** + * @brief How well this position estimate has matched previous measurments + */ double_t weight; + /** + * @brief Create a particle + */ Particle(const tf2::Transform & pose, double weight) : pose(pose), weight(weight) {} }; +/** + * @brief Hold a vector of Particles + */ class ParticleFilter { public: std::vector particles; + /** + * @brief Normalize the weight of all particles such that the sum is 1 + */ void normalize_weights(); + + /** + * @brief Normalize the weight of all particles such that the sum is 1 + * + * This function avoids calculating the total_weight if it is already known. + */ void normalize_weights(double total_weight); + + /** + * @brief Calculate the effective sample size + * + * The effective sample size can be used as a criterion for deciding when to perform the + * resampling step. It is a measure how well the current particle set represents the target + * posterior. + */ double calc_effective_sample_size() const; + + /** + * @brief Calculate a mean pose with covariance of all the particles in the filter + */ PoseWithCovariance get_pose_with_covariance() const; }; } // namespace ruvu_mcl diff --git a/ruvu_mcl/src/resamplers/low_variance.cpp b/ruvu_mcl/src/resamplers/low_variance.cpp index 6bd4505..b2535d8 100644 --- a/ruvu_mcl/src/resamplers/low_variance.cpp +++ b/ruvu_mcl/src/resamplers/low_variance.cpp @@ -14,7 +14,7 @@ namespace ruvu_mcl { LowVariance::LowVariance(const std::shared_ptr & rng) : rng_(rng) {} -bool LowVariance::resample(ParticleFilter * pf, int needed_particles) +void LowVariance::resample(ParticleFilter * pf, int needed_particles) { assert(!pf->particles.empty()); ROS_DEBUG_NAMED(name, "resample"); @@ -37,6 +37,5 @@ bool LowVariance::resample(ParticleFilter * pf, int needed_particles) } pf_resampled.normalize_weights(); *pf = pf_resampled; - return true; } } // namespace ruvu_mcl diff --git a/ruvu_mcl/src/resamplers/low_variance.hpp b/ruvu_mcl/src/resamplers/low_variance.hpp index 5315260..2affb99 100644 --- a/ruvu_mcl/src/resamplers/low_variance.hpp +++ b/ruvu_mcl/src/resamplers/low_variance.hpp @@ -11,16 +11,15 @@ namespace ruvu_mcl // forward declare class Rng; +/** + * @brief Implementation of the low-variance sampler + */ class LowVariance : public Resampler { public: explicit LowVariance(const std::shared_ptr & rng); - /* - * @brief Resample a particle filter using the low-variance method - * @param pf Particle filter to use - * @return if it was succesful - */ - bool resample(ParticleFilter * pf, int needed_particles) override; + + void resample(ParticleFilter * pf, int needed_particles) override; private: std::shared_ptr rng_; diff --git a/ruvu_mcl/src/resamplers/resampler.hpp b/ruvu_mcl/src/resamplers/resampler.hpp index 767afc7..ac6ea1b 100644 --- a/ruvu_mcl/src/resamplers/resampler.hpp +++ b/ruvu_mcl/src/resamplers/resampler.hpp @@ -7,16 +7,22 @@ namespace ruvu_mcl // forward declare class ParticleFilter; +/** + * @brief Base class for all resampling algorithms + * + * The resampling step has the important function to force particles back to the posterior belief. + * See also Probablistic robotics for this. + */ class Resampler { public: virtual ~Resampler(); - /* - * @brief Resample a particle filter + /** + * @brief Resample the particles of a particle filter * @param pf Particle filter to use - * @return if it was succesful + * @param needed_particles How many particles are needed after resampling */ - virtual bool resample(ParticleFilter * pf, int needed_particles) = 0; + virtual void resample(ParticleFilter * pf, int needed_particles) = 0; }; } // namespace ruvu_mcl diff --git a/ruvu_mcl/src/rng.hpp b/ruvu_mcl/src/rng.hpp index 096efb3..31517bf 100644 --- a/ruvu_mcl/src/rng.hpp +++ b/ruvu_mcl/src/rng.hpp @@ -7,10 +7,24 @@ namespace ruvu_mcl { +/** + * @brief Utility class for generating random numbers + */ class Rng : public std::enable_shared_from_this { public: + /** + * @brief Construct Rng class with a random seed + */ Rng(); + + /** + * @brief Construct Rng class with a fixed seed + * + * This means that the class produces random numbers that are deterministic. Two classes with the + * same seed produce the same sequence of random numbers. This can be used for example in CI to + * create test cases that deterministic (i.e. not flaky). + */ explicit Rng(uint_fast32_t seed); /** diff --git a/ruvu_mcl/src/sensor_models/beam_model.hpp b/ruvu_mcl/src/sensor_models/beam_model.hpp index 9fa65b3..b1525d5 100644 --- a/ruvu_mcl/src/sensor_models/beam_model.hpp +++ b/ruvu_mcl/src/sensor_models/beam_model.hpp @@ -13,6 +13,9 @@ namespace ruvu_mcl // forward declare struct OccupancyMap; +/** + * @brief Implements the beam range finder model from Probablistic Robotics + */ class BeamModel : public Laser { public: @@ -21,12 +24,6 @@ class BeamModel : public Laser */ BeamModel(const BeamModelConfig & config, const std::shared_ptr & map); - /* - * @brief Run a sensor update on laser - * @param pf Particle filter to use - * @param data Laser data to use - * @return if it was succesful - */ void sensor_update(ParticleFilter * pf, const LaserData & data) override; private: diff --git a/ruvu_mcl/src/sensor_models/gaussian_landmark_model.cpp b/ruvu_mcl/src/sensor_models/gaussian_landmark_model.cpp index bc4a4f4..414b704 100644 --- a/ruvu_mcl/src/sensor_models/gaussian_landmark_model.cpp +++ b/ruvu_mcl/src/sensor_models/gaussian_landmark_model.cpp @@ -53,6 +53,20 @@ GaussianLandmarkModel::GaussianLandmarkModel( { assert(config.z_rand >= 0 && config.z_rand <= 1); + /** + * Our original algorithm scaled linearly with the size of the landmark map. This meant that for + * maps in the range of 1000 landmarks, our algorithm quickly became unresponsive. We needed to + * find a way to quickly discard landmarks in the landmark map that don't contribute to the + * particle weight. + * + * The chance that a measurment has a smaller distance than max_confidence_range_ is + * landmark_max_r_confidence. Suppose this confidence range is 99%. If we skip measurments that + * are bigger than this distance, then we skip only 1% of each measurments. This means the + * particle update stays approximately the same. + * + * We can then use a KD tree to only evaluate landmarks that are within this + * max_confidence_range_. + */ max_confidence_range_ = config.landmark_sigma_r * F_inv(config.landmark_max_r_confidence); ROS_INFO_NAMED( name, "%f confidence is %f meter (%f sigma)", config.landmark_max_r_confidence, @@ -91,9 +105,10 @@ void GaussianLandmarkModel::sensor_update(ParticleFilter * pf, const LandmarkLis for (const auto & measurement : data.landmarks) { double pz_arg = std::numeric_limits::max(); auto measurement_length = measurement.pose.getOrigin().length(); - // Only landmarks withing max_range around the laser have a chance to influence the particle - // weight. We can use this to quickly prune the map with a KDTree. - double max_range = measurement.pose.getOrigin().length() + max_confidence_range_; + // Only landmarks withing max_range around the laser can have a r_hat_diff that is smaller + // than max_confidence_range_. Only these landmarks have a chance to influence the particle + // weight. We use this fact to quickly prune the map with a KDTree. + double max_range = measurement_length + max_confidence_range_; const auto & l = laser_pose.getOrigin(); std::vector results; KDTreeNode node{Landmark{tf2::Transform{ diff --git a/ruvu_mcl/src/sensor_models/gaussian_landmark_model.hpp b/ruvu_mcl/src/sensor_models/gaussian_landmark_model.hpp index fb22428..433714c 100644 --- a/ruvu_mcl/src/sensor_models/gaussian_landmark_model.hpp +++ b/ruvu_mcl/src/sensor_models/gaussian_landmark_model.hpp @@ -28,6 +28,9 @@ struct KDTreeNode using KDTreeType = KDTree::KDTree<2, KDTreeNode>; +/** + * @brief Implements the landmark model known correspondence from Probablistic Robotics + */ class GaussianLandmarkModel : public LandmarkModel { public: @@ -36,12 +39,6 @@ class GaussianLandmarkModel : public LandmarkModel */ GaussianLandmarkModel(const GaussianLandmarkModelConfig & config, const LandmarkList & map); - /* - * @brief Run a sensor update on the particles with landmark data - * @param pf Particle filter to use - * @param data Landmark data to use - * @return if it was succesful - */ void sensor_update(ParticleFilter * pf, const LandmarkList & data); private: diff --git a/ruvu_mcl/src/sensor_models/landmark.hpp b/ruvu_mcl/src/sensor_models/landmark.hpp index 792cd72..c8890a9 100644 --- a/ruvu_mcl/src/sensor_models/landmark.hpp +++ b/ruvu_mcl/src/sensor_models/landmark.hpp @@ -22,46 +22,86 @@ namespace ruvu_mcl // forward declare class ParticleFilter; +/** + * @brief The a single Landmark measurment + */ struct Landmark { explicit Landmark(const tf2::Transform & pose) : pose(pose), id(0) {} Landmark(const tf2::Transform & pose, int id) : pose(pose), id(id) {} /** + * @brief Pose of the landmark + * * Landmarks that are from the landmark map are assumed to be transformed to the global frame. * Landmark from measurements are assumed to be in the sensor frame. */ tf2::Transform pose; + + /** + * @brief optional identity of the landmark. If the id is 0, it has no identity. + */ int id; }; +/** + * @brief Landmark measurment data + * + * LandmarkList is used in two cases. One as a map of landmarks. Two as Landmark measurment data. + */ class LandmarkList { public: + /** + * @brief Create an empty LandmarkList + */ LandmarkList() : header(), pose(tf2::Transform::getIdentity()), landmarks() {} + /** + * @brief Create a landmark list from a ruvu_mcl_msgs::LandmarkList + * @param msg ruvu_mcl_msgs::LandmarkList + * @param pose For landmark measurments, the pose of the sensor relative to the base_link must be + * given. + */ explicit LandmarkList( const ruvu_mcl_msgs::LandmarkList & msg, const tf2::Transform & pose = tf2::Transform::getIdentity()); + /** + * @brief timestamp in the header is the acquisition time of the measurement + */ std_msgs::Header header; + /** - * LandmarkLists from measurements should contain a pose with the transform from robot frame to sensor frame. + * @brief LandmarkLists from measurements should contain a pose with the transform from robot frame to sensor frame. */ tf2::Transform pose; + + /** + * @brief List of all detected landmarks + */ std::vector landmarks; }; +/** + * @brief Base class for landmark sensor models + * + * Base class for all landmark sensor models. A sensor model tries to accurately model the specific + * types of uncertainty that exist in a sensor. + * + * Sensor models take a measurment as input and update the weight of the particles according to how + * good the position estimate explains the sensor measurment. See also chapter 6 of Probabilistc + * Robotics. + */ class LandmarkModel { public: virtual ~LandmarkModel(); - /* + /** * @brief Run a sensor update on the particles with landmark data * @param pf Particle filter to use * @param data Landmark data to use - * @return if it was succesful */ virtual void sensor_update(ParticleFilter * pf, const LandmarkList & data) = 0; }; diff --git a/ruvu_mcl/src/sensor_models/landmark_likelihood_field_model.hpp b/ruvu_mcl/src/sensor_models/landmark_likelihood_field_model.hpp index 94edf9b..e84b4b7 100644 --- a/ruvu_mcl/src/sensor_models/landmark_likelihood_field_model.hpp +++ b/ruvu_mcl/src/sensor_models/landmark_likelihood_field_model.hpp @@ -15,6 +15,9 @@ ROS_DECLARE_MESSAGE(LandmarkList) namespace ruvu_mcl { +/** + * @brief Impelements the likelihood field range finder model from Probablistic Robotics for landmark measurments + */ class LandmarkLikelihoodFieldModel : public LandmarkModel { public: @@ -24,12 +27,6 @@ class LandmarkLikelihoodFieldModel : public LandmarkModel LandmarkLikelihoodFieldModel( const LandmarkLikelihoodFieldModelConfig & config, const LandmarkList & landmarks); - /* - * @brief Run a sensor update on the particles with landmark data - * @param pf Particle filter to use - * @param data Landmark data to use - * @return if it was succesful - */ void sensor_update(ParticleFilter * pf, const LandmarkList & data) override; private: diff --git a/ruvu_mcl/src/sensor_models/laser.hpp b/ruvu_mcl/src/sensor_models/laser.hpp index 9507774..250dae8 100644 --- a/ruvu_mcl/src/sensor_models/laser.hpp +++ b/ruvu_mcl/src/sensor_models/laser.hpp @@ -18,34 +18,79 @@ namespace ruvu_mcl { class ParticleFilter; +/** + * @brief Measurment data from a laser scanner + */ class LaserData { public: using RangeType = double; + /** + * @brief Construct a LaserData from a ROS message + * @param scan ROS message + * @param pose The position of the scanner relative to base_link + */ LaserData(const sensor_msgs::LaserScan & scan, const tf2::Transform & pose); + /** + * @brief Calculate the angle of the i'th beam + */ double get_angle(std::size_t i) const; + /** + * @brief timestamp in the header is the acquisition time of the first ray in the scan. + */ std_msgs::Header header; - tf2::Transform pose; // how the laser is mounted relative to base_link + /** + * @brief how the laser is mounted relative to base_link + */ + tf2::Transform pose; + /** + * @brief start angle of the scan + */ double angle_min; + + /** + * @brief angular distance between measurements [rad] + */ double angle_increment; + + /** + * @brief maximum range value [m] + */ double range_max; + + /** + * @brief range data [m] + * + * If a range is -infinity, the object too close to measure. + * If a range is infinity, no objects are detected in range. + * If the range is nan, it is an erroneous, invalid, or missing measurement + */ std::vector ranges; }; +/** + * @brief Base class for laser sensor models + * + * Base class for all laser sensor models. A sensor model tries to accurately model the specific + * types of uncertainty that exist in a sensor. + * + * Sensor models take a measurment as input and update the weight of the particles according to how + * good the position estimate explains the sensor measurment. See also chapter 6 of Probabilistc + * Robotics. + */ class Laser { public: virtual ~Laser(); - /* - * @brief Run a sensor update on laser - * @param pf Particle filter to use + /** + * @brief Update particles based on a laser measurement + * @param pf The particle filter to update * @param data Laser data to use - * @return if it was succesful */ virtual void sensor_update(ParticleFilter * pf, const LaserData & data) = 0; }; diff --git a/ruvu_mcl/src/sensor_models/likelihood_field_model.hpp b/ruvu_mcl/src/sensor_models/likelihood_field_model.hpp index 3cc0094..3986df9 100644 --- a/ruvu_mcl/src/sensor_models/likelihood_field_model.hpp +++ b/ruvu_mcl/src/sensor_models/likelihood_field_model.hpp @@ -13,6 +13,9 @@ namespace ruvu_mcl // forward declare struct DistanceMap; +/** + * @brief Impelements the likelihood field range finder model from Probablistic Robotics + */ class LikelihoodFieldModel : public Laser { public: @@ -22,12 +25,6 @@ class LikelihoodFieldModel : public Laser LikelihoodFieldModel( const LikelihoodFieldModelConfig & config, const std::shared_ptr & map); - /* - * @brief Run a sensor update on laser - * @param pf Particle filter to use - * @param data Laser data to use - * @return if it was succesful - */ void sensor_update(ParticleFilter * pf, const LaserData & data) override; private: