Template Function beluga::estimate(const Poses&, const 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::SE3<Scalar>>>>
std::pair<Sophus::SE3<Scalar>, Sophus::Matrix6<Scalar>> beluga::estimate(const Poses &poses, const 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 and rotation parts to create a 6x6 covariance matrix. NOTE: We represent this estimate as a large

noiseless SE3 pose and a covariance in se3, the tangent space of the SE3 manifold. Users may perform the appropriate conversions to get the covariance matrix into their parametrization of interest. This reasoning is based on “Characterizing the Uncertainty of Jointly

Distributed Poses in the Lie Algebra” by Mangelson et al. (

https://robots.et.byu.edu/jmangelson/pubs/2020/mangelson2020tro.pdf

). See section III. E) “Defining Random Variables

over Poses” for more context.

Template Parameters:

  • Poses – A sized range type whose value type is Sophus::SE3<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 – 3D poses to estimate mean and covariances from.

  • weights – Weights for the poses with matching indices.

Returns:

The estimated pose and its 6x6 covariance matrix.