Skip to content

Commit

Permalink
more cleaning
Browse files Browse the repository at this point in the history
  • Loading branch information
@mayarajan3
mayarajan3 committed Nov 1, 2024
1 parent e06d4fb commit 469858c
Show file tree
Hide file tree
Showing 3 changed files with 115 additions and 113 deletions.
88 changes: 46 additions & 42 deletions extensions/src/doodlebot/LineFollowing.ts
View file
Original file line number Diff line number Diff line change
@@ -1,9 +1,12 @@
import * as Spline from "cubic-spline";
import * as Bezier from "bezier-js";
import { rebalanceCurve, rotateCurve } from "curve-matcher";
import { procrustes, applyTranslation, cutOffLineAtOverlap } from "./Procrustes";
import { procrustes } from "./Procrustes";
import { type Point, type ProcrustesResult, type RobotPosition, type Command, applyTranslation, cutOffLineAtOverlap, distanceBetweenPoints } from './LineHelper';

// CONSTANTS
const maxDistance = 100;
const epsilon = 0.4;
const bezierSamples = 3;
const controlLength = .02;
const lookahead = 0.06;
Expand All @@ -14,17 +17,25 @@ const verticalFOV = 41.41;
const cameraHeight = 0.098;
const tiltAngle = 41.5;

type Command = { radius: number, angle: number };
type Point = number[];
type RobotPosition = { x: number, y: number, angle: number };
type ProcrustesResult = { rotation: number, translation: number[] };

function cutOffLineOnDistance(line, maxDistance) {
let filteredLine = [line[0]]; // Start with the first point

for (let i = 1; i < line.length; i++) {
const point1 = line[i - 1];
const point2 = line[i];
const distance = distanceBetweenPoints(point1, point2);

// If the distance exceeds the threshold, stop adding points
if (distance > maxDistance) {
break;
}

function distanceBetweenPoints(x1: number, y1: number, x2: number, y2: number) {
const dx = x2 - x1;
const dy = y2 - y1;
return Math.sqrt(dx * dx + dy * dy);
filteredLine.push(point2); // Add the current point if within distance
}

return filteredLine;
}


function findPointAtDistanceWithIncrements(spline: Spline, increment: number, desiredDistance: number): number {
let totalDistance = 0;
Expand All @@ -43,7 +54,7 @@ function findPointAtDistanceWithIncrements(spline: Spline, increment: number, de

// Calculate distance between current and next increment
if (!isNaN(nextY)) {
const distance = distanceBetweenPoints(currentX, currentY, nextXIncrement, nextY);
const distance = distanceBetweenPoints([currentX, currentY], [nextXIncrement, nextY]);

totalDistance += distance;
if (totalDistance >= desiredDistance) {
Expand Down Expand Up @@ -135,10 +146,10 @@ function groupByYInterval(points: Point[], intervalSize: number): Point[] {
// If no point is far enough, return the endpoints.
return [points[0], points[points.length - 1]];
}
}
}


export function simplifyLine(points: Point[], epsilon: number, intervalSize: number): number[][] {
function simplifyLine(points: Point[], epsilon: number, intervalSize: number): number[][] {
// Group points by y-interval and pick the median x-coordinate in each group
const medianPoints = groupByYInterval(points, intervalSize);

Expand All @@ -147,16 +158,16 @@ function groupByYInterval(points: Point[], intervalSize: number): Point[] {
}


function perpendicularDistance(point: number[], lineStart: number[], lineEnd: number[]): number {
const [x, y] = point;
const [x1, y1] = lineStart;
const [x2, y2] = lineEnd;
const num = Math.abs((y2 - y1) * x - (x2 - x1) * y + x2 * y1 - y2 * x1);
const den = Math.sqrt(Math.pow(y2 - y1, 2) + Math.pow(x2 - x1, 2));
return den === 0 ? 0 : num / den;
}
function perpendicularDistance(point: number[], lineStart: number[], lineEnd: number[]): number {
const [x, y] = point;
const [x1, y1] = lineStart;
const [x2, y2] = lineEnd;

const num = Math.abs((y2 - y1) * x - (x2 - x1) * y + x2 * y1 - y2 * x1);
const den = Math.sqrt(Math.pow(y2 - y1, 2) + Math.pow(x2 - x1, 2));

return den === 0 ? 0 : num / den;
}


function rotateAndTranslateLine(line: Point[], angle: number, translation: number[]) {
Expand Down Expand Up @@ -290,21 +301,25 @@ function blendLines(entireLine: Point[], worldPoints: Point[], transitionLength:



export function followLine(previousPoints: Point[], worldPoints: Point[], delay: number, previousSpeed: number, previousCommands: {radius: number, angle: number}[]) {
let robotPosition = {x:0, y:0, angle:0};
export function followLine(previousLine: Point[], pixels: Point[], delay: number, previousSpeed: number, previousCommands: {radius: number, angle: number}[]) {
let worldPoints = simplifyLine(pixels, epsilon, 0.1);
worldPoints = cutOffLineOnDistance(worldPoints.filter((point: Point) => point[1] < 370), maxDistance);
worldPoints = worldPoints.map(point => pixelToGroundCoordinates(point));

let robotPosition = {x:0, y:0, angle:0};
for (const command of previousCommands) {
robotPosition = getRobotPositionAfterArc(command, robotPosition);
}

let wholeLine = rotateAndTranslateLine(previousPoints, -1*robotPosition.angle, [-1*robotPosition.x, -1*robotPosition.y]);
let wholeLine = rotateAndTranslateLine(previousLine, -1*robotPosition.angle, [-1*robotPosition.x, -1*robotPosition.y]);

// Cutting off segments to the overlap portion
let segment1 = showLineAboveY(wholeLine, Math.max(worldPoints[0][1], wholeLine[0][1]));
let segment2 = showLineBelowY(worldPoints, Math.min(wholeLine[wholeLine.length - 1][1], worldPoints[worldPoints.length - 1][1]))

let worldDistance = 0;
for (let i = 0; i < worldPoints.length - 1; i++) {
worldDistance += distanceBetweenPoints(worldPoints[i][0], worldPoints[i][1], worldPoints[i + 1][0], worldPoints[i + 1][1]);
worldDistance += distanceBetweenPoints(worldPoints[i], worldPoints[i + 1]);
}

let procrustesResult: ProcrustesResult;
Expand Down Expand Up @@ -366,9 +381,9 @@ export function followLine(previousPoints: Point[], worldPoints: Point[], delay:
y: point1.y + unitDy * controlLength
};

const x3 = spline.xs[0];
const x3 = previousSpeed*delay;
const point3 = {x: spline.at(x3), y: x3}
const reference1 = [spline.at(x3), 0]
const reference1 = [spline.at(spline.xs[0]), 0]
const reference2 = [0, 0]

let xOffset = reference1[0] - reference2[0];
Expand All @@ -380,7 +395,7 @@ export function followLine(previousPoints: Point[], worldPoints: Point[], delay:
point2
);

const motorCommands: { radius: number, angle: number}[] = [];
const motorCommands: Command[] = [];

const bezierPoints = bezierCurvePoints(bezier, bezierSamples);
for (let i = 0; i < bezierPoints.length - 1; i++) {
Expand All @@ -392,7 +407,7 @@ export function followLine(previousPoints: Point[], worldPoints: Point[], delay:
}


export function pixelToGroundCoordinates(pixelCoords: Point): { x: number, y: number } {
export function pixelToGroundCoordinates(pixelCoords: Point): Point {
// Based on Franklin's algorithm
const verticalPixels = imageDimensions[1]/verticalFOV;
const angleP = pixelCoords[1]/verticalPixels;
Expand Down Expand Up @@ -451,16 +466,9 @@ function calculateCurveBetweenPoints(pointA: RobotPosition, pointB: RobotPositio
const { x: x1, y: y1, angle: theta1Rad } = pointA;
const { x: x2, y: y2, angle: theta2Rad } = pointB;

// Calculate midpoint between points A and B
const midX = (x1 + x2) / 2;
const midY = (y1 + y2) / 2;

// Distance between points A and B
const distanceAB = Math.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2);

// Calculate the bisector of A and B in terms of orientation (90 degrees)
const angleBisector = Math.atan2(y2 - y1, x2 - x1) + Math.PI / 2;

// Determine the radius by projecting from the midpoint to a perpendicular circle center
const tanAngle = Math.tan((theta2Rad - theta1Rad) / 2);

Expand All @@ -471,10 +479,6 @@ function calculateCurveBetweenPoints(pointA: RobotPosition, pointB: RobotPositio
// Calculate the radius of the arc
const radius = distanceAB / (2 * tanAngle);

// Calculate the center of the circle based on direction (left or right of the line A-B)
const centerX = midX + radius * Math.cos(angleBisector);
const centerY = midY + radius * Math.sin(angleBisector);

// Calculate the angle to travel on the circumference in radians
const angleRad = 2 * Math.atan2(distanceAB, 2 * radius);

Expand Down
65 changes: 65 additions & 0 deletions extensions/src/doodlebot/LineHelper.ts
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,65 @@
export type Command = { radius: number, angle: number };
export type Point = number[];
export type RobotPosition = { x: number, y: number, angle: number };
export type ProcrustesResult = { rotation: number, translation: number[] };

export function distanceBetweenPoints(p1: Point, p2: Point): number {
return Math.sqrt((p1[0] - p2[0]) ** 2 + (p1[1] - p2[1]) ** 2);
}

function findClosestPoint(line: Point[], targetPoint: Point): number {
let closestPointIndex = 0;
let minDistance = Infinity;

line.forEach((point, index) => {
const distance = distanceBetweenPoints(point, targetPoint);
if (distance < minDistance) {
minDistance = distance;
closestPointIndex = index;
}
});

return closestPointIndex; // Return the index of the closest point
}

export function cutOffLineAtOverlap(line1: Point[], line2: Point[]): { line: Point[], distance: number, overlap: Boolean } {
const line2StartPoint = line2[0];
let closestPointIndex: number;
const line1End = line1[line1.length - 1];
const line2End = line2[line2.length - 1];

let finalLine: Point[];
let overlap = false;

closestPointIndex = findClosestPoint(line1, line2StartPoint);
if (line2End[1] > line1End[1]) {
finalLine = line1.slice(0, closestPointIndex + 1);

} else {
finalLine = line1;
overlap = true;
}

const trimmedLine = line1.slice(0, closestPointIndex + 1);

let totalDistance = 0;

for (let i = 0; i < trimmedLine.length - 1; i++) {
const point1 = trimmedLine[i];
const point2 = trimmedLine[i + 1];

const distance = Math.sqrt(
Math.pow(point2[0] - point1[0], 2) + Math.pow(point2[1] - point1[1], 2)
);
totalDistance += distance;
}

return {line: finalLine, distance: totalDistance, overlap};
}

export function applyTranslation(line: Point[], translationVector: number[]) {
return line.map(point => [
point[0] + translationVector[0],
point[1] + translationVector[1]
]);
}
75 changes: 4 additions & 71 deletions extensions/src/doodlebot/Procrustes.ts
View file
Original file line number Diff line number Diff line change
@@ -1,18 +1,14 @@
//import { procrustes } from "./transform-lines-2";
import {
procrustesNormalizeCurve,
procrustesNormalizeRotation,
findProcrustesRotationAngle,
rebalanceCurve,
shapeSimilarity,
rotateCurve
} from 'curve-matcher';

type Point = number[];
import { type Point, type ProcrustesResult, applyTranslation, distanceBetweenPoints } from './LineHelper';

function distanceBetweenPoints(p1: Point, p2: Point): number {
return Math.sqrt((p1[0] - p2[0]) ** 2 + (p1[1] - p2[1]) ** 2);
}

function calculateCentroid(line: Point[]) {
const n = line.length;
Expand All @@ -23,13 +19,6 @@ function calculateCentroid(line: Point[]) {
return [sum[0] / n, sum[1] / n]; // Return the average of the x and y coordinates
}

export function applyTranslation(line: Point[], translationVector: number[]) {
return line.map(point => [
point[0] + translationVector[0],
point[1] + translationVector[1]
]);
}

function getSublinesOfLength(line: Point[], totalDistance: number) {
let sublines = [];

Expand All @@ -55,7 +44,6 @@ function getSublinesOfLength(line: Point[], totalDistance: number) {
return sublines;
}


function findOptimalTranslation(line1: Point[], line2: Point[]) {
const centroid1 = calculateCentroid(line1);
const centroid2 = calculateCentroid(line2);
Expand All @@ -81,8 +69,8 @@ function mapLine(line: Point[]) {
return line.map((point: Point) => ({ x: point[0], y: point[1] }))
}

function mapCurve(line: Point[]) {
return line.map((point: Point) => [point.x, point.y]);
function mapCurve(line: {x: number, y: number}[]) {
return line.map((point: {x: number, y: number}) => [point.x, point.y]);
}

function getError(line1: Point[], line2: Point[]) {
Expand All @@ -102,58 +90,7 @@ function getError(line1: Point[], line2: Point[]) {
return {curve1: translatedLine1, curve2: points2, rotation: rotation, translation: translationVector}
}

function findClosestPoint(line: Point[], targetPoint: Point): number {
let closestPointIndex = 0;
let minDistance = Infinity;

line.forEach((point, index) => {
const distance = distanceBetweenPoints(point, targetPoint);
if (distance < minDistance) {
minDistance = distance;
closestPointIndex = index;
}
});

return closestPointIndex; // Return the index of the closest point
}

export function cutOffLineAtOverlap(line1: Point[], line2: Point[]): { line: Point[], distance: number, overlap: Boolean } {
const line2StartPoint = line2[0];
let closestPointIndex: number;
const line1End = line1[line1.length - 1];
const line2End = line2[line2.length - 1];

let finalLine: Point[];
let overlap = false;

closestPointIndex = findClosestPoint(line1, line2StartPoint);
if (line2End[1] > line1End[1]) {
finalLine = line1.slice(0, closestPointIndex + 1);

} else {
finalLine = line1;
overlap = true;
}

const trimmedLine = line1.slice(0, closestPointIndex + 1);

let totalDistance = 0;

for (let i = 0; i < trimmedLine.length - 1; i++) {
const point1 = trimmedLine[i];
const point2 = trimmedLine[i + 1];

const distance = Math.sqrt(
Math.pow(point2[0] - point1[0], 2) + Math.pow(point2[1] - point1[1], 2)
);
totalDistance += distance;
}

return {line: finalLine, distance: totalDistance, overlap};
}


export function procrustes(line1: Point[], line2: Point[], ratio=0.5): { rotation: number, translation: number[] } {
export function procrustes(line1: Point[], line2: Point[], ratio=0.5): ProcrustesResult {
line1 = mapCurve(rebalanceCurve(mapLine(line1), {}));
line2 = mapCurve(rebalanceCurve(mapLine(line2), {}));

Expand Down Expand Up @@ -185,10 +122,6 @@ export function procrustes(line1: Point[], line2: Point[], ratio=0.5): { rotatio
}

const { rotation, translation } = getError(maxLine, line2Filtered);
let rotatedCurve1 = rotateCurve(mapLine(line1), rotation);
let translatedLine1 = applyTranslation(mapCurve(rotatedCurve1), translation);

const end = cutOffLineAtOverlap(translatedLine1, line2Filtered);

return {rotation, translation };
}
Expand Down

0 comments on commit 469858c

Please sign in to comment.