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