PCL :: Search - PCL - Point Cloud Library (PCL) · KdTree3D Nearest Neighbor SearchHigh Dimensional Nearest Neighbor SearchOctree Nearest Neighbor Search I Nearest neighbor search

KdTree 3D Nearest Neighbor Search High Dimensional Nearest Neighbor Search Octree

PCL :: SearchMarius Muja and Julius Kammerl

July 1, 2011


Motivation

I Nearest neighbor search is an inner loop of many parts ofPCL (filters, surface, features, registration)

I Needs to be as fast as possibleI FLANN - Fast Library for Appproximate Nearest Neighbors

I http://www.cs.ubc.ca/~mariusm/flannI C, C++, Matlab and Python bindingsI Exact nearest neighbor search in low dimensional spaces

(3D) using kd-treesI Approximate nearest neighbor search in high dimensional

spacesI Octree - 3D search

Point Cloud Library (PCL)

http://www.cs.ubc.ca/~mariusm/flann


Nearest Neighbor Search

I Nearest neighbor search problemI Given a set of points P = p1,p2, ...,pn in a metric space X,

preprocess them in such a way that given a new pointq ∈ X finding the closest pi to q can be done easily

I K-Nearest neighbor searchI find the closest K neighbors

I Radius nearest neighbor searchI find all the neighbors within a certain radius



Outline

1. KdTree

2. 3D Nearest Neighbor Search

3. High Dimensional Nearest Neighbor Search

4. Octree



The KD-Tree

I recursively divide the data points based on a singledimension

I how to choose the dimension in which to divide the data?I where to divide?

I binary treeI when searching entire branches can be ignored due to

being too far away from the query pointI very efficient for low dimensionality data



KD-Tree Example



KD-Tree Example



KD-Tree Example



KD-Tree Example



KD-Tree Example



KD-Tree Example



KD-Tree Example



KD-Tree Example



GSOC

I during 2011 GSOC, 2 students are working on a GPUbased kd-tree implemenattion

I encouraging preliminary results, speedups of 8-10xcompared to CPU implementation



FLANN in PCL

I Header: #include <pcl/kdtree/kdtree_flann.h>

I Class: template<typename PointT> class pcl::KdTreeFLANN

I K-nearest neighbor searchint nearestKSearch (const PointT &point, int k,

vector<int> &k_indices, vector<float> &k_distances);

I Radius searchint radiusSearch (const PointT &point, double radius,

vector<int> &k_indices,vector<float> &k_distances, int max_nn = -1);



KNN Search Example

PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>);PointXYZ searchPoint;

// ... populate the cloud and the search point

// create a kd-tree instanceKdTreeFLANN<PointXYZ> kdtree;

// assign a point cloud - this builds the treekdtree.setInputCloud (cloud);

// pre-allocate the neighbor index and// distance vectorsint K = 10;std::vector<int> pointsIdx(K);std::vector<float> pointsSquaredDist(K);

// K nearest neighbor searchkdtree.nearestKSearch (searchPoint, K, pointsIdx, pointsSquaredDist);



Radius Search Example

PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>);PointXYZ searchPoint;

// ... populate the cloud and the search point

// create a kd-tree instanceKdTreeFLANN<PointXYZ> kdtree;

// assign a point cloud - this builds the treekdtree.setInputCloud (cloud);

std::vector<int> pointIdxRadius;std::vector<float> pointsSquaredDistRadius;float radius = ...;

// radius searchint count = kdtree.radiusSearch (searchPoint, radius,

pointIdxRadiusSearch, pointsSquaredDistRadius);



Compile & Try

$ cd $PCL_ROOT/doc/tutorials/content/sources/kdtree_search$ mkdir build$ cd build$ cmake ..$ make$ ./kdtree_searchK nearest neighbor search at (701.248 662.202 554.841) with K=10

702.91 601.583 521.043 (squared distance: 4819.7)676.792 699.92 482.203 (squared distance: 7297.07)731.215 717.665 491.714 (squared distance: 7959.15)670.142 707.355 476.051 (squared distance: 9214.31)681.636 728.872 479.31 (squared distance: 10534.5)683.843 581.742 492.494 (squared distance: 10663.9)696.085 705.888 457.71 (squared distance: 11369.7)683.603 667.109 430.477 (squared distance: 15801.9)721.228 684.503 430.334 (squared distance: 16398.5)829.566 676.396 560.64 (squared distance: 16700.6)

Neighbors within radius search at (701.248 662.202 554.841) withradius=114.069

702.91 601.583 521.043 (squared distance: 4819.7)676.792 699.92 482.203 (squared distance: 7297.07)731.215 717.665 491.714 (squared distance: 7959.15)670.142 707.355 476.051 (squared distance: 9214.31)681.636 728.872 479.31 (squared distance: 10534.5)683.843 581.742 492.494 (squared distance: 10663.9)696.085 705.888 457.71 (squared distance: 11369.7)



Outline

1. KdTree

2. 3D Nearest Neighbor Search

3. High Dimensional Nearest Neighbor Search

4. Octree



Motivation

I Object recognition (TOD)

I Image stitching (AutoStitch, Hugin)I 3D Reconstruction (Photosynth)I 3D object classification (VFH)I Content based image retrievalI Visual SLAM

TOD c©Willow GaragePoint Cloud Library (PCL)


Motivation

I Object recognition (TOD)I Image stitching (AutoStitch, Hugin)

I 3D Reconstruction (Photosynth)I 3D object classification (VFH)I Content based image retrievalI Visual SLAM

AutoStitch c©Matthew BrownPoint Cloud Library (PCL)


Motivation

I Object recognition (TOD)I Image stitching (AutoStitch, Hugin)I 3D Reconstruction (Photosynth)

I 3D object classification (VFH)I Content based image retrievalI Visual SLAM

Photosynth c©Microsoft CorporationPoint Cloud Library (PCL)


Motivation

I Object recognition (TOD)I Image stitching (AutoStitch, Hugin)I 3D Reconstruction (Photosynth)I 3D object classification (VFH)

I Content based image retrievalI Visual SLAM

VFH Classification examplePoint Cloud Library (PCL)


Motivation

I Object recognition (TOD)I Image stitching (AutoStitch, Hugin)I 3D Reconstruction (Photosynth)I 3D object classification (VFH)I Content based image retrieval

I Visual SLAM



Motivation

I Object recognition (TOD)I Image stitching (AutoStitch, Hugin)I 3D Reconstruction (Photosynth)I 3D object classification (VFH)I Content based image retrievalI Visual SLAM



High Dimensional Search

I For high dimensionality data, no exact algorithm fasterthan linear search is known

I Approximate nearest neighbor search is used to obtainlarge speedups

I FLANN contains several algorithms for high dimensionalapproximate nearest neighbor search

I KDTreeIndex (randomized kd-tree forest)I KMeansIndex (hierarchical k-means tree)I HierarchicalClusteringIndex (clustering tree in a generic

metric space)∗I LshIndex (locality sensitive hashing)∗



Randomized KD-Trees

I multiple trees are build in parallelI the split dimension is chosen randomly from the first D

dimensions with greatest variance (D=5)I at search time a single priority queue is used across all

treesI search is terminated after a predefined number of tree

leafs are checked



Hierarchical K-Means Tree

I Building the treeI built by splitting the data at

each level of the tree usingk-means clustering

I apply the same procedurerecursively on each cluster

I just a few iterations of thek-means clustering give goodresults

I Exploring the treeI unexplored branches are added

to a priority queue whiletraversing the tree

I restart search from best branchin the priority queue

which with the cited number of 2000 stable features per

frame amounts to about 50 training images in the database.

Lowe’s approach has been used on around 5000 objects

in a commercial application, but we are not aware of an

academic reference describing these results.

For the most part, the above approaches keep amounts

of data around in the database that is on the order of

magnitude as large as the image patches themselves, or

at least the region descriptors. However, the compactness

of the database is very important for query efficiency in

a large database. With our vocabulary tree approach, the

representation of an image patch is simply one or two

integers, which should be contrasted to the hundreds of

bytes or floats used for a descriptor vector.

Compactness is also the most important difference

between our approach and the hierarchical approach used

by Grauman and Darrell [5]. They use a pyramid of

histograms, at each level doubling the number of bins along

each axis without considering the distribution of data. By

using a vocabulary adapted to the likely distribution of

data, we can use a much smaller tree, resulting in better

resolution while maintaining a compact representation. We

also estimate that our approach is around a factor 1000

faster.

For feature extraction, we use our own implementation

of Maximally Stable Extremal Regions (MSERs) [10].

They have been found to perform well in thorough

performance evaluation [13, 4]. We warp an elliptical

patch around each MSER region into a circular patch.

The remaining portion of our feature extraction is then

implemented according to the SIFT feature extraction

pipeline by Lowe [9]. Canonical directions are found based

on an orientation histogram formed on the image gradients.

SIFT descriptors are then extracted relative to the canonical

directions. The SIFT descriptors have been found highly

distinctive in performance evaluation [12]. The normalized

SIFT descriptors are then quantized with the vocabulary

tree. Finally, a hierarchical scoring scheme is applied to

retrieve images from a database.

3. Building and Using the Vocabulary Tree

The vocabulary tree defines a hierarchical quantization

that is built by hierarchical k-means clustering. A large

set of representative descriptor vectors are used in the

unsupervised training of the tree.

Instead of k defining the final number of clusters or

quantization cells, k defines the branch factor (number of

children of each node) of the tree. First, an initial k-

means process is run on the training data, defining k cluster

centers. The training data is then partitioned into k groups,

where each group consists of the descriptor vectors closest

to a particular cluster center.

The same process is then recursively applied to

Figure 2. An illustration of the process of building the vocabulary

tree. The hierarchical quantization is defined at each level by kcenters (in this case k = 3) and their Voronoi regions.

each group of descriptor vectors, recursively defining

quantization cells by splitting each quantization cell into knew parts. The tree is determined level by level, up to some

maximum number of levels L, and each division into k parts

is only defined by the distribution of the descriptor vectors

that belong to the parent quantization cell. The process is

illustrated in Figure 2.

In the online phase, each descriptor vector is simply

propagated down the tree by at each level comparing

the descriptor vector to the k candidate cluster centers

(represented by k children in the tree) and choosing the

closest one. This is a simple matter of performing kdot products at each level, resulting in a total of kL dot

products, which is very efficient if k is not too large. The

path down the tree can be encoded by a single integer and

is then available for use in scoring.

Note that the tree directly defines the visual vocabulary

and an efficient search procedure in an integrated

manner. This is different from for example defining a

visual vocabulary non-hierarchically, and then devising

an approximate nearest neighbor search in order to find

visual words efficiently. We find the seamless choice

more appealing, although the latter approach also defines

quantization cells in the original space if used consistently

and deterministically. The hierarchical approach also gives

more flexibility to the subsequent scoring procedure.

While the computational cost of increasing the size of

the vocabulary in a non-hierarchical manner would be very

high, the computational cost in the hierarchical approach is

(Nistér & Stewénius, 2006)



FLANN Usage

I Header: #include <flann/flann.h>

I Class: template<typename Distance> class flann::Index

I K-nearest neighbor searchvoid knnSearch(const Matrix<ElementType>& queries,

Matrix<int>& indices, Matrix<DistanceType>& dists,int knn, const SearchParams& params)

I Radius searchint radiusSearch(const Matrix<ElementType>& query,

Matrix<int>& indices, Matrix<DistanceType>& dists,float radius, const SearchParams& params)



FLANN Search Example

flann::Matrix<float> data;flann::Matrix<float> queries;// populate the matrix with features// one feature/row

// build randomized kd-tree index (4 trees)flann::Index< L2<float> > index(data, flann::KDTreeIndexParams(4));index.buildIndex();

// allocate memory for resultsint k = 10;int n = queries.rows;flann::Matrix<int> k_indices(new int[n*k], n, k);flann::Matrix<float> k_distances(new float[n*k], n, k);

// KNN searchindex.knnSearch (queries, k_indices, k_distances, k,

flann::SearchParams(256));



Search Precision

I Speedup with precision for different dataset sizes

50 60 70 80 90 100

100

102

104

106

Correct neighbors (%)

Spe

edup

ove

r lin

ear

sear

ch

k−means tree − sift 31Mrand. kd−trees − sift 31Mk−means tree − sift 1Mrand. kd−trees − sift 1Mk−means tree − sift 100Krand. kd−trees − sift 100K



VFH Recognition



VFH Recognition



Compile & Try

I See tutorial at: http://www.pointclouds.org/documentation/tutorials/vfh_recognition.php

$ cd $PCL_ROOT/doc/tutorials/content/sources/vfh_recognition$ wget http://dev.pointclouds.org/attachments/download/216/vfh_recognition_tutorial_data.tbz$ tar -xzf vfh_recognition_tutorial_data.tbz$ mkdir build$ cd build$ cmake ..$ make$ cd ..$ ./build/build_tree data...$ ./build/nearest_neighbors -k 16 -thresh 50 data/000.580.67/1258730231333_cluster_0_nxyz_vfh.pcd


http://www.pointclouds.org/documentation/tutorials/vfh_recognition.php

http://www.pointclouds.org/documentation/tutorials/vfh_recognition.php


Octree Overview

Octree - 3D hierachical spatial tree data structureI Recursive divide & conquer algorithmI Binary subdivision of occupied cells into 8 octants (voxels)

2D Example (Quadtree):



Octree Overview

Serialized Octree:00000100 01000001 00011000 00100000

00000100

01000001

00011000 00100000

I Root node describes a cubic bounding box whichencapsulates all points

I Child nodes recursively subdivide point spaceI Nodes have up to eight children ⇒ Byte encoding



Octree Usage

Instantiate octree:float voxelSize = 0.01f; // voxel resolutionOctreePointCloud<PointXYZ> octree (voxelSize);

Set input point cloud (via Boost shared pointers):octree.setInputCloud (cloud);

Define octree bounding box (optional):// calculate bounding box of input cloudoctree.defineBoundingBox ();// manually define bounding boxoctree.defineBoundingBox (minX, minY, minZ, maxX, maxY, maxZ);

Add points from input cloud to octree:octree.addPointsFromInputCloud ();

Delete octree data structure:(pushes allocated nodes to memory pool!)octree.deleteTree ();



Data/Voxel Access

Check if voxel at given point coordinates exist:double X,Y,Z;bool occuppied;X = 1.0; Y=2.0; Z=3.0;occuppied = octree.isVoxelOccupiedAtPoint (X, Y, Z);

Get center points of all occupied voxels:(voxel grid filter/downsampling)std::vector<PointXYZ> pointGrid;octree.getOccupiedVoxelCenters (pointGrid);

Delete voxel:pcl::PointXYZ point_arg( 1.0, 2.0, 3.0 );octree.deleteVoxelAtPoint ( point );



Octree Applications

Provided algorithms in PCL using octrees for spatialdecomposition:

I Search operations (neighbor search, radius search, voxelsearch)

I Downsampling (Voxel-grid / Voxel-centroid filter)I Point cloud compressionI Spatial change detectionI Spatial point density analysisI Occupancy checks/mapsI Collision detectionI ...



Neighbor Search I

Points within radius searchI Depth first tree explorationI At every node investigate occupied child voxels that

overlap with search sphere

K nearest neighbor search:I Priority queue (binary heap) of nodes and point candidatesI Investigate occupied child voxels (closest voxel first)I Radius search with radius=distance to Kth point candidateI Update radius with every new point candidate



Neighbor Search II

Define search precision / error bound:octree.setEpsilon (double eps); // default: 0.0

Neighbors within voxel search:std::vector<int> pointIdxVec;

if (octree.voxelSearch (searchPoint, pointIdxVec)){for (size_t i = 0; i < pointIdxVec.size (); ++i)std::cerr << " " << cloud->points[pointIdxVec[i]].x<< " " << cloud->points[pointIdxVec[i]].y<< " " << cloud->points[pointIdxVec[i]].z << std::endl;

}

K nearest neighbor search:int K = 10;std::vector<int> pointIdxNKNSearch;std::vector<float> pointNKNSquaredDistance;

if ( octree.nearestKSearch (searchPoint, K,pointIdxNKNSearch, pointNKNSquaredDistance) > 0 )

{...

}Point Cloud Library (PCL)


Neighbor search III

Neighbors within radius search:std::vector<int> pointIdxRadiusSearch;std::vector<float> pointRadiusSquaredDistance;float radius = 0.1;

if ( octree.radiusSearch (searchPoint, radius,pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 )

{...

}

Approx. neighbors within radius search:(only scans points within “search point voxel”)std::vector<int> pointIdxRadiusSearch;std::vector<float> pointRadiusSquaredDistance;float radius = 0.1;

if ( octree.approxNearestSearch (searchPoint, radius,pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 )

{...

}



Octree Implementation

Template configuration:

OctreePointCloud< PointT, LeafT, OctreeT >

OctreeBase Octree2BufBase OctreeLowMemBase

OctreeLeafEmpty OctreeLeafDataT OctreeLeafDataTVector

Optimized performance&memory usage:I Select octree base implementationI Select/define leaf node classI Serialization callbacks (serializeLeafCallback,

deserializeLeafCallback, serializeNewLeafCallback)



Octree instantiation

OctreePointCloud classes:float resolution = 0.01f;

// equal to OctreePointCloudPointVector<PointXYZ>OctreePointCloud<PointXYZ> octreeA (resolution);

// manages indices vectors in leaf nodesOctreePointCloudPointVector<PointXYZ> octreeB (resolution);// keeps a single point indices in leaf nodesOctreePointCloudSinglePoint<PointXYZ> octreeC (resolution);// does not store any point information in leaf nodeOctreePointCloudOccupancy<PointXYZ> octreeD (resolution);

Octree-Base selection via typedefs:OctreePointCloud<PointXYZ>::SingleBuffer octreeSB (resolution);OctreePointCloud<PointXYZ>::DoubleBuffer octreeDB (resolution);OctreePointCloud<PointXYZ>::LowMem octreeLM (resolution);



Double Buffering

Octree2BufBase implementation:I Create octrees at high rateI Advanced memory management:

I Previous tree structure is kept in memoryI Maximum reusage of already allocate branch&leaf nodesI Unused node instances are pushed to a memory pool for

later reusageI Enables comparison of octree structure (change detection)

Switching between octree buffers:octree.switchBuffers ();



Change Detection

class SimpleSpatialChangeDetection{public:OctreePointCloudChangeDetector<PointXYZRGB>* octree;...voidcloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &cloud){if (!viewer.wasStopped ()){// Switch octree buffersoctree.switchBuffers ();

// Add points from cloud to octreeoctree.setInputCloud (cloud);octree.addPointsFromInputCloud ();

std::vector<int> newPointIdxVector;

/* Get vector of point indices from octree voxelswhich did not exist in previous buffer */

octree.getPointIndicesFromNewVoxels (newPointIdxVector);...

}}

};Point Cloud Library (PCL)


Change Detection

I Real-time spatial change detectionbased on XOR comparison of octree structure

DEMO: See /visualization/tool/openni_change_viewer



Extending the Octree

Example: Point density estimationDesign your own leaf node class:template<typename DataT>class OctreePointCloudDensityLeaf : public OctreeLeafAbstract<DataT>{public:...

virtual voidsetData (const DataT& point_arg){pointCounter_++;

}

unsigned intgetPointCounter (){return pointCounter_;

}

...

private:unsigned int pointCounter_;

};Point Cloud Library (PCL)


Extending the Octree

.. and your own OctreePointCloud class:

class OctreePointCloudDensity : public OctreePointCloud<PointT, OctreePointCloudDensityLeaf<int> , OctreeT>

{public:...

unsigned intgetVoxelDensityAtPoint (const PointT& point_arg) const{unsigned int pointCount = 0;

OctreePointCloudDensityLeaf<int>* leaf =this->findLeafAtPoint (point_arg);

if (leaf) pointCount = leaf->getPointCounter ();

return pointCount;}

};



Point Cloud CompressionEncoding Pipeline:

Octree Structure

Point Component Encoding

Entropy Encoding

Point Detail Encoding Position Detail Coefficients

BinarySerialization

Compressed PCPoint Cloud

Component Voxel Avg. + Detail Coefficients

Example:/* for a full list of profiles see:

/io/include/pcl/compression/compression_profiles.h */compression_Profiles_e compressionProfile =pcl::octree::MED_RES_ONLINE_COMPRESSION_WITH_COLOR;

// instantiate point cloud compression for encoding and decodingPointCloudCompression<PointXYZ> PointCloudEncoder (compressionProfile);PointCloudCompression<PointXYZ> PointCloudDecoder ();...// iostream to read/write compressed point cloud datastd::stringstream compressedData;

// compress & decompress point cloudPointCloudEncoder->encodePointCloud (cloud, compressedData);PointCloudDecoder->decodePointCloud (compressedData, cloudOut);



Compile & Try

I See octree search tutorial at:http://pointclouds.org/documentation/tutorials/octree.php

I See point cloud compression tutorial at:http://pointclouds.org/documentation/tutorials/compression.php

I See change detection tutorial at:http://pointclouds.org/documentation/tutorials/octree_change.php

I Point cloud compression and streaming app:PCL_ROOT/apps/openni_stream_compression

I Change detection app:PCL_ROOT/visualization/tools/openni_change_viewer


http://pointclouds.org/documentation/tutorials/octree.php

http://pointclouds.org/documentation/tutorials/octree.php

http://pointclouds.org/documentation/tutorials/compression.php

http://pointclouds.org/documentation/tutorials/compression.php

http://pointclouds.org/documentation/tutorials/octree_change.php

http://pointclouds.org/documentation/tutorials/octree_change.php

PCL_ROOT/apps/openni_stream_compression

PCL_ROOT/visualization/tools/openni_change_viewer

PCL_ROOT/visualization/tools/openni_change_viewer

Documents

PCL :: Search - PCL - Point Cloud Library (PCL) · KdTree3D Nearest Neighbor SearchHigh Dimensional Nearest Neighbor SearchOctree Nearest Neighbor Search I Nearest neighbor search