1. EECS 598-2: Algorithms for Robotics
L10. Laser Scanners
EECS 598-2: Algorithms for Robotics

2. Administrative
PS2 is due today
PS3 posted later tonight.

4. Last Time Topological Mapping Data Association Key ideas
Nearest Neighbor
Joint Compatibility
RANSAC
Decompose environment into “places”. Places are connected to each other.
Relationship of topological/metrical maps.
What do humans do? Local Perceptual Map (LPM) + topology?

5. Today Finish up Data Association Laser scanners Feature Extraction
Principles of operation, noise characterization
Feature Extraction
Contours
Lines
Split and Fit
Agglomerative Line Fitting
Matching Features

6. Data Association: NN Simplest data association scheme:
Nearest neighbor.
We have probabilistic estimates of where our landmarks are
When we get an observation, which landmark does it match “best”?
E.g., use Mahalanobis distance
Landmark 1
Landmark 2
Observation
Mahl=1.2
Mahl = 4.2

7. Computing Joint Compatibility
Hypothetical data association
observed three landmarks:
(Nice linear observation model this time)
What is the Mahalanobis distance of the observation with respect to the noisy observation of the RHS?
What is the mean and covariance of the RHS?
\left[ \begin{array}{c} z_1 \\ z_2 \\ z_3 \\ z_4 \\ z_5 \\ z_6 \end{array} \right]
= f(x, w) =
\left[ \begin{array}{c} x_3 + w_1 \\ x_4 + w_2 \\ x_{11} + w_3 \\ x_{12} + w_4 \\ x_7 + w_5 \\ x_8 + w_6 \end{array} \right]
\Sigma_f = J_x\Sigma_xJ_x^T + J_w\Sigma_wJ_w^T

8. RANSAC: Preview RANdom SAmple Consensus:
Randomly generate models
Test the model.
Pick the best model.
Alternative approach to NN, JCBB
(Typically) non-probabilistic
Can be applied to many problems, not just data association:
Outlier rejection
Line fitting

9. RANSAC
Idea: If we knew the rigid body transformation that aligned us with the global coordinate frame, NN would work well.
Let’s focus our attention on computing the RBT!
Method:
Randomly pick two landmark pairs, compute the rigid- body transformation that they imply.
Project all observations using that transformation
How many observations have good NN matches to landmarks?
Our “Consensus” Score
Keep the best RBT. (Optional: Refit the RBT based on the consensus set.)

10. RANSAC How many random trials do we need?
We have to guess “right” twice in order to get a correct RBT.
Suppose probability of guessing right is p.
Probability of guessing right twice in a row is p2.
If we have 1/p2 trials, we expect to find the right answer about once.
Use K/p2 trials to “make sure we aren’t unlucky”.

11. RANSAC example

12. RANSAC
Even with small values of ‘p’, we don’t need that many iterations…
a few thousand, maybe.
In some cases, we can exhaustively try all possible combinations. (“exhaustive RANSAC”?)

13. RANSAC: Discussion Complexity? Upsides: Downsides:
What about higher dimensions/more complex models?
Upsides:
Works well in low-noise/high-ambiguity situations
Allows us to compute a good hypothesis
Very fast in low dimensions
Useful in many contexts: a great general-purpose tool
Easy to incorporate negative information
Downsides:
Not very rigorous
Parameters to tune by hand
What is “close enough” to increase consensus score?
2D RBTs require two point correspondences. N^2 possible correspondences, so N^4 possible pairs of correspondences. But N^2 of them are correct.
Higher dimensions: Like fitting the parameters of a lens (9+ parameters)
We need O(N^2) iterations, each one costs O(N). I.e., O(N^3), rather than exponential (JCBB)

14. Graph Methods
Most of value of Joint Compatibility can be captured by considering only pairwise compatibility.
Consistency graph
Nodes in the graph are hypotheses (observation i corresponds to landmark j)
Edges represent pairwise consistency
No edge means the two hypotheses are not simultaneously compatible.

15. Graph Methods: CCDA Combined Constraint Data Association (CCDA):
Boolean edges (e.g., thresholded Mahalanobis distances)
Find the largest clique in the graph: these are the correct associations. H1 H2 H4 H3 H5
CCDA: H1, H2, H4

16. Graph Methods: SCGP Single Cluster Graph Partitioning (SCGP)
Edges have weights (more expressive)
Finds densely-connected subgraph
Doesn’t have to be a clique
Detects ambiguity (two dense subgraphs possible?)
O(N2) H1 H2 H4 H3 H5
CCDA: ?? SCGP: H1, H2, H4, H5 H1 H2 H4 H3 H5
CCDA, SCGP: H1, H2, H4

17. SCGP sketch Indicator vector: Quantity to maximize:
Vector of {0,1} which says which nodes to keep
Quantity to maximize:
Differentiate λ with respect to v, obtain:
Sum of all pair-wise consistencies between hypotheses in v
Number of hypotheses in v
vTAv
vTv
λ =
How to maximize lambda? Set v to be dominant eigenvalue of A.
Av = λv

18. Laser Scanners Measure range via time-of-flight SICK Hokuyo
Industrial safety
180 samples, 1 degree spacing, 75Hz
Resolution ~1cm, ~0.25 deg.
Interlacing
Max range: “80m”  30m fairly reliable
Intensity
$4500
Hokuyo
~520 samples, 0.25deg spacing, 15Hz
$ $5000
Note limited field of view… what can they see? Not glass, not banister railings, not reflective surfaces.

19. Laser Scanners

20. Laser Scanners

21. Aligning Scans = Measuring motion
Two important cases:
Incremental motion. Use lidar to augment (or replace!) odometry.
Loop closing. Identify places we’ve been before to over-constrain robot trajectory.

22. Feature Extraction Our plan: Possible features:
Extract features from two scans
Match features
Compute rigid-body transformation
Possible features:
Lines
Corners
Occlusion boundaries/depth discontinuities
Trees (!)

23. Line Extraction Given laser scan, produce a set of 2D line segments
Two basic approaches:
Divisive (“Split N Fit”)
Agglomerative
Our plan:
If we know which points belong to a line, how do we fit a line to those points?
How do we determine which points belong together?

24. Optimal Line Derivation
Assume we know a set of points belong to a line. How do we compute parameters of the line?
How to parameterize the line?
Slope + y intercept?
Endpoints of line
A point on the line + unit vector
R + theta
Lines have 2 degrees of freedom, but these have singularities.
Slope+y intercept is BAD: singularity for vertical lines
Endpoints: over-determined, hard to optimize (4DOF for
Point + unit vector: also over-determined

25. Optimal Line Derivation
Error for one point
Cost function
Solution found by minimizing pi e
pi-q
q, a point on the line θ
|e| = (p_i - q) \cdot \hat{n}
err^2 = \sum_i \left((p_i - q) \cdot \hat{n}\right)^2
\hat{n} = \left[ \begin{array}{c} -sin(\theta) \\ cos(\theta) \end{array} \right]

26. Optimal Line Derivation
Method: work in terms of moments:
err^2 = \sum_i \left((p_{i_x}-q_x)\hat{n}_x + (p_{i_y}-q_y)\hat{n}_y \right)^2
q_x = \frac{1}{N}\sum p_x
M_x = \sum p_x \\
M_y = \sum p_y \\
M_{xx} = \sum p_x^2 \\
M_{xy} = \sum p_x p_y \\
M_{yy} = \sum p_y^2
q_x &= &\frac{1}{N}M_x \\
q_y &= &\frac{1}{N}M_y \\
\theta &= &\frac{\pi}{2} + \frac{1}{2}{\rm atan2}(-2C_{xy}, C_{yy} - C_{xx})
C_{xx} &= &\frac{1}{N}M_{xx} - \left(\frac{M_x}{N}\right)^2 \\
C_{xy} &= &\frac{1}{N}M_{xy} - \frac{M_x}{N}\frac{M_y}{N} \\
C_{yy} &= &\frac{1}{N}M_{yy} - \left(\frac{M_y}{N}\right)^2 \\

27. Optimal Line Derivation
Solution:
Because all quantities are written in terms of moments, we can compute this quantity incrementally and without storing all the points!

28. Line Fitting: Divisive
Init: All points belong to a single line
Split-N-Fit(points)
Fit line to points
if line error < thresh then
return the line
else
Which point fits the worst?
Split the line into two lines at that point
return Split-N-Fit(leftpoints) U Split-N-Fit(rightpoints)

29. Line Fitting: Divisive

30. Line Fitting: Divisive
Pros:
Easy to implement
Cons:
Assumes points are in scan order
Doesn’t always generate good answers
Can split points that should belong together
“Split-N-Fit-N-Merge”
Complexity?
Complexity: You might think O(log N), but there’s no guarantee that the worst exemplar is at the halfway mark.

31. Line Fitting: Agglomerative
Agglomerative-Fit(points)
Init: create N-1 lines for each pair of adjacent points
do forever
for each pair of adjacent lines i, i+1
Compute error for line that merges those two lines
if minimum error > thresh then
return lines
else
merge lines with minimum error
Opposite of divisive!

32. Line Fitting: Agglomerative

33. Line Fitting: Agglomerative
Pros:
Good quality, matches human expectations
Cons:
Assumes points are in scan order
A little bit tricky to implement quickly (use a min- heap)
Complexity?
Complexity: at most N joins. Each min-heap operation is O(n). Assume (for now) line fitting cost is O(1).  O(n log n)

34. Line Fitting: RANSAC Pros: Cons: Easy
Does not require points to be in scan order
Cons:
Hard to exploit points being in scan order
Not obvious how to extract more than one line

35. Line Fitting: Hough Transform
Each point is evidence for a set of lines
Vote for each of the possible lines.
Lines with lots of evidence get many votes.

36. Other Features Corners Trees (Victoria Park) Depth Discontinuities
Intersections of nearby (?) lines.
Easy to extract from lines.
Only need ONE corner correspondence to compute RBT.
Trees (Victoria Park)
Depth Discontinuities
Beware of viewpoint effects
Lines don’t actually have to be nearby… i.e., corner doesn’t actually have to exist in order for it to be useful.

37. Contour Extraction
Divisive/Agglomerative line fittings need points to be grouped together
Consider stair railing
Simple agglomerative process works well!

38. Aligning scans using features
RANSAC!
Randomly guess enough correspondences to fit a model
Pick the model with the best consensus score.
Can incorporate negative information
Penalize consensus if features are missing.

39. Aligning scans without features
ICP/ICL
Correlation-based scanmatching

40. Iterative Closest Point (ICP)
until converged:
For each point in scan A, find the closest point in scan B.
Compute the RBT that best aligns the correspondences from the previous step.
Robustness tweaks:
Outliers: If distance between corresponding points is too large, delete the correspondence.
Also match from B to A: if the correspondences are very different in distance, delete the correspondence.

41. Iterative Closest Line (ICL)
LIDARs sample space at essentially arbitrary locations.
No reason to think that the exact points sampled in scan A were observed in scan B
Solution: interpolate lines between points in scan B, find closest line instead of closest point.

42. Computing the RBT We need to compute the rigid-body transformation
3DOFs:
Translation in x, y
Rotation
Simple two point method
Optimal n point method

43. Naïve two point method
Compute translation component by aligning centroids
Red Centroid
Blue Centroid

44. Naïve two point method
Compute translation component by aligning centroids
Rotate (so that lines between points have same angle)
Red Centroid θ
Blue Centroid

45. Optimal N-point method
From Horn, “Closed-form solution of absolute orientation using unit quaternions”
Actually a 3D method, but 2D derivation is much easier
Formulation
Minimize mean squared error between corresponding points
Potential weakness: outliers

46. Optimal N-point method
Assume that points x and points y are centered at zero. (This assumption can be patched up later)
y \approx R(x) + t
\chi^2 &= &\sum e_i^T e_i \\
&= &\sum y_i^Ty_i - R(x_i)^TR(x_i) + t^Tt - 2y_i^TR(x_i) - 2y_i^Tt + 2t^TR(x_i)

47. Optimal N-point method
Let’s compute the optimal Translation
Recall: we assumed points x & points y are centered at origin. i.e.,
And thus:
In other words, when the centroids are aligned, the optimal translation is zero. Thus, the optimal translation is the translation that aligns the centroids.
\frac{\partial \chi^2}{\partial t} = \sum {2t - 2y_i + 2R(x_i) } = 0
\sum y_i = \sum R(x_i) = 0

48. Optimal N-point method
Let’s compute the optimal rotation
Letting t = 0 per previous slide:
Note that yTy and R(x)TR(x) are the lengths of the vectors, and are independent of the rotation. We can thus drop those terms.
\chi^2 &= &\sum y_i^Ty_i - R(x_i)^TR(x_i) - 2y_i^TR(x_i)

49. Optimal N-point method
Recall that:
Collect sin/cos terms, we can write:
R(x_i) = \left[\begin{array}{c} \cos(\theta)x_{i_0} - \sin(\theta)x_{i_1} \\
\sin(\theta)x_{i_0} + \cos(\theta)x_{i_1} \end{array} \right]
\chi^2 &= &\sum y_i^TR(x_i) \\
&= &\sum \cos(\theta)x_{i_0}y_{i_0} - \sin(\theta)x_{i_1}y_{i_0} + \sin(\theta)x_{i_0}y_{i_1}
+ \cos(\theta)x_{i_1}y_{i_1}
\chi^2 &= &M \sin(\theta) + N \cos(\theta) \\
\frac{\partial \chi^2}{\partial \theta} &= &M\cos(\theta) - N\sin(\theta) = 0 \\
\theta &= &{\rm atan2}(M,N)

50. Correlation-based scan matching
How well do the scans “line up”?
Consider all plausible RBTs, project points from scan A to scan B. How close is each point to a point from the other scan?
Pick the RBT with the best score.
Local search (“hill climbing”)
Fast but not very robust
“Exhaustive” search
Slow but super robust

51. Feature vs. non-feature matching
Features (pros)
Data reduction
Noise filtering
Extraction + Matching is often fast
Non-Feature Matching (pros)
General purpose: don’t require world to contain our features
Very robust

52. Next Time
Aligning scans without extracting features