Program Listing for File bearing_sensor_model.hpp

↰ Return to documentation for file (include/beluga/sensor/bearing_sensor_model.hpp)

// Copyright 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_SENSOR_BEARING_SENSOR_MODEL_HPP
#define BELUGA_SENSOR_BEARING_SENSOR_MODEL_HPP

// external
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/all.hpp>
#include <range/v3/view/transform.hpp>
#include <sophus/se2.hpp>
#include <sophus/se3.hpp>

// standard library
#include <cmath>
#include <random>
#include <utility>
#include <vector>

// project
#include <beluga/types/landmark_detection_types.hpp>

namespace beluga {

struct BearingModelParam {
  double sigma_bearing{1.0};
  Sophus::SE3d sensor_pose_in_robot;
};


template <class LandmarkMap, class StateType>
class BearingSensorModel {
 public:
  using state_type = StateType;
  using weight_type = double;
  using measurement_type = std::vector<LandmarkBearingDetection>;
  using map_type = LandmarkMap;
  using param_type = BearingModelParam;


  template <class... Args>
  explicit BearingSensorModel(param_type params, LandmarkMap landmark_map)
      : params_{std::move(params)}, landmark_map_{std::move(landmark_map)} {}


  [[nodiscard]] auto operator()(measurement_type&& detections) const {
    return [this, detections = std::move(detections)](const state_type& state) -> weight_type {
      Sophus::SE3d robot_pose_in_world;

      if constexpr (std::is_same_v<state_type, Sophus::SE3d>) {
        // The robot pose state is already given in 3D,
        robot_pose_in_world = state;
      } else {
        // The robot pose state is given in 2D. Notice that in this case
        // the 2D pose of the robot is assumed to be that of the robot footprint (projection of the robot
        // on the z=0 plane of the 3D world frame). This is so that we can tie the sensor reference frame
        // to the world frame where the landmarks are given without additional structural information.
        robot_pose_in_world = Sophus::SE3d{
            Sophus::SO3d::rotZ(state.so2().log()),
            Eigen::Vector3d{state.translation().x(), state.translation().y(), 0.0}};
      }

      // precalculate the sensor pose in the world frame
      const auto sensor_pose_in_world = robot_pose_in_world * params_.sensor_pose_in_robot;

      const auto detection_weight = [this, &sensor_pose_in_world](const auto& detection) {
        // find the landmark the most closely matches the sample bearing vector
        const auto opt_landmark_bearing_in_sensor = landmark_map_.find_closest_bearing_landmark(
            detection.detection_bearing_in_sensor, detection.category, sensor_pose_in_world);

        // if we did not find a matching landmark, return 0.0
        if (!opt_landmark_bearing_in_sensor) {
          return 0.0;
        }

        // recover sample bearing vector
        const auto detection_bearing_in_sensor = detection.detection_bearing_in_sensor.normalized();

        // recover landmark bearing vector
        const auto& landmark_bearing_in_sensor = *opt_landmark_bearing_in_sensor;

        // calculate the aperture angle between the detection and the landmark
        const auto cos_aperture = detection_bearing_in_sensor.dot(landmark_bearing_in_sensor);
        const auto sin_aperture = detection_bearing_in_sensor.cross(landmark_bearing_in_sensor).norm();

        // calculate the angle between the sample and the landmark
        const auto bearing_error = std::atan2(sin_aperture, cos_aperture);

        // model the probability of the landmark being detected as depending on the bearing error
        const auto bearing_error_prob =
            std::exp(-bearing_error * bearing_error / (2. * params_.sigma_bearing * params_.sigma_bearing));

        return bearing_error_prob;
      };

      return std::transform_reduce(detections.cbegin(), detections.cend(), 1.0, std::multiplies{}, detection_weight);
    };
  }

  void update_map(map_type&& map) { landmark_map_ = std::move(map); }

 private:
  param_type params_;
  LandmarkMap landmark_map_;
};

template <class LandmarkMap>
using BearingSensorModel2d = BearingSensorModel<LandmarkMap, Sophus::SE2d>;

template <class LandmarkMap>
using BearingSensorModel3d = BearingSensorModel<LandmarkMap, Sophus::SE3d>;

}  // namespace beluga

#endif