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

Administrative PS2 is due today PS3 posted later tonight.

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?

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

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

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

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

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

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

RANSAC example

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”?)

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)

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.

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

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

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

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 $2600 - $5000 Note limited field of view… what can they see? Not glass, not banister railings, not reflective surfaces.

Laser Scanners

Laser Scanners

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.

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 (!)

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?

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

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]

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 \\

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!

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)

Line Fitting: Divisive

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.

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!

Line Fitting: Agglomerative

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)

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

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.

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.

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

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.

Aligning scans without features ICP/ICL Correlation-based scanmatching

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.

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.

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

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

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

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

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)

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

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)

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)

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

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

Next Time Aligning scans without extracting features