Template Function beluga::estimate(Poses&&)

Function Documentation

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>> beluga::estimate(Poses &&poses)

Returns a pair consisting of the estimated mean pose and its covariance.

Given a range of poses, computes the estimated pose by averaging the translation and rotation parts, assuming all poses are equally weighted. Computes the covariance matrix of the translation parts and the circular variance of the rotation angles to create a 3x3 covariance matrix. It does not take into account the particle weights. This is appropriate for use with filter update cycles that resample the particle set at every iteration, since in that case the belief is fully represented by the spatial distribution of the particles, and the particle weights provide no additional information.

Template Parameters:

  • Poses – A sized range type whose value type is Sophus::SE2<Scalar>.

  • Pose – The pose value type of the given range.

  • Scalar – A scalar type, e.g. double or float.

Parameters:

poses – Poses of equally weighted 2D poses.

Returns:

The estimated pose and its 3x3 covariance matrix.