Program Listing for File estimation.hpp
↰ Return to documentation for file (include/beluga/algorithm/estimation.hpp)
// Copyright 2022-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_ALGORITHM_ESTIMATION_HPP
#define BELUGA_ALGORITHM_ESTIMATION_HPP
#include <range/v3/algorithm/count_if.hpp>
#include <range/v3/numeric/accumulate.hpp>
#include <range/v3/range/access.hpp>
#include <range/v3/range/primitives.hpp>
#include <range/v3/view/common.hpp>
#include <range/v3/view/repeat_n.hpp>
#include <range/v3/view/transform.hpp>
#include <sophus/se2.hpp>
#include <sophus/types.hpp>
#include <numeric>
namespace beluga {
template <class Range, class WeightsRange, class Scalar>
Sophus::Matrix2<Scalar>
calculate_covariance(Range&& range, WeightsRange&& normalized_weights, const Sophus::Vector2<Scalar>& mean) {
auto translations_view = range | ranges::views::common;
auto normalized_weights_view = normalized_weights | ranges::views::common;
auto calculate_weighted_sample_covariance = [mean](const auto& value, const auto& weight) {
const auto centered = value - mean;
return Sophus::Vector3<Scalar>{
weight * centered.x() * centered.x(), // weighted sample x autocovariance
weight * centered.x() * centered.y(), // weighted sample xy cross-covariance
weight * centered.y() * centered.y(), // weighted sample y autocovariance
};
};
// calculate the averaging factor for the weighted covariance estimate
// See https://en.wikipedia.org/wiki/Sample_mean_and_covariance#Weighted_samples
const auto squared_weight_sum = std::transform_reduce(
normalized_weights_view.begin(), normalized_weights_view.end(), Scalar{0.0}, std::plus{},
[](const auto& weight) { return (weight * weight); });
// calculate the x autocovariance, xy cross-covariance, and y autocovariance
Sophus::Vector3<Scalar> coefficients =
std::transform_reduce(
translations_view.begin(), translations_view.end(), normalized_weights_view.begin(),
Sophus::Vector3<Scalar>::Zero().eval(), std::plus{}, calculate_weighted_sample_covariance) /
(1.0 - squared_weight_sum);
// create the symmetric 2x2 translation covariance matrix from the coefficients
auto covariance_matrix = Sophus::Matrix2<Scalar>{};
covariance_matrix << coefficients(0), coefficients(1), coefficients(1), coefficients(2);
return covariance_matrix;
}
template <class Range, class Scalar>
Sophus::Matrix2<Scalar> calculate_covariance(Range&& range, const Sophus::Vector2<Scalar>& mean) {
const auto sample_count = range.size();
return calculate_covariance(
range,
ranges::views::repeat_n(1.0 / static_cast<Scalar>(sample_count), static_cast<std::ptrdiff_t>(sample_count)),
mean);
}
template <
class Poses,
class Weights,
class Pose = ranges::range_value_t<Poses>,
class Scalar = typename Pose::Scalar,
typename = std::enable_if_t<std::is_same_v<Pose, typename Sophus::SE2<Scalar>>>>
std::pair<Sophus::SE2<Scalar>, Sophus::Matrix3<Scalar>> estimate(Poses&& poses, Weights&& weights) {
auto translation_view = poses | ranges::views::transform([](const auto& pose) { return pose.translation(); });
auto weights_view = weights | ranges::views::common;
const auto weights_sum = std::accumulate(weights_view.begin(), weights_view.end(), 0.0);
auto normalized_weights_view =
weights_view | ranges::views::transform([weights_sum](const auto& weight) { return weight / weights_sum; });
// map sophus pose 2D pose into a 4D Eigen vector. Mapping exploits that Sophus stores the 2D transform
// as two elements for the linear translation, and two more for the orientation (in complex number form)
const auto pose_to_weighted_eigen_vector = [](const auto& pose, const auto& weight) {
return Eigen::Map<const Sophus::Vector4<Scalar>>{pose.data()} * weight;
};
// Compute the average of all the coefficients of the SE2 group elements and construct a new SE2 element. Notice
// that after averaging the complex representation of the orientation the resulting complex is not on the unit circle.
// This is expected and the value will be renormalized after having used the non-normal result to estimate the
// orientation autocovariance.
const Sophus::Vector4<Scalar> mean_pose_vector = std::transform_reduce(
poses.begin(), poses.end(), normalized_weights_view.begin(), Sophus::Vector4<Scalar>::Zero().eval(), std::plus{},
pose_to_weighted_eigen_vector);
// Calculate the weighted pose estimation
Sophus::SE2<Scalar> estimated_pose = Eigen::Map<const Sophus::SE2<Scalar>>{mean_pose_vector.data()};
Sophus::Matrix3<Scalar> covariance_matrix = Sophus::Matrix3<Scalar>::Zero();
// Compute the covariance of the translation part.
covariance_matrix.template topLeftCorner<2, 2>() =
calculate_covariance(translation_view, normalized_weights_view, estimated_pose.translation());
// Compute the orientation variance and re-normalize the rotation component.
if (estimated_pose.so2().unit_complex().norm() < std::numeric_limits<double>::epsilon()) {
// Handle the case where both averages are too close to zero.
// Return zero yaw and infinite variance.
covariance_matrix.coeffRef(2, 2) = std::numeric_limits<double>::infinity();
estimated_pose.so2() = Sophus::SO2<Scalar>{0.0};
} else {
// See circular standard deviation in
// https://en.wikipedia.org/wiki/Directional_statistics#Dispersion.
covariance_matrix.coeffRef(2, 2) = -2.0 * std::log(estimated_pose.so2().unit_complex().norm());
estimated_pose.so2().normalize();
}
return std::pair{estimated_pose, covariance_matrix};
}
template <
class Scalars,
class Weights,
class Scalar = ranges::range_value_t<Scalars>,
typename = std::enable_if_t<std::is_arithmetic_v<Scalar>>>
std::pair<Scalar, Scalar> estimate(Scalars&& scalars, Weights&& weights) {
auto weights_view = weights | ranges::views::common;
const auto weights_sum = ranges::accumulate(weights, 0.0, std::plus<>{});
auto normalized_weights_view =
weights_view | ranges::views::transform([weights_sum](auto weight) { return weight / weights_sum; });
const Scalar weighted_mean = std::transform_reduce(
scalars.begin(), scalars.end(), normalized_weights_view.begin(), 0.0, std::plus<>{}, std::multiplies<>{});
const Scalar weighted_squared_deviations = std::transform_reduce(
scalars.begin(), scalars.end(), normalized_weights_view.begin(), 0.0, std::plus<>{},
[weighted_mean](const auto& scalar, const auto& weight) {
return weight * (scalar - weighted_mean) * (scalar - weighted_mean);
});
const auto number_of_non_zero_weights =
static_cast<Scalar>(ranges::count_if(weights_view, [&](auto weight) { return weight > 0; }));
const Scalar weighted_sd =
std::sqrt(weighted_squared_deviations * number_of_non_zero_weights / (number_of_non_zero_weights - 1));
return std::pair{weighted_mean, weighted_sd};
}
template <
class Poses,
class Pose = ranges::range_value_t<Poses>,
class Scalar = typename Pose::Scalar,
typename = std::enable_if_t<std::is_same_v<Pose, typename Sophus::SE2<Scalar>>>>
std::pair<Sophus::SE2<Scalar>, Sophus::Matrix3<Scalar>> estimate(Poses&& poses) {
return beluga::estimate(poses, ranges::views::repeat_n(1.0, static_cast<std::ptrdiff_t>(poses.size())));
}
} // namespace beluga
#endif