board rotation
This commit is contained in:
chk
@@ -175,3 +175,217 @@ export async function removeMarkerAssignment(robotPath, { markerId, removeFrom }
|
||||
|
||||
return { changed: false, error: `Marker-ID ${id} nicht gefunden.` };
|
||||
}
|
||||
|
||||
// ── Aktion 3: Sets untereinander justieren (2D+Z Kabsch) ─────────────────────
|
||||
|
||||
/**
|
||||
* Optimale 2D-Rotation (um Z-Achse) + 3D-Translation, die model→measured minimiert.
|
||||
* Gibt { tx, ty, tz, theta, thetaDeg } zurück.
|
||||
* modelPts / measuredPts: Arrays von [x, y, z] in mm.
|
||||
*/
|
||||
function computeRigid2DZ(modelPts, measuredPts) {
|
||||
const n = modelPts.length;
|
||||
if (n === 0) return { tx: 0, ty: 0, tz: 0, theta: 0, thetaDeg: 0 };
|
||||
|
||||
// Schwerpunkte
|
||||
let mCx = 0, mCy = 0, mCz = 0, pCx = 0, pCy = 0, pCz = 0;
|
||||
for (let i = 0; i < n; i++) {
|
||||
mCx += modelPts[i][0]; mCy += modelPts[i][1]; mCz += modelPts[i][2];
|
||||
pCx += measuredPts[i][0]; pCy += measuredPts[i][1]; pCz += measuredPts[i][2];
|
||||
}
|
||||
mCx /= n; mCy /= n; mCz /= n;
|
||||
pCx /= n; pCy /= n; pCz /= n;
|
||||
|
||||
// 2D-Kreuzkovarianz für Kabsch-Rotation in XY-Ebene
|
||||
let H00 = 0, H01 = 0, H10 = 0, H11 = 0;
|
||||
for (let i = 0; i < n; i++) {
|
||||
const ax = modelPts[i][0] - mCx, ay = modelPts[i][1] - mCy;
|
||||
const bx = measuredPts[i][0] - pCx, by = measuredPts[i][1] - pCy;
|
||||
H00 += ax * bx; H01 += ax * by;
|
||||
H10 += ay * bx; H11 += ay * by;
|
||||
}
|
||||
|
||||
// Optimaler Drehwinkel (2D-Sonderfall von Kabsch / SVD → atan2)
|
||||
const theta = Math.atan2(H10 - H01, H00 + H11);
|
||||
const cos = Math.cos(theta);
|
||||
const sin = Math.sin(theta);
|
||||
|
||||
// Translation: gemessener Schwerpunkt − R × Modell-Schwerpunkt
|
||||
const tx = pCx - (cos * mCx - sin * mCy);
|
||||
const ty = pCy - (sin * mCx + cos * mCy);
|
||||
const tz = pCz - mCz;
|
||||
|
||||
return { tx, ty, tz, theta, thetaDeg: theta * 180 / Math.PI };
|
||||
}
|
||||
|
||||
function applyRigid2DZ([x, y, z], { tx, ty, tz, theta }) {
|
||||
const cos = Math.cos(theta);
|
||||
const sin = Math.sin(theta);
|
||||
return [
|
||||
Math.round((cos * x - sin * y + tx) * 100) / 100,
|
||||
Math.round((sin * x + cos * y + ty) * 100) / 100,
|
||||
Math.round((z + tz) * 100) / 100,
|
||||
];
|
||||
}
|
||||
|
||||
/**
|
||||
* Richtet alle Marker des Sets `setToMove` rigid an ihren triangulieren 3b-Positionen aus.
|
||||
* Das andere Set bleibt unberührt – die Transformation wird aus den Paaren
|
||||
* (Modellposition, Messposition) der matching Marker berechnet.
|
||||
*
|
||||
* setToMove: Name des zu verschiebenden Sets (z.B. "rail")
|
||||
* extraMarkers: Marker aus aruco_marker_poses.json (mit marker_id, position_mm)
|
||||
*
|
||||
* Gibt { numChanged, numMatchingPts, transform: {tx, ty, tz, thetaDeg} } zurück,
|
||||
* oder { error } bei Fehler.
|
||||
*/
|
||||
export async function alignSetToMeasured(robotPath, { setToMove, extraMarkers = [] }) {
|
||||
const robot = await readRobot(robotPath);
|
||||
const links = robot.links ?? {};
|
||||
|
||||
// Alle Marker des zu verschiebenden Sets sammeln
|
||||
const setMarkers = [];
|
||||
for (const [linkName, linkData] of Object.entries(links)) {
|
||||
for (const marker of (linkData.markers ?? [])) {
|
||||
if (marker.set === setToMove) {
|
||||
setMarkers.push({ marker, linkName });
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (setMarkers.length === 0) {
|
||||
return { error: `Set "${setToMove}" nicht gefunden oder leer.` };
|
||||
}
|
||||
|
||||
// Gemessene Positionen aus 3b indizieren
|
||||
const measuredMap = new Map();
|
||||
for (const em of extraMarkers) {
|
||||
if (Array.isArray(em.position_mm) && em.position_mm.length >= 3) {
|
||||
measuredMap.set(Number(em.marker_id), em.position_mm.map(Number));
|
||||
}
|
||||
}
|
||||
|
||||
// Matching-Paare für Kabsch (nur Marker, die sowohl Modellposition als auch Messung haben)
|
||||
const modelPts = [], measuredPts = [];
|
||||
for (const { marker } of setMarkers) {
|
||||
if (!Array.isArray(marker.position) || marker.position.length < 3) continue;
|
||||
const mpos = measuredMap.get(Number(marker.id));
|
||||
if (!mpos) continue;
|
||||
modelPts.push(marker.position.map(Number));
|
||||
measuredPts.push(mpos);
|
||||
}
|
||||
|
||||
if (modelPts.length < 2) {
|
||||
return {
|
||||
error: `Zu wenig Messpunkte für Set "${setToMove}" (${modelPts.length} von ${setMarkers.length} Markern gemessen). ` +
|
||||
`Bitte Board-Run mit ≥2 Kameras durchführen.`,
|
||||
};
|
||||
}
|
||||
|
||||
const transform = computeRigid2DZ(modelPts, measuredPts);
|
||||
|
||||
// Transformation auf ALLE Marker des Sets anwenden (auch nicht gemessene)
|
||||
let numChanged = 0;
|
||||
for (const { marker } of setMarkers) {
|
||||
if (!Array.isArray(marker.position) || marker.position.length < 3) continue;
|
||||
marker.position = applyRigid2DZ(marker.position.map(Number), transform);
|
||||
numChanged++;
|
||||
}
|
||||
|
||||
robot.links = links;
|
||||
await writeRobot(robotPath, robot);
|
||||
|
||||
return {
|
||||
numChanged,
|
||||
numMatchingPts: modelPts.length,
|
||||
transform: {
|
||||
tx: Math.round(transform.tx * 100) / 100,
|
||||
ty: Math.round(transform.ty * 100) / 100,
|
||||
tz: Math.round(transform.tz * 100) / 100,
|
||||
thetaDeg: Math.round(transform.thetaDeg * 100) / 100,
|
||||
},
|
||||
};
|
||||
}
|
||||
|
||||
// ── Aktion 4: Einzelnen Marker per ID hinzufügen / aktualisieren ──────────────
|
||||
|
||||
/**
|
||||
* Fügt einen Marker per ID zu Set und Link hinzu oder aktualisiert einen bestehenden.
|
||||
*
|
||||
* - Existiert der Marker bereits in robot.json: set und/oder link werden aktualisiert.
|
||||
* - Existiert er noch nicht: Position wird aus extraMarkers (3b-Output) geholt und
|
||||
* der Marker wird dem angegebenen Link hinzugefügt.
|
||||
*
|
||||
* Gibt { changed, change } oder { changed: false, error } zurück.
|
||||
*/
|
||||
export async function assignMarkerId(robotPath, { markerId, set, link, extraMarkers = [] }) {
|
||||
const id = Number(markerId);
|
||||
if (!Number.isFinite(id) || id < 0) return { changed: false, error: 'Ungültige Marker-ID.' };
|
||||
|
||||
const robot = await readRobot(robotPath);
|
||||
const links = robot.links ?? {};
|
||||
|
||||
// Marker in robot.json suchen
|
||||
let foundMarker = null, foundLink = null;
|
||||
outer: for (const [linkName, linkData] of Object.entries(links)) {
|
||||
for (const m of (linkData.markers ?? [])) {
|
||||
if (Number(m.id) === id) { foundMarker = m; foundLink = linkName; break outer; }
|
||||
}
|
||||
}
|
||||
|
||||
if (foundMarker) {
|
||||
// Bestehenden Marker: Set und ggf. Link aktualisieren
|
||||
const change = {
|
||||
action: 'updated',
|
||||
markerId: id,
|
||||
oldLink: foundLink,
|
||||
oldSet: foundMarker.set ?? '',
|
||||
newLink: foundLink,
|
||||
newSet: foundMarker.set ?? '',
|
||||
};
|
||||
|
||||
if (set !== undefined && set !== '') { foundMarker.set = set; change.newSet = set; }
|
||||
|
||||
if (link && link !== foundLink) {
|
||||
// In neuen Link verschieben
|
||||
links[foundLink].markers = links[foundLink].markers.filter(m => Number(m.id) !== id);
|
||||
if (!links[link]) links[link] = { markers: [] };
|
||||
if (!links[link].markers) links[link].markers = [];
|
||||
links[link].markers.push(foundMarker);
|
||||
change.newLink = link;
|
||||
}
|
||||
|
||||
robot.links = links;
|
||||
await writeRobot(robotPath, robot);
|
||||
return { changed: true, change };
|
||||
}
|
||||
|
||||
// Neuer Marker: Position aus 3b-Output holen
|
||||
const em = extraMarkers.find(m => Number(m.marker_id) === id);
|
||||
if (!em) {
|
||||
return {
|
||||
changed: false,
|
||||
error: `Marker ${id} ist nicht in robot.json und nicht im letzten 3b-Run vorhanden.`,
|
||||
};
|
||||
}
|
||||
if (!link) {
|
||||
return { changed: false, error: 'Link muss angegeben werden, um einen neuen Marker hinzuzufügen.' };
|
||||
}
|
||||
|
||||
const newMarker = {
|
||||
id,
|
||||
position: em.position_mm.map(v => Math.round(Number(v) * 100) / 100),
|
||||
};
|
||||
if (set) newMarker.set = set;
|
||||
|
||||
if (!links[link]) links[link] = { markers: [] };
|
||||
if (!links[link].markers) links[link].markers = [];
|
||||
links[link].markers.push(newMarker);
|
||||
|
||||
robot.links = links;
|
||||
await writeRobot(robotPath, robot);
|
||||
return {
|
||||
changed: true,
|
||||
change: { action: 'added', markerId: id, oldLink: null, oldSet: '', newLink: link, newSet: set ?? '' },
|
||||
};
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user