Template Function beluga::estimate(Poses&&, Weights&&)

Function Documentation

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

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. 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.

Template Parameters:

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

  • Weights – A sized range type whose value type is 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.

  • weights – Poses of equally weighted 2D poses.

Returns:

The estimated pose and its 3x3 covariance matrix.