Mster

Robtica 2016/2017

Sistemas de Percepcin en Robtica Mvil

PRACTICE | Line Fitting and extraction for robot localization

1. Introduction
It is essential for mobile robots to have a sufficient knowledge of the environment. In
this way the can perform tasks with highly autonomy since they are able to localize in
this environment.
The exercise presented here is motivated by an autonomous vehicle carrying goods
across the corridors of a warehouse. In order to navigate from one place to another,
the vehicle would need to know its position and orientation (heading) in the
warehouse. On this way, it might come across walls, doors, windows, all of which are
seen as measurements located along lines by a laser scanner. This scanner would be
mounted in such a way that the scanning plane is parallel to the ground.
The next chapter explains the line extraction process in terms of equations and the
algorithm in pseudo-code is presented. In this section some exercises are proposed to
test the line fitting algorithm.

2. Line extraction
A range scan extracts 2D information from the environment. To describe the points of
the scene using a range scan, a polar coordinate system can be stablished. This system
has the origin at the location of the sensor. In the literature, it is common to model the
noise of the measurements as a Gaussian distribution with zero mean, some range
variance and negligible angular uncertainty.
A line in polar parameters (,) can be defined by the line equation (1) for the
Cartesian coordinates (,) of the points lying on the line

+ = ,






(1)

where < is the angle between the x-axis and the shortest connection
between the origin and the line. This connections length is 0 (see Figure 1).

Mster Robtica 2016/2017

Sistemas de Percepcin en Robtica Mvil

Figure 1: Fitting line parameters: D is the fitting error we aim to minimize expressing a
line in polar parameters (,).

2.1.

Split-and-merge

We employ the Split-and-Merge line extraction algorithm to divide the obtained

range measurements (points) into segments of points lying roughly on a common line.

2.2.

Algorithm in matlab

The Split-and-Merge algorithm is implemented inside the function ! , ! , =

! , ! . A crucial part of this function is the line fitting step.

Mster Robtica 2016/2017

Sistemas de Percepcin en Robtica Mvil

Exercise 1
Edit fitLine.m and follow the instructions to complete the mathematical formula for
computing line regression (line fitting) using a set of points in Cartesian coordinates
after reading the following theory. The aim of the function is to minimize the sum of
squared errors:

(2)

where ! , ! are the input points in Cartesian coordinates. The solution of (r,) can
be found by imposing: S = 0 . The solution for is then

(3)

(4)

(5)

where (xc,yc) are the Cartesian coordinate of the (xi,yi)s centroid (see instructions
in fitLine.m). In order to solve for r consider the equation (1) and a point that will
surely lie on the line.
As soon as the lines are correctly fitted, the algorithm performs Split-and-Merge and
extracts the endpoints of each segment.
Validation: Run test/testLineExtraction.m to check if the code is correctly completed.
If not it will show a figure with the measured points and expected lines (in yellow) as
together with the found lines and segments (in red, green and blue). To test only the
fitLine function on artifical data use test/testLineFitting.m.

Exercise 2
Once the code has been validated you can include some new lines in the code so that
this results can be visualized.

Insert a call to plotLinesAndMeasurementsPolar (theta', rho', zT, segendsT,
z) in test/testLineExtraction.m to plot the lines detected for each set of range data.

Mster Robtica 2016/2017

2.3.

Sistemas de Percepcin en Robtica Mvil

Kalman algorithm in matlab

The RANSAC (Random Sample Consensus) is an algorithm to estimate robustly the

parameters of a model from a given data in the presence of outliers.

Exercise 3

The aim of this exercise is to try another line extraction algorithm. Try to implement
the KALMAN algorithm from scratch using as an input any of the available range sensor
data set.
You can call this function testLineExtractionRANSAC.m and save it in the test/ folder.

References
[1] Roland Siegwart, Illah Nourbakhsh, and Davide Scaramuzza. Introduction to
Autonomous Mobile Robots. MIT Press, 2nd edition, 2011.