Template Function beluga::estimate(const Poses&, const Weights&)
Defined in File estimation.hpp
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.