Exercise 3 | Line fitting and extraction for robot ... · function [alpha, r] = fitLine(XY) %...
Transcript of Exercise 3 | Line fitting and extraction for robot ... · function [alpha, r] = fitLine(XY) %...
![Page 1: Exercise 3 | Line fitting and extraction for robot ... · function [alpha, r] = fitLine(XY) % Compute the centroid of the point set (xmw, ymw) considering that % the centroid of a](https://reader034.fdocuments.us/reader034/viewer/2022052010/603a0a55a7f1c0299973ac47/html5/thumbnails/1.jpg)
|Autonomous Mobile RobotsRoland Siegwart, Margarita Chli, Nick Lawrance
ASL Autonomous Systems Lab
Lukas Bernreiter and Hermann Blum
2019 | Exercise 3 1
Exercise 3 | Line fitting and extraction for robotlocalization
Spring 2019
![Page 2: Exercise 3 | Line fitting and extraction for robot ... · function [alpha, r] = fitLine(XY) % Compute the centroid of the point set (xmw, ymw) considering that % the centroid of a](https://reader034.fdocuments.us/reader034/viewer/2022052010/603a0a55a7f1c0299973ac47/html5/thumbnails/2.jpg)
|Autonomous Mobile RobotsRoland Siegwart, Margarita Chli, Nick Lawrance
ASL Autonomous Systems Lab
2019 | Exercise 3 2
Line extraction, EKF, SLAM
Exercise 3- Line extraction- Line fitting
Exercise 4- EKF- Localization:
Line extraction,given map
- Wheel odometry
Exercise 5- Simultaneous Localization
and Mapping (SLAM)- Unknown environment
(a-priori)
![Page 3: Exercise 3 | Line fitting and extraction for robot ... · function [alpha, r] = fitLine(XY) % Compute the centroid of the point set (xmw, ymw) considering that % the centroid of a](https://reader034.fdocuments.us/reader034/viewer/2022052010/603a0a55a7f1c0299973ac47/html5/thumbnails/3.jpg)
|Autonomous Mobile RobotsRoland Siegwart, Margarita Chli, Nick Lawrance
ASL Autonomous Systems Lab
2019 | Exercise 3 3
Describing lines using polar coordinates
Two parameter description, i.e.
Why switch to polar parameters?
Vertical lines are not representable in Cartesian coordinates (linear eq.)
In general simpler representation for e.g. lines or circles
Where does the line equation expressed in polar coordinates come from?
Pythagorean theorem yields
With and
![Page 4: Exercise 3 | Line fitting and extraction for robot ... · function [alpha, r] = fitLine(XY) % Compute the centroid of the point set (xmw, ymw) considering that % the centroid of a](https://reader034.fdocuments.us/reader034/viewer/2022052010/603a0a55a7f1c0299973ac47/html5/thumbnails/4.jpg)
|Autonomous Mobile RobotsRoland Siegwart, Margarita Chli, Nick Lawrance
ASL Autonomous Systems Lab
2019 | Exercise 3 5
Line fitting / Line regression
Squared error between the line and all points
Solution for the line:
Task 1:
Derive the line parameters using least squares, i.e.
![Page 5: Exercise 3 | Line fitting and extraction for robot ... · function [alpha, r] = fitLine(XY) % Compute the centroid of the point set (xmw, ymw) considering that % the centroid of a](https://reader034.fdocuments.us/reader034/viewer/2022052010/603a0a55a7f1c0299973ac47/html5/thumbnails/5.jpg)
|Autonomous Mobile RobotsRoland Siegwart, Margarita Chli, Nick Lawrance
ASL Autonomous Systems Lab
2019 | Exercise 3 8
Line fitting / Line regression
Task 1:
![Page 6: Exercise 3 | Line fitting and extraction for robot ... · function [alpha, r] = fitLine(XY) % Compute the centroid of the point set (xmw, ymw) considering that % the centroid of a](https://reader034.fdocuments.us/reader034/viewer/2022052010/603a0a55a7f1c0299973ac47/html5/thumbnails/6.jpg)
|Autonomous Mobile RobotsRoland Siegwart, Margarita Chli, Nick Lawrance
ASL Autonomous Systems Lab
2019 | Exercise 3 11
Line fitting / Line regression
![Page 7: Exercise 3 | Line fitting and extraction for robot ... · function [alpha, r] = fitLine(XY) % Compute the centroid of the point set (xmw, ymw) considering that % the centroid of a](https://reader034.fdocuments.us/reader034/viewer/2022052010/603a0a55a7f1c0299973ac47/html5/thumbnails/7.jpg)
|Autonomous Mobile RobotsRoland Siegwart, Margarita Chli, Nick Lawrance
ASL Autonomous Systems Lab
2019 | Exercise 3 16
Line fitting / Line regression
Trigonometric identities
![Page 8: Exercise 3 | Line fitting and extraction for robot ... · function [alpha, r] = fitLine(XY) % Compute the centroid of the point set (xmw, ymw) considering that % the centroid of a](https://reader034.fdocuments.us/reader034/viewer/2022052010/603a0a55a7f1c0299973ac47/html5/thumbnails/8.jpg)
|Autonomous Mobile RobotsRoland Siegwart, Margarita Chli, Nick Lawrance
ASL Autonomous Systems Lab
2019 | Exercise 3 17
Split and Merge
Solved: Fitting a line to a set of points that should be on a line
What do we do in case ofmultiple lines?
Find sets of points, then fit a line to the separate sets!
![Page 9: Exercise 3 | Line fitting and extraction for robot ... · function [alpha, r] = fitLine(XY) % Compute the centroid of the point set (xmw, ymw) considering that % the centroid of a](https://reader034.fdocuments.us/reader034/viewer/2022052010/603a0a55a7f1c0299973ac47/html5/thumbnails/9.jpg)
|Autonomous Mobile RobotsRoland Siegwart, Margarita Chli, Nick Lawrance
ASL Autonomous Systems Lab
2019 | Exercise 3 18
Split and Merge
while not finished do fit a line to the current set of points
![Page 10: Exercise 3 | Line fitting and extraction for robot ... · function [alpha, r] = fitLine(XY) % Compute the centroid of the point set (xmw, ymw) considering that % the centroid of a](https://reader034.fdocuments.us/reader034/viewer/2022052010/603a0a55a7f1c0299973ac47/html5/thumbnails/10.jpg)
|Autonomous Mobile RobotsRoland Siegwart, Margarita Chli, Nick Lawrance
ASL Autonomous Systems Lab
2019 | Exercise 3 19
Split and Merge
while not finished do fit a line to the current set of points
check distance to line for each point
![Page 11: Exercise 3 | Line fitting and extraction for robot ... · function [alpha, r] = fitLine(XY) % Compute the centroid of the point set (xmw, ymw) considering that % the centroid of a](https://reader034.fdocuments.us/reader034/viewer/2022052010/603a0a55a7f1c0299973ac47/html5/thumbnails/11.jpg)
|Autonomous Mobile RobotsRoland Siegwart, Margarita Chli, Nick Lawrance
ASL Autonomous Systems Lab
2019 | Exercise 3 20
Split and Merge
while not finished do fit a line to the current set of points
check distance to line for each point
if max(distances) > threshold then split current set of points else select next set of points endend
![Page 12: Exercise 3 | Line fitting and extraction for robot ... · function [alpha, r] = fitLine(XY) % Compute the centroid of the point set (xmw, ymw) considering that % the centroid of a](https://reader034.fdocuments.us/reader034/viewer/2022052010/603a0a55a7f1c0299973ac47/html5/thumbnails/12.jpg)
|Autonomous Mobile RobotsRoland Siegwart, Margarita Chli, Nick Lawrance
ASL Autonomous Systems Lab
2019 | Exercise 3 21
Split and Merge
while not finished do fit a line to the current set of points
check distance to line for each point
if max(distances) > threshold then split current set of points else select next set of points endend
![Page 13: Exercise 3 | Line fitting and extraction for robot ... · function [alpha, r] = fitLine(XY) % Compute the centroid of the point set (xmw, ymw) considering that % the centroid of a](https://reader034.fdocuments.us/reader034/viewer/2022052010/603a0a55a7f1c0299973ac47/html5/thumbnails/13.jpg)
|Autonomous Mobile RobotsRoland Siegwart, Margarita Chli, Nick Lawrance
ASL Autonomous Systems Lab
2019 | Exercise 3 22
Split and Merge
while not finished do fit a line to the current set of points
check distance to line for each point
if max(distances) > threshold then split current set of points else select next set of points endend
merge collinear lines
![Page 14: Exercise 3 | Line fitting and extraction for robot ... · function [alpha, r] = fitLine(XY) % Compute the centroid of the point set (xmw, ymw) considering that % the centroid of a](https://reader034.fdocuments.us/reader034/viewer/2022052010/603a0a55a7f1c0299973ac47/html5/thumbnails/14.jpg)
|Autonomous Mobile RobotsRoland Siegwart, Margarita Chli, Nick Lawrance
ASL Autonomous Systems Lab
2019 | Exercise 3 23
Split and Merge
while not finished do fit a line to the current set of points
check distance to line for each point
if max(distances) > threshold then split current set of points else select next set of points endend
merge collinear lines
good processingcan preventoutliers
![Page 15: Exercise 3 | Line fitting and extraction for robot ... · function [alpha, r] = fitLine(XY) % Compute the centroid of the point set (xmw, ymw) considering that % the centroid of a](https://reader034.fdocuments.us/reader034/viewer/2022052010/603a0a55a7f1c0299973ac47/html5/thumbnails/15.jpg)
|Autonomous Mobile RobotsRoland Siegwart, Margarita Chli, Nick Lawrance
ASL Autonomous Systems Lab
2019 | Exercise 3 24
Line fitting / Line regressionfunction [alpha, r] = fitLine(XY)% Compute the centroid of the point set (xmw, ymw) considering that% the centroid of a finite set of points can be computed as% the arithmetic mean of each coordinate of the points.
% XY(1,:) contains x position of the points% XY(2,:) contains y position of the points xc = TODO yc = TODO
% compute parameter alpha (see exercise pages) num = TODO denom = TODO alpha = TODO
% compute parameter r (see exercise pages) r = TODO
% Eliminate negative radii if r < 0 alpha = alpha + pi; if alpha > pi, alpha = alpha - 2 * pi; end r = -r; endend
![Page 16: Exercise 3 | Line fitting and extraction for robot ... · function [alpha, r] = fitLine(XY) % Compute the centroid of the point set (xmw, ymw) considering that % the centroid of a](https://reader034.fdocuments.us/reader034/viewer/2022052010/603a0a55a7f1c0299973ac47/html5/thumbnails/16.jpg)
|Autonomous Mobile RobotsRoland Siegwart, Margarita Chli, Nick Lawrance
ASL Autonomous Systems Lab
2019 | Exercise 3 25
Split-and-Merge algorithm
function splitPos = findSplitPosInD(d, params) splitPos = TODOend
Task 2: Fill in the solution for line fitting Implement the split function
![Page 17: Exercise 3 | Line fitting and extraction for robot ... · function [alpha, r] = fitLine(XY) % Compute the centroid of the point set (xmw, ymw) considering that % the centroid of a](https://reader034.fdocuments.us/reader034/viewer/2022052010/603a0a55a7f1c0299973ac47/html5/thumbnails/17.jpg)
|Autonomous Mobile RobotsRoland Siegwart, Margarita Chli, Nick Lawrance
ASL Autonomous Systems Lab
2019 | Exercise 3 26
Line fitting / Line regressionfunction [alpha, r] = fitLine(XY)% Compute the centroid of the point set (xmw, ymw) considering that% the centroid of a finite set of points can be computed as% the arithmetic mean of each coordinate of the points.
% XY(1,:) contains x position of the points% XY(2,:) contains y position of the points len = size(XY, 2); xc = sum(XY(1, :)) / len; yc = sum(XY(2, :)) / len;
% compute parameter alpha (see exercise pages) dX = (xc - XY(1, :)); dY = (yc - XY(2, :)); num = -2 * sum(dX.*dY); denom = sum(dY.*dY - dX.*dX); alpha = atan2(num, denom) / 2;
% compute parameter r by inserting the centroid % into the line equation and solve for r r = xc * cos(alpha) + yc * sin(alpha); % Eliminate negative radii if r < 0 alpha = alpha + pi; if alpha > pi, alpha = alpha - 2 * pi; end r = -r; endend
Test with testLineFitting.m
Testing line fitting 1 : OKTesting line fitting 2 : OKTesting line fitting 3 : OKTesting line fitting 4 : OKTesting line fitting 5 : OKTesting line fitting 6 : OKTesting line fitting 7 : OKTesting line fitting 8 : OKTesting line fitting 9 : OKTesting line fitting 10 : OKTesting line fitting 11 : OKTesting line fitting 12 : OKTesting line fitting 13 : OKTesting line fitting 14 : OKTesting line fitting 15 : OKTesting line fitting 16 : OKTesting line fitting 17 : OKTesting line fitting 18 : OKTesting line fitting 19 : OKTesting line fitting 20 : OK…
![Page 18: Exercise 3 | Line fitting and extraction for robot ... · function [alpha, r] = fitLine(XY) % Compute the centroid of the point set (xmw, ymw) considering that % the centroid of a](https://reader034.fdocuments.us/reader034/viewer/2022052010/603a0a55a7f1c0299973ac47/html5/thumbnails/18.jpg)
|Autonomous Mobile RobotsRoland Siegwart, Margarita Chli, Nick Lawrance
ASL Autonomous Systems Lab
2019 | Exercise 3 27
Split-and-Merge algorithm
function splitPos = findSplitPosInD(d, params) N = length(d); d = abs(d); mask = d > params.LINE_POINT_DIST_THRESHOLD; if isempty(find(mask, 1)) splitPos = -1; return; end [~, splitPos] = max(d); if (splitPos == 1), splitPos = 2; end if (splitPos == N), splitPos = N-1; endend
function splitPos = findSplitPosInD(d, params) N = length(d); % Find the local maximum set (2 points) farOnPositiveSideB = d > params.LINE_POINT_DIST_THRESHOLD; farOnNegativeSideB = d < -params.LINE_POINT_DIST_THRESHOLD; neigborsFarAwayOnTheSameSideI = find((farOnPositiveSideB(1:N-1) & farOnPositiveSideB(2:N)) | (farOnNegativeSideB(1:N-1) & farOnNegativeSideB(2:N))); if isempty(neigborsFarAwayOnTheSameSideI) splitPos = -1; else absDPairSum = abs(d(neigborsFarAwayOnTheSameSideI)) + abs(d(neigborsFarAwayOnTheSameSideI+1)); [~, splitPos] = max(absDPairSum); splitPos = neigborsFarAwayOnTheSameSideI(splitPos); if abs(d(splitPos)) <= abs(d(splitPos + 1)) splitPos = splitPos + 1; end end % If the split position is toward either end of % the segment, find otherway to split. if (splitPos ~= -1 && (splitPos < 3 || splitPos > N-2)) [~, splitPos] = max(abs(d)); if (splitPos == 1), splitPos = 2; end if (splitPos == N), splitPos = N-1; end end end
![Page 19: Exercise 3 | Line fitting and extraction for robot ... · function [alpha, r] = fitLine(XY) % Compute the centroid of the point set (xmw, ymw) considering that % the centroid of a](https://reader034.fdocuments.us/reader034/viewer/2022052010/603a0a55a7f1c0299973ac47/html5/thumbnails/19.jpg)
|Autonomous Mobile RobotsRoland Siegwart, Margarita Chli, Nick Lawrance
ASL Autonomous Systems Lab
2019 | Exercise 3 28
Line fitting implementation