landmark_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_LANDMARK_SENSOR_MODEL_HPP
#define BELUGA_SENSOR_LANDMARK_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/so2.hpp>
 
// standard library
#include <cmath>
#include <random>
#include <vector>
 
// project
#include <beluga/types/landmark_detection_types.hpp>
 
namespace beluga {
 
 
struct LandmarkModelParam {
  double sigma_range{1.0};    
  double sigma_bearing{1.0};  
};
 
 
template <class LandmarkMap, class StateType>
class LandmarkSensorModel {
 public:
  using state_type = StateType;
  using weight_type = double;
  using measurement_type = std::vector<LandmarkPositionDetection>;
  using map_type = LandmarkMap;
  using param_type = LandmarkModelParam;
 
 
  explicit LandmarkSensorModel(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}};
      }
 
      const auto detection_weight = [this, &robot_pose_in_world](const auto& detection) {
        // calculate range and detection_bearing_in_robot to the detection from the robot
        // the detection is already in robot frame
        const auto& detection_category = detection.category;
        const auto& detection_position_in_robot = detection.detection_position_in_robot;
 
        const auto detection_range_in_robot = detection_position_in_robot.norm();
        const auto detection_bearing_in_robot = detection_position_in_robot.normalized();
 
        // convert the detection to the world frame to query the map
        const auto detection_position_in_world = robot_pose_in_world * detection_position_in_robot;
 
        // find the closest matching landmark in the world map
        const auto opt_landmark_position_in_world =
            landmark_map_.find_nearest_landmark(detection_position_in_world, detection_category);
 
        // if we did not find a matching landmark, return 0.0
        if (!opt_landmark_position_in_world) {
          return 0.0;
        }
 
        // convert landmark pose to world frame
        // ignore height, because we are modelling the detection in 2D
        const auto& landmark_position_in_world = *opt_landmark_position_in_world;
        const auto landmark_in_robot_position = robot_pose_in_world.inverse() * landmark_position_in_world;
 
        const auto landmark_range_in_robot = landmark_in_robot_position.norm();
        const auto landmark_bearing_in_robot = landmark_in_robot_position.normalized();
 
        // calculate the aperture angle between the detection and the landmark
        const auto cos_aperture = landmark_bearing_in_robot.dot(detection_bearing_in_robot);
        const auto sin_aperture = landmark_bearing_in_robot.cross(detection_bearing_in_robot).norm();
        const auto bearing_error = std::atan2(sin_aperture, cos_aperture);
 
        const auto range_error = detection_range_in_robot - landmark_range_in_robot;
 
        // apply the error model from Probabilistic Robotics
        const auto range_error_prob =
            std::exp(-range_error * range_error / (2. * params_.sigma_range * params_.sigma_range));
        const auto bearing_error_prob =
            std::exp(-bearing_error * bearing_error / (2. * params_.sigma_bearing * params_.sigma_bearing));
 
        // We'll assume the probability of identification error to be zero
        return range_error_prob * 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 LandmarkSensorModel2d = LandmarkSensorModel<LandmarkMap, Sophus::SE2d>;
 
template <class LandmarkMap>
using LandmarkSensorModel3d = LandmarkSensorModel<LandmarkMap, Sophus::SE3d>;
 
}  // namespace beluga
 
#endif