models_sym[obj_id])
]
elif p['error_type'] in ['ad', 'add', 'adi']:
if not spheres_overlap:
# Infinite error if the bounding spheres do not overlap. With
# typically used values of the correctness threshold for the AD
# error (e.g. k*diameter, where k = 0.1), such pose estimates
# would be considered incorrect anyway.
e = [float('inf')]
else:
if p['error_type'] == 'ad':
if obj_id in dp_model['symmetric_obj_ids']:
e = [
pose_error.adi(
R_e, t_e, R_g, t_g,
models[obj_id]['pts'])
]
else:
e = [
pose_error.add(
R_e, t_e, R_g, t_g,
models[obj_id]['pts'])
]
elif p['error_type'] == 'add':
e = [
pose_error.add(R_e, t_e, R_g, t_g,
models[obj_id]['pts'])
]
def find_closest(Re, te, Rs, ts, metric, pts, syms, r2):
"""
Given a pose estimate [Re, te], finds the closest plausible pose in [Rs, ts] wrt [metric].
:param Re: Rotation estimate.
:param te: translation estimate.
:param Rs: Plausible rotations.
:param ts: Plausible translations.
:param metric: Pose-error function to be used. Valid choices are [te, frobenius, add, adi, mssd].
:param pts: Point cloud [xyz] of the target object.
:param syms: Set of transformations representing the symmetric poses of the object. Used by [frobenius, adi, mssd].
:param r2: Scaling parameter for the rotation term in the Frobenius-based metric.
:return: Rotation, translation and distance wrt [metric] of the closest plausible pose.
"""
if metric == 'te':
dist = np.linalg.norm(ts.reshape(-1, 3) - te.reshape(-1, 3), axis=1, ord=2)
elif metric == 'frobenius':
# eval metric defined in Brégier et al., "Defining the Pose of any 3D Rigid Object and an Associated Distance"
# dist = np.sqrt(np.linalg.norm(ts.reshape(-1, 3) - te.reshape(-1, 3), axis=1, ord=2)**2
# + r2*np.linalg.norm(Rs - Re, axis=(1, 2), ord='fro')**2)
# symmetric version
dists = []
for sym in syms:
# symmetric pose candidate
Rsym = Re @ sym['R']
tsym = (Re @ sym['t']).reshape(3, 1) + te.reshape(3, 1)
dists.append(np.sqrt(np.linalg.norm(ts.reshape(-1, 3) - tsym.reshape(-1, 3), axis=1, ord=2) ** 2
+ r2 * np.linalg.norm(Rs - Rsym, axis=(1, 2), ord='fro') ** 2))
dist = np.min(dists, axis=0)
elif metric == 'add':
# serial evaluation (takes twice as long)
# dist = [error.add(R.reshape(3, 3), t.reshape(3, 1), Re, te, pts) for R, t in zip(Rs, ts.T)]
# precompute relative rotation and relative translation of all plausible poses to speed-up computation
R_ = (Re - Rs).reshape(-1, 3, 3)
t_ = (te.reshape(1, 3) - ts.reshape(-1, 3))
dist = [np.linalg.norm(R @ pts.T + t.reshape(3, 1), axis=0, ord=2).mean() for R, t in zip(R_, t_)]
elif metric == 'adi':
dist = [error.adi(R.reshape(3, 3), t.reshape(3, 1), Re, te, pts) for R, t in zip(Rs, ts)]
elif metric == 'mssd':
# serial evaluation (takes twice as long)
# dist = [error.mssd(R.reshape(3, 3), t.reshape(3, 1), Re, te, pts, syms) for R, t in zip(Rs, ts)]
dists = []
for sym in syms:
# symmetric pose candidate
Rsym = Re @ sym['R']
tsym = (Re @ sym['t']).reshape(3, 1) + te.reshape(3, 1)
# precompute relative rotation and relative translation of all plausible poses to speed-up computation
R_ = (Rsym - Rs).reshape(-1, 3, 3)
t_ = (tsym.reshape(1, 3) - ts.reshape(-1, 3))
dists.append([np.linalg.norm(R @ pts.T + t.reshape(3, 1), axis=0, ord=2).max() for R, t in zip(R_, t_)])
dist = np.min(dists, axis=0)
else:
raise ValueError(f"Parameter 'metric' must be one of 'te', 'frobenius', 'add', 'adi' or 'mssd'.")
closest = np.argmin(dist)
return Rs[closest], ts[closest], np.min(dist)