bearing_sensor_model.hpp

Go to the documentation of this file.

// 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