The Development of a Relative Point SLAM Algorithm and a

Relative Plane SLAM Algorithm

• I would like to thank my Advisor Dr McLeod and the examining committee – Dr. J Peters– Dr W.K. Fung– Dr. J Baltes– Dr. R. Palmer

for reviewing my thesis

Robot map making without accurate location data

• Given an unknown environment make a map, however:

• How do you make a map if the location is not known and how do you solve the location if the map is not known.

• This is referred to as the Simultaneous Localization and Mapping (SLAM) problem.

• Many different ways to look at this problem, this thesis takes the approach of using 3D landmark information and no odometry estimate.

• “Put differently, instead of relying on a single “best guess” as to what might be the case, probabilistic algorithms represent information by probability distributions over a whole space of guesses (EKF or Particle Filters).”

• “In contrast with traditional programming techniques in robotics such as model-based motion planning techniques or reactive behavior-based approaches… tend to be more robust.”

Previous Work

Inspiration• Solve SLAM using

software engineering techniques (structures and algorithms) by storing and using past and present planar data.

Many Attempts

• Learned identifying dynamic corner points was harder then filtering the noise.

• Why use current position?

Relative Plane algorithm

Plane 1

Plane 2

1

2

1

2

1

2

Iteration 1 I2 I3

I4

1

2

I5

1

2

I6

ydiff

xdiff

ydiff

xdiffydiff

xdiff

ydiff

xdiff

ydiffxdiff xdiff

ydiff

Note that the (xdiff,ydiff) is translational invariant but not rotational invariant

To solve the rotational invariant problem, plane 1 must be rotated so its angle is at 0 degrees and plane 2 must be rotated by the same amount. Now (xdiff,ydiff) will always be the same regardless of the robots rotational angle

Plane 1

Plane 2

Iteration 1

1

2

I6

ydiff

xdiff

xdiff

ydiff

Rotate both by –90 degrees

Plane 1

Plane 2ydiff

xdiff

Plane 1

Plane 2ydiff

xdiffRotate both by –60 degrees

• Store planes in groups based on visibility. Calculate relative maps for each group and combine into global map. Solve current position by comparing an iteration’s observations versus the global map.

Plane 1

Plane 2

1

2

1

2

• How to find which keypoints are valid?

• Use both of the planes top corner points and its known maximum size to create two assumed midpoints.

Plane 1

Plane 2

1

2

1

2

• We now have four comparison pairs. For each pair in addition to calculating the average relative location also calculate the standard deviation. At least one of the pairs will has a low standard deviation compared to the others. This pair contains the keypoints.

• There is a special case where there are two parallel planes that have two valid keypoint pairs. This case is handled elsewhere in the algorithm.

• Notice that planes 4 location is completely defined by X2 which is calculated in the interval when planes 3,4,5 are visible together. This means that plane 4’s location is not recalculated when plane 4 is only partially visible.

• If plane 4’s maximum size is known at this point no further work is required. However as it is likely to grow the only thing that needs to be done is figure out its new maximum size. This can be done with a motion model.

X2

Plane 4

Plane 3Plane 5

Issues

• Plane segmentation• Several architecture issues for future work such as.

– Only 2 1/2D– Groups of planes hard linked necessitating use of roll

back mechanism… for the plane group reordering which is not implemented that well.

• However it was realized that the algorithm does two things well, it filters noise and it identifies dynamic features in the same framework. Perhaps this can be applied to point landmark SLAM.

Relative Point algorithm

(x,y,z)iteration

Untransformed location

Iteration

1

2

3

5

…

N

(x1,y1, z1,)

4

(x2,y2,z2)

(x3,y3,y3)

(x4,y4,y4)

(x5,y5,y5)

(xn,yn,zn)

Point

RLTPoint

RltPoint 1

RltPoint 3

RltPoint 2

RltPoint 4

RltPoint 5

RltPoint 6

RltPoint 7

RltPoint 8

Time 0 10 20 30

RltPointCharting

x

y

Overlap is set to maximum matching bounds

OVLQuadTree

Point Matching

RltPoint 1

RltPoint 3

RltPoint 2

RltPoint 4

RltPoint 5

RltPoint 6

RltPoint 7

RltPoint 8

Time 0 10 20 30

RltPointCharting Group Creation

Sorted interval list (really two lists, grouped plus ungrouped interval list)

RltPoint 4

RltPoint 6

RltPoint 2

RltPoint 8

RltPoint 3

RltPoint 5

RltPoint 1

RltPoint 7

Time 0 10 20 30

RltUngroupedList RltInterval

RltPoint 4

RltPoint 6

RltPoint 2

RltPoint 8

RltPoint 3

RltPoint 5

RltPoint 1

RltPoint 7

Time 0 10 20 30

Sorted interval list

19X4 = 76

RltPoint 4

RltPoint 6

RltPoint 2

RltPoint 8

RltPoint 3

RltPoint 5

RltPoint 1

RltPoint 7

Time 0 10 20 30

Sorted interval list

13X5 = 65

• Group creation flags the RLTGroupRef of each RltPoint as grouped for the LSF and ungrouped RLTPoint that required mapping.

• Multiple levels of grouping (currently 3). First group just to add RltPoint to the map. Second runs the dynamic detection. Third is for accuracy.

• When a lower level group has no exclusive RltPoint that are not in a higher level group, that group is removed.

RltGroup

RltGroupRef

RltLSF

Map Creation

Global RltPoint matching

• Occurs when backtracking or closing the loop.

• Can use current position.

• Can use RltPoint merging instead at a higher cost but at the benefit of removing the use of current position from the algorithm.

Dynamic point detection

• Dynamic point detection is performed at group creation time.

• Since in our case a static basis is not known, every point needs to be compared against every other point to both find the static basis and to group points using this basis. This would normally be O(n2).

5 6 7 81 2 3 4

Place each RltPoint in a bin

5 6 7 81 2 3 4

Compare every bin to one other bin calculating first the average distance and then the standard deviation over the RltGroup’s interval. Save the standard deviation

.02 .06 .4 .03

Sort standard deviation to determine a threshold

.02, .03, .06, .4

Look for a large change in size, set threshold to a small amount above the last static standard deviation

.02, .03, .06, .4 Threshold set at .08

Bin, bins with standard deviation below threshold, maintain list and hash of previous comparisons so they do not occur again

5 61

2

3

4

7

8

5 61

2

3

4

7

8

.05 .3

651

2

3

4

7

8

61

2

3

4

5

7

8

.02

.03

6 cannot be compared to the “1” bin since 5 has already been compared to 6

1 2 3 4 5 7 66

No more comparisons possible

This chart corresponds the number of comparisons required given a percentage of uncorrelated points and the number of points. As the uncorrelated points approach 90% the standard deviation constant is found to be too high and the algorithm bins all the points into one bin in some cases.

Evaluating the run time of the binning algorithm

0

5000

10000

15000

20000

25000

30000

35000

40000

45000

50000

20 40 60 80 100 120 140 160 180 200 220 240 260 280 300

Num be r o f P o in ts

Nu

mb

er

of

Co

mp

ari

so

ns

0%

10%

20%

30%

40%

50%

60%

70%

70%

80%

90%

100%

It can be seen that the algorithm is only O(nlogn) when only 10% of the points are not correlated

Evaluating the run time of the binning algorithm

0

500

1000

1500

2000

2500

3000

3500

20 40 60 80 100 120 140

Num be r o f P o in ts

Nu

mb

er

of

Co

mp

ari

so

ns

0%

10%

20%

30%

40%

50%

nlogn

The algorithm is retested with the heuristic of using a priority queue to decided which bins to use for the comparisons. Bins that have a previous successful match have a higher priority than those that do not. Note that the algorithm now runs at or below O(nlogn) even with 40% of the points uncorrelated

Evaluation of binning with priority queue

0

100

200

300

400

500

600

700

800

20 40 60 80 100 120 140 160 180 200 220 240 260 280 300

Num be r o f P o in ts

Co

mp

ari

so

ns

0%

10%

20%

30%

40%

nlogn

Looking from 40-50% uncorrelated points line, the algorithm only really degrades at about the 47% line.

Evaluation of priority queue 50% threshold

0

2 0 0 0

4 0 0 0

6 0 0 0

8 0 0 0

1 0 0 0 0

1 2 0 0 0

1 4 0 0 0

2 0 4 0 6 0 8 0 1 0 0 1 2 0 1 4 0 1 6 0 1 8 0 2 0 0 2 2 0 2 4 0 2 6 0 2 8 0 3 0 0

Nu m b e r o f p o in ts

Co

mp

ari

so

ns

4 0 %

4 1 %

4 2 %

4 3 %

4 4 %

4 5 %

4 6 %

4 7 %

4 8 %

4 9 %

5 0 %

n lo g n

n * n /7

Relative Algorithm Performance on figure eight

The execution time does not go up as the number of RltPoint increase. Every part of the algorithm is designed to only work on the given observations.

Execution time

0

0.001

0.002

0.003

0.004

0.005

0.006

1

50

2

10

03

15

04

20

05

25

06

30

07

35

08

40

09

45

10

50

11

55

12

60

13

65

14

Ite ra tion

Tim

e (

se

co

nd

s)

Execution time ma

Linear (Execution time ma)

As the point density increases, the computation time becomes polynomial, why?

T o ta l c o m p u ta tio n tim e

0

0.002

0.004

0.006

0.008

0.01

0.012

0.014

100 150 200 250 300 350 400

Po in t d e n s ity

Tim

e (

se

co

nd

s)

nlogn

Algorithm time

The polynomial appears to come from the initial point matching which is affected by the efficiency of the quadtree

W h e re th e g a p co m e s fro m

-0 .0 0 0 5

0

0 .0 0 0 5

0 .0 0 1

0 .0 0 1 5

0 .0 0 2

0 .0 0 2 5

1 0 0 1 5 0 2 0 0 2 5 0 3 0 0 3 5 0 4 0 0

Point density

Tim

e (s

ec

on

ds

)

lo a d tim e g a p

to ta l g a p

Quadtree with Octtree replacement at 400

0

0.002

0.004

0.006

0.008

0.01

0.012

0.014

0.016

0.018

0.02

100 150 200 250 300 350 400 400 450 500 550

P o in t d e n sity

Tim

e (

se

co

nd

s)

Quad combo with Oct

nlogn

n

Accuracy vs 6D no odometry EKF

• 6D no odometry range bearing EKF is obtained from the mobile robot toolkit.

• It is tested in the figure eight.

White gaussian noise

Figure eight landmark error

0

5

10

15

20

25

30

35

40

Iterations

RLT landmark error

EKF Landmark error

White noise

Figure eight runtime

0

0 .2

0 .4

0 .6

0 .8

1

1 .2

1 .4

1 .6

1 .8

1

51

10

1

15

1

20

1

25

1

30

1

35

1

40

1

45

1

50

1

55

1

60

1

65

1

70

1

75

1

80

1

85

1

90

1

95

1

10

01

10

51

11

01

Iteration

Tim

e (s

ec

on

ds

)

E K F ru n tim e

R lt ru n tim

Figure 8 point count

0

20

40

60

80

100

120

140

1

54

10

7

16

0

21

3

26

6

31

9

37

2

42

5

47

8

53

1

58

4

63

7

69

0

74

3

79

6

84

9

90

2

95

5

10

08

10

61

11

14

Ite ra tion

Po

ints Points per iteration

Total Points

• The testing is not to rank EKF vs Relative for accuracy purposes. It is to use the EKF as a baseline to verify that the Relative Point algorithm accuracy is reasonable.

• There are other algorithms FastSLAM and EKF that use submapping in terms of execution speed comparisons.

Performance Summary

• The Relative Point algorithm is a fast O(nslog(ns)) as long as the data structures are chosen correctly.

• The Relative Point algorithm is able to identify dynamic points.

• The Relative Point algorithm accuracy is shown to be comparable to the EKF.

Software Engineering

• The successful implementation of the Relative Point algorithm took many iterations of possible solutions and using what was learned to improve the approach taken.

• It is also curious to note that in regards to the Second System Effect, “the Relative Point algorithm seem to be implemented approximately correct…..”

Contribution• “In fact, certain

probabilistic algorithms are currently the only known working solutions to hard robotic estimation problems… the problem of building accurate maps of very large environments.”