Outdoor SLAM using Visual Appearance and Laser Ranging P. Newman, D. Cole and K. Ho ICRA 2006 Jackie Libby Advisor: George Kantor

Main Idea laser and camera for 3d SLAM system Laser: builds 3d point cloud map Camera: detects loop closure from sequences of images First working implementation in outdoor environment

Outline Slam framework Laser data representation How loop is detected with vision How loop is closed once detected results

SLAM representation: state x(k+1|k) = state vector of vehicle poses from t = 1, …, k+1 given observations from t = 1, …, k x vn = n th vehicle pose in state vector u(k+1) = odometry between k and k+1 = SΕ 3 transformation composition operator (motion model)

SLAM representation: covariance P v = covariance of newly added vehicle state (bottom left) P vp = ? (my guess: covariance between new state and all previous states)

Scan-match framework Nodding “yes-yes” laser – Returns planar scans at different elevations each vehicle pose corresponds to a laser scan – x v (k) -> S k – timesteps not constant x v (i), x v (j) -> S i, S j -> T i,j T i,j = rigid transformation – Observation -> EKF equations – i, j sequential -> T i,j can replace u – loop closing -> j >> i T i,j used to correct position

State vector with growing uncertainty outdoor data set x,y,z grid (20m marks) 1 σ x,y,z uncertainty ellipsoids Effect of long loops How to detect loop closure 1)slam pdf, small Mahalanobis distance - Ellipsoids overconfident -> this method fails 2)Vision system - Matches image sequences - similarity in appearance

My work with SLAM: CASC – Advisor: George Kantor – Sanjiv Singh – Marcel Bergerman – Ben Grocholsky – Brad Hamner Retroreflective cones no-no nodding SICK laser

Detecting loop closure (high level) Assumption: 2 images look similar -> close in space Not close in time -> loop detected Problem: Just one image pair -> false positive “repetitious low level descriptors” “common texture” leaves, bricks on buildings “background similarity” “common large scale features” plants, windows Solution: sequences of image pairs increases confidence

Image, IuDetect interest pointsSIFT DescriptorsClustered into wordsVocab = set of all wordsAssign weight to each wordImage = vector of weights Similarity between two images Image, I v

Thanks Martial Hebert! Image, I u Detect interest points Harris Affine detector Scale invariant, affine invariant Example: corners -> still a corner from any angle or scale

Detect interest pointsSIFT Descriptors {d 1, …, d n } 16x16 pixel window around interest point Assign each pixel a gradient orientation (out of 8 values) For each 4x4 window, make histogram of orientations 16 histograms * 8 values = 128 = dimension of SIFT vector (ignore blue circles) Thanks Martial Hebert!

SIFT Descriptors {d 1, …, d n }Clustered into words,Vocab = set of all words SIFT Descriptors: n is different for different images word, = {d 1, …, d k } clustering happens in an offline learning process Vocabulary, V future work: different vocabularies for different settings urban, park, indoors

Vocab = set of all wordsweight for each word The more frequent the word, the less descriptive it is inverse weighting frequency scheme *: *K. S. Jones, “Exhaustivity and specificity,” Journal of Documentation, vol. 28, no. 1, pp. 11–21, w i = weight for index word, N = total # images n i = # images where this word appears

weight for each wordImage  vector of weights Image has been transformed into a vector vector is long, |V|, but many elements are zero

Similarity Image Cosine distance: cos(0) = 1 The closer the vectors -> the smaller the angle between them the greater the similarity

Image, IuDetect interest pointsSIFT Descriptors {d1, …, dn}Clustered into words,Vocab = set of all wordsweight for each wordImage  vector of weights Similarity Image, I v

Boxes = false positives one to many mapping: a i b i+1, b i+2, b i+3 … b i a i+1, a i+2, a i+3 … causes: vehicle stopped repetitive low-level structure (windows, bricks, leaves) distant images Similarity Matrix Similarity matrix, M M i,j = S(i,j) darker means more similar axes = timesteps same for x and y comparing each image against every other one main diagonal is line of reflection Loop closure = off diagonal streaks a i b i a i+1 b i+1

Sequence extraction (finding streaks) Modified Smith-Waterman algorithm dynamic programming penalty terms avoid boxes allow for curved lines (i.e. change in velocity) α term allows gaps Maximum H i,j = η A,B Maximal cumulative similarity

Removing “common mode similarity” (finding boxes) Decompose M into sum of outer products “Dominant structure” = repetitive structure dominant structure  largest eigenvalues/vectors Eigenface: First three outer products More repetition -> more range in eigenvalues Relative significance: Maximize entropy:

Sequence Significance problem: Maximum H i,j = η A,B  this doesn’t mean there’s a loop solution: randomly shuffle rows and columns of M, recompute η A,B look at distribution: Extreme value distribution (EVD) Probability that sequence could be random η A,B = real score η = random score threshold at 0.5%

Estimating Loop Closure Geometry We have detected a loop (with image sequence) Now how do we close it? (how do we find T ij ?) One solution: iterative scan matching – η A,B  a i, b j  x vi, x vj  S i, S j  T ij – Problem: local minima Better solution: projective model – Essential matrix – 5 point algorithm with Ransac loop – User lasers to: Remove scale ambiguity Fine-tune with iterative scan matching – quality of final scan match is another quality check

Enforcing loop closure Naïve method: single EKF update step only works for small errors, because of linear approximation Better method: constrained non-linear optimization incremental changes [x v1, …., x vn ]  [T 1,2, … T n-1,n, T n,1 ] [Σ 1,2, …, Σ n,1 ] (from scan-matching) Want new poses, [T* 1,2, … T* n-1,n, T* n,1 ] Subject to constraint: Minimize:

Results Resulting estimated map And vehicle trajectory successfully applied to several data sets 98% runtime spent on laser registration -> bottleneck 1/3 real time most expensive part of vision subsystem is feature detector/descriptor (Harris Affine/ SIFT)

Conclusions and future work Conclusions – SLAM system for outdoor applications – Works for challenging urban environment – Complementary vision laser system Vision for loop closing Laser data for geometry map building – First working implementation Future work – SLAM formulation not efficient – Laser scan matching is bottle neck – Learning vocabularies for distinct domains (urban, park, indoors) Different similarity matrix if domain switches