0
# Uncertainty
1

2
Operations on uncertain transformations with statistical estimation, covariance propagation, and sensor fusion for handling measurement uncertainties in 3D transformations.
3

4
## Capabilities
5

6
### Uncertain Transformation Composition
7

8
Functions for propagating uncertainty through transformation operations.
9

10
```python { .api }
11
def concat_globally_uncertain_transforms(A2B, cov_A2B, B2C, cov_B2C):
12
    """
13
    Concatenate transformations with global uncertainty propagation.
14
    
15
    Parameters:
16
    - A2B: array, shape (4, 4) - First transformation
17
    - cov_A2B: array, shape (6, 6) - Covariance of first transformation
18
    - B2C: array, shape (4, 4) - Second transformation  
19
    - cov_B2C: array, shape (6, 6) - Covariance of second transformation
20
    
21
    Returns:
22
    - A2C: array, shape (4, 4) - Composed transformation
23
    - cov_A2C: array, shape (6, 6) - Propagated covariance
24
    """
25

26
def concat_locally_uncertain_transforms(A2B, cov_A2B, B2C, cov_B2C):
27
    """
28
    Concatenate transformations with local uncertainty propagation.
29
    
30
    Parameters:
31
    - A2B: array, shape (4, 4) - First transformation
32
    - cov_A2B: array, shape (6, 6) - Local covariance of first transformation
33
    - B2C: array, shape (4, 4) - Second transformation
34
    - cov_B2C: array, shape (6, 6) - Local covariance of second transformation
35
    
36
    Returns:
37
    - A2C: array, shape (4, 4) - Composed transformation
38
    - cov_A2C: array, shape (6, 6) - Propagated local covariance
39
    """
40

41
def invert_uncertain_transform(A2B, cov_A2B):
42
    """
43
    Invert uncertain transformation with covariance propagation.
44
    
45
    Parameters:
46
    - A2B: array, shape (4, 4) - Transformation matrix
47
    - cov_A2B: array, shape (6, 6) - Covariance matrix
48
    
49
    Returns:
50
    - B2A: array, shape (4, 4) - Inverted transformation
51
    - cov_B2A: array, shape (6, 6) - Propagated covariance
52
    """
53
```
54

55
### Statistical Estimation
56

57
Functions for estimating mean transformations from samples with uncertainty quantification.
58

59
```python { .api }
60
def frechet_mean(poses, weights=None, init_pose=None, max_iter=100, tol=1e-9):
61
    """
62
    Compute Fréchet mean of pose samples.
63
    
64
    Parameters:
65
    - poses: array, shape (n_poses, 4, 4) - Transformation samples
66
    - weights: array, shape (n_poses,), optional - Sample weights
67
    - init_pose: array, shape (4, 4), optional - Initial estimate
68
    - max_iter: int - Maximum iterations
69
    - tol: float - Convergence tolerance
70
    
71
    Returns:
72
    - mean_pose: array, shape (4, 4) - Mean transformation
73
    """
74

75
def estimate_gaussian_transform_from_samples(A2Bs, max_iter=100):
76
    """
77
    Estimate Gaussian distribution parameters from transformation samples.
78
    
79
    Parameters:
80
    - A2Bs: array, shape (n_samples, 4, 4) - Transformation samples
81
    - max_iter: int - Maximum iterations for convergence
82
    
83
    Returns:
84
    - mean_A2B: array, shape (4, 4) - Mean transformation
85
    - cov_A2B: array, shape (6, 6) - Covariance matrix
86
    """
87

88
def estimate_gaussian_rotation_matrix_from_samples(Rs, max_iter=100):
89
    """
90
    Estimate mean rotation and covariance from rotation matrix samples.
91
    
92
    Parameters:
93
    - Rs: array, shape (n_samples, 3, 3) - Rotation matrix samples
94
    - max_iter: int - Maximum iterations
95
    
96
    Returns:
97
    - mean_R: array, shape (3, 3) - Mean rotation matrix
98
    - cov_R: array, shape (3, 3) - Rotation covariance
99
    """
100
```
101

102
### Sensor Fusion
103

104
Multi-sensor pose fusion with uncertainty-weighted combination.
105

106
```python { .api }
107
def pose_fusion(poses, covariances, check_inputs=True):
108
    """
109
    Fuse multiple pose estimates with uncertainties.
110
    
111
    Parameters:
112
    - poses: list of arrays, shape (4, 4) - Pose estimates
113
    - covariances: list of arrays, shape (6, 6) - Covariance matrices
114
    - check_inputs: bool - Validate input poses and covariances
115
    
116
    Returns:
117
    - fused_pose: array, shape (4, 4) - Fused pose estimate
118
    - fused_cov: array, shape (6, 6) - Fused covariance
119
    """
120
```
121

122
### Uncertainty Visualization
123

124
Functions for visualizing uncertainty as ellipsoids and projections.
125

126
```python { .api }
127
def to_ellipsoid(mean, cov, n_steps=20):
128
    """
129
    Convert covariance matrix to ellipsoid surface points.
130
    
131
    Parameters:
132
    - mean: array, shape (3,) - Mean position
133
    - cov: array, shape (3, 3) - Position covariance matrix
134
    - n_steps: int - Number of surface discretization steps
135
    
136
    Returns:
137
    - ellipsoid: array, shape (n_steps**2, 3) - Ellipsoid surface points
138
    """
139

140
def to_projected_ellipsoid(mean, cov, n_steps=20):
141
    """
142
    Project 3D uncertainty ellipsoid to 2D plane.
143
    
144
    Parameters:
145
    - mean: array, shape (3,) - Mean position
146
    - cov: array, shape (3, 3) - Position covariance
147
    - n_steps: int - Discretization steps
148
    
149
    Returns:
150
    - projection: array, shape (n_steps, 2) - 2D ellipse points
151
    """
152

153
def plot_projected_ellipsoid(ax, mean, cov, n_steps=20, **kwargs):
154
    """
155
    Plot projected uncertainty ellipsoid on 2D axis.
156
    
157
    Parameters:
158
    - ax: matplotlib axis - 2D plotting axis
159
    - mean: array, shape (3,) - Mean position
160
    - cov: array, shape (3, 3) - Position covariance
161
    - n_steps: int - Ellipse discretization
162
    """
163
```
164

165
## Usage Examples
166

167
### Uncertain Transform Composition
168

169
```python
170
import numpy as np
171
import pytransform3d.uncertainty as pu
172
import pytransform3d.transformations as pt
173

174
# Create two uncertain transformations
175
T1 = pt.transform_from(p=[1, 0, 0])
176
cov1 = np.diag([0.01, 0.01, 0.01, 0.001, 0.001, 0.001])  # [translation, rotation] variances
177

178
T2 = pt.transform_from(p=[0, 1, 0]) 
179
cov2 = np.diag([0.02, 0.02, 0.02, 0.002, 0.002, 0.002])
180

181
# Compose with uncertainty propagation
182
T_composed, cov_composed = pu.concat_globally_uncertain_transforms(T1, cov1, T2, cov2)
183

184
print(f"Composed transformation:\n{T_composed}")
185
print(f"Propagated uncertainty diagonal: {np.diag(cov_composed)}")
186
```
187

188
### Multi-Sensor Fusion
189

190
```python
191
import numpy as np
192
import pytransform3d.uncertainty as pu
193
import pytransform3d.transformations as pt
194

195
# Simulate multiple pose measurements
196
true_pose = pt.transform_from(p=[2, 1, 0.5])
197

198
# Sensor 1: High precision in position, low precision in orientation
199
pose1 = true_pose + np.random.normal(0, 0.01, (4, 4))
200
pose1 = pt.transform_from(p=pose1[:3, 3])  # Extract position
201
cov1 = np.diag([0.01, 0.01, 0.01, 0.1, 0.1, 0.1])
202

203
# Sensor 2: Low precision in position, high precision in orientation  
204
pose2 = true_pose + np.random.normal(0, 0.05, (4, 4))
205
pose2 = pt.transform_from(p=pose2[:3, 3])
206
cov2 = np.diag([0.05, 0.05, 0.05, 0.01, 0.01, 0.01])
207

208
# Sensor 3: Medium precision overall
209
pose3 = true_pose + np.random.normal(0, 0.02, (4, 4))
210
pose3 = pt.transform_from(p=pose3[:3, 3])
211
cov3 = np.diag([0.02, 0.02, 0.02, 0.02, 0.02, 0.02])
212

213
# Fuse measurements
214
poses = [pose1, pose2, pose3]
215
covariances = [cov1, cov2, cov3]
216

217
fused_pose, fused_cov = pu.pose_fusion(poses, covariances)
218

219
print(f"True position: {true_pose[:3, 3]}")
220
print(f"Fused position: {fused_pose[:3, 3]}")
221
print(f"Position error: {np.linalg.norm(fused_pose[:3, 3] - true_pose[:3, 3]):.4f}")
222
print(f"Fused uncertainty (diagonal): {np.diag(fused_cov)}")
223
```
224

225
### Statistical Estimation
226

227
```python
228
import numpy as np
229
import pytransform3d.uncertainty as pu
230
import pytransform3d.transformations as pt
231

232
# Generate noisy pose samples around true pose
233
true_pose = pt.transform_from(p=[1, 2, 3])
234
n_samples = 100
235

236
pose_samples = []
237
for _ in range(n_samples):
238
    # Add noise to position and orientation
239
    noise_pos = np.random.normal(0, 0.1, 3)
240
    noise_rot = np.random.normal(0, 0.05, 3)
241
    
242
    # Create noisy pose
243
    T_noise = pt.transform_from(p=noise_pos)
244
    R_noise = pr.matrix_from_euler(noise_rot, "xyz", extrinsic=True)
245
    T_noise[:3, :3] = R_noise
246
    
247
    noisy_pose = pt.concat(true_pose, T_noise)
248
    pose_samples.append(noisy_pose)
249

250
pose_samples = np.array(pose_samples)
251

252
# Estimate mean and covariance
253
mean_pose, cov_estimate = pu.estimate_gaussian_transform_from_samples(pose_samples)
254

255
print(f"True position: {true_pose[:3, 3]}")
256
print(f"Estimated position: {mean_pose[:3, 3]}")
257
print(f"Position error: {np.linalg.norm(mean_pose[:3, 3] - true_pose[:3, 3]):.4f}")
258
print(f"Estimated covariance diagonal: {np.diag(cov_estimate)}")
259
```
260

261
### Uncertainty Visualization
262

263
```python
264
import numpy as np
265
import matplotlib.pyplot as plt
266
import pytransform3d.uncertainty as pu
267

268
# Define uncertain position
269
mean_pos = np.array([1, 2, 0])
270
cov_pos = np.array([[0.1, 0.05, 0.02],
271
                    [0.05, 0.2, 0.01], 
272
                    [0.02, 0.01, 0.05]])
273

274
# Generate ellipsoid surface
275
ellipsoid_points = pu.to_ellipsoid(mean_pos, cov_pos, n_steps=20)
276

277
# 3D visualization
278
fig = plt.figure(figsize=(12, 5))
279

280
# 3D ellipsoid
281
ax1 = fig.add_subplot(121, projection='3d')
282
ax1.scatter(ellipsoid_points[:, 0], ellipsoid_points[:, 1], ellipsoid_points[:, 2], 
283
           alpha=0.6, s=1)
284
ax1.scatter(*mean_pos, c='red', s=100, label='Mean')
285
ax1.set_xlabel('X')
286
ax1.set_ylabel('Y')
287
ax1.set_zlabel('Z')
288
ax1.legend()
289
ax1.set_title('3D Uncertainty Ellipsoid')
290

291
# 2D projection
292
ax2 = fig.add_subplot(122)
293
projection = pu.to_projected_ellipsoid(mean_pos, cov_pos[:2, :2])
294
ax2.plot(projection[:, 0], projection[:, 1], 'b-', alpha=0.7)
295
ax2.scatter(*mean_pos[:2], c='red', s=100, label='Mean')
296
ax2.set_xlabel('X')
297
ax2.set_ylabel('Y')
298
ax2.legend()
299
ax2.set_title('2D Projection')
300
ax2.set_aspect('equal')
301

302
plt.tight_layout()
303
plt.show()
304
```
305

306
### Uncertainty Propagation Chain
307

308
```python
309
import numpy as np
310
import pytransform3d.uncertainty as pu
311
import pytransform3d.transformations as pt
312

313
# Create chain of uncertain transformations
314
transforms = []
315
covariances = []
316

317
# Base to sensor mount
318
T_base_sensor = pt.transform_from(p=[0.1, 0, 0.5])
319
cov_base_sensor = np.diag([0.001, 0.001, 0.001, 0.0001, 0.0001, 0.0001])
320
transforms.append(T_base_sensor)
321
covariances.append(cov_base_sensor)
322

323
# Sensor mount to camera
324
T_sensor_cam = pt.transform_from(p=[0.05, 0.02, 0])
325
cov_sensor_cam = np.diag([0.002, 0.002, 0.002, 0.0005, 0.0005, 0.0005])
326
transforms.append(T_sensor_cam)
327
covariances.append(cov_sensor_cam)
328

329
# Propagate uncertainty through chain
330
T_result = transforms[0]
331
cov_result = covariances[0]
332

333
for i in range(1, len(transforms)):
334
    T_result, cov_result = pu.concat_globally_uncertain_transforms(
335
        T_result, cov_result, transforms[i], covariances[i]
336
    )
337

338
print(f"Final transformation: {T_result[:3, 3]}")
339
print(f"Final uncertainty (diagonal): {np.diag(cov_result)}")
340
print(f"Position std dev: {np.sqrt(np.diag(cov_result)[:3])}")
341
print(f"Orientation std dev: {np.sqrt(np.diag(cov_result)[3:])}")
342
```