pytransform3d.uncertainty.concat_globally_uncertain_transforms#

pytransform3d.uncertainty.concat_globally_uncertain_transforms(mean_A2B, cov_A2B, mean_B2C, cov_B2C)[source]#

Concatenate two independent globally uncertain transformations.

We assume that the two distributions are independent.

Each of the two transformations is globally uncertain (not in the local / body frame), that is, samples are generated through

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

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_locally_uncertain_transforms().

Hence, the full model is

\[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 \(_B\boldsymbol{\xi} \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{\Sigma}_{BA})\), \(_C\boldsymbol{\xi} \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{\Sigma}_{CB})\), and \(_C\boldsymbol{\xi'} \sim \mathcal{N}(\boldsymbol{0}, \boldsymbol{\Sigma}_{CA})\).

This version of Barfoot and Furgale [1] approximates the covariance up to 4th-order terms. Note that it is still an approximation of the covariance after concatenation of the two transforms.

Parameters:
mean_A2Barray, shape (4, 4)

Mean of transform from A to B.

cov_A2Barray, shape (6, 6)

Covariance of transform from A to B. Models uncertainty in frame B.

mean_B2Carray, shape (4, 4)

Mean of transform from B to C.

cov_B2Carray, shape (6, 6)

Covariance of transform from B to C. Models uncertainty in frame C.

Returns:
mean_A2Carray, shape (4, 4)

Mean of new pose.

cov_A2Carray, shape (6, 6)

Covariance of new pose. Models uncertainty in frame C.

See also

concat_locally_uncertain_transforms

Concatenate two independent locally uncertain transformations.

pytransform3d.transformations.concat

Concatenate two transformations.

References

[1]

Barfoot, T. D., Furgale, P. T. (2014). Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems. IEEE Transactions on Robotics, 30(3), pp. 679-693, doi: 10.1109/TRO.2014.2298059.

Examples using pytransform3d.uncertainty.concat_globally_uncertain_transforms#

Concatenate Uncertain Transforms

Concatenate Uncertain Transforms