Program Listing for File landmark_sensor_model.hpp
↰ Return to documentation for file (include/beluga/sensor/landmark_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_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