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 Iteration 1I2I3 I4 1 2 I5 1 2 I6 ydiff xdiff ydiff xdiff ydiff xdiff ydiff xdiff ydiff 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 I6 ydiff xdiff ydiff Rotate both by –90 degrees Plane 1 Plane 2 ydiff xdiff Plane 1 Plane 2 ydiff xdiff Rotate 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 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 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 3 Plane 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 … N (x 1,y 1, z 1, ) 4 (x 2,y 2,z 2 ) (x 3,y 3, y 3 ) (x 4,y 4,y 4 ) (x 5,y 5,y 5 ) (x n,y n,z n ) Point RLTPoint RltPoint 1 RltPoint 3 RltPoint 2 RltPoint 4 RltPoint 5 RltPoint 6 RltPoint 7 RltPoint 8 Time 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 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 RltUngroupedList RltInterval

RltPoint 4 RltPoint 6 RltPoint 2 RltPoint 8 RltPoint 3 RltPoint 5 RltPoint 1 RltPoint 7 Time Sorted interval list 19X4 = 76 RltPoint 4 RltPoint 6 RltPoint 2 RltPoint 8 RltPoint 3 RltPoint 5 RltPoint 1 RltPoint 7 Time 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(n 2 ).

Place each RltPoint in a bin 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 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,.4Threshold set at.08 Bin, bins with standard deviation below threshold, maintain list and hash of previous comparisons so they do not occur again cannot be compared to the “1” bin since 5 has already been compared to 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.

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

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

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

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.

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

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

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 White noise

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(n s log(n s )) 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.”