Add new discrete landmark and bearing sensor models to the beluga lib…

…rary (#268)

- Adds a slightly polished version of the landmark and bearing sensor
models created during last week's hackaton.
- Both have been extended to both 2D and 3D particle states (since it
was trivial).
 - Tests have been improved.
 - A residual bug in the bearing model was found and fixed.
- Minor refactoring to avoid crowding the `sensors.hpp` file with
interfaces.

Signed-off-by: Gerardo Puga <[email protected]>

beluga/include/beluga/interfaces/bearing_sensor_model_interface.hpp

@@ -0,0 +1,69 @@
+// Copyright 2022-2023 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_INTERFACES_BEARING_SENSOR_MODEL_INTERFACE_HPP
+#define BELUGA_INTERFACES_BEARING_SENSOR_MODEL_INTERFACE_HPP
+
+// standard library
+#include <cstdint>
+#include <vector>
+
+// project
+#include <beluga/types/landmark_detection_types.hpp>
+
+/**
+ * \file
+ * \brief Includes the interface for bearing sensor models.
+ */
+
+namespace beluga {
+
+/// Pure abstract class representing the bearing sensor model interface.
+/**
+ * \tparam Map Bearing landmarks representation type.
+ */
+template <class Map>
+struct BearingSensorModelInterface {
+  /// Measurement type of the sensor: vector of landmark detections
+  using measurement_type = std::vector<LandmarkBearingDetection>;
+
+  /// Virtual destructor.
+  virtual ~BearingSensorModelInterface() = default;
+
+  /// Update the sensor model with the measured points.
+  /**
+   * This method updates the sensor model with the information
+   * it needs to compute the weight of each particle.
+   * The weight of each particle is calculated by subsequent calls to
+   * the `importance_weight()` method provided by the same mixin
+   * component.
+   *
+   * \param points The discrete sensor detections.
+   */
+  virtual void update_sensor(measurement_type&& points) = 0;
+
+  /// Update the sensor model with a new map.
+  /**
+   * This method updates the sensor model with a new map,
+   * i.e. a representation of the environment, that it needs
+   * to compute the weight of each particle.
+   *
+   * \param map The range finder map.
+   */
+  virtual void update_map(Map&& map) = 0;
+};
+
+}  // namespace beluga
+
+#endif

beluga/include/beluga/interfaces/landmark_sensor_model_interface.hpp

@@ -0,0 +1,69 @@
+// Copyright 2022-2023 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_INTERFACES_LANDMARK_SENSOR_MODEL_INTERFACE_HPP
+#define BELUGA_INTERFACES_LANDMARK_SENSOR_MODEL_INTERFACE_HPP
+
+// standard library
+#include <cstdint>
+#include <vector>
+
+// project
+#include <beluga/types/landmark_detection_types.hpp>
+
+/**
+ * \file
+ * \brief Includes the interface for generic landmark sensor models.
+ */
+
+namespace beluga {
+
+/// Pure abstract class representing a generic landmark sensor model.
+/**
+ * \tparam Landmark information representation type.
+ */
+template <class Map>
+struct LandmarkSensorModelInterface {
+  /// Measurement type of the sensor, detection position in robot frame
+  using measurement_type = std::vector<LandmarkPositionDetection>;
+
+  /// Virtual destructor.
+  virtual ~LandmarkSensorModelInterface() = default;
+
+  /// Update the sensor model with the measured points.
+  /**
+   * This method updates the sensor model with the information
+   * it needs to compute the weight of each particle.
+   * The weight of each particle is calculated by subsequent calls to
+   * the `importance_weight()` method provided by the same mixin
+   * component.
+   *
+   * \param points The discrete sensor detections in the reference frame of the particle.
+   */
+  virtual void update_sensor(measurement_type&& points) = 0;
+
+  /// Update the sensor model with a new map.
+  /**
+   * This method updates the sensor model with a new map,
+   * i.e. a representation of the environment, that it needs
+   * to compute the weight of each particle.
+   *
+   * \param map The range finder map.
+   */
+  virtual void update_map(Map&& map) = 0;
+};
+
+}  // namespace beluga
+
+#endif

beluga/include/beluga/interfaces/laser_sensor_model_interface_2d.hpp

@@ -0,0 +1,64 @@
+// Copyright 2022-2023 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_INTERFACES_LASER_SENSOR_MODEL_INTERFACE_2D_HPP
+#define BELUGA_INTERFACES_LASER_SENSOR_MODEL_INTERFACE_2D_HPP
+
+#include <vector>
+
+/**
+ * \file
+ * \brief Includes the interface for laser sensor models.
+ */
+
+namespace beluga {
+
+/// Pure abstract class representing the laser sensor model interface.
+/**
+ * \tparam Map Environment representation type.
+ */
+template <class Map>
+struct LaserSensorModelInterface2d {
+  /// Measurement type of the sensor: a point cloud for the range finder.
+  using measurement_type = std::vector<std::pair<double, double>>;
+
+  /// Virtual destructor.
+  virtual ~LaserSensorModelInterface2d() = default;
+
+  /// Update the sensor model with the measured points.
+  /**
+   * This method updates the sensor model with the information
+   * it needs to compute the weight of each particle.
+   * The weight of each particle is calculated by subsequent calls to
+   * the `importance_weight()` method provided by the same mixin
+   * component.
+   *
+   * \param points The range finder points in the reference frame of the particle.
+   */
+  virtual void update_sensor(measurement_type&& points) = 0;
+
+  /// Update the sensor model with a new map.
+  /**
+   * This method updates the sensor model with a new map,
+   * i.e. a representation of the environment, that it needs
+   * to compute the weight of each particle.
+   *
+   * \param map The range finder map.
+   */
+  virtual void update_map(Map&& map) = 0;
+};
+
+}  // namespace beluga
+
+#endif

beluga/include/beluga/sensor.hpp

@@ -15,12 +15,9 @@
 #ifndef BELUGA_SENSOR_HPP
 #define BELUGA_SENSOR_HPP
 
-#include <beluga/sensor/beam_model.hpp>
-#include <beluga/sensor/likelihood_field_model.hpp>
-
 /**
  * \file
- * \brief Includes all Beluga sensor models.
+ * \brief Includes all Beluga sensor models and their interfaces.
  */
 
 /**
@@ -49,45 +46,19 @@
  * \section SensorModelLinks See also
  * - beluga::LikelihoodFieldModel
  * - beluga::BeamSensorModel
+ * - beluga::LandmarkSensorModel2d
+ * - beluga::BearingSensorModel2d
  */
 
-namespace beluga {
-
-/// Pure abstract class representing the laser sensor model interface.
-/**
- * \tparam Map Environment representation type.
- */
-template <class Map>
-struct LaserSensorModelInterface2d {
-  /// Measurement type of the sensor: a point cloud for the range finder.
-  using measurement_type = std::vector<std::pair<double, double>>;
+// interfaces
+#include <beluga/interfaces/bearing_sensor_model_interface.hpp>
+#include <beluga/interfaces/landmark_sensor_model_interface.hpp>
+#include <beluga/interfaces/laser_sensor_model_interface_2d.hpp>
 
-  /// Virtual destructor.
-  virtual ~LaserSensorModelInterface2d() = default;
-
-  /// Update the sensor model with the measured points.
-  /**
-   * This method updates the sensor model with the information
-   * it needs to compute the weight of each particle.
-   * The weight of each particle is calculated by subsequent calls to
-   * the `importance_weight()` method provided by the same mixin
-   * component.
-   *
-   * \param points The range finder points in the reference frame of the particle.
-   */
-  virtual void update_sensor(measurement_type&& points) = 0;
-
-  /// Update the sensor model with a new map.
-  /**
-   * This method updates the sensor model with a new map,
-   * i.e. a representation of the environment, that it needs
-   * to compute the weight of each particle.
-   *
-   * \param map The range finder map.
-   */
-  virtual void update_map(Map&& map) = 0;
-};
-
-}  // namespace beluga
+// implementations
+#include <beluga/sensor/beam_model.hpp>
+#include <beluga/sensor/bearing_sensor_model.hpp>
+#include <beluga/sensor/landmark_sensor_model.hpp>
+#include <beluga/sensor/likelihood_field_model.hpp>
 
 #endif