PoseManifoldGaussian.h
Go to the documentation of this file.
1 #pragma once
2
3 #include <Eigen/Core>
4 #include <Eigen/Geometry>
5
6
7 namespace armarx::objpose
8 {
9
10  /**
11  * @brief A "gaussian" distribution in pose space (i.e. pose manifold).
12  *
13  * The mean is a specific pose, represented as 4x4 homogeneous matrix.
14  *
15  * The covariance is a 6 x 6 covariance matrix.
16  * The first 3 dimensions represent the position (translation) part,
17  * the second 3 dimensions represent the orientation (rotation) part.
18  * A column of the pure orientational part (lower right 3 x 3 matrix)
19  * is a scaled rotation axis, i.e. rotation axis whose norm is the
20  * rotation angle (in this case the "angle variance").
21  *
22  * Note that the lower left and upper right 3x3 parts of the covariance
23  * matrix can be non-zero.
24  */
26  {
27  public:
28
29  /// The mean (i.e. a pose).
31  /// The covariance matrix.
33
34
35  public:
36
37  /// Get the pure positional part of the covariance matrix.
38  Eigen::Block<Eigen::Matrix<float, 6, 6>, 3, 3>
40  {
41  return covariance.block<3, 3>(0, 0);
42  }
43  Eigen::Block<const Eigen::Matrix<float, 6, 6>, 3, 3>
45  {
46  return covariance.block<3, 3>(0, 0);
47  }
48
49  /// Get the pure orientational part of the covariance matrix.
50  Eigen::Block<Eigen::Matrix<float, 6, 6>, 3, 3>
52  {
53  return covariance.block<3, 3>(3, 3);
54  }
55  Eigen::Block<const Eigen::Matrix<float, 6, 6>, 3, 3>
57  {
58  return covariance.block<3, 3>(3, 3);
59  }
60
61
62  struct Ellipsoid
63  {
64  Eigen::Vector3f center;
66  Eigen::Vector3f size;
67  };
68
69  /// Get the parameters of a 3D ellipsoid illustrating this gaussian.
71  getPositionEllipsoid() const;
72
73
74  /**
75  * @brief Get a column of the pure orientational covariance matrix
76  * as axis-angle rotation.
77  *
78  * @param index
79  * The column's index.
80  * @param global
81  * If true, rotate the axis by the mean orientation.
82  * @return
83  */
84  Eigen::AngleAxisf
85  getScaledRotationAxis(int index, bool global = false) const;
86
87
88  /**
89  * @brief Transform this gaussian by the given transform (e.g. to another frame).
90  * @param transform The transform to apply.
91  * @return The transformed gaussian.
92  */
95
96  /**
97  * @brief Transform this gaussian from one base coordinate system to another one.
98  * @param fromFrame The original (current) base coordinate system' pose.
99  * @param toFrame The new base coordinate system' pose.
100  * @return The same gaussian in thenew coordinate system.
101  */
103  getTransformed(const Eigen::Matrix4f& fromFrame, const Eigen::Matrix4f& toFrame) const;
104
105  };
106
107 }
armarx::objpose::PoseManifoldGaussian::Ellipsoid::orientation
Eigen::Matrix3f orientation
Definition: PoseManifoldGaussian.h:65
index
uint8_t index
Definition: EtherCATFrame.h:59
armarx::objpose::PoseManifoldGaussian::mean
Eigen::Matrix4f mean
The mean (i.e. a pose).
Definition: PoseManifoldGaussian.h:30
armarx::objpose::PoseManifoldGaussian::Ellipsoid::size
Eigen::Vector3f size
Definition: PoseManifoldGaussian.h:66
armarx::objpose::PoseManifoldGaussian
A "gaussian" distribution in pose space (i.e.
Definition: PoseManifoldGaussian.h:25
armarx::objpose::PoseManifoldGaussian::covariance
Eigen::Matrix< float, 6, 6 > covariance
The covariance matrix.
Definition: PoseManifoldGaussian.h:32
armarx::objpose::PoseManifoldGaussian::getScaledRotationAxis
Eigen::AngleAxisf getScaledRotationAxis(int index, bool global=false) const
Get a column of the pure orientational covariance matrix as axis-angle rotation.
Definition: PoseManifoldGaussian.cpp:46
armarx::objpose::PoseManifoldGaussian::getTransformed
PoseManifoldGaussian getTransformed(const Eigen::Matrix4f &transform) const
Transform this gaussian by the given transform (e.g.
Definition: PoseManifoldGaussian.cpp:60
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
armarx::objpose::PoseManifoldGaussian::getPositionEllipsoid
Ellipsoid getPositionEllipsoid() const
Get the parameters of a 3D ellipsoid illustrating this gaussian.
Definition: PoseManifoldGaussian.cpp:11
armarx::objpose
This file is part of ArmarX.
Definition: objpose.h:7
armarx::objpose::PoseManifoldGaussian::orientationCovariance
Eigen::Block< Eigen::Matrix< float, 6, 6 >, 3, 3 > orientationCovariance()
Get the pure orientational part of the covariance matrix.
Definition: PoseManifoldGaussian.h:51
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:600
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
armarx::transform
auto transform(const Container< InputT, Alloc > &in, OutputT(*func)(InputT const &)) -> Container< OutputT, typename std::allocator_traits< Alloc >::template rebind_alloc< OutputT > >
Convenience function (with less typing) to transform a container of type InputT into the same contain...
Definition: algorithm.h:315
armarx::objpose::PoseManifoldGaussian::Ellipsoid::center
Eigen::Vector3f center
Definition: PoseManifoldGaussian.h:64
Eigen::Matrix< float, 6, 6 >
armarx::objpose::PoseManifoldGaussian::positionCovariance
Eigen::Block< Eigen::Matrix< float, 6, 6 >, 3, 3 > positionCovariance()
Get the pure positional part of the covariance matrix.
Definition: PoseManifoldGaussian.h:39
armarx::objpose::PoseManifoldGaussian::positionCovariance
Eigen::Block< const Eigen::Matrix< float, 6, 6 >, 3, 3 > positionCovariance() const
Definition: PoseManifoldGaussian.h:44
armarx::objpose::PoseManifoldGaussian::orientationCovariance
Eigen::Block< const Eigen::Matrix< float, 6, 6 >, 3, 3 > orientationCovariance() const
Definition: PoseManifoldGaussian.h:56