1. B659: Principles of Intelligent Robot Motion Spring 2013 Kris Hauser
3D Sensing and Mapping
B659: Principles of Intelligent Robot Motion
Spring 2013
Kris Hauser

2. Agenda
A high-level overview of visual sensors and perception algorithms
Core concepts
Camera / projective geometry
Point clouds
Occupancy grids
Iterative closest points algorithm

3. Proprioceptive: sense one’s own body
Motor encoders (absolute or relative)
Contact switches (joint limits)
Inertial: sense accelerations of a link
Accelerometers
Gyroscopes
Inertial Measurement Units (IMUs)
Visual: sense 3D scene with reflected light
RGB: cameras (monocular, stereo)
Depth: lasers, radar, time-of-flight, stereo+projection
Infrared, etc
Tactile: sense forces
Contact switches
Force sensors
Pressure sensors
Other
Motor current feedback: sense effort
GPS
Sonar

4. Sensing vs Perception Sensing: acquisition of signals from hardware
Perception: processing of “raw” signals into “meaningful” representations
Example:
Reading pixels from a camera is sensing.
Declaring “it’s a rounded shape with skin color”, “it’s a face”, or “it’s a smiling face” are different levels of perception.
Especially at lower levels of perception like signal processing, the line is blurry, and the often the processed results can be essentially considered “sensed”.
Digits

5. 3D Perception Topics
Sensors: visible-light cameras, depth sensors, laser sensors
(Some) perception tasks:
Stereo reconstruction
Object recognition
3D mapping
Object pose recognition
Key issues:
How to represent and optimize camera transforms?
How to fit models in the presence of noise?
How to represent large 3D models?

6. Visual Sensors
Visible-light cameras: cheap, low power, high resolution, high frame rates
Data: 2D field of RGB pixels
Stereo cameras
Depth field sensors
Two major types: infrared pattern projection (Kinect, ASUS, PrimeSense) and time-of-flight
Data: 2D field of depth values (SwissRanger)
Sweeping laser sensors
Data: 1D field of depth values
Hokuyo, SICK, Velodyne
Can be mounted on a tilt/spin mount to get 3D field of view
Bumblebee stereo camera
ASUS Xtion depth sensor
Hokuyo laser sensor

7. Sensors vary in strengths / weaknesses
Velodyne (DARPA grand challenge)
1.3 million readings/s
$75k price tag

8. Image formation
Light bounces off an object, passes through a lens and lands on a CCD pixel on the image plane
Depth of focus
Illumination and aperture
Color: accomplished through use of filters, e.g. Bayer filter
Each channel’s in-between pixels are interpolated

9. Idealized projective geometry
Let:
Zim: distance from image plane to focal point along the depth axis
(X,Y,Z): point in 3D space relative to focal point, Z > 0
Then, image space point is:
Xc = Zim X / Z
Yc = Zim Y / Z
…Which get scaled and offset to get pixel coordinates x
Image plane
(X,Z)
(Xc,Zim)
Focal point z

10. Issues with real sensors
Motion blur
Distortion caused by lenses
Non-square pixels
Exposure
Noise
Film grain
Salt-and-pepper noise
Shot noise
Motion blur
Distortion
Exposure

11. Calibration Determine camera’s intrinsic parameters
Focal length
Field of view
Pixel dimensions
Radial distortion
That determine the mapping from image pixels to an idealized pinhole camera
Rectification

12. Stereo vision processing
Dense reconstruction
Given two rectified images, find binocular disparity at each pixel
Take a small image patch around each pixel in the left image, search for the best horizontal shifted copy in the right image
What size patch? What search size? What matching criterion?
Works best for highly textured scenes

13. Point Clouds Unordered list of 3D points P={p1,…,pn}
Each point optionally annotated by:
Color (RGB)
Sensor reading ID# (why?)
Estimated surface normal (nx,ny,nz)
No information about objects, occlusions, topology
Point Cloud Library (PCL)

14. 3D Mapping
Each frame of a depth sensor gives a narrow snapshot of the world geometry from a given position
3D mapping is the process of stitching multiple views into a global model

15. Three scenarios:
Consider two raw point clouds P1 and P2 from cameras with transformations T1 and T2.
Goal: build point cloud P in frame T1 (assume identity)
Case 1: relative transformations known
Simple union P = P1  (T2-1  P2)
Case 2: small transformation
Pose registration problem
Vast majority of points correspond between scenes
Case 3: large transformation
Significant fraction of points do not correspond, lighting differences, more occlusions
Pose registration must be more robust to outliers

16. Case 2: Small transformations
Visual odometry: estimate relative motion of subsequent frames using optical flow
Define feature points in P1 (e.g., corner detector)
Estimate a transformation of an image patch around feature that best matches P2 (defines optical flow field)
Transformation: translation, rotation, scale
Fit T2 to match these feature transforms

17. Case 3: Large transformations
Iterative closest point algorithm
Input: initial guess for T2
Repeat until convergence:
Find nearest neighbor pairings between P1 and T2P2
Select those pairs that fall below some distance threshold (outlier rejection)
Assign an error metric and optimize T2 to minimize this metric

18. Case 3: Large transformations
Iterative closest point algorithm
Input: initial guess for T2
Repeat until convergence:
Find nearest neighbor pairings between P1 and T2P2
Select those pairs that fail to satisfy some distance criteria (outlier rejection)
Assign an error metric and optimize T2 to minimize this metric
What metric?
What criteria?
How to minimize?

19. What metrics for matching?
Position
Surface normal
Color
Nearest neighbor methods
Fast data structures, e.g. K-D trees
For large scans usually want a constant sized subsample
Projection-based methods
Render scene from perspective of T1 to determine a match
Very fast (used in Kinect Fusion algorithm)
Only uses position information

20. What criteria for outlier rejection?
Distance too large (e.g., top X%)
Inconsistencies with neighboring pairs
On boundary of scan

21. How to optimize?
Want to find rotation R, translation t of T2 to minimize some error function
Sum of squared point-to-point differences
𝐸 𝑅,𝑡 = 𝑖=1 𝑛 𝑝 𝑖 −(𝑅 𝑞 𝑖 +𝑡) 2
Closed-form solution (SVD)
Very fast per step
Sum of squared point-to-plane differences
𝐸 𝑅,𝑡 = 𝑖=1 𝑛 𝑛 𝑖 𝑇 𝑝 𝑖 − 𝑛 𝑖 𝑇 (𝑅 𝑞 𝑖 +𝑡) 2
Must use numerical methods
Deal with rotation variable
Tends to lead ICP to converge with fewer iterations

22. Other applications of ICP
Fitting 3D triangulated models to point clouds for object recognition / pose estimation

23. Stitching multiple scans
In its most basic form, multi-view 3D mapping is simply a repetition of the two-camera case
But two major issues:
Drift and “closing the loop”
Point clouds become massive after many scans
This time

24. Point cloud growth problem
With N points, at F frames/sec, and T seconds of run time, NFT points are gathered
Kinect: N=307200, F=30, T=60 => 552,960,000 points
With RGB in 4 bytes, XYZ in 12 bytes => 8 GB / min
Solutions:
Forget earlier scans (short term memory)
Build persistent, “collapsed” representation of environment geometry
Polygon meshes
Occupancy grids
Key issue: how to estimate with low # of points and update later?

25. Occupancy grids Store a grid G with a fixed minimum resolution
Mark which cells (voxels) are occupied by a point
Representation size is independent of T
Two options for updating on a new scan
Compute ICP to align current scan to prior scan, then add points to G
Modify ICP to work directly with the representation G

26. Probabilistic Occupancy Grids with Ray Casting
Scans are noisy, so simply adding points is likely to overestimate occupied cells
Ray casting approaches:
Each cell has a probability of being free/occupied/unseen
Each scan defines a line segment that passes through free space and ends in an occupied cell (or near one)
Walk along the segment, increasing P(free(c)) of each encountered cell c, and finally increase P(occupied(c)) for the terminal cell c

27. Probabilistic Occupancy Grids with Ray Casting
Scans are noisy, so simply adding points is likely to overestimate occupied cells
Ray casting approaches:
Each cell has a probability of being free/occupied/unseen
Each scan defines a line segment that passes through free space and ends in an occupied cell (or near one)
Walk along the segment, increasing P(free(c)) of each encountered cell c, and finally increase P(occupied(c)) for the terminal cell c

28. Compact geometry representations within a cell
On-line averaging
On-line least squares estimation of fitting plane

29. Handling large 3D grids
Problem: tabular 3D grid storage increases with O(N3)
10243 is 1Gb
Solutions
Store hash table only of occupied cells
Octree data structure
OctoMap Library (

30. Dynamic Environments
Real environments have people, animals, objects that move around
Two options:
Map static parts by assuming dynamic objects will average out as noise over time (probabilistic occupancy grids)
Segment (and possibly model) dynamic objects

31. Related topics Sensor fusion Object segmentation and recognition
Simultaneous Localization and Mapping (SLAM)
Next-best-view planning

32. Next time Kalman filtering Welch and Bishop (2001) Principles Ch. 8
Zeeshan