44 lines
1.8 KiB
Python
44 lines
1.8 KiB
Python
#!/usr/bin/env python3
|
|
"""Compare two homogeneous-extrinsic JSON files in parameter space and on SE(3)."""
|
|
|
|
import argparse
|
|
import json
|
|
from pathlib import Path
|
|
|
|
import numpy as np
|
|
from scipy.spatial.transform import Rotation
|
|
|
|
|
|
def main() -> int:
|
|
parser = argparse.ArgumentParser(description=__doc__)
|
|
parser.add_argument("--reference", type=Path, required=True)
|
|
parser.add_argument("--candidate", type=Path, required=True)
|
|
parser.add_argument("--output", type=Path, required=True)
|
|
args = parser.parse_args()
|
|
reference = json.loads(args.reference.read_text(encoding="utf-8-sig"))
|
|
candidate = json.loads(args.candidate.read_text(encoding="utf-8-sig"))
|
|
a = np.asarray(reference["matrix_4x4"], dtype=float)
|
|
b = np.asarray(candidate["matrix_4x4"], dtype=float)
|
|
delta = np.linalg.inv(a) @ b
|
|
result = {
|
|
"convention": "delta = inverse(reference) @ candidate",
|
|
"reference": str(args.reference.resolve()),
|
|
"candidate": str(args.candidate.resolve()),
|
|
"candidate_minus_reference_translation_xyz_m": (b[:3, 3] - a[:3, 3]).tolist(),
|
|
"candidate_minus_reference_rpy_xyz_deg": (
|
|
np.asarray(candidate["rotation_rpy_deg_xyz"], float)
|
|
- np.asarray(reference["rotation_rpy_deg_xyz"], float)
|
|
).tolist(),
|
|
"relative_translation_norm_m": float(np.linalg.norm(delta[:3, 3])),
|
|
"relative_rotation_deg": float(np.degrees(Rotation.from_matrix(delta[:3, :3]).magnitude())),
|
|
"relative_matrix_4x4": delta.tolist(),
|
|
}
|
|
args.output.parent.mkdir(parents=True, exist_ok=True)
|
|
args.output.write_text(json.dumps(result, ensure_ascii=False, indent=2), encoding="utf-8")
|
|
print(json.dumps(result, ensure_ascii=False, indent=2))
|
|
return 0
|
|
|
|
|
|
if __name__ == "__main__":
|
|
raise SystemExit(main())
|