The Development of a Relative Point SLAM Algorithm and a Relative Plane SLAM Algorithm

1. The Development of a Relative Point SLAM Algorithm and a Relative Plane SLAM Algorithm

2. 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

3. 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.

4. Previous Work • “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.”

5. Inspiration • Solve SLAM using software engineering techniques (structures and algorithms) by storing and using past and present planar data.

6. Many Attempts • Learned identifying dynamic corner points was harder then filtering the noise. • Why use current position?

7. 2 2 2 2 2 ydiff ydiff ydiff ydiff 1 1 1 1 1 xdiff xdiff xdiff xdiff Relative Plane algorithm Plane 2 Plane 1 Iteration 1 I2 I3 ydiff ydiff xdiff xdiff I4 I5 I6 Note that the (xdiff,ydiff) is translational invariant but not rotational invariant

8. 2 ydiff 1 xdiff 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 2 Rotate both by –90 degrees xdiff Plane 1 Plane 2 ydiff Plane 1 Iteration 1 xdiff Rotate both by –60 degrees ydiff Plane 2 xdiff ydiff Plane 1 I6

9. 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.

10. 2 Plane 1 • 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. 1 2 Plane 2 1 2 Plane 1 • 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. 1 2 Plane 2 1

11. Plane 4 X2 Plane 3 Plane 5 • 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.

12. 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.

13. Relative Point algorithm

14. Point Matching RltPointCharting Time 0 10 20 30 RltPoint 1 RltPoint 2 Untransformed location Iteration RltPoint 3 (x1,y1, z1,) RltPoint 4 1 RltPoint 5 (x2,y2,z2) 2 RltPoint 6 (x3,y3,y3) 3 RltPoint 7 4 (x4,y4,y4) RltPoint 8 5 (x5,y5,y5) … N (xn,yn,zn) (x,y,z) iteration Point RLTPoint OVLQuadTree x Overlap is set to maximum matching bounds y

15. Group Creation Time 0 10 20 30 RltPoint 4 RltPoint 2 RltPoint 6 RltPoint 8 RltPoint 3 RltPoint 5 RltPoint 1 RltPoint 7 RltPointCharting Time 0 10 20 30 RltPoint 1 RltPoint 2 RltPoint 3 RltInterval RltUngroupedList RltPoint 4 RltPoint 5 RltPoint 6 RltPoint 7 RltPoint 8 Sorted interval list (really two lists, grouped plus ungrouped interval list)

16. Sorted interval list Time 0 10 20 30 RltPoint 4 RltPoint 2 19X4 = 76 RltPoint 6 RltPoint 8 RltPoint 3 RltPoint 5 RltPoint 1 RltPoint 7 Sorted interval list Time 0 10 20 30 13X5 = 65 RltPoint 4 RltPoint 2 RltPoint 6 RltPoint 8 RltPoint 3 RltPoint 5 RltPoint 1 RltPoint 7

17. 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.

18. Map Creation RltGroup RltGroupRef RltLSF

19. 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.

20. 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).

21. Place each RltPoint in a bin 6 7 8 1 2 3 4 5 1 2 3 4 5 6 7 8 .05 .3 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 1 2 3 4 6 5 7 8 1 2 3 4 5 6 7 8 .02 .06 .4 .03 .02 Sort standard deviation to determine a threshold 6 cannot be compared to the “1” bin since 5 has already been compared to 6 .02, .03, .06, .4 1 2 3 4 5 6 7 8 Look for a large change in size, set threshold to a small amount above the last static standard deviation .03 .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 No more comparisons possible 1 2 3 4 5 7 6 6 1 2 3 4 5 6 7 8

22. 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.

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

24. 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

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

26. Relative Algorithm Performance on figure eight

28. 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.

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

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

32. 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.

33. White gaussian noise White noise

35. 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.

36. 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.

37. 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…..”

38. 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.”