Program Listing for File beam_model.hpp
↰ Return to documentation for file (include/beluga/sensor/beam_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_BEAM_MODEL_HPP
#define BELUGA_SENSOR_BEAM_MODEL_HPP
#include <algorithm>
#include <cmath>
#include <random>
#include <vector>
#include <beluga/algorithm/raycasting.hpp>
#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>
namespace beluga {
struct BeamModelParam {
double z_hit{0.5};
double z_short{0.5};
double z_max{0.05};
double z_rand{0.05};
double sigma_hit{0.2};
double lambda_short{0.1};
double beam_max_range{60};
};
template <class OccupancyGrid>
class BeamSensorModel {
public:
using state_type = Sophus::SE2d;
using weight_type = double;
using measurement_type = std::vector<std::pair<double, double>>;
using map_type = OccupancyGrid;
using param_type = BeamModelParam;
explicit BeamSensorModel(const param_type& params, OccupancyGrid grid) : params_{params}, grid_{std::move(grid)} {}
[[nodiscard]] auto operator()(measurement_type&& points) const {
return [this, points = std::move(points)](const state_type& state) -> weight_type {
const auto beam = Ray2d{grid_, state, params_.beam_max_range};
const double n = 1. / (std::sqrt(2. * M_PI) * params_.sigma_hit);
return std::transform_reduce(
points.cbegin(), points.cend(), 0.0, std::plus{}, [this, &beam, n](const auto& point) {
// TODO(Ramiro): We're converting from range + bearing to cartesian points in the ROS node, but we want
// range
// + bearing here. We might want to make that conversion in the likelihood model instead, and let the
// measurement type be range, bearing instead of x, y.
// Compute the range according to the measurement.
const double z = std::sqrt(point.first * point.first + point.second * point.second);
// dirty hack to prevent SO2d from calculating the hypot again to normalize the vector.
auto beam_bearing = Sophus::SO2d{};
beam_bearing.data()[0] = point.first / z;
beam_bearing.data()[1] = point.second / z;
// Compute range according to the map (raycasting).
const double z_mean = beam.cast(beam_bearing).value_or(params_.beam_max_range);
// 1: Correct range with local measurement noise.
const double eta_hit =
2. / (std::erf((params_.beam_max_range - z_mean) / (std::sqrt(2.) * params_.sigma_hit)) -
std::erf(-z_mean / (std::sqrt(2.) * params_.sigma_hit)));
const double d = (z - z_mean) / params_.sigma_hit;
double pz = params_.z_hit * eta_hit * n * std::exp(-(d * d) / 2.);
// 2: Unexpected objects.
if (z < z_mean) {
const double eta_short = 1. / (1. - std::exp(-params_.lambda_short * z_mean));
pz += params_.z_short * params_.lambda_short * eta_short * std::exp(-params_.lambda_short * z);
}
// 3 and 4: Max range return or random return.
if (z < params_.beam_max_range) {
pz += params_.z_rand / params_.beam_max_range;
} else {
pz += params_.z_max;
}
// TODO(glpuga): Investigate why AMCL and QuickMCL both use this formula for the weight.
// See https://github.com/Ekumen-OS/beluga/issues/153
return pz * pz * pz;
});
};
}
void update_map(map_type&& map) { grid_ = std::move(map); }
private:
param_type params_;
OccupancyGrid grid_;
};
} // namespace beluga
#endif