X-Position

X-Position from Fotos. Kallibrierung

public/calculateAngles.js

@@ -4,24 +4,127 @@
  * Browser + Server + Jest (CJS) kompatibel
  */
 
+function calculateXPos(listIdAndX, jsonRobot){
+    const partsMovingFixedX = new Set(['Base', 'Arm1', 'Joint1']);
+    const markersMovingFixedX = jsonRobot.Marker.filter(m => partsMovingFixedX.has(m.on));
+  // Join: Robot-Marker ↔ Found-Marker
+
+  const markersListeWithRobotInfo = jsonRobot.Marker
+  .filter(m => partsMovingFixedX.has(m.on))   // nur relevante Teile
+  .map(m => {
+    const found = listIdAndX.get(m.id);
+    if (!found) return null;
+
+    return {
+      id: m.id,
+      relPos: m.relPos,   // oder m.relPos, je nach Struktur
+      position: found,
+      Px: found[0] - m.relPos[0]
+    };
+  })
+  .filter(Boolean); // nulls entfernen
+
+  const pxValues = markersListeWithRobotInfo.map(m => m.Px);
+  const meanPx =  pxValues.reduce((sum, x) => sum + x, 0) / pxValues.length;
+
+  const variancePx =  pxValues.reduce((sum, x) => sum + Math.pow(x - meanPx, 2), 0) / pxValues.length;
+  const stdDevPx = Math.sqrt(variancePx);
+
+  return { meanPx, stdDevPx };
+}
+
+
+function optimizeRobot(listFoundMarkers, jsonRobot) {
+  
+  const map = new Map();
+
+  for (const foundMarkers of listFoundMarkers) {
+    var x_222_226 = null;
+    var x_222_226_count = 0;
+    
+    for (const mark of foundMarkers.markers.filter(m => m.id === 226).map(f => [f.id, f.position_mm])) {     
+      x_222_226 = mark[1][0];
+      x_222_226_count++;
+    }
+    if (x_222_226_count > 0) {
+      x_222_226 = x_222_226 / x_222_226_count;
+    }
+    else{
+      continue; // Wenn weder 222 noch 226 gefunden wurden, überspringen
+    }
+
+    for (const mark of foundMarkers.markers.map(f => [f.id, f.position_mm ])){
+      const id = mark[0];
+      const dx_222_226 = mark[1][0] - x_222_226;
+      if (!map.has(id)) {
+        map.set(id, []); // Initialisiere mit  x_222_226, damit wir später die Abweichung berechnen können
+      }
+      map.get(id).push(dx_222_226);
+    }
+  }
+
+  const result = Array.from(map, ([id, mm]) => ({ id, mm })).filter(m => [198,200,204,229,243].includes(m.id));
+
+  
+  const withStats = result.map(entry => {
+    const { mm } = entry;
+    const n = mm.length;
+
+    if (n === 0) {
+      return {
+        ...entry,
+        n: 0,
+        average: null,
+        deviation: null
+      };
+    }
+
+    const average = mm.reduce((a, b) => a + b, 0) / n;
+
+    const deviation = Math.sqrt(
+      mm.reduce((sum, x) => sum + Math.pow(x - average, 2), 0) / n
+    );
+
+    return {
+      ...entry,
+      n,
+      average,
+      deviation
+    };
+  });
+
+  return withStats;
+}
+function calculateRotAngle(listIdAndX, jsonRobot, jointName){
+  // Achse finden
+  const jointInfo = jsonRobot.Joints[jointName];
+  if(!jointInfo){return null, null; }    
+  if(jointInfo.type !== 'revolute'){ return null, null; }
+  if(!(jointInfo.origin)){ return null, null; }
+  if(!(jointInfo.axis)){ return null, null; }
+  if(!(jointInfo.child)){ return null, null; }
+  
+  markerUsed = jsonRobot.Marker.filter(m => m.on === jointInfo.child)
+  if(markerUsed.length === 0){ return null, null; }
+
+  var markerFound = []
+  for (const m of markerUsed){
+    markerFound.push(listIdAndX.get(m.id));
+  }
+
+
+  var angle = 0;
+  var varAngle = 0;
+
+  return angle, varAngle;
+}
+
 function calculate(foundMarkers, jsonRobot) {
-  // --- X of the Hand-Joint ---
-  const validOns = new Set(['Base', 'Arm1', 'Joint1']);
-  const foundById = new Map(foundMarkers.map(f => [f.id, f]));
 
-  const markersMovingFixedX = jsonRobot.Marker.filter(m => validOns.has(m.on));
-  const markersXPosition = new Map();
+  const foundById = new Map(foundMarkers.markers.map(f => [f.id, f.position_mm ]));
+  const { meanPx: x, stdDevPx: varx } = calculateXPos(foundById, jsonRobot);
 
-  const xs = markersMovingFixedX
-    .map(m => {
-      const f = foundById.get(m.id);
-      return f ? f.position[0] - m.relPos[0] : null;
-    })
-    .filter(x => x !== null);
-
-  const xAvg = xs.length
-    ? xs.reduce((a, b) => a + b, 0) / xs.length
-    : 0;
+  const { meanY: y, stdDevY: vary } = calculateRotAngle(foundById, jsonRobot, "jointB");
 
   return {
     meta: {
@@ -34,10 +137,11 @@ function calculate(foundMarkers, jsonRobot) {
     },
     status: 'ok',
     result: {
-      xAvg
+      x: x,
+      varx: varx 
     }
   };
 }
 
 
-module.exports = { calculate };
+module.exports = { calculate, optimizeRobot };