Multipoint Schritt 3
This commit is contained in:
chk
@@ -20,9 +20,15 @@ Four switchable methods (robot.json -> pose_estimation.method):
|
||||
global_ba : all variables jointly, position + normal residuals, robust loss
|
||||
hybrid : sequential_fk init -> global_ba refine (default, most stable)
|
||||
|
||||
Observation input:
|
||||
marker_observation = "corner_pose" -> aruco_marker_poses.json (pos + measured normal)
|
||||
marker_observation = "center_point" -> aruco_positions_*.json (pos only)
|
||||
Observation input (robot.json -> pose_estimation.marker_observation):
|
||||
"corner_pose" (default) -> aruco_marker_poses.json: 3 pos + 3 normal residuals/marker
|
||||
"corner_points" -> aruco_marker_poses.json: 12 corner residuals for
|
||||
robot-link markers (4 triangulated corners vs FK
|
||||
corners; no separate normal), 1 center residual for
|
||||
root-link (Board: floor/rail) markers whose spin is
|
||||
uncalibrated. Robust loss acts per corner. Opt-in;
|
||||
needs `corners_m`. Links via corner_point_links.
|
||||
"center_point" -> aruco_positions_*.json: position only
|
||||
|
||||
Homing integration (appRobotHoming, see doc/Homing_5_Pose.md):
|
||||
--from-state <json> seed/init state (flat {var: value}, or the
|
||||
@@ -89,6 +95,14 @@ DEFAULT_CFG: Dict[str, Any] = {
|
||||
"min_cameras_per_marker": 2,
|
||||
"finger_block_joints": ["b", "c", "e"],
|
||||
"per_link_method": {},
|
||||
# Nur im marker_observation="corner_points"-Modus relevant: welche Links die
|
||||
# 4 Eck-Residuen nutzen. None/absent = alle Nicht-Root-Links (= der Roboter);
|
||||
# der Root-Link (Board mit Boden-/Rail-Markern) nutzt ein Center-Residuum.
|
||||
# Hintergrund: nur die Roboter-Marker-Spins sind kalibriert/verifiziert; die
|
||||
# Board/Rail-Spins nicht — deren Eckreihenfolge wäre unzuverlässig. Board ist
|
||||
# zudem Root (Residuum konstant bzgl. der Gelenke). Explizite Liste möglich,
|
||||
# z.B. ["Arm1","Ellbow","Arm2","Hand","Palm","FingerA","FingerB"].
|
||||
"corner_point_links": None,
|
||||
# One switch: if set to a link name (e.g. "Arm1"), that link's
|
||||
# jointToParent.origin Y/Z is fit together with the normal pose (same
|
||||
# global_ba solve) and the result is written back into robot.json
|
||||
@@ -111,8 +125,12 @@ def load_pose_cfg(robot_data: Dict[str, Any]) -> Dict[str, Any]:
|
||||
def load_observations(path: str, use_normals: bool, min_cams: int = 2) -> Dict[int, Dict[str, Any]]:
|
||||
"""
|
||||
Load marker observations. Accepts aruco_marker_poses.json (with measured
|
||||
normal + num_cameras) or aruco_positions_*.json (position only).
|
||||
Returns: marker_id -> {pos_mm:(3,), normal:(3,)|None, link:str, n_cams:int}
|
||||
normal + num_cameras + 4 triangulated corners) or aruco_positions_*.json
|
||||
(position only).
|
||||
Returns: marker_id -> {pos_mm:(3,), normal:(3,)|None, corners_mm:(4,3)|None,
|
||||
link:str, n_cams:int, weight:float}
|
||||
corners_mm (aus `corners_m`, m→mm) speist den marker_observation=
|
||||
"corner_points"-Modus in residual_vector().
|
||||
"""
|
||||
data = json.load(open(path, "r", encoding="utf-8"))
|
||||
out: Dict[int, Dict[str, Any]] = {}
|
||||
@@ -135,7 +153,14 @@ def load_observations(path: str, use_normals: bool, min_cams: int = 2) -> Dict[i
|
||||
nn = np.linalg.norm(nv)
|
||||
if nn > 1e-9:
|
||||
nrm = nv / nn
|
||||
out[mid] = {"pos_mm": pos, "normal": nrm, "link": m.get("link", "?"), "n_cams": n_cams,
|
||||
corners_mm = None
|
||||
cm = m.get("corners_m")
|
||||
if cm is not None:
|
||||
arr = np.array(cm, dtype=float)
|
||||
if arr.shape == (4, 3):
|
||||
corners_mm = arr * 1000.0
|
||||
out[mid] = {"pos_mm": pos, "normal": nrm, "corners_mm": corners_mm,
|
||||
"link": m.get("link", "?"), "n_cams": n_cams,
|
||||
"weight": float(m.get("weight", 1.0))}
|
||||
return out
|
||||
|
||||
@@ -268,14 +293,64 @@ def model_markers(fk: RobotFK, state: Dict[str, float]) -> Dict[int, Dict[str, n
|
||||
return fk.all_markers_world(T) # mid -> {world_mm, normal_world, link, local_mm}
|
||||
|
||||
|
||||
def _resolve_corner_links(fk: RobotFK, cfg: Dict[str, Any]) -> set:
|
||||
"""
|
||||
Welche Links im "corner_points"-Modus die 4 Eck-Residuen nutzen.
|
||||
Explizite Liste in cfg["corner_point_links"], sonst alle Nicht-Root-Links
|
||||
(der Root-Link Board trägt die unkalibrierten Boden-/Rail-Marker und nutzt
|
||||
ein Center-Residuum).
|
||||
"""
|
||||
explicit = cfg.get("corner_point_links")
|
||||
if isinstance(explicit, list) and explicit:
|
||||
return set(explicit)
|
||||
links = fk.links
|
||||
roots = {ln for ln, ld in links.items()
|
||||
if not ld.get("parent") or ld.get("parent") not in links}
|
||||
return set(links.keys()) - roots
|
||||
|
||||
|
||||
def residual_vector(state: Dict[str, float], fk: RobotFK, obs: Dict[int, Dict[str, Any]],
|
||||
marker_ids: List[int], cfg: Dict[str, Any]) -> np.ndarray:
|
||||
"""Position (mm) + optional normal (scaled) residuals over the given markers."""
|
||||
"""
|
||||
Residuen über die gegebenen Marker. Modus via pose_estimation.marker_observation:
|
||||
|
||||
"corner_pose" (Default): 3 Position (mm) + optional 3 Normale
|
||||
(×normal_weight) je Marker — wie bisher.
|
||||
"corner_points": 12 Eck-Residuen (4 Ecken × xyz, mm) je Marker auf
|
||||
einem Roboter-Link (corner_point_links), KEINE
|
||||
separate Normale (Orientierung steckt in den
|
||||
Ecken). Mehr unabhängige Messpunkte, robuster
|
||||
Verlust greift auf Eck-Ebene. Marker auf dem
|
||||
Root-Link (Board: Boden-/Rail-Marker mit
|
||||
unkalibriertem Spin) oder ohne `corners_mm`
|
||||
nutzen EIN Center-Residuum (3, "ein Punkt pro
|
||||
Marker") — gleiche mm-Skala → ein huber_delta_mm.
|
||||
"""
|
||||
model = model_markers(fk, state)
|
||||
res: List[float] = []
|
||||
use_mw = bool(cfg.get("use_marker_weight", False))
|
||||
obs_mode = str(cfg.get("marker_observation", "corner_pose")).lower()
|
||||
|
||||
if obs_mode == "corner_points":
|
||||
corner_links = _resolve_corner_links(fk, cfg)
|
||||
for mid in marker_ids:
|
||||
if mid not in model or mid not in obs:
|
||||
continue
|
||||
mw = float(obs[mid].get("weight", 1.0)) if use_mw else 1.0
|
||||
mm = model[mid]
|
||||
oc = obs[mid].get("corners_mm")
|
||||
mc = mm.get("corners_world")
|
||||
if mm.get("link") in corner_links and oc is not None and mc is not None:
|
||||
dc = (np.asarray(mc, float) - np.asarray(oc, float)) * mw # (4,3)
|
||||
res.extend(dc.reshape(-1).tolist()) # 12 Werte
|
||||
else:
|
||||
dp = (np.asarray(mm["world_mm"], float) - obs[mid]["pos_mm"]) * mw
|
||||
res.extend(dp.tolist()) # Center (1 Punkt)
|
||||
return np.asarray(res, dtype=float)
|
||||
|
||||
# Default: Center (mm) + optionale Normale (skaliert)
|
||||
w_n = float(cfg.get("normal_weight", 30.0))
|
||||
use_n = bool(cfg.get("use_normals", True))
|
||||
use_mw = bool(cfg.get("use_marker_weight", False))
|
||||
for mid in marker_ids:
|
||||
if mid not in model or mid not in obs:
|
||||
continue
|
||||
@@ -646,6 +721,9 @@ def main() -> None:
|
||||
ap.add_argument("-robot", "--robot", required=True)
|
||||
ap.add_argument("-out", "--out", default=None)
|
||||
ap.add_argument("--method", default=None, help="override robot.json method")
|
||||
ap.add_argument("--marker-observation", default=None, dest="marker_observation",
|
||||
help="override robot.json marker_observation "
|
||||
"(corner_pose | corner_points | center_point)")
|
||||
ap.add_argument("--from-state", default=None, metavar="JSON",
|
||||
help="Seed/init state (flat {var:value} or {accumulated_state:{...}} as "
|
||||
"written by 4b_revolute_angle.py). Used as x0 for global_ba/hybrid "
|
||||
@@ -656,6 +734,8 @@ def main() -> None:
|
||||
cfg = load_pose_cfg(robot_data)
|
||||
if args.method:
|
||||
cfg["method"] = args.method
|
||||
if args.marker_observation:
|
||||
cfg["marker_observation"] = args.marker_observation
|
||||
|
||||
fk = RobotFK(robot_data)
|
||||
obs = load_observations(args.markers, cfg.get("use_normals", True),
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@@ -192,14 +192,109 @@ class RobotFK:
|
||||
"""Transform a local marker position → world (mm)."""
|
||||
return transform_point(transforms.get(link_name, np.eye(4)), local_pos)
|
||||
|
||||
# ── Marker-Eckpunkte (Mehrpunkt-Residuen, doc/Homing_5_Pose_MultiPoint_Weighted.md Schritt 3) ──
|
||||
|
||||
@staticmethod
|
||||
def _shortest_arc_R(normal: Sequence[float]) -> np.ndarray:
|
||||
"""
|
||||
Rotationsmatrix [0,0,1] → normal (kürzester Bogen).
|
||||
|
||||
Repliziert THREE.Quaternion.setFromUnitVectors(vFrom=[0,0,1], vTo=n)
|
||||
exakt (inkl. antiparallelem Sonderfall), damit die hier erzeugten
|
||||
Ecken zur visuell verifizierten Orientierungs-Konvention in
|
||||
boardViewer.html passen (qNormalLoc dort).
|
||||
"""
|
||||
n = np.asarray(normal, dtype=float)
|
||||
nn = float(np.linalg.norm(n))
|
||||
n = n / nn if nn > 1e-12 else np.array([0.0, 0.0, 1.0])
|
||||
# three.js: quat = (cross([0,0,1], n), dot([0,0,1], n) + 1), dann normalisieren.
|
||||
r = float(n[2]) + 1.0
|
||||
if r < 1e-12:
|
||||
# antiparallel (n == [0,0,-1]): three.js else-Zweig für vFrom=[0,0,1] → (0,-1,0,0)
|
||||
qx, qy, qz, qw = 0.0, -1.0, 0.0, 0.0
|
||||
else:
|
||||
qx, qy, qz, qw = -float(n[1]), float(n[0]), 0.0, r # cross([0,0,1], n) = (-ny, nx, 0)
|
||||
ql = math.sqrt(qx * qx + qy * qy + qz * qz + qw * qw)
|
||||
qx, qy, qz, qw = qx / ql, qy / ql, qz / ql, qw / ql
|
||||
return np.array([
|
||||
[1 - 2 * (qy * qy + qz * qz), 2 * (qx * qy - qz * qw), 2 * (qx * qz + qy * qw)],
|
||||
[2 * (qx * qy + qz * qw), 1 - 2 * (qx * qx + qz * qz), 2 * (qy * qz - qx * qw)],
|
||||
[2 * (qx * qz - qy * qw), 2 * (qy * qz + qx * qw), 1 - 2 * (qx * qx + qy * qy)],
|
||||
])
|
||||
|
||||
@staticmethod
|
||||
def _marker_plane_corners(half: float) -> np.ndarray:
|
||||
"""
|
||||
Die 4 Eckpunkte in der Marker-Ebene (+Z = Normale), in DERSELBEN
|
||||
Reihenfolge wie die von 3b_corner_marker_poses.py triangulierten
|
||||
`corners_m` (ArUco 0..3, im Uhrzeigersinn von der Vorderseite gesehen).
|
||||
|
||||
Ecke 0 zeigt in Richtung (+h, +h) — dieselbe Konvention wie der visuell
|
||||
kalibrierte Orientierungszeiger (1,1,0) in boardViewer.html. Damit passt
|
||||
sie zu den manuell/visuell gesetzten `spin`-Werten der ARM-Marker, die
|
||||
im Homing die Gelenkwinkel bestimmen (gegen echte corners_m am
|
||||
Seed-Pose verifiziert, test_robot_fk_corners.py, RMS ~1 mm).
|
||||
|
||||
Hinweis: Die Board-Referenzmarker (Set A0) sind ~90° anders kalibriert,
|
||||
ihre Eckreihenfolge passt unter dieser Konvention NICHT — egal, weil
|
||||
Board der Root-Link ist: ihr Eck-Residuum ist konstant bzgl. der
|
||||
Gelenkvariablen und beeinflusst die Schätzung nicht (der robuste
|
||||
Huber-Verlust dämpft es als Ausreißer). Siehe Doc-Notiz.
|
||||
|
||||
Die Drehrichtung 0→1→2→3 ist so, dass 3b's `corner_plane_normal()`
|
||||
(outward = -cross(e01,e02)) wieder +Z liefert — identisch zur
|
||||
Beobachtungs-Konvention.
|
||||
"""
|
||||
h = float(half)
|
||||
return np.array([
|
||||
[ h, h, 0.0], # 0
|
||||
[ h, -h, 0.0], # 1
|
||||
[-h, -h, 0.0], # 2
|
||||
[-h, h, 0.0], # 3
|
||||
])
|
||||
|
||||
@classmethod
|
||||
def marker_corners_local(cls,
|
||||
position: Sequence[float],
|
||||
normal: Sequence[float],
|
||||
size_mm: float,
|
||||
spin_deg: float = 0.0) -> np.ndarray:
|
||||
"""
|
||||
Die 4 Marker-Ecken im LINK-lokalen Frame (mm), Reihenfolge wie `corners_m`.
|
||||
|
||||
Orientierung = Spin um die Normale ∘ Minimal-Rotation [0,0,1]→Normale,
|
||||
exakt wie boardViewer.html (qSpinLoc.multiply(qNormalLoc)).
|
||||
"""
|
||||
n = np.asarray(normal, dtype=float)
|
||||
R = _rot_axis_angle(n, float(spin_deg)) @ cls._shortest_arc_R(n)
|
||||
plane = cls._marker_plane_corners(float(size_mm) / 2.0) # (4,3)
|
||||
return np.asarray(position, dtype=float) + (R @ plane.T).T # (4,3)
|
||||
|
||||
@classmethod
|
||||
def marker_corners_world(cls,
|
||||
transforms: Dict[str, np.ndarray],
|
||||
link_name: str,
|
||||
position: Sequence[float],
|
||||
normal: Sequence[float],
|
||||
size_mm: float,
|
||||
spin_deg: float = 0.0) -> np.ndarray:
|
||||
"""Die 4 Marker-Ecken im Weltframe (mm), Reihenfolge wie `corners_m`."""
|
||||
T = transforms.get(link_name, np.eye(4))
|
||||
local = cls.marker_corners_local(position, normal, size_mm, spin_deg)
|
||||
return np.array([transform_point(T, c) for c in local])
|
||||
|
||||
def all_markers_world(self,
|
||||
transforms: Dict[str, np.ndarray]
|
||||
) -> Dict[int, Dict[str, Any]]:
|
||||
"""
|
||||
Returns
|
||||
-------
|
||||
dict marker_id -> {world_mm, local_mm, link, normal_world}
|
||||
dict marker_id -> {world_mm, local_mm, link, normal_world, corners_world}
|
||||
|
||||
corners_world: (4,3) Welt-mm in `corners_m`-Reihenfolge (für den
|
||||
marker_observation="corner_points"-Modus in 5_pose_estimation.py).
|
||||
"""
|
||||
default_size = float((self.robot.get("markerDefaults", {}) or {}).get("size", 25.0))
|
||||
result: Dict[int, Dict[str, Any]] = {}
|
||||
for lname, ldata in self.links.items():
|
||||
T = transforms.get(lname, np.eye(4))
|
||||
@@ -210,11 +305,15 @@ class RobotFK:
|
||||
continue
|
||||
loc = np.array(m["position"], dtype=float)
|
||||
nor = np.array(m.get("normal", [0, 0, 1]), dtype=float)
|
||||
size_mm = float(m.get("size", default_size))
|
||||
spin_deg = float(m.get("spin", 0.0))
|
||||
local_corners = self.marker_corners_local(loc, nor, size_mm, spin_deg)
|
||||
result[mid] = {
|
||||
"world_mm": transform_point(T, loc),
|
||||
"local_mm": loc,
|
||||
"link": lname,
|
||||
"normal_world": (R @ nor) / max(np.linalg.norm(R @ nor), 1e-12),
|
||||
"corners_world": np.array([transform_point(T, c) for c in local_corners]),
|
||||
}
|
||||
return result
|
||||
|
||||
|
||||
Reference in New Issue
Block a user