336 lines
18 KiB
Python
336 lines
18 KiB
Python
#!/usr/bin/env python3
|
|
"""Refine the accepted five-photo O30 calibration with eight side views.
|
|
|
|
One joint calibration is shared by all poses/views. Image landmarks are manual
|
|
estimates, and camera poses are nuisance parameters, not measured extrinsics.
|
|
"""
|
|
from __future__ import annotations
|
|
|
|
import argparse
|
|
import json
|
|
import os
|
|
from pathlib import Path
|
|
import sys
|
|
import xml.etree.ElementTree as ET
|
|
|
|
if '--render' in sys.argv:
|
|
os.environ.setdefault('MUJOCO_GL', 'egl')
|
|
|
|
import numpy as np
|
|
from scipy.optimize import least_squares
|
|
from scipy.spatial.transform import Rotation
|
|
import fit_photos as photo
|
|
|
|
base = photo.base
|
|
SIDE = Path(__file__).with_name('side_landmarks.json')
|
|
PREVIOUS = base.MODEL_DIR / 'o30_right_photo_calibration_v2.json'
|
|
OUTPUT_JSON = base.MODEL_DIR / 'o30_right_multiview_calibration.json'
|
|
OUTPUT_URDF = base.MODEL_DIR / 'linkerhand_o30_right_multiview_calibrated.urdf'
|
|
REPORT = base.ROOT / 'outputs/reports/O30/o30_right_multiview_calibration'
|
|
JOINT_PAD_COUNT = 56
|
|
|
|
|
|
class MultiviewCalibration(photo.PhotoCalibration):
|
|
def __init__(self):
|
|
super().__init__()
|
|
self.previous = json.loads(PREVIOUS.read_text())
|
|
self.v2 = np.array(self.previous['parameter_vector'])
|
|
self.side_annotations = json.loads(SIDE.read_text())
|
|
if self.side_annotations['image_size'] != [self.width, self.height]:
|
|
raise ValueError('Side annotations must use the same pixel coordinate size')
|
|
sides = self.side_annotations['photos']
|
|
expected = [f'{view}_{finger}' for view in ['left', 'right'] for finger in base.FINGERS[1:]]
|
|
if [p['name'] for p in sides] != expected:
|
|
raise ValueError('Incorrect side-photo order')
|
|
landmarks = {name: (int(self.model.jnt_bodyid[self.model.joint(name).id]), np.zeros(3))
|
|
for name in base.NAMES}
|
|
# Reuse the exact visible tip definition used by the original photos.
|
|
for old in self.photos:
|
|
for k, point in enumerate(old['points']):
|
|
if point[0].endswith('_tip'):
|
|
i = self.photos.index(old)
|
|
landmarks[point[0]] = (self.photo_data[i][0][k], self.photo_data[i][1][k])
|
|
for side in sides:
|
|
self.photo_commands.append(next(s['command'] for s in self.samples if s['finger'] == side['pose']))
|
|
points = side['points']
|
|
body = np.array([landmarks[p[0]][0] for p in points])
|
|
local = np.array([landmarks[p[0]][1] for p in points])
|
|
observed = np.array([p[1:3] for p in points], dtype=float)
|
|
sigma = np.array([p[3] for p in points], dtype=float)
|
|
if not np.all(np.isfinite(observed)) or np.any(sigma <= 0):
|
|
raise ValueError('Invalid side annotations')
|
|
self.photo_data.append((body, local, observed, sigma))
|
|
self.photos.extend(sides)
|
|
self.view_count = len(self.photos)
|
|
|
|
def camera_bounds(self):
|
|
return (np.r_[np.tile([-6, -6, -6, -.3, -.2, .2], self.view_count), np.log(500)],
|
|
np.r_[np.tile([6, 6, 6, .3, .3, 1.5], self.view_count), np.log(2000)])
|
|
|
|
def camera_initial(self, q):
|
|
focal = self.previous['photo_fit']['focal_px']
|
|
cameras = list(np.array(self.previous['camera_parameters'][:30]).reshape(5, 6))
|
|
for i, side in enumerate(self.photos[5:], 5):
|
|
rotation = np.array([[1, 0, 0], [0, 0, -1], [0, 1, 0]]) if side['view'] == 'left' else np.array(
|
|
[[-1, 0, 0], [0, 0, -1], [0, -1, 0]])
|
|
world = self.world_points(i, q)
|
|
_, _, observed, sigma = self.photo_data[i]
|
|
best = None
|
|
for yaw in [-.25, 0, .25]:
|
|
rv = (Rotation.from_matrix(rotation)*Rotation.from_euler('z', yaw)).as_rotvec()
|
|
result = least_squares(
|
|
lambda v: ((self.project(world, v, focal)-observed)/sigma[:, None]).ravel(),
|
|
np.r_[rv, 0, .08, .4], max_nfev=200,
|
|
bounds=([-6, -6, -6, -.3, -.2, .2], [6, 6, 6, .3, .3, 1.5]))
|
|
if best is None or result.cost < best.cost:
|
|
best = result
|
|
cameras.append(best.x)
|
|
return np.r_[np.array(cameras).ravel(), np.log(focal)]
|
|
|
|
def photo_residual(self, q, cameras):
|
|
poses, focal = cameras[:-1].reshape(self.view_count, 6), np.exp(cameras[-1])
|
|
residual = []
|
|
for i in range(self.view_count):
|
|
_, _, observed, sigma = self.photo_data[i]
|
|
predicted = self.project(self.world_points(i, q), poses[i], focal)
|
|
residual.extend(self.robust_photo_residual((predicted-observed)/sigma[:, None]).ravel())
|
|
residual.append((cameras[-1]-np.log(self.previous['photo_fit']['focal_px']))/.35)
|
|
return residual
|
|
|
|
def joint_residual(self, vector):
|
|
q = vector[:JOINT_PAD_COUNT]
|
|
residual = list(base.Calibration.residual(self, q))
|
|
residual.extend(self.photo_residual(q, vector[JOINT_PAD_COUNT:]))
|
|
# Stay near the accepted V2, rather than refitting from a different hand.
|
|
residual.extend((q[:20]-self.v2[:20])/.087*.6)
|
|
residual.extend((q[20:40]-self.v2[20:40])/.2*.6)
|
|
for i in range(4):
|
|
self.set_pose(q, self.commands[i])
|
|
_, tn = self.world_pad(q, i, 'thumb')
|
|
_, fn = self.world_pad(q, i, self.samples[i]['finger'])
|
|
residual.extend((tn+fn)/.065)
|
|
sigma = np.full(20, 3.)
|
|
sigma[[0, 1, 6, 15]] = 5.
|
|
residual.extend(self.positions(q, self.zero_command)/np.deg2rad(sigma))
|
|
return np.array(residual)
|
|
|
|
def metrics_photos(self, q, cameras):
|
|
poses, focal = cameras[:-1].reshape(self.view_count, 6), np.exp(cameras[-1])
|
|
groups = {k: [] for k in ['all', 'front', 'side', 'side_motors', 'side_pip_dip_ip']}
|
|
rows = []
|
|
for i, image in enumerate(self.photos):
|
|
_, _, observed, sigma = self.photo_data[i]
|
|
predicted = self.project(self.world_points(i, q), poses[i], focal)
|
|
error = np.linalg.norm(predicted-observed, axis=1)
|
|
groups['all'].extend(error)
|
|
groups['front' if i < 5 else 'side'].extend(error)
|
|
details = []
|
|
for point, obs, pred, s, e in zip(image['points'], observed, predicted, sigma, error):
|
|
name = point[0]
|
|
if i >= 5:
|
|
target = image['pose']
|
|
if name in [target+'_mcp_pitch', target+'_pip', target+'_dip', 'thumb_mcp', 'thumb_ip']:
|
|
groups['side_motors'].append(e)
|
|
if name in [target+'_pip', target+'_dip', 'thumb_ip']:
|
|
groups['side_pip_dip_ip'].append(e)
|
|
details.append(dict(name=name, observed_px=obs.tolist(), predicted_px=pred.tolist(),
|
|
sigma_px=float(s), error_px=float(e)))
|
|
rows.append(dict(photo=image['name'], points=len(error), rms_px=float(np.sqrt(np.mean(error**2))),
|
|
points_detail=details))
|
|
return dict(focal_px=float(focal), photos=rows,
|
|
**{key+'_rms_px': float(np.sqrt(np.mean(np.array(values)**2))) for key, values in groups.items()})
|
|
|
|
def accept(self, q, camera, reference):
|
|
base.check_metrics(self.metrics(q))
|
|
self.set_pose(q, self.zero_command)
|
|
if max([0.] + [-c.dist for c in self.data.contact]) > .0002:
|
|
raise RuntimeError('Zero pose collision regression')
|
|
score = self.metrics_photos(q, camera)
|
|
if score['front_rms_px'] > reference['front_rms_px']+2.5:
|
|
raise RuntimeError('Original front-view fit regressed beyond tolerance')
|
|
if score['photos'][4]['rms_px'] > reference['photos'][4]['rms_px']+1.5:
|
|
raise RuntimeError('Zero photo regressed beyond tolerance')
|
|
if score['side_pip_dip_ip_rms_px'] >= reference['side_pip_dip_ip_rms_px']:
|
|
raise RuntimeError('Visible side-view intermediate hinges did not improve')
|
|
max_change = max(float(np.max(abs(self.positions(q, command)-self.positions(self.v2, command))))
|
|
for command in self.photo_commands[:5])
|
|
if max_change > np.deg2rad(18):
|
|
raise RuntimeError('Too large a joint-pose change from the accepted V2')
|
|
return score
|
|
|
|
def search(self, initial, starts, max_nfev):
|
|
initial_camera = self.camera_initial(self.v2)
|
|
reference_camera = self.fit_cameras(self.v2, initial_camera)
|
|
reference = self.metrics_photos(self.v2, reference_camera)
|
|
if initial is None:
|
|
initial = np.r_[self.v2, reference_camera]
|
|
low, high = self.bounds()
|
|
# Explicit trust region as well as soft regularization around V2.
|
|
low[:20] = np.maximum(low[:20], self.v2[:20]-np.deg2rad(8))
|
|
high[:20] = np.minimum(high[:20], self.v2[:20]+np.deg2rad(8))
|
|
low[20:40] = np.maximum(low[20:40], self.v2[20:40]-.3)
|
|
high[20:40] = np.minimum(high[20:40], self.v2[20:40]+.3)
|
|
clow, chigh = self.camera_bounds()
|
|
bounds = np.r_[low, clow], np.r_[high, chigh]
|
|
rng, best, history = np.random.default_rng(20260922), None, []
|
|
for attempt in range(starts):
|
|
start = initial.copy() if best is None else best.x.copy()
|
|
if attempt:
|
|
start[:40] += rng.normal(0, .018, 40)
|
|
result = least_squares(self.joint_residual, np.clip(start, bounds[0]+1e-10, bounds[1]-1e-10),
|
|
bounds=bounds, max_nfev=max_nfev, diff_step=1e-4, x_scale='jac',
|
|
ftol=1e-7, xtol=1e-9)
|
|
reason = ''
|
|
try:
|
|
score = self.accept(result.x[:JOINT_PAD_COUNT], result.x[JOINT_PAD_COUNT:], reference)
|
|
accepted = True
|
|
except RuntimeError as exc:
|
|
accepted, reason, score = False, str(exc), None
|
|
history.append(dict(start=attempt+1, cost=float(result.cost), nfev=result.nfev,
|
|
success=bool(result.success), accepted=accepted, reason=reason,
|
|
message=result.message, score=None if score is None else {
|
|
k: v for k, v in score.items() if k.endswith('_rms_px')}))
|
|
print(json.dumps(history[-1]), flush=True)
|
|
if accepted and (best is None or result.cost < best.cost):
|
|
best = result
|
|
if best is None:
|
|
raise RuntimeError('No refinement passed acceptance; V2 remains available')
|
|
return best.x, history, reference_camera, reference
|
|
|
|
|
|
def provenance():
|
|
return {'stock_urdf_sha256': base.STOCK_URDF, 'stock_xml_sha256': base.STOCK_XML,
|
|
'samples_sha256': base.SAMPLES, 'annotations_sha256': photo.ANNOTATIONS,
|
|
'side_annotations_sha256': SIDE, 'v2_sha256': PREVIOUS}
|
|
|
|
|
|
def save(cal, vector, history, reference_camera, reference):
|
|
q, camera = vector[:56], vector[56:]
|
|
score = cal.accept(q, camera, reference)
|
|
base.CAL_URDF, base.CAL_JSON = OUTPUT_URDF, OUTPUT_JSON
|
|
info = base.save_solution(cal, q, dict(history=history, objective_evaluations=cal.evaluations,
|
|
seed=20260922, method='Bounded multistart photogeometric refinement'))
|
|
tree = ET.parse(OUTPUT_URDF)
|
|
for link in tree.getroot().findall('link'):
|
|
color = link.find('visual/material/color')
|
|
if color is not None:
|
|
color.set('rgba', '0.48 0.50 0.52 1' if link.get('name', '').endswith('_distal') else '0.68 0.70 0.72 1')
|
|
tree.getroot().insert(0, ET.Comment(' Multiview refinement of V2; offsets in joint origins, travels in limits. See o30_right_multiview_calibration.json. '))
|
|
ET.indent(tree, space=' ')
|
|
tree.write(OUTPUT_URDF, encoding='utf-8', xml_declaration=True)
|
|
info.update({key: base.sha256(path) for key, path in provenance().items()})
|
|
info.update(status='13-photo refinement; hardware validation pending',
|
|
method='MuJoCo contact and manual image landmark fitting, with shared joint calibration and per-view cameras',
|
|
limitations='Manually estimated 2-D landmarks, unknown cameras and lens distortion. '
|
|
'Image residual is training fit, not measured 3-D accuracy. '
|
|
'Full stroke, unseen poses and hardware dynamics remain unvalidated.',
|
|
calibrated_urdf_sha256=base.sha256(OUTPUT_URDF),
|
|
zero_command=cal.zero_command.tolist(), camera_parameters=camera.tolist(),
|
|
baseline_camera_parameters=reference_camera.tolist(), previous_v2=reference, photo_fit=score,
|
|
pose_changes_deg={cal.photos[i]['name']: dict(zip(base.NAMES, np.rad2deg(
|
|
cal.positions(q, cmd)-cal.positions(cal.v2, cmd)).tolist()))
|
|
for i, cmd in enumerate(cal.photo_commands[:5])})
|
|
OUTPUT_JSON.write_text(json.dumps(info, indent=2, ensure_ascii=False)+'\n')
|
|
return info
|
|
|
|
|
|
def load(cal):
|
|
info = json.loads(OUTPUT_JSON.read_text())
|
|
for key, path in {**provenance(), 'calibrated_urdf_sha256': OUTPUT_URDF}.items():
|
|
if info[key] != base.sha256(path):
|
|
raise ValueError(f'Calibration input/output changed: {path}')
|
|
vector = np.r_[info['parameter_vector'], info['camera_parameters']]
|
|
if vector.shape != (56+6*cal.view_count+1,) or not np.all(np.isfinite(vector)):
|
|
raise ValueError('Invalid calibration vector')
|
|
if info['zero_command'] != cal.zero_command.tolist():
|
|
raise ValueError('Zero command changed')
|
|
cal.accept(vector[:56], vector[56:], info['previous_v2'])
|
|
return info, vector
|
|
|
|
|
|
def render(cal, vector):
|
|
from PIL import Image, ImageDraw, ImageFont
|
|
REPORT.mkdir(parents=True, exist_ok=True)
|
|
model, data = cal.model, cal.data
|
|
model.vis.global_.offwidth, model.vis.global_.offheight = cal.width, cal.height
|
|
for g in range(model.ngeom):
|
|
if model.geom_group[g] == 2:
|
|
model.geom_rgba[g] = [.48, .50, .52, 1] if model.body(model.geom_bodyid[g]).name.endswith('_distal') else [.68, .70, .72, 1]
|
|
cameras, focal = vector[56:-1].reshape(cal.view_count, 6), np.exp(vector[-1])
|
|
font = ImageFont.truetype('/usr/share/fonts/truetype/dejavu/DejaVuSans.ttf', 19)
|
|
row_height = cal.height+40
|
|
with base.mujoco.Renderer(model, cal.height, cal.width) as renderer:
|
|
for group, indices in [('front_zero', range(5)), ('left', range(5, 9)), ('right', range(9, 13))]:
|
|
sheet = Image.new('RGB', (cal.width*len(indices), row_height*2), '#f1f5f9')
|
|
header = ImageDraw.Draw(sheet)
|
|
for row, q in enumerate([cal.v2, vector[:56]]):
|
|
for col, i in enumerate(indices):
|
|
cal.set_pose(q, cal.photo_commands[i])
|
|
renderer.update_scene(data)
|
|
rotation = Rotation.from_rotvec(cameras[i, :3]).as_matrix()
|
|
eye = -rotation.T@cameras[i, 3:]
|
|
forward, up = rotation.T@np.array([0, 0, 1]), rotation.T@np.array([0, -1, 0])
|
|
for camera in renderer.scene.camera:
|
|
camera.pos[:], camera.forward[:], camera.up[:] = eye, forward, up
|
|
camera.frustum_near, camera.frustum_far = .01, 5.
|
|
camera.frustum_top = .01*cal.height/2/focal
|
|
camera.frustum_bottom = -camera.frustum_top
|
|
camera.frustum_center = 0
|
|
camera.frustum_width = .01*cal.width/2/focal
|
|
for light in renderer.scene.lights[:renderer.scene.nlight]:
|
|
light.pos[:], light.dir[:] = eye, forward
|
|
light.ambient[:], light.diffuse[:] = [.25]*3, [.65]*3
|
|
image = Image.fromarray(renderer.render())
|
|
sheet.paste(image, (col*cal.width, row*row_height+40))
|
|
header.text((col*cal.width+12, row*row_height+10),
|
|
f'V{row+2} / {cal.photos[i]["name"]}', fill='#172033', font=font)
|
|
if row == 1:
|
|
image.save(REPORT/f'{cal.photos[i]["name"]}_pose.png')
|
|
draw = ImageDraw.Draw(image)
|
|
predicted = cal.project(cal.world_points(i, q), cameras[i], focal)
|
|
for observed, point in zip(cal.photo_data[i][2], predicted):
|
|
draw.line([tuple(observed), tuple(point)], fill='yellow', width=2)
|
|
draw.ellipse([observed[0]-4, observed[1]-4, observed[0]+4, observed[1]+4], outline='cyan', width=2)
|
|
draw.ellipse([point[0]-2, point[1]-2, point[0]+2, point[1]+2], fill='red')
|
|
draw.text((10, 8), 'Cyan: manual photo points / red: model', fill='white', font=font)
|
|
image.save(REPORT/f'{cal.photos[i]["name"]}_landmarks.png')
|
|
sheet.save(REPORT/f'{group}_comparison.png')
|
|
print(REPORT/f'{group}_comparison.png')
|
|
|
|
|
|
def main():
|
|
parser = argparse.ArgumentParser(description=__doc__)
|
|
parser.add_argument('--fit', action='store_true')
|
|
parser.add_argument('--check', action='store_true')
|
|
parser.add_argument('--render', action='store_true')
|
|
parser.add_argument('--starts', type=int, default=4)
|
|
parser.add_argument('--max-nfev', type=int, default=180)
|
|
parser.add_argument('--preview', choices=['index', 'middle', 'ring', 'pinky', 'zero'])
|
|
args = parser.parse_args()
|
|
if not any([args.fit, args.check, args.render, args.preview]):
|
|
parser.error('Choose --fit, --check, --render or --preview')
|
|
if args.starts < 1 or args.max_nfev < 1:
|
|
parser.error('Optimization budgets must be positive')
|
|
cal = MultiviewCalibration()
|
|
if args.preview:
|
|
photo.preview(cal, args.preview)
|
|
return
|
|
if args.fit:
|
|
initial = load(cal)[1] if OUTPUT_JSON.exists() else None
|
|
vector, history, reference_camera, reference = cal.search(initial, args.starts, args.max_nfev)
|
|
info = save(cal, vector, history, reference_camera, reference)
|
|
print('Exported URDF; regenerate with O30_RIGHT_PREPARE_ONLY=1 ./hands/O30/scripts/run_o30_right_mujoco_plan.sh before --check')
|
|
else:
|
|
info, vector = load(cal)
|
|
if args.check:
|
|
info['live_validation'] = photo.validate(cal, vector)
|
|
REPORT.mkdir(parents=True, exist_ok=True)
|
|
(REPORT/'validation.json').write_text(json.dumps(info, indent=2, ensure_ascii=False)+'\n')
|
|
print(json.dumps(info['live_validation'], indent=2))
|
|
if args.render:
|
|
render(cal, vector)
|
|
|
|
|
|
if __name__ == '__main__':
|
|
main()
|