iDynTree/SO3Utils.h file

Classes

struct iDynTree::GeodesicL2MeanOptions: Struct containing the options for geodesicL2MeanRotation and geodesicL2WeightedMeanRotation.

auto isValidRotationMatrix(const iDynTree::Rotation& r) -> bool: Check if the rotation matrix is valid.
auto geodesicL2Distance(const iDynTree::Rotation& rotation1, const iDynTree::Rotation& rotation2) -> double: Computes the geodesic distance between two rotation matrices.
auto geodesicL2MeanRotation(const std::vector<iDynTree::Rotation>& inputRotations, iDynTree::Rotation& meanRotation, const GeodesicL2MeanOptions& options = GeodesicL2MeanOptions()) -> bool: Computes the geodesic mean amongst the provided rotations.
auto geodesicL2WeightedMeanRotation(const std::vector<iDynTree::Rotation>& inputRotations, const std::vector<double>& weights, iDynTree::Rotation& meanRotation, const GeodesicL2MeanOptions& options = GeodesicL2MeanOptions()) -> bool: Computes the weighted geodesic mean amongst the provided rotations.

Check if the rotation matrix is valid.

Parameters
r	The input rotation
Returns	True if it is a rotation matrix.

It checks that the determinant is 1, that the Frobenius norm is finite and that it is orthogonal.

Computes the geodesic distance between two rotation matrices.

Parameters
rotation1	The first rotation.
rotation2	The other rotation.
Returns	the geodesic L2 distance between the two rotation matrices.

It implements the angular distance presented in Sec. 4 of "Rotation Averaging" (available at http://users.cecs.anu.edu.au/~hongdong/rotationaveraging.pdf), in particular .

Computes the geodesic mean amongst the provided rotations.

Parameters
inputRotations	The rotations to average.
meanRotation	The mean rotation.
options	The options for the inner optimization (refinement) loop.
Returns	false in case of failure, true otherwise.

It implements Algorithm 1 in Sec. 5.3 of "Rotation Averaging" (available at http://users.cecs.anu.edu.au/~hongdong/rotationaveraging.pdf).

Inside it calls geodesicL2WeightedMeanRotation.

Computes the weighted geodesic mean amongst the provided rotations.

Parameters
inputRotations	The rotations to average.
weights	The weights for each rotation. If this vector is null assumes that each weight is 1.0 (equivalent to geodesicL2MeanRotation)
meanRotation	The weighted mean rotation.
options	The options for the inner optimization (refinement) loop.
Returns	false in case of failure, true otherwise.

It implements Algorithm 1 in Sec. 5.3 of "Rotation Averaging" (available at http://users.cecs.anu.edu.au/~hongdong/rotationaveraging.pdf), with a small modification to take into accounts weights different from 1.