the development of a relative point slam algorithm and a relative plane slam algorithm
DESCRIPTION
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. - PowerPoint PPT PresentationTRANSCRIPT
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.”