updates

extensions/src/doodlebot/LineFollowing.ts

@@ -2,7 +2,7 @@ import * as Spline from "cubic-spline";
 import * as Bezier from "bezier-js";
 import { rebalanceCurve, rotateCurve } from "curve-matcher";
 import { procrustes } from "./Procrustes";
-import { type Point, type ProcrustesResult, type RobotPosition, type Command, applyTranslation, cutOffLineAtOverlap, distanceBetweenPoints } from './LineHelper';
+import { type Point, type ProcrustesResult, type RobotPosition, type Command, calculateLineError, applyTranslation, cutOffLineAtOverlap, distanceBetweenPoints } from './LineHelper';
 
 // CONSTANTS
 const maxDistance = 100;
@@ -366,9 +366,9 @@ export function followLine(previousLine: Point[], pixels: Point[], next: Point[]
     for (const command of previousCommands) {
       if (command.radius == Infinity) {
         robotPosition.y = robotPosition.y + command.distance;
-    } else {
-        robotPosition = getRobotPositionAfterArc(command, robotPosition);
-    }
+      } else {
+          robotPosition = getRobotPositionAfterArc(command, robotPosition);
+      }
     }
 
     // Guess the location of the previous line
@@ -389,8 +389,31 @@ export function followLine(previousLine: Point[], pixels: Point[], next: Point[]
     if (previousCommands.length == 0) {
         procrustesResult = procrustes(segment1, segment2);
     } else if (worldDistance > 0.05) {
-        // TODO: check if line 2 is much smaller than line 1, then use segment1 and segment2. Otherwise, use guessLine and worldPoints
-        procrustesResult = procrustes(guessLine, worldPoints, 0.5);
+        const scaleValues = [];
+        const start = 0.1;
+        const end = 1;
+        const interval = 0.01;
+        for (let value = start; value <= end; value += interval) {
+            scaleValues.push(value); // Keeps precision at 1 decimal
+        }
+        let lowestError = Infinity;
+        let bestResult = null;
+        scaleValues.forEach(scale => {
+            // Apply the Procrustes transformation
+            let result = procrustes(guessLine, worldPoints, scale);
+            // Calculate cumulative error
+            console.log(guessLine);
+            let guessLine2 = rotateCurve(guessLine.map(point => ({x: point[0], y: point[1]})), result.rotation).map((point: { x: number, y: number }) => [point.x, point.y]);
+            guessLine2 = applyTranslation(guessLine2, result.translation);    
+            guessLine2 = showLineAboveY(guessLine2, 0);
+            let cumulativeError = calculateLineError(worldPoints, guessLine2)
+            // Update if we find a lower cumulative error
+            if (cumulativeError < lowestError) {
+                lowestError = cumulativeError;
+                bestResult = result;
+            }
+        });
+        procrustesResult = bestResult;
     } else {
         // If the current frame doesn't contain that many points, just use previous guess
         procrustesResult = { translation: [0, 0], rotation: 0, distance: 0 };

extensions/src/doodlebot/LineHelper.ts

@@ -1,12 +1,17 @@
 export type Command = { radius: number, angle: number, distance: number };
 export type Point = number[];
+export type PointObject = {x: number, y: number};
 export type RobotPosition = { x: number, y: number, angle: number };
 export type ProcrustesResult = { rotation: number, translation: number[], distance: number };
 
 export function distanceBetweenPoints(p1: Point, p2: Point): number {
     return Math.sqrt((p1[0] - p2[0]) ** 2 + (p1[1] - p2[1]) ** 2);
 }
 
+function distance(p1: PointObject, p2: PointObject): number {
+    return Math.sqrt((p2.x - p1.x) ** 2 + (p2.y - p1.y) ** 2);
+}
+
 function findClosestPoint(line: Point[], targetPoint: Point): number {
     let closestPointIndex = 0;
     let minDistance = Infinity;
@@ -61,4 +66,109 @@ export function applyTranslation(line: Point[], translationVector: number[]) {
         point[0] + translationVector[0],
         point[1] + translationVector[1]
     ]);
+}
+
+export function calculateLineError(line1: Point[], line2: Point[]) {
+    // Filter line points based on the overlapping y-range
+    const yMin = Math.max(
+        Math.min(...line1.map(([x, y]) => y)),
+        Math.min(...line2.map(([x, y]) => y))
+    );
+    const yMax = Math.min(
+        Math.max(...line1.map(([x, y]) => y)),
+        Math.max(...line2.map(([x, y]) => y))
+    );
+
+    // Filter both lines to keep only points within the overlapping y range
+    const line1Filtered = line1.filter(([x, y]) => y >= yMin && y <= yMax);
+    const line2Filtered = line2.filter(([x, y]) => y >= yMin && y <= yMax);
+
+    // Calculate cumulative translation error by finding the closest point
+    let cumulativeError = 0;
+    let count = 0;
+
+    for (const [x1, y1] of line1Filtered) {
+        let minDistance = Infinity;
+
+        for (const [x2, y2] of line2Filtered) {
+            // Calculate Euclidean distance between points
+            const distance = Math.sqrt((x1 - x2) ** 2 + (y1 - y2) ** 2);
+            if (distance < minDistance) {
+                minDistance = distance;
+            }
+        }
+
+        // Sum the minimum distance found for this point in line1
+        cumulativeError += minDistance;
+        count++;
+    }
+
+    // Calculate average translation error
+    const averageError = count > 0 ? cumulativeError / count : 0;
+    return averageError;
+}
+
+function lerp(a: PointObject, b: PointObject, t: number): PointObject {
+    return {
+        x: a.x + (b.x - a.x) * t,
+        y: a.y + (b.y - a.y) * t
+    };
+}
+
+function bezierMidpoint(P1: PointObject, C1: PointObject, C2: PointObject, P2: PointObject): PointObject {
+    const A = lerp(P1, C1, 0.5);
+    const B = lerp(C1, C2, 0.5);
+    const C = lerp(C2, P2, 0.5);
+    const D = lerp(A, B, 0.5);
+    const E = lerp(B, C, 0.5);
+    return lerp(D, E, 0.5);
+}
+
+function findSingleCircle(
+P1: PointObject,
+P2: PointObject,
+midpoint: PointObject
+): { center: PointObject; radius: number; angle: number } | null {
+    const mid1 = { x: (P1.x + midpoint.x) / 2, y: (P1.y + midpoint.y) / 2 };
+    const mid2 = { x: (P2.x + midpoint.x) / 2, y: (P2.y + midpoint.y) / 2 };
+
+    const dir1 = { x: midpoint.y - P1.y, y: P1.x - midpoint.x };
+    const dir2 = { x: midpoint.y - P2.y, y: P2.x - midpoint.x };
+
+    const det = dir1.x * dir2.y - dir1.y * dir2.x;
+    if (Math.abs(det) < 1e-9) return null;
+
+    const dx = mid2.x - mid1.x;
+    const dy = mid2.y - mid1.y;
+    const u = (dy * dir2.x - dx * dir2.y) / det;
+
+    const center = {
+        x: mid1.x + u * dir1.x,
+        y: mid1.y + u * dir1.y
+    };
+
+    const radiusInPixels = distance(center, P1);
+    const radiusInInches = radiusInPixels * 39.3701; // Convert pixels to inches
+
+    // Calculate angles in radians
+    const angle1 = Math.atan2(P1.y - center.y, P1.x - center.x);
+    const angle2 = Math.atan2(P2.y - center.y, P2.x - center.x);
+
+    // Calculate the angle difference
+    let angleInDegrees = (angle2 - angle1) * (180 / Math.PI);
+
+    // Normalize the angle to range [-180, 180]
+    if (angleInDegrees > 180) {
+        angleInDegrees -= 360;
+    } else if (angleInDegrees < -180) {
+        angleInDegrees += 360;
+    }
+
+    return { center, radius: radiusInInches, angle: -1*angleInDegrees };
+}
+
+
+export function approximateBezierWithArc(P1: PointObject, C1: PointObject, C2: PointObject, P2: PointObject): { center: PointObject; radius: number; angle: number } | null {
+    const midpoint = bezierMidpoint(P1, C1, C2, P2);
+    return findSingleCircle(P1, P2, midpoint);
 }