Skip to content

Commit

Permalink
commenting
Browse files Browse the repository at this point in the history
  • Loading branch information
@mayarajan3
mayarajan3 committed Nov 1, 2024
1 parent 469858c commit 3d91321
Show file tree
Hide file tree
Showing 2 changed files with 93 additions and 70 deletions.
136 changes: 76 additions & 60 deletions extensions/src/doodlebot/LineFollowing.ts
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@ const verticalFOV = 41.41;
const cameraHeight = 0.098;
const tiltAngle = 41.5;

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

for (let i = 1; i < line.length; i++) {
Expand Down Expand Up @@ -227,9 +227,10 @@ function getRobotPositionAfterArc(command: Command, initialPosition: RobotPositi
function showLineAboveY(line: Point[], yLimit: number) {
const newLine: Point[] = [];

// Only add point if y is above limit
for (let i = 0; i < line.length; i++) {
const [x, y] = line[i];
if (y > yLimit) {
if (y >= yLimit) {
newLine.push([x, y]);
}
}
Expand All @@ -242,9 +243,10 @@ function showLineAboveY(line: Point[], yLimit: number) {
function showLineBelowY(line: Point[], yLimit: number) {
const newLine: Point[] = [];

// Only add point is y is below limit
for (let i = 0; i < line.length; i++) {
const [x, y] = line[i];
if (y < yLimit) {
if (y <= yLimit) {
newLine.push([x, y]);
}
}
Expand All @@ -254,53 +256,59 @@ function showLineBelowY(line: Point[], yLimit: number) {


function smoothLine(line: Point[], windowSize = 3) {
const smoothedLine: Point[] = [];
for (let i = 0; i < line.length; i++) {
let start = Math.max(0, i - Math.floor(windowSize / 2));
let end = Math.min(line.length, i + Math.floor(windowSize / 2) + 1);
let sumX = 0;
let sumY = 0;

for (let j = start; j < end; j++) {
sumX += line[j][0];
sumY += line[j][1];
}
const smoothedLine: Point[] = [];

for (let i = 0; i < line.length; i++) {
// Define the range of indices for the smoothing window
let start = Math.max(0, i - Math.floor(windowSize / 2));
let end = Math.min(line.length, i + Math.floor(windowSize / 2) + 1);

// Sum the x and y values within the window
let sumX = 0;
let sumY = 0;
for (let j = start; j < end; j++) {
sumX += line[j][0];
sumY += line[j][1];
}

let count = end - start;
smoothedLine.push([sumX / count, sumY / count]);
}
// Push the averaged point to the smoothed line
let count = end - start;
smoothedLine.push([sumX / count, sumY / count]);
}

return smoothedLine;
return smoothedLine;
}



function blendLines(entireLine: Point[], worldPoints: Point[], transitionLength: number = 3) {
// Start with the non-overlapping portion of entireLine
const blendedLine = [...cutOffLineAtOverlap(entireLine, worldPoints).line];

const blendedLine = [...cutOffLineAtOverlap(entireLine, worldPoints).line];

if (entireLine.length < transitionLength || worldPoints.length < transitionLength) {
return [...entireLine, ...worldPoints];
}
// If either line is too short for blending, return them joined directly
if (entireLine.length < transitionLength || worldPoints.length < transitionLength) {
return [...entireLine, ...worldPoints];
}

for (let i = 0; i < transitionLength; i++) {
const t = i / (transitionLength - 1); // Transition factor (0 to 1)
const [x1, y1] = entireLine[entireLine.length - transitionLength + i];
const [x2, y2] = worldPoints[i];
// Blend the transition region between entireLine and worldPoints
for (let i = 0; i < transitionLength; i++) {
const t = i / (transitionLength - 1); // Interpolation factor (0 to 1)
const [x1, y1] = entireLine[entireLine.length - transitionLength + i];
const [x2, y2] = worldPoints[i];

const blendedX = (1 - t) * x1 + t * x2;
const blendedY = (1 - t) * y1 + t * y2;
// Linearly interpolate between corresponding points
const blendedX = (1 - t) * x1 + t * x2;
const blendedY = (1 - t) * y1 + t * y2;

blendedLine.push([blendedX, blendedY]);
}
blendedLine.push([blendedX, blendedY]);
}

blendedLine.push(...worldPoints.slice(transitionLength));
// Append remaining points from worldPoints after the transition region
blendedLine.push(...worldPoints.slice(transitionLength));

return blendedLine;
return blendedLine;
}



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);
Expand All @@ -311,81 +319,87 @@ export function followLine(previousLine: Point[], pixels: Point[], delay: number
robotPosition = getRobotPositionAfterArc(command, robotPosition);
}

let wholeLine = rotateAndTranslateLine(previousLine, -1*robotPosition.angle, [-1*robotPosition.x, -1*robotPosition.y]);
// Guess the location of the previous line
let guessLine = 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 segment1 = showLineAboveY(guessLine, Math.max(worldPoints[0][1], guessLine[0][1]));
let segment2 = showLineBelowY(worldPoints, Math.min(guessLine[guessLine.length - 1][1], worldPoints[worldPoints.length - 1][1]))

// Distance of the world line
let worldDistance = 0;
for (let i = 0; i < worldPoints.length - 1; i++) {
worldDistance += distanceBetweenPoints(worldPoints[i], worldPoints[i + 1]);
}

// Collect the error between guess and reality
let procrustesResult: ProcrustesResult;
if (previousCommands.length == 0) {
procrustesResult = procrustes(segment1, segment2);
} else if (worldDistance > 0.05) {
// TODO: check if line2 is much smaller than line 1, then use segment1 and segment2. Otherwise, use all the lines
procrustesResult = procrustes(wholeLine, worldPoints, 0.5);
procrustesResult = procrustes(guessLine, worldPoints, 0.5);
} else {
// If the current frame doesn't contain that many points, just use previous guess
procrustesResult = { translation: [0, 0], rotation: 0 };
}

wholeLine = rotateCurve(wholeLine.map((point: Point) => ({x: point[0], y: point[1]})), procrustesResult.rotation).map((point: number) => [point.x, point.y]);
wholeLine = applyTranslation(wholeLine, procrustesResult.translation);
wholeLine = showLineAboveY(wholeLine, 0);
// Correct the guess of the previous line
let line = rotateCurve(guessLine.map((point: Point) => ({x: point[0], y: point[1]})), procrustesResult.rotation).map((point: number) => [point.x, point.y]);
line = applyTranslation(line, procrustesResult.translation);
line = showLineAboveY(line, 0);

if (worldDistance > 0.05) {
let trimmedLine = cutOffLineAtOverlap(wholeLine, worldPoints);
wholeLine = trimmedLine.overlap ? trimmedLine.line : blendLines(trimmedLine.line, worldPoints);
// If we have enough points to append, add the new portion of the current camera frame
let trimmedLine = cutOffLineAtOverlap(line, worldPoints);
line = trimmedLine.overlap ? trimmedLine.line : blendLines(trimmedLine.line, worldPoints);
}

wholeLine = smoothLine(wholeLine);
wholeLine = rebalanceCurve(wholeLine.map((point: Point) => ({x: point[0], y: point[1]})), {}).map(point => [point.x, point.y]);
line = smoothLine(line);
line = rebalanceCurve(line.map((point: Point) => ({x: point[0], y: point[1]})), {}).map((point: {x: number, y: number}) => [point.x, point.y]);

// Remove duplicate y values
const seenY = new Set();
wholeLine = wholeLine.filter((point: Point) => {
line = line.filter((point: Point) => {
const y = point[1];
if (seenY.has(y)) {
return false;
}
seenY.add(y);
return true;
});
const xs = wholeLine.map((point: Point) => point[0]);
const ys = wholeLine.map((point: Point) => point[1]);


// Create the spline
const xs = line.map((point: Point) => point[0]);
const ys = line.map((point: Point) => point[1]);
const spline = new Spline.default(ys, xs); // Opposite so we get the x values

// Find the end point for the Bezier curve
const distance = previousSpeed*delay + lookahead;

const x1 = findPointAtDistanceWithIncrements(spline, 0.001, distance - .01);
const x2 = findPointAtDistanceWithIncrements(spline, 0.001, distance);

const point1 = {x: spline.at(x1), y: x1}
const point2 = {x: spline.at(x2), y: x2}

// Calculate the direction vector from point2 to point1
// Extend point1 in the direction of the unit vector to make the Bezier control point
const dx = point1.x - point2.x;
const dy = point1.y - point2.y;

// Normalize the direction vector
const length = Math.sqrt(dx * dx + dy * dy);
const unitDx = dx / length;
const unitDy = dy / length;

// Extend point1 in the direction of the unit vector

const extendedPoint1 = {
x: point1.x + unitDx * controlLength,
y: point1.y + unitDy * controlLength
};

const x3 = previousSpeed*delay;
// Find the start point for the Bezier curve -- account for camera latency
const x3 = previousSpeed * delay;
const point3 = {x: spline.at(x3), y: x3}

// Find the x offset to correct
const reference1 = [spline.at(spline.xs[0]), 0]
const reference2 = [0, 0]

let xOffset = reference1[0] - reference2[0];

const bezier = new Bezier.Bezier(
Expand All @@ -397,12 +411,14 @@ export function followLine(previousLine: Point[], pixels: Point[], delay: number

const motorCommands: Command[] = [];

// Split the Bezier curve into a series of arcs
const bezierPoints = bezierCurvePoints(bezier, bezierSamples);
for (let i = 0; i < bezierPoints.length - 1; i++) {
const command = calculateCurveBetweenPoints(bezierPoints[i], bezierPoints[i+1]);
motorCommands.push(command);
}
return {motorCommands, bezierPoints, line: [...wholeLine]};

return {motorCommands, bezierPoints, line};

}

Expand Down
27 changes: 17 additions & 10 deletions extensions/src/doodlebot/Procrustes.ts
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,11 +53,11 @@ function findOptimalTranslation(line1: Point[], line2: Point[]) {
centroid1[1] - centroid2[1]
];

const translatedLine2 = applyTranslation(line2, translationVector);
const translatedLine = applyTranslation(line2, translationVector);

return {
translationVector,
translatedLine2
translatedLine
};
}

Expand All @@ -74,45 +74,51 @@ function mapCurve(line: {x: number, y: number}[]) {
}

function getError(line1: Point[], line2: Point[]) {
// Normalize and balance each curve
const balanced1 = procrustesNormalizeCurve(rebalanceCurve(mapLine(line1), {}));
const balanced2 = procrustesNormalizeCurve(rebalanceCurve(mapLine(line2), {}))
const rotation = findProcrustesRotationAngle(balanced1, balanced2);

// Find the rotation between the two lines
const rotation = findProcrustesRotationAngle(balanced1, balanced2);

const points1 = rebalanceLine(line1);
const points2 = rebalanceLine(line2);

let rotatedCurve1 = rotateCurve(mapLine(points1), rotation);

const {translatedLine2, translationVector} = findOptimalTranslation(points2, mapCurve(rotatedCurve1));
const translatedLine1 = translatedLine2;
// Find the translation between the two lines
const {translatedLine, translationVector} = findOptimalTranslation(points2, mapCurve(rotatedCurve1));

return {curve1: translatedLine1, curve2: points2, rotation: rotation, translation: translationVector}
return {curve1: translatedLine, curve2: points2, rotation: rotation, translation: translationVector}
}

export function procrustes(line1: Point[], line2: Point[], ratio=0.5): ProcrustesResult {
// Balance each line to have the same number of points
line1 = mapCurve(rebalanceCurve(mapLine(line1), {}));
line2 = mapCurve(rebalanceCurve(mapLine(line2), {}));

const yValues = line2.map(point => point[1]);
const minY = Math.min(...yValues);
const maxY = Math.max(...yValues);

let midY: number;
// Get the first ratio % of the 2nd line
const range = maxY - minY;
midY = minY + range*ratio;
const midY = minY + range*ratio;
const line2Filtered = line2.filter(point => point[1] >= minY && point[1] <= midY);

// Get the distance of the filtered line
let totalDistance = 0;
for (let i = 0; i < line2Filtered.length - 1; i++) {
totalDistance += distanceBetweenPoints(line2Filtered[i], line2Filtered[i + 1]);
}

// Get a list of segments from the first line with around the same distance as the first line
// TODO: Make this a range instead of a value
let sublines = getSublinesOfLength(line1, totalDistance);
sublines = [...sublines]

// Get the most similar segment to the filtered second line
let maxSimilarity = 0;
let maxLine = sublines[0];

for (const line of sublines) {
const similarity = shapeSimilarity(mapLine(line), mapLine(line2Filtered), {checkRotations: false});
if (similarity > maxSimilarity) {
Expand All @@ -121,6 +127,7 @@ export function procrustes(line1: Point[], line2: Point[], ratio=0.5): Procruste
}
}

// Calculate the error between the most similar segments
const { rotation, translation } = getError(maxLine, line2Filtered);

return {rotation, translation };
Expand Down

0 comments on commit 3d91321

Please sign in to comment.