Reihenfolge sinnvoll gestalten
This commit is contained in:
@@ -230,11 +230,29 @@ def analyze_chain(fk: RobotFK) -> Dict[str, Any]:
|
|||||||
for x in pending:
|
for x in pending:
|
||||||
var_block[x] = len(blocks) - 1
|
var_block[x] = len(blocks) - 1
|
||||||
|
|
||||||
|
# subtree_markers[L] = L's own markers + all descendants' markers. Lets
|
||||||
|
# observability() credit a block whose own link saw nothing this capture
|
||||||
|
# but whose CHILD link did (e.g. Ellbow has no visible markers, but Arm2's
|
||||||
|
# markers still constrain z through the chain — same idea as 4b's
|
||||||
|
# Fallback-1, just for confidence reporting here, not for the fit itself).
|
||||||
|
children: Dict[str, List[str]] = defaultdict(list)
|
||||||
|
for ln, ld in links.items():
|
||||||
|
p = ld.get("parent")
|
||||||
|
if p:
|
||||||
|
children[p].append(ln)
|
||||||
|
subtree_markers: Dict[str, List[int]] = {}
|
||||||
|
for ln in reversed(topo):
|
||||||
|
ids = list(link_markers.get(ln, []))
|
||||||
|
for c in children.get(ln, []):
|
||||||
|
ids.extend(subtree_markers.get(c, []))
|
||||||
|
subtree_markers[ln] = ids
|
||||||
|
|
||||||
return {
|
return {
|
||||||
"ordered_vars": ordered_vars,
|
"ordered_vars": ordered_vars,
|
||||||
"var_type": var_type,
|
"var_type": var_type,
|
||||||
"var_links": dict(var_links),
|
"var_links": dict(var_links),
|
||||||
"link_markers": link_markers,
|
"link_markers": link_markers,
|
||||||
|
"subtree_markers": subtree_markers,
|
||||||
"blocks": blocks,
|
"blocks": blocks,
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -518,16 +536,25 @@ def observability(chain: Dict[str, Any], obs: Dict[int, Dict[str, Any]]) -> Dict
|
|||||||
driven by markers-per-variable in that block:
|
driven by markers-per-variable in that block:
|
||||||
high : >= 2 markers per variable (well over-determined)
|
high : >= 2 markers per variable (well over-determined)
|
||||||
medium : >= 1 marker per variable
|
medium : >= 1 marker per variable
|
||||||
low : fewer markers than variables (under-determined — distrust!)
|
low : fewer markers than variables (under-determined — distrust!),
|
||||||
none : no markers at all (variable left at 0)
|
OR no own markers seen but a child link's markers were
|
||||||
|
(indirect evidence through the chain, e.g. Ellbow via Arm2)
|
||||||
|
none : no markers at all, not even indirectly (variable left at 0)
|
||||||
"""
|
"""
|
||||||
info: Dict[str, Dict[str, Any]] = {}
|
info: Dict[str, Dict[str, Any]] = {}
|
||||||
|
subtree_markers = chain.get("subtree_markers", {})
|
||||||
for block in chain["blocks"]:
|
for block in chain["blocks"]:
|
||||||
seen = [m for m in block["markers"] if m in obs]
|
seen = [m for m in block["markers"] if m in obs]
|
||||||
|
indirect = False
|
||||||
|
if not seen and block["anchor"]:
|
||||||
|
seen = [m for m in subtree_markers.get(block["anchor"], []) if m in obs]
|
||||||
|
indirect = bool(seen)
|
||||||
nvars = max(1, len(block["vars"]))
|
nvars = max(1, len(block["vars"]))
|
||||||
ratio = len(seen) / nvars
|
ratio = len(seen) / nvars
|
||||||
if len(seen) == 0:
|
if len(seen) == 0:
|
||||||
conf = "none"
|
conf = "none"
|
||||||
|
elif indirect:
|
||||||
|
conf = "low" # indirect/coupled through a child link, not direct
|
||||||
elif ratio >= 2.0:
|
elif ratio >= 2.0:
|
||||||
conf = "high"
|
conf = "high"
|
||||||
elif ratio >= 1.0:
|
elif ratio >= 1.0:
|
||||||
@@ -537,7 +564,7 @@ def observability(chain: Dict[str, Any], obs: Dict[int, Dict[str, Any]]) -> Dict
|
|||||||
for v in block["vars"]:
|
for v in block["vars"]:
|
||||||
info[v] = {"observable": len(seen) > 0, "n_markers": len(seen),
|
info[v] = {"observable": len(seen) > 0, "n_markers": len(seen),
|
||||||
"block_vars": len(block["vars"]), "confidence": conf,
|
"block_vars": len(block["vars"]), "confidence": conf,
|
||||||
"block_anchor": block["anchor"]}
|
"block_anchor": block["anchor"], "indirect": indirect}
|
||||||
return info
|
return info
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
Binary file not shown.
@@ -288,7 +288,7 @@
|
|||||||
"name": "Joint1",
|
"name": "Joint1",
|
||||||
"type": "revolute",
|
"type": "revolute",
|
||||||
"axis": [-1, 0, 0],
|
"axis": [-1, 0, 0],
|
||||||
"origin": [110, 107.457, 37.242],
|
"origin": [110, 101.1, 55.2],
|
||||||
"rotation": [0, 0, 0],
|
"rotation": [0, 0, 0],
|
||||||
"variable": "y"
|
"variable": "y"
|
||||||
},
|
},
|
||||||
|
|||||||
Reference in New Issue
Block a user