Write user guide for uncertainty module

doc/source/user_guide/index.rst

@@ -14,4 +14,5 @@ User Guide
    transformation_modeling
    transform_manager
    transformation_over_time
+   uncertainty
    camera

doc/source/user_guide/uncertainty.rst

@@ -0,0 +1,97 @@
+==============================
+Uncertainty in Transformations
+==============================
+
+In computer vision and robotics we are never absolutely certain about
+transformations. It is often a good idea to express the uncertainty explicitly
+and use it. The module :mod:`pytransform3d.uncertainty` offers tools to handle
+with uncertain transformations.
+
+----------------------------------------
+Gaussian Distribution of Transformations
+----------------------------------------
+
+Uncertain transformations are often represented by Gaussian distributions,
+where the mean of the distribution is represented by a transformation matrix
+:math:`\boldsymbol{T} \in SE(3)` and the covariance is defined in the tangent space
+through exponential coordinates
+:math:`\boldsymbol{\Sigma} \in \mathbb{R}^{6 \times 6}`.
+
+We can use
+:func:`~pytransform3d.uncertainty.estimate_gaussian_transform_from_samples`
+to estimate a Gaussian distribution of transformations. We can sample from
+a known Gaussian distribution of transformations with
+:func:`~pytransform3d.transformations.random_transform` as illustrated in
+the Example :ref:`sphx_glr__auto_examples_plots_plot_sample_transforms.py`.
+
+.. figure:: ../_auto_examples/plots/images/sphx_glr_plot_sample_transforms_001.png
+   :target: ../_auto_examples/plots/plot_sample_transforms.html
+   :align: center
+
+----------------------------
+Visualization of Uncertainty
+----------------------------
+
+A typical visual representation of Gaussian distributions in 3D coordinates
+are equiprobable ellipsoids (obtained with
+:func:`~pytransform3d.uncertainty.to_ellipsoid`). This is equivalent to showing
+the :math:`k\sigma, k \in \mathbb{R}` intervals of a 1D Gaussian distribution.
+However, for transformations are also interactions between rotation and
+translation components so that an ellipsoid is not an appropriate
+representation to visualize the distribution of transformations in 3D. We have
+to project a 6D hyper-ellipsoid to 3D (for which we can use
+:func:`~pytransform3d.uncertainty.to_projected_ellipsoid`), which
+can result in banana-like shapes.
+
+------------------------------------------
+Concatenation of Uncertain Transformations
+------------------------------------------
+
+There are two different ways of defining uncertainty of transformations when
+we concatenate multiple transformations. We can define uncertainty
+in the global frame of reference or in the local frame of reference
+and it makes a difference.
+
+Global frame of reference
+(:func:`~pytransform3d.uncertainty.concat_globally_uncertain_transforms`):
+
+.. math::
+
+   Exp(_C\boldsymbol{\xi'}) \overline{\boldsymbol{T}}_{CA} = Exp(_C\boldsymbol{\xi}) \overline{\boldsymbol{T}}_{CB} Exp(_B\boldsymbol{\xi}) \overline{\boldsymbol{T}}_{BA},
+
+where :math:`_B\boldsymbol{\xi} \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{\Sigma}_{BA})`,
+:math:`_C\boldsymbol{\xi} \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{\Sigma}_{CB})`,
+and :math:`_C\boldsymbol{\xi'} \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{\Sigma}_{CA})`.
+
+This method of concatenating uncertain transformation is used in Example
+:ref:`sphx_glr__auto_examples_plots_plot_concatenate_uncertain_transforms.py`,
+which illustrates how the banana distribution is generated.
+
+.. figure:: ../_auto_examples/plots/images/sphx_glr_plot_concatenate_uncertain_transforms_001.png
+   :target: ../_auto_examples/plots/plot_concatenate_uncertain_transforms.html
+   :align: center
+
+Local frame of reference
+(:func:`~pytransform3d.uncertainty.concat_locally_uncertain_transforms`):
+
+.. math::
+
+   \overline{\boldsymbol{T}}_{CA} Exp(_A\boldsymbol{\xi'}) = \overline{\boldsymbol{T}}_{CB} Exp(_B\boldsymbol{\xi}) \overline{\boldsymbol{T}}_{BA} Exp(_A\boldsymbol{\xi}),
+
+where :math:`_B\boldsymbol{\xi} \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{\Sigma}_B)`,
+:math:`_A\boldsymbol{\xi} \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{\Sigma}_A)`,
+and :math:`_A\boldsymbol{\xi'} \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{\Sigma}_{A,total})`.
+
+This method of concatenating uncertain transformations is used in Example
+:ref:`sphx_glr__auto_examples_visualizations_vis_probabilistic_robot_kinematics.py`,
+which illustrates probabilistic robot kinematics.
+
+.. figure:: ../_auto_examples/visualizations/images/sphx_glr_vis_probabilistic_robot_kinematics_001.png
+   :target: ../_auto_examples/visualizations/vis_probabilistic_robot_kinematics.html
+   :align: center
+
+-------------------------
+Fusion of Uncertain Poses
+-------------------------
+
+TODO

examples/visualizations/vis_probabilistic_robot_kinematics.py

@@ -215,7 +215,7 @@ def geometries(self):
 
 def animation_callback(
         step, n_frames, tm, graph, joint_names, thetas, covs, surface):
-    angle = 0.5 * np.cos(2.0 * np.pi * (step / n_frames))
+    angle = 0.5 * np.cos(2.0 * np.pi * (0.5 + step / n_frames))
     thetas_t = angle * thetas
     for joint_name, value in zip(joint_names, thetas_t):
         tm.set_joint(joint_name, value)
@@ -262,7 +262,7 @@ def animation_callback(
 fig.plot_transform(np.eye(4), s=0.3)
 surface = Surface(x, y, z, c=(0, 0.5, 0.5))
 surface.add_artist(fig)
-fig.view_init(elev=20)
+fig.view_init(elev=-20)
 n_frames = 200
 if "__file__" in globals():
     fig.animate(animation_callback, n_frames, loop=True,

pytransform3d/uncertainty.py

@@ -1,4 +1,7 @@
-"""Operations related to uncertain transformations."""
+"""Operations related to uncertain transformations.
+
+See :doc:`user_guide/uncertainty` for more information.
+"""
 import numpy as np
 from .transformations import (
     invert_transform, transform_from, concat, adjoint_from_transform,