PCL Tutorial:

The Point Cloud Library By Example

Jeff Delmerico
Vision and Perceptual Machines Lab
106 Davis Hall
UB North Campus

[email protected]

February 11, 2013

Jeff Delmerico February 11, 2013 1/38

Point Clouds
Definition
A point cloud is a data structure used to represent a collection of
multi-dimensional points and is commonly used to represent
three-dimensional data.
In a 3D point cloud, the points usually represent the X, Y, and Z
geometric coordinates of an underlying sampled surface. When
color information is present, the point cloud becomes 4D.

Jeff Delmerico February 11, 2013 Introduction 2/38

Where do point clouds come from?

I RGB-D cameras
I Stereo cameras
I 3D laser scanners
I Time-of-flight cameras
I Sythetically from software
(e.g. Blender)

Jeff Delmerico February 11, 2013 Introduction 3/38

Point Cloud Library
I PCL is a large scale, open project for 2D/3D image and point
cloud processing (in C++, w/ new python bindings).
I The PCL framework contains numerous state-of-the art
algorithms including filtering, feature estimation, surface
reconstruction, registration, model fitting and segmentation.
I PCL is cross-platform, and has been successfully compiled and
deployed on Linux, MacOS, Windows, and Android/iOS.
I Website: pointclouds.org

Jeff Delmerico February 11, 2013 Introduction 4/38

Getting PCL

I First, download PCL for your system from:

http://pointclouds.org/downloads/
I If you want to try the python bindings (currently for only a
subset of the full PCL functionality), go here:
http://strawlab.github.com/python-pcl/
I PCL provides the 3D processing pipeline for ROS, so you can
also get the perception pcl stack and still use PCL standalone.
I PCL depends on Boost, Eigen, FLANN, and VTK.

Jeff Delmerico February 11, 2013 Using PCL 5/38

Basic Structures
The basic data type in PCL is a PointCloud. A PointCloud is a
templated C++ class which contains the following data fields:
I width (int) - secifies the width of the point cloud dataset in
the number of points.
I the total number of points in the cloud (equal with the
number of elements in points) for unorganized datasets
I the width (total number of points in a row) of an organized
point cloud dataset
I height (int) - Specifies the height of the point cloud dataset
in the number of points.
I set to 1 for unorganized point clouds
I the height (total number of rows) of an organized point cloud
dataset
I points (std::vectorhPointTi) - Contains the data array
where all the points of type PointT are stored.
Jeff Delmerico February 11, 2013 Using PCL 6/38
Basic Structures

I is dense (bool) - Specifies if all the data in points is finite

(true), or whether the XYZ values of certain points might
contain Inf/NaN values (false).
I sensor origin (Eigen::Vector4f) - Specifies the sensor
acquisition pose (origin/translation). This member is usually
optional, and not used by the majority of the algorithms in
PCL.
I sensor orientation (Eigen::Quaternionf) - Specifies the
sensor acquisition pose (orientation). This member is usually
optional, and not used by the majority of the algorithms in
PCL.

Jeff Delmerico February 11, 2013 Using PCL 7/38

Point Types

I PointXYZ - float x, y, z
I PointXYZI - float x, y, z, intensity
I PointXYZRGB - float x, y, z, rgb
I PointXYZRGBA - float x, y, z, uint32 t rgba
I Normal - float normal[3], curvature
I PointNormal - float x, y, z, normal[3], curvature
I Histogram - float histogram[N]
I And many, many, more. Plus you can define new types to suit
your needs.

Jeff Delmerico February 11, 2013 Using PCL 8/38

Building PCL Projects

PCL relies on CMake as a build tool. CMake just requires that

you place a file called CMakeLists.txt somewhere on your project
path.
CMakeLists.txt
cmake minimum required(VERSION 2.6 FATAL ERROR)
project(MY GRAND PROJECT)
find package(PCL 1.3 REQUIRED COMPONENTS common io)
include directories($PCL INCLUDE DIRS)
link directories($PCL LIBRARY DIRS)
add definitions($PCL DEFINITIONS)
add executable(pcd write test pcd write.cpp)
target link libraries(pcd write test $PCL COMMON LIBRARIES
$PCL IO LIBRARIES)

Jeff Delmerico February 11, 2013 Using PCL 9/38

Building PCL Projects

Generating the Makefile & Building the Project

$ cd /PATH/TO/MY/GRAND/PROJECT
$ mkdir build
$ cd build
$ cmake ..
$ make

Jeff Delmerico February 11, 2013 Using PCL 10/38

PCD File Format
A simple file format for storing multi-dimensional point data. It
consists of a text header (with the fields below), followed by the
data in ASCII (w/ points on separate lines) or binary (a memory
copy of the points vector of the PC).
I VERSION - the PCD file version (usually .7)
I FIELDS - the name of each dimension/field that a point can have (e.g. FIELDS
xyz)
I SIZE - the size of each dimension in bytes (e.g. a float is 4)
I TYPE - the type of each dimension as a char (I = signed, U = unsigned, F =
float)
I COUNT - the number of elements in each dimension (e.g. x, y, or z would only
have 1, but a histogram would have N)
I WIDTH - the width of the point cloud
I HEIGHT - the height of the point cloud
I VIEWPOINT - an acquisition viewpoint for the points: translation (tx ty tz) +
quaternion (qw qx qy qz)
I POINTS - the total number of points in the cloud
I DATA - the data type that the point cloud data is stored in (ascii or binary)
Jeff Delmerico February 11, 2013 I/O 11/38
 PCD Example
# .PCD v.7 - Point Cloud Data file format
VERSION .7
FIELDS x y z rgb
SIZE 4 4 4 4
TYPE F F F F
COUNT 1 1 1 1
WIDTH 213
HEIGHT 1
VIEWPOINT 0 0 0 1 0 0 0
POINTS 213
DATA ascii
0.93773 0.33763 0 4.2108e+06
0.90805 0.35641 0 4.2108e+06
0.81915 0.32 0 4.2108e+06
0.97192 0.278 0 4.2108e+06
0.944 0.29474 0 4.2108e+06
0.98111 0.24247 0 4.2108e+06
0.93655 0.26143 0 4.2108e+06
0.91631 0.27442 0 4.2108e+06
0.81921 0.29315 0 4.2108e+06
0.90701 0.24109 0 4.2108e+06
0.83239 0.23398 0 4.2108e+06
0.99185 0.2116 0 4.2108e+06
0.89264 0.21174 0 4.2108e+06
.
.
.

Jeff Delmerico February 11, 2013 I/O 12/38

Writing PCD Files

write pcd.cpp
#i n c l u d e <p c l / i o / p c d i o . h>
#i n c l u d e <p c l / p o i n t t y p e s . h>

int
main ( i n t a r g c , c h a r ∗∗ a r g v )
{
p c l : : P o i n t C l o u d<p c l : : PointXYZ> c l o u d ;

// F i l l i n t h e c l o u d d a t a
cloud . width = 50;
cloud . height = 1;
cloud . is dense = f a l s e ;
cloud . p o i n t s . r e s i z e ( cloud . width ∗ cloud . height ) ;
f o r ( s i z e t i = 0 ; i < c l o u d . p o i n t s . s i z e ( ) ; ++i )
{
c l o u d . p o i n t s [ i ] . x = 1024 ∗ r a n d ( ) / (RAND MAX + 1 . 0 f ) ;
c l o u d . p o i n t s [ i ] . y = 1024 ∗ r a n d ( ) / (RAND MAX + 1 . 0 f ) ;
c l o u d . p o i n t s [ i ] . z = 1024 ∗ r a n d ( ) / (RAND MAX + 1 . 0 f ) ;
}

p c l : : i o : : s a v e P C D F i l e A S C I I ( ” t e s t p c d . pcd ” , c l o u d ) ;
return (0);
}

Jeff Delmerico February 11, 2013 I/O 13/38

Reading PCD Files

read pcd.cpp
#i n c l u d e <p c l / i o / p c d i o . h>
#i n c l u d e <p c l / p o i n t t y p e s . h>

int
main ( i n t a r g c , c h a r ∗∗ a r g v )
{
p c l : : P o i n t C l o u d<p c l : : PointXYZ >:: P t r c l o u d ( new p c l : : P o i n t C l o u d<p c l : : PointXYZ >);

// Load t h e f i l e
i f ( p c l : : i o : : l o a d P C D F i l e<p c l : : PointXYZ> ( ” t e s t p c d . pcd ” , ∗ c l o u d ) == −1)
{
PCL ERROR ( ” C o u l d n ’ t r e a d f i l e t e s t p c d . pcd \n” ) ;
r e t u r n ( −1);
}

// Do some p r o c e s s i n g on t h e c l o u d h e r e

return (0);
}

Jeff Delmerico February 11, 2013 I/O 14/38

Getting Point Clouds from OpenNI

openni grabber.cpp
#i n c l u d e <p c l / i o / o p e n n i g r a b b e r . h>
#i n c l u d e <p c l / v i s u a l i z a t i o n / c l o u d v i e w e r . h>

c l a s s SimpleOpenNIViewer
{
public :
S i m p l e O p e n N I V i e w e r ( ) : v i e w e r ( ”PCL OpenNI V i e w e r ” ) {}
v o i d c l o u d c b ( c o n s t p c l : : P o i n t C l o u d<p c l : : PointXYZRGBA >:: C o n s t P t r &c l o u d )
{
i f ( ! v i e w e r . w a sS t o pp e d ( ) )
v i e w e r . showCloud ( c l o u d ) ;
}

p c l : : v i s u a l i z a t i o n : : CloudViewer viewer ;

Jeff Delmerico February 11, 2013 I/O 15/38

Getting Point Clouds from OpenNI

openni grabber.cpp
void run ( )
{
p c l : : G r a b b e r ∗ i n t e r f a c e = new p c l : : OpenNIGrabber ( ) ;
b o o s t : : f u n c t i o n <v o i d ( c o n s t p c l : : P o i n t C l o u d<p c l : : PointXYZRGBA >:: C o n s t P t r&)> f =
b o o s t : : b i n d (& S i m p l e O p e n N I V i e w e r : : c l o u d c b , t h i s , 1 ) ;
i n t e r f a c e −>r e g i s t e r C a l l b a c k ( f ) ;
i n t e r f a c e −>s t a r t ( ) ;
w h i l e ( ! v i e w e r . w a s St o p pe d ( ) )
{
boost : : t h i s t h r e a d : : s l e e p ( boost : : posix time : : seconds ( 1 ) ) ;
}
i n t e r f a c e −>s t o p ( ) ;
}
};

i n t main ( )
{
SimpleOpenNIViewer v ;
v . run ( ) ;
return 0;
}

Jeff Delmerico February 11, 2013 I/O 16/38

Normal Estimation
compute normals.cpp
void
downsample ( p c l : : P o i n t C l o u d<p c l : : PointXYZRGB >:: P t r &p o i n t s , f l o a t l e a f s i z e ,
p c l : : P o i n t C l o u d<p c l : : PointXYZRGB >:: P t r &d o w n s a m p l e d o u t )
{
p c l : : V o x e l G r i d<p c l : : PointXYZRGB> v o x g r i d ;
vox grid . setLeafSize ( leaf size , leaf size , l e a f s i z e );
vox grid . setInputCloud ( points );
v o x g r i d . f i l t e r (∗ d o w n s a m p l e d o u t ) ;
}

v o i d c o m p u t e s u r f a c e n o r m a l s ( p c l : : P o i n t C l o u d<p c l : : PointXYZRGB >:: P t r &p o i n t s ,

f l o a t n o r m a l r a d i u s , p c l : : P o i n t C l o u d<p c l : : Normal >:: P t r &n o r m a l s o u t )
{
p c l : : N o r m a l E s t i m a t i o n<p c l : : PointXYZRGB , p c l : : Normal> n o r m e s t ;
// Use a FLANN−b a s e d KdTree t o p e r f o r m n e i g h b o r h o o d s e a r c h e s
n o r m e s t . s e t S e a r c h M e t h o d ( p c l : : s e a r c h : : KdTree<p c l : : PointXYZRGB >:: P t r
( new p c l : : s e a r c h : : KdTree<p c l : : PointXYZRGB >));
// S p e c i f y t h e l o c a l n e i g h b o r h o o d s i z e f o r c o m p u t i n g t h e s u r f a c e n o r m a l s
norm est . setRadiusSearch ( normal radius ) ;
// S e t t h e i n p u t p o i n t s
norm est . setInputCloud ( points ) ;
// E s t i m a t e t h e s u r f a c e n o r m a l s and s t o r e t h e r e s u l t i n ” n o r m a l s o u t ”
n o r m e s t . compute (∗ n o r m a l s o u t ) ;
}

Jeff Delmerico February 11, 2013 3D Features 17/38

compute normals.cpp
v o i d v i s u a l i z e n o r m a l s ( c o n s t p c l : : P o i n t C l o u d<p c l : : PointXYZRGB >:: P t r p o i n t s ,
c o n s t p c l : : P o i n t C l o u d<p c l : : PointXYZRGB >:: P t r n o r m a l p o i n t s ,
c o n s t p c l : : P o i n t C l o u d<p c l : : Normal >:: P t r n o r m a l s )
{
pcl : : v i s u a l i z a t i o n : : PCLVisualizer viz ;
v i z . addPointCloud ( points , ” p o i n t s ” ) ;
v i z . addPointCloud ( normal points , ” normal points ” ) ;
v i z . a d d P o i n t C l o u d N o r m a l s<p c l : : PointXYZRGB , p c l : : Normal> ( n o r m a l p o i n t s , n o r m a l s , 1 , 0 .
viz . spin ();
}

i n t main ( i n t a r g c , c h a r ∗∗ a r g v )
{
// Load d a t a from pcd . . .

p c l : : P o i n t C l o u d<p c l : : PointXYZRGB >:: P t r d s ( new p c l : : P o i n t C l o u d<p c l : : PointXYZRGB >);

p c l : : P o i n t C l o u d<p c l : : Normal >:: P t r n o r m a l s ( new p c l : : P o i n t C l o u d<p c l : : Normal >);
// Downsample t h e c l o u d
const f l o a t v o x e l g r i d l e a f s i z e = 0 . 0 1 ;
downsample ( c l o u d , v o x e l g r i d l e a f s i z e , d s ) ;
// Compute s u r f a c e n o r m a l s
const f l o a t normal radius = 0 . 0 3 ;
c o m p u t e s u r f a c e n o r m a l s ( ds n o r m a l r a d i u s , normals ) ;
// V i s u a l i z e t h e n o r m a l s
v i s u a l i z e n o r m a l s ( c l o u d , ds , n o r m a l s ) ;
return (0);
}

Jeff Delmerico February 11, 2013 3D Features 18/38

Computing 3D Features

setInputCloud = False setInputCloud = True

setSearchSurface = False compute on all points, compute on a subset,
using all points using all points
setSearchSurface = True compute on all points, compute on a subset,
using a subset using a subset

Jeff Delmerico February 11, 2013 3D Features 19/38

Filtering

When working with 3D data, there are many reasons for filtering
your data:

I Restricting range (PassThrough)

I Downsampling (VoxelGrid)
I Outlier removal
(StatisticalOutlierRemoval /
RadiusOutlierRemoval)
I Selecting indices

Jeff Delmerico February 11, 2013 filtering 20/38

PassThrough Filter

Filter out points outside a specified range in one dimension. (Or

filter them in with setFilterLimitsNegative)
filtering.cpp
p c l : : P o i n t C l o u d <p c l : : PointXYZ > : : P t r c l o u d
( new p c l : : P o i n t C l o u d <p c l : : PointXYZ >);
p c l : : P o i n t C l o u d <p c l : : PointXYZ > : : P t r c l o u d f i l t e r e d
( new p c l : : P o i n t C l o u d <p c l : : PointXYZ >);

// P a s s T h r o u g h f i l t e r
p c l : : PassThrough<p c l : : PointXYZ> p a s s ;
pass . setInputCloud ( cloud ) ;
pass . s e t F i l t e r F i e l d N a m e (”x” ) ;
pa ss . s e t F i l t e r L i m i t s ( −0.75 , 0 . 5 ) ;
// p a s s . s e t F i l t e r L i m i t s N e g a t i v e ( t r u e ) ;
pass . f i l t e r (∗ c l o u d f i l t e r e d ) ;

Jeff Delmerico February 11, 2013 filtering 21/38

Downsampling to a Voxel Grid

Voxelize the cloud to a 3D grid. Each occupied voxel is

approximated by the centroid of the points inside of it.
filtering.cpp
// Downsample t o v o x e l g r i d
p c l : : V o x e l G r i d <p c l : : PointXYZ> vg ;
vg . s e t I n p u t C l o u d ( c l o u d ) ;
vg . s e t L e a f S i z e ( 0 . 0 1 f , 0 . 0 1 f , 0 . 0 1 f ) ;
vg . f i l t e r ( ∗ c l o u d f i l t e r e d ) ;

Jeff Delmerico February 11, 2013 filtering 22/38

Statistical Outlier Removal

Filter points based on their local point densities. Remove points

that are sparse relative to the mean point density of the whole
cloud.
filtering.cpp
// S t a t i s t i c a l O u t l i e r Removal
pcl : : S t a t i s t i c a l O u t l i e r R e m o v a l <p c l : : PointXYZ> s o r ;
sor . setInputCloud ( cloud ) ;
sor . setMeanK ( 5 0 ) ;
sor . setStddevMulThresh ( 1 . 0 ) ;
sor . f i l t e r (∗ c l o u d f i l t e r e d ) ;

Jeff Delmerico February 11, 2013 filtering 23/38

What is a keypoint?

A keypoint (also known as an “interest point”) is simply a point

that has been identied as a relevant in some way. A good keypoint
detector will find points with the following properties:
I Sparseness: Typically, only a small subset of the points in the
scene are keypoints.
I Repeatiblity: If a point was determined to be a keypoint in
one point cloud, a keypoint should also be found at the
corresponding location in a similar point cloud. (Such points
are often called ”stable”.)
I Distinctiveness: The area surrounding each keypoint should
have a unique shape or appearance that can be captured by
some feature descriptor.

Jeff Delmerico February 11, 2013 Keypoints 24/38

Why compute keypoints?

I Some features are expensive to compute, and it would be

prohibitive to compute them at every point. Keypoints
identify a small number of locations where computing
feature descriptors is likely to be most effective.
I When searching for corresponding points, features computed
at non-descriptive points will lead to ambiguous feature
corespondences. By ignoring non-keypoints, one can reduce
error when matching points.

Jeff Delmerico February 11, 2013 Keypoints 25/38

Detecting 3D SIFT Keypoints

keypoints.cpp
void
d e t e c t k e y p o i n t s ( p c l : : P o i n t C l o u d<p c l : : PointXYZRGB >:: P t r &p o i n t s , f l o a t m i n s c a l e ,
int nr octaves , int nr scales per octave , float min contrast ,
p c l : : P o i n t C l o u d<p c l : : P o i n t W i t h S c a l e >:: P t r &k e y p o i n t s o u t )
{
p c l : : S I F T K e y p o i n t<p c l : : PointXYZRGB , p c l : : P o i n t W i t h S c a l e> s i f t d e t e c t ;

// Use a FLANN−b a s e d KdTree t o p e r f o r m n e i g h b o r h o o d s e a r c h e s

s i f t d e t e c t . s e t S e a r c h M e t h o d ( p c l : : s e a r c h : : KdTree<p c l : : PointXYZRGB >:: P t r
( new p c l : : s e a r c h : : KdTree<p c l : : PointXYZRGB >));

// S e t t h e d e t e c t i o n p a r a m e t e r s
s i f t d e t e c t . setScales ( min scale , nr octaves , nr scales per octave ) ;
s i f t d e t e c t . setMinimumContrast ( m i n c o n t r a s t ) ;

// S e t t h e i n p u t
s i f t d e t e c t . setInputCloud ( points );

// D e t e c t t h e k e y p o i n t s and s t o r e them i n ” k e y p o i n t s o u t ”
s i f t d e t e c t . compute (∗ k e y p o i n t s o u t ) ;
}

Jeff Delmerico February 11, 2013 Keypoints 26/38

Computing PFH Features at Keypoints

keypoints.cpp
void
P F H f e a t u r e s a t k e y p o i n t s ( p c l : : P o i n t C l o u d<p c l : : PointXYZRGB >:: P t r &p o i n t s ,
p c l : : P o i n t C l o u d<p c l : : Normal >:: P t r &n o r m a l s ,
p c l : : P o i n t C l o u d<p c l : : P o i n t W i t h S c a l e >:: P t r &k e y p o i n t s ,
float feature radius ,
p c l : : P o i n t C l o u d<p c l : : P F H S i g n a t u r e 1 2 5 >:: P t r & d e s c r i p t o r s o u t )
{
// C r e a t e a P F H E s t i m a t i o n o b j e c t
p c l : : P FHEstimation<p c l : : PointXYZRGB , p c l : : Normal , p c l : : P F H S i g n a t u r e 1 2 5> p f h e s t ;
p f h e s t . s e t S e a r c h M e t h o d ( p c l : : s e a r c h : : KdTree<p c l : : PointXYZRGB >:: P t r
( new p c l : : s e a r c h : : KdTree<p c l : : PointXYZRGB >));
// S p e c i f y t h e r a d i u s o f t h e PFH f e a t u r e
pfh est . setRadiusSearch ( feature radius );
// Copy XYZ d a t a f o r u s e i n e s t i m a t i n g f e a t u r e s
p c l : : P o i n t C l o u d<p c l : : PointXYZRGB >:: P t r k e y p o i n t s x y z r g b
( new p c l : : P o i n t C l o u d<p c l : : PointXYZRGB >);
p c l : : c o p y P o i n t C l o u d (∗ k e y p o i n t s , ∗ k e y p o i n t s x y z r g b ) ;
// Use a l l o f t h e p o i n t s f o r a n a l y z i n g t h e l o c a l s t r u c t u r e o f t h e c l o u d
pfh est . setSearchSurface ( points );
p f h e s t . setInputNormals ( normals ) ;
// But o n l y compute f e a t u r e s a t t h e k e y p o i n t s
pfh est . setInputCloud ( keypoints xyzrgb );
// Compute t h e f e a t u r e s
p f h e s t . compute (∗ d e s c r i p t o r s o u t ) ;
}
Jeff Delmerico February 11, 2013 Keypoints 27/38
Finding Correspondences

keypoints.cpp
void
f e a t u r e c o r r e s p o n d e n c e s ( p c l : : P o i n t C l o u d<p c l : : P F H S i g n a t u r e 1 2 5 >:: P t r &s o u r c e d e s c r i p t o r s ,
p c l : : P o i n t C l o u d<p c l : : P F H S i g n a t u r e 1 2 5 >:: P t r & t a r g e t d e s c r i p t o r s ,
s t d : : v e c t o r <i n t > &c o r r e s p o n d e n c e s o u t ,
s t d : : v e c t o r <f l o a t > &c o r r e s p o n d e n c e s c o r e s o u t )
{
// R e s i z e t h e o u t p u t v e c t o r
c o r r e s p o n d e n c e s o u t . r e s i z e ( s o u r c e d e s c r i p t o r s −>s i z e ( ) ) ;
c o r r e s p o n d e n c e s c o r e s o u t . r e s i z e ( s o u r c e d e s c r i p t o r s −>s i z e ( ) ) ;

// Use a KdTree t o s e a r c h f o r t h e n e a r e s t m a t c h e s i n f e a t u r e s p a c e
p c l : : s e a r c h : : KdTree<p c l : : P F H S i g n a t u r e 1 2 5> d e s c r i p t o r k d t r e e ;
descriptor kdtree . setInputCloud ( target descriptors );

// F i n d t h e i n d e x o f t h e b e s t match f o r e a c h k e y p o i n t
const i n t k = 1;
s t d : : v e c t o r <i n t > k i n d i c e s ( k ) ;
s t d : : v e c t o r <f l o a t > k s q u a r e d d i s t a n c e s ( k ) ;
f o r ( s i z e t i = 0 ; i < s o u r c e d e s c r i p t o r s −>s i z e ( ) ; ++i )
{
d e s c r i p t o r k d t r e e . n e a r e s t K S e a r c h (∗ s o u r c e d e s c r i p t o r s , i , k ,
k indices , k squared distances );
correspondences out [ i ] = k indices [ 0 ] ;
correspondence scores out [ i ] = k squared distances [ 0 ] ;
}
Jeff Delmerico February 11, 2013 Keypoints 28/38
}
K-d Trees

Jeff Delmerico February 11, 2013 Trees 29/38

KdTree Neighbor Search

kdtree.cpp
#i n c l u d e <p c l / k d t r e e / k d t r e e f l a n n . h>
...
p c l : : KdTreeFLANN<p c l : : PointXYZ> k d t r e e ;
kdtree . setInputCloud ( cloud ) ;
...
// K n e a r e s t n e i g h b o r s e a r c h
int K = 10;
p c l : : PointXYZ s e a r c h P o i n t ;
s t d : : v e c t o r <i n t > p o i n t I d x N K N S e a r c h (K ) ;
s t d : : v e c t o r <f l o a t > p o i n t N K N S q u a r e d D i s t a n c e (K ) ;
i f ( k d t r e e . n e a r e s t K S e a r c h ( s e a r c h P o i n t , K, pointIdxNKNSearch ,
pointNKNSquaredDistance ) > 0 )
{
...
}

// N e i g h b o r s w i t h i n r a d i u s s e a r c h
s t d : : v e c t o r <i n t > p o i n t I d x R a d i u s S e a r c h ;
s t d : : v e c t o r <f l o a t > p o i n t R a d i u s S q u a r e d D i s t a n c e ;
f l o a t r a d i u s = 2 5 6 . 0 f ∗ r a n d ( ) / (RAND MAX + 1 . 0 f ) ;
i f ( kdtree . radiusSearch ( searchPoint , radius , pointIdxRadiusSearch ,
pointRadiusSquaredDistance ) > 0 )
{
...
}
Jeff Delmerico February 11, 2013 Trees 30/38
Octrees

Jeff Delmerico February 11, 2013 Trees 31/38

octree.cpp
#i n c l u d e <p c l / o c t r e e / o c t r e e . h>
...
f l o a t r e s o l u t i o n = 128.0 f ;
p c l : : o c t r e e : : O c t r e e P o i n t C l o u d S e a r c h <p c l : : PointXYZ> o c t r e e ( r e s o l u t i o n ) ;
octree . setInputCloud ( cloud ) ;
octree . addPointsFromInputCloud ( ) ;
...
// N e i g h b o r s w i t h i n v o x e l s e a r c h
i f ( octree . voxelSearch ( searchPoint , pointIdxVec ))
{
...
}

// K n e a r e s t n e i g h b o r s e a r c h
int K = 10;
i f ( o c t r e e . n e a r e s t K S e a r c h ( s e a r c h P o i n t , K,
pointIdxNKNSearch , pointNKNSquaredDistance ) > 0)
{
...
}

// N e i g h b o r s w i t h i n r a d i u s s e a r c h
i f ( octree . radiusSearch ( searchPoint , radius ,
p o i n t I d x R a d i u s S e a r c h , p o i n t R a d i u s S q u a r e d D i s t a n c e ) > 0)
{
...
}

Jeff Delmerico February 11, 2013 Trees 32/38

Sample Consensus
The Random Sample Consensus (RANSAC) algorithm assumes the
data is comprised of both inliers and outliers. The distribution of
inliers can be explained by a set of parameters and a model. The
outlying data does not fit the model.

Jeff Delmerico February 11, 2013 Sample Consensus & Segmentation 33/38
Plane Fitting with RANSAC

sample consensus.cpp
#i n c l u d e <p c l / s a m p l e c o n s e n s u s / r a n s a c . h>
#i n c l u d e <p c l / s a m p l e c o n s e n s u s / s a c m o d e l p l a n e . h>
#i n c l u d e <p c l / s a m p l e c o n s e n s u s / s a c m o d e l s p h e r e . h>
...
s t d : : v e c t o r <i n t > i n l i e r s ;

// c r e a t e d RandomSampleConsensus o b j e c t and compute t h e model

p c l : : S a m p l e C o n s e n s u s M o d e l P l a n e<p c l : : PointXYZ >:: P t r
m o d e l p ( new p c l : : S a m p l e C o n s e n s u s M o d e l P l a n e<p c l : : PointXYZ> ( c l o u d ) ) ;
p c l : : RandomSampleConsensus<p c l : : PointXYZ> r a n s a c ( m o d e l p ) ;
ransac . setDistanceThreshold ( . 0 1 ) ;
r a n s a c . computeModel ( ) ;
ransac . g e t I n l i e r s ( i n l i e r s ) ;

// c o p i e s a l l i n l i e r s o f t h e model computed t o a n o t h e r P o i n t C l o u d
p c l : : c o p y P o i n t C l o u d<p c l : : PointXYZ >(∗c l o u d , i n l i e r s , ∗ f i n a l ) ;

Jeff Delmerico February 11, 2013 Sample Consensus & Segmentation 34/38
euclidean cluster extraction.cpp
#i n c l u d e <p c l / s e g m e n t a t i o n / e x t r a c t c l u s t e r s . h>

p c l : : s e a r c h : : KdTree<p c l : : PointXYZ >:: P t r t r e e ( new p c l : : s e a r c h : : KdTree<p c l : : PointXYZ >);

t r e e −>s e t I n p u t C l o u d ( c l o u d f i l t e r e d ) ;

s t d : : v e c t o r <p c l : : P o i n t I n d i c e s > c l u s t e r i n d i c e s ;
p c l : : E u c l i d e a n C l u s t e r E x t r a c t i o n <p c l : : PointXYZ> e c ;
e c . s e t C l u s t e r T o l e r a n c e ( 0 . 0 2 ) ; // 2cm
ec . s e t M i n C l u s t e r S i z e ( 1 0 0 ) ;
ec . s e t M a x C l u s t e r S i z e ( 2 5 0 0 0 ) ;
ec . setSearchMethod ( t r e e ) ;
ec . s e t I n p u t C l o u d ( c l o u d f i l t e r e d ) ;
ec . e x t r a c t ( c l u s t e r i n d i c e s ) ;

f o r ( s t d : : v e c t o r <p c l : : P o i n t I n d i c e s >:: c o n s t i t e r a t o r i t = c l u s t e r i n d i c e s . begin ( ) ;

i t != c l u s t e r i n d i c e s . end ( ) ; ++i t )
{
p c l : : P o i n t C l o u d<p c l : : PointXYZ >:: P t r c l o u d c l u s t e r
( new p c l : : P o i n t C l o u d<p c l : : PointXYZ >);
f o r ( s t d : : v e c t o r <i n t >:: c o n s t i t e r a t o r p i t = i t −>i n d i c e s . b e g i n ( ) ;
p i t != i t −>i n d i c e s . end ( ) ; p i t ++)
c l o u d c l u s t e r −>p o i n t s . p u s h b a c k ( c l o u d f i l t e r e d −>p o i n t s [ ∗ p i t ] ) ;

c l o u d c l u s t e r −>w i d t h = c l o u d c l u s t e r −>p o i n t s . s i z e ();

c l o u d c l u s t e r −>h e i g h t = 1 ;
c l o u d c l u s t e r −>i s d e n s e = t r u e ;
}

Jeff Delmerico February 11, 2013 Sample Consensus & Segmentation 35/38
 Iterative Closest Point

ICP iteratively revises the transformation (translation,

rotation) needed to minimize the distance between the
points of two raw scans.

Inputs: points from two raw scans, initial estimation of the

transformation, criteria for stopping the iteration.

Output: refined transformation.

The algorithm steps are :

1. Associate points by the nearest neighbor criteria.
2. Estimate transformation parameters using a mean
square cost function.
3. Transform the points using the estimated parameters.
4. Iterate (re-associate the points and so on).
Jeff Delmerico February 11, 2013 Registration 36/38
Iterative Closest Point

icp.cpp
#i n c l u d e <p c l / r e g i s t r a t i o n / i c p . h>
...
p c l : : I t e r a t i v e C l o s e s t P o i n t <p c l : : PointXYZRGB , p c l : : PointXYZRGB> i c p ;
icp . setInputCloud ( cloud2 ) ;
icp . setInputTarget ( cloud1 ) ;
icp . setMaximumIterations (20);
icp . setMaxCorrespondenceDistance ( 0 . 1 ) ;
Eigen : : Matrix4f t r a f o ;
i c p . a l i g n (∗ c l o u d 2 ) ;
(∗ c l o u d 2 ) += ∗( c l o u d 1 ) ;
...

Jeff Delmerico February 11, 2013 Registration 37/38

Conclusion
PCL has many more tutorials and lots sample code here:
http://pointclouds.org/documentation/tutorials/. And
the tutorials only cover a small portion of its overall functionality.

I hope you find a use for PCL in your own projects, and you should
feel free to ask me any PCL-related questions in the future
([email protected]).

Jeff Delmerico February 11, 2013 Conclusion 38/38