pypose.metric.rpe

class pypose.metric.rpe(rstamp, rpose, estamp, epose, etype='translation', diff=0.01, offset=0.0, align=False, scale=False, nposes=- 1, origin=False, associate='frame', delta=1.0, rtol=0.1, all=False, thresh=0.3, rpair=False, otype='All')

Compute the Relative Pose Error (RPE) between two trajectories.

Parameters:

• rstamp (array-like of float or None) – The timestamps of the reference trajectory. Must have the same length as rpose. For example, torch.tensor([…]) or None.

• rpose (array-like of SE3) – The poses of the reference trajectory. Must have the same length as rstamp. For example, pypose.SE3(torch.rand(10, 7)).

• estamp (array-like of float or None) – The timestamps of the estimated trajectory. Must have the same length as epose. For example, torch.tensor([…]) or None.

• epose (array-like of SE3) – The poses of the estimated trajectory. Must have the same length as estamp. For example, pypose.SE3(torch.rand(10, 7)).

• etype (str, optional) –

  The type of pose error. Supported options include:

  'translation': \(||{R_{r}^{rel}}^T * t_{r}^{rel} - {R_{e}^{rel}}^T * t_{e}^{rel}||_2\)

  'rotation': \(|| {R_{r}^{rel}}^T * R_{e}^{rel} - I_3 ||_2\)

  'pose': \(|| {T_{r}^{rel}}^{-1} * T_{e}^{rel} - I_4 ||_2\)

  'radian': \(|| \mathrm{Log}({R_{r}^{rel}}^T * R_{e}^{rel}) ||_2\)

  'degree': \(\mathrm{Degree}(|| \mathrm{Log}({R_{r}^{rel}}^T * R_{e}^{rel}) ||_2))\)

  Default: 'translation'

• diff (float, optional) – The maximum allowed absolute time difference (in seconds) for associating poses. Defaults to 0.01.

• offset (float, optional) – The aligned offset (in seconds) for the second timestamps. Defaults to 0.0.

• align (bool, optional) – If True, the trajectory is aligned using a scaled SVD method. Defaults to False.

• scale (bool, optional) – If True, the scale is corrected using the SVD method. Defaults to False.

• nposes (int, optional) – The number of poses to use for alignment. If -1, all poses are used. Defaults to -1.

• origin (bool, optional) – If True, the trajectory is aligned by the first pose. Defaults to False.

• associate (str, optional) – The method used to associate pairs between the two trajectories. Supported options: ‘frame’, ‘distance’. Defaults to ‘frame’.

• delta (float, optional) – The interval step used to select the pair. For example, when associate=’distance’, it can represent the distance step in meters. Defaults to 1.0.

• rtol (float, optional) – The relative tolerance for accepting or rejecting deltas. Defaults to 0.1.

• all (bool, optional) – If True, all associated pairs are used for evaluation. Defaults to False.

• thresh (float, optional) – The threshold for valid matching pairs. If the ratio of matching pairs is below this threshold, a warning is issued. Defaults to 0.3.

• rpair (bool, optional) – Use reference trajectory to compute the pairing indices or not. Defaults to False.

• otype (str, optional) –

  The output type for the metric. Supported options include:

  'All': All metrics will be computed and returned.

  'Max': The Max error is computed and returned.

  'Min': The Min error is computed and returned.

  'Mean': The Mean error is computed and returned.

  'Median': The Median error is computed and returned.

  'RMSE': The root mean square error (RMSE) is computed and returned.

  'SSE': The sum of square error (SSE) is computed and returned.

  'STD': The standard deviation (STD) is computed and returned.

  Defaults to 'All'.

Returns:

The computed statistics of the RPE (Relative Pose Error). The return is a dict if otype is not 'all', otherwise a Tensor.

Return type:

dict or Tensor

Examples

>>> import torch
>>> import pypose as pp
>>> rstamp = torch.tensor([1311868163.8696999550, 1311868163.8731000423,
...                        1311868163.8763999939])
>>> rpose = pp.SE3([[-0.1357000023, -1.4217000008,  1.4764000177,
...                   0.6452999711, -0.5497999787,  0.3362999856, -0.4101000130],
...                 [-0.1357000023, -1.4218000174,  1.4764000177,
...                    0.6453999877, -0.5497000217,  0.3361000121, -0.4101999998],
...                 [-0.1358000040, -1.4219000340,  1.4764000177,
...                   0.6455000043, -0.5498999953,  0.3357999921, -0.4101000130]])
>>> estamp = torch.tensor([1311868164.3631811142, 1311868164.3990259171,
...                        1311868164.4309399128])
>>> epose = pp.SE3([[0.0000000000, 0.0000000000, 0.0000000000,
...                  0.0000000000, 0.0000000000, 0.0000000000, 1.0000000000],
...                 [-0.0005019300, 0.0010138600, -0.0020097860,
...                  -0.0020761820,-0.0010706080, -0.0007627490, 0.9999969602],
...                 [0.0004298200, 0.0019603260, -0.0048985220,
...                 -0.0043526068, -0.0036625920, -0.0023494449, 0.9999810457]])
>>> pp.metric.rpe(rstamp, rpose, estamp, epose)
{'Max': tensor(0.0032, dtype=torch.float64),
 'Min': tensor(0.0023, dtype=torch.float64),
 'Mean': tensor(0.0027, dtype=torch.float64),
 'Median': tensor(0.0023, dtype=torch.float64),
 'RMSE': tensor(0.0028, dtype=torch.float64),
 'SSE': tensor(1.5428e-05, dtype=torch.float64),
 'STD': tensor(0.0006, dtype=torch.float64)}
>>> pp.metric.rpe(rstamp, rpose, estamp, epose, otype='Mean')
tensor(0.0027, dtype=torch.float64)