Template Function beluga::estimate(const 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::SE3<Scalar>>>>
std::pair<Sophus::SE3<Scalar>, Sophus::Matrix6<Scalar>> beluga::estimate(const 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. Computes the covariance matrix of the translation and rotation parts to create a 6x6 covariance matrix, assuming all the poses are equally weighted.

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

Returns:

The estimated pose and its 6x6 covariance matrix.