pytransform3d.uncertainty.concat_locally_uncertain_transforms#

pytransform3d.uncertainty.concat_locally_uncertain_transforms(mean_A2B, mean_B2C, cov_A, cov_B)[source]#

Concatenate two independent locally uncertain transformations.

We assume that the two distributions are independent.

Each of the two transformations is locally uncertain (not in the global / world frame), that is, samples are generated through

\[\boldsymbol{T} = \overline{\boldsymbol{T}} Exp(\boldsymbol{\xi}),\]

where \(\boldsymbol{T} \in SE(3)\) is a sampled transformation matrix, \(\overline{\boldsymbol{T}} \in SE(3)\) is the mean transformation, and \(\boldsymbol{\xi} \in \mathbb{R}^6\) are exponential coordinates of transformations and are distributed according to a Gaussian distribution with zero mean and covariance \(\boldsymbol{\Sigma} \in \mathbb{R}^{6 \times 6}\), that is, \(\boldsymbol{\xi} \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{\Sigma})\).

The concatenation order is the same as in concat(), that is, the transformation B2C is left-multiplied to A2B. Note that the order of arguments is different from concat_globally_uncertain_transforms().

Hence, the full model is

\[\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 \(_B\boldsymbol{\xi} \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{\Sigma}_B)\), \(_A\boldsymbol{\xi} \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{\Sigma}_A)\), and \(_A\boldsymbol{\xi'} \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{\Sigma}_{A,total})\).

This version of Meyer et al. [1] approximates the covariance up to 2nd-order terms.

Parameters:
mean_A2Barray, shape (4, 4)

Mean of transform from A to B: \(\overline{\boldsymbol{T}}_{BA}\).

mean_B2Carray, shape (4, 4)

Mean of transform from B to C: \(\overline{\boldsymbol{T}}_{CB}\).

cov_Aarray, shape (6, 6)

Covariance of noise in frame A: \(\boldsymbol{\Sigma}_A\). Noise samples are right-multiplied with the mean transform A2B.

cov_Barray, shape (6, 6)

Covariance of noise in frame B: \(\boldsymbol{\Sigma}_B\). Noise samples are right-multiplied with the mean transform B2C.

Returns:
mean_A2Carray, shape (4, 4)

Mean of new pose.

cov_A_totalarray, shape (6, 6)

Covariance of accumulated noise in frame A.

See also

concat_globally_uncertain_transforms

Concatenate two independent globally uncertain transformations.

pytransform3d.transformations.concat

Concatenate two transformations.

References

[1]

Meyer, L., Strobl, K. H., Triebel, R. (2022). The Probabilistic Robot Kinematics Model and its Application to Sensor Fusion. In IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Kyoto, Japan (pp. 3263-3270), doi: 10.1109/IROS47612.2022.9981399. https://elib.dlr.de/191928/1/202212_ELIB_PAPER_VERSION_with_copyright.pdf

Examples using pytransform3d.uncertainty.concat_locally_uncertain_transforms#

Probabilistic Product of Exponentials

Probabilistic Product of Exponentials