Only released in EOL distros:   fuerte groovy hydro

Overview

One of the main problems with registration solutions is that their efficiency often depends on the application. The modular_cloud_matcher package provides a convenient way to tune and test several registration solutions using the same node. This packages holds a single node. It is based on libpointmachter which aims at providing registration evaluation and optimization independently from ROS. Using this library, modular_cloud_matcher allows a complete configuration of the registration chain through parameters, see the parameter section for a list of possible values. This leads to a real-time tracker in 3D pose. You can see this package in action in our ROS contest entry: openni/Contests/ROS 3D/Automatic Calibration of Extrinsic Parameters. On this page, you will also find the necessary information on how to use our tracker with your own Kinect.

The following table shows an overview of the implemented solutions.

For installation instructions, see the ethzasl_icp_mapping stack page.

Registration Chain

The registration chain performs a generalization of Iterative Closest Point algorithm. In this chain, a point cloud is a pair of vectors. The first vector is the feature vector, and typically holds the coordinates of the points. The second vector is the descriptor vector, and holds additional informations such as the normals to the points, their colors, etc. The descriptor vector is either empty or of the same size of the feature vector.

The registration chain takes two point clouds as input, the reading and the reference. The registration algorithm tries to align the reading onto the reference. To do so, it first applies some filtering to the cloud, and then iterates. In each iteration, it associates points in reading to points in reference and finds a transformation of reading such as to minimize an alignment error. The following sketch illustrates the process:

Data Filters

A data filter takes a point cloud as input, transforms it, and produces another point cloud as output. For instance, the transformation might add descriptors, such as SurfaceNormalDataPointsFilter, or change the number of points, such as FixStepSamplingDataPointsFilter, or do both, such as SamplingSurfaceNormalDataPointsFilter.

Data filters can be chained.

Matchers

A matcher links points in the reading to points in the reference, for now, we provide KDTreeMatcher, based on the fast nearset-neighbor library libnabo.

Feature Outlier Filters

A feature outlier filter removes links between points in reading and their matched points in reference, depending on some criteria. For example, if the distance between the points exceeds a threshold, as in MaxDistOutlierFilter, or if it exceeds a certain number of times the median distance, as in MedianDistOutlierFilter.

Points with no link will be ignored in the subsequent minimization step. Feature outlier filters can be chained.

Error Minimizers

An error minimizer will compute a transformation matrix such as to minimize the error between the reading and the reference. There are different error functions available, such as PointToPointErrorMinimizer or PointToPlaneErrorMinimizer.

Transformation Checkers

A transformation checker can stop the iteration depending on some conditions. For example, a condition can be the number of times the loop was executed, as in CounterTransformationChecker, or it can be related to the matching error, as in DifferentialTransformationChecker.

Transformation checkers can be chained.

Inspector

The inspector allows to log data at the different steps, for analysis.

Citing

A paper using this node to evaluate a Kinect tracker was presented at IROS 2011 and can be found here (fulltext). It also introduces the functionalities of the library.

If you use modular_cloud_matcher in an academic context, please cite the following publication:

@INPROCEEDINGS{pomerleau11tracking,
        author = {François Pomerleau and Stéphane Magnenat and Francis Colas and Ming Liu and Roland Siegwart},
        title = {Tracking a Depth Camera: Parameter Exploration for Fast ICP},
        booktitle = {Proc. of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},
        publisher = {IEEE Press},
        pages = {3824--3829},
        year = {2011}
}

Quick Start

Since this node is highly modular, it has many parameters. You can play with it directly if you have a kinect, provided that you have the openni_kinect driver installed. Just run:

roslaunch modular_cloud_matcher openni_kinect_tracker_viewer.launch

You can then have a look into the file launch/openni_kinect_tracker.launch to see how the kinect tracker is configured. This file corresponds to the following chain:

Starting from this chain, and using the documentation in the rest of this page, you can develop your own chain.

Node: cloud_matcher_node

This node performs continuous registration of point clouds and publishes the resulting pose on as a tf. The type of topic messages must be sensor_msgs/PointCloud2. This effectively implements a real-time tracker in 3D pose.

In addition to tf, this node sends StampedTransform representing the transformation matrix between the most recent one and the previous if there is a subscriber to the corresponding topic.

Published Topics

/tf

• A transformation frame between /world and /openni_rgb_optical_frame. You can change the name of these frames using the ~fixedFrame and ~sensorFrame parameters.

/tracker_path

• The tracked path between the same frames as in /tf. You can change the name of this topic using the ~path parameter.

/openni_delta_pose

• The delta poses. You can change the name of this topic using the ~deltaPoseTopic parameter.

Subscribed Topics

/camera/rgb/points (sensor_msgs/PointCloud2)

• You can change the name of this topic using the ~cloudTopic parameter.

Parameters

~config (string, no default)

• Name of the YAML file configuring the modular ICP chain, see Configuration section.

~useROSLogger (bool, default: false)

• If true, use ROS console for logging, otherwise use logger defined in config file.

~cloudTopic (string, default: "/camera/rgb/points")

• The node listens to the topic whose name is defined by this parameter and expects a sensor_msgs/PointCloud2.

~deltaPoseTopic (string, default: "/openni_delta_pose")

• Topic on which to send delta pose.

~fixedFrame (string, default: "/world")

• Frame name used as fixed frame in the tf published by the node.

~sensorFrame (string, default: "/openni_rgb_optical_frame")

• Frame name used as moving frame in the tf published by the node.

~startupDropCount (unsigned, default: 0)

• Number of clouds to drop on startup, useful if sensor is producing garbage in a startup phase.

Node: cloud_matcher_service

This node provides a service to match two point clouds.

Services offered

matchClouds (MatchClouds.srv - custom messages in the package)

• This service is available if the node is not running with the -topic argument. In that case, this service takes two sensor_msgs/PointCloud2 messages as inputs (the first is the reference and the second the reading) and provides a geometry_msgs/Transform message as output.

Parameters

The parameters to configure the chain are the same as the one for cloud_matcher_node. In addition, a parameter allows to configure the name of the service:

serviceName (string, default: matchClouds)

• name of the service.

~config (string, no default)

• Name of the YAML file configuring the modular ICP chain, see Configuration section.

~useROSLogger (bool, default: false)

• If true, use ROS console for logging, otherwise use logger defined in config file.

Chain configuration

You configure the ICP chain through a yaml file. You can see an example for the kinect (launch/kinect.yaml). The rest of this section shows all available options. Note that you can retrieve this list using the pmicp -l command.

Note that some configurations are invalid, for instance, the PointToPlaneErrorMinimizer depends on the reference cloud having normals in its descriptors. Thus, to use this error minimizer, you must put SurfaceNormalDataPointsFilter or SamplingSurfaceNormalDataPointsFilter as data-point filters to the reference. If the node encounters an invalid chain, it will display an error message and abort.

modular_cloud_matcher-specific

~ratioToSwitchKeyframe (double, default: 0.8)

• To avoid drift when the sensor is not moving, the node stores a key frame and the registration will refer to this frame as long as the ratio of matching points is larger than ratioToSwitchKeyframe. If not, the node stores a new key frame.

Transformation

TransformFeatures

Transform features.

• no parameters

TransformNormals

Rotate normals.

• no parameters

DataPointsFilter

FixStepSamplingDataPointsFilter

Subsampling. This filter reduces the size of the point cloud by only keeping one point over step ones; with step varying in time from startStep to endStep, each iteration getting multiplied by stepMult. If use as prefilter (i.e. before the iterations), only startStep is used.

startStep (default: 10, min: 1, max: 2147483647)

• initial number of point to skip (initial decimation factor)

endStep (default: 10, min: 1, max: 2147483647)

• maximal or minimal number of points to skip (final decimation factor)

stepMult (default: 1, min: 0.0000001, max: inf)

• multiplication factor to compute the new decimation factor for each iteration

IdentityDataPointsFilter

Does nothing.

• no parameters

MaxDistDataPointsFilter

Subsampling. Filter points beyond a maximum distance measured on a specific axis. If dim is set to -1, points are filtered based on a maximum radius.

dim (default: -1, min: -1, max: 2)

• dimension on which the filter will be applied. x=0, y=1, z=2, radius=-1

maxDist (default: 1, min: 0, max: inf)

• maximum distance authorized. All points beyond that will be filtered.

MaxQuantileOnAxisDataPointsFilter

Subsampling. Filter points beyond a maximum quantile measured on a specific axis.

dim (default: 0, min: 0, max: 2)

• dimension on which the filter will be applied. x=0, y=1, z=2

ratio (default: 0.5, min: 0.0000001, max: 0.9999999)

• maximum quantile authorized. All points beyond that will be filtered.

MinDistDataPointsFilter

Subsampling. Filter points before a minimum distance measured on a specific axis. If dim is set to -1, points are filtered based on a minimum radius.

dim (default: -1, min: -1, max: 2)

• dimension on which the filter will be applied. x=0, y=1, z=2, radius=-1

minDist (default: 1, min: 0, max: inf)

• minimum distance authorized. All points before that will be filtered.

OrientNormalsDataPointsFilter

Normals. Reorient normals so that they all point in the same direction, with respect to the origin of the point cloud.

towardCenter (default: 1)

• If set to true(1), all the normals will point inside the surface (i.e. toward the center of the point cloud).

RandomSamplingDataPointsFilter

Subsampling. This filter reduces the size of the point cloud by randomly dropping points. Based on [1]

prob (default: 0.75, min: 0, max: 1)

• probability to keep a point, one over decimation factor

SamplingSurfaceNormalDataPointsFilter

Subsampling, Normals. This filter decomposes the point-cloud space in boxes, by recursively splitting the cloud through axis-aligned hyperplanes such as to maximize the evenness of the aspect ratio of the box. When the number of points in a box reaches a value binSize or lower, the filter computes the center of mass of these points and its normal by taking the eigenvector corresponding to the smallest eigenvalue of all points in the box.

binSize (default: 7, min: 3, max: 2147483647)

• limit over which a box is splitted in two

averageExistingDescriptors (default: 1)

• whether the filter keep the existing point descriptors and average them or should it drop them

keepNormals (default: 1)

• whether the normals should be added as descriptors to the resulting cloud

keepDensities (default: 0)

• whether the point densities should be added as descriptors to the resulting cloud

keepEigenValues (default: 0)

• whether the eigen values should be added as descriptors to the resulting cloud

keepEigenVectors (default: 0)

• whether the eigen vectors should be added as descriptors to the resulting cloud

SurfaceNormalDataPointsFilter

Normals. This filter extracts the normal to each point by taking the eigenvector corresponding to the smallest eigenvalue of its nearest neighbors.

knn (default: 5, min: 3, max: 2147483647)

• number of nearest neighbors to consider, including the point itself

epsilon (default: 0, min: 0, max: inf)

• approximation to use for the nearest-neighbor search

keepNormals (default: 1)

• whether the normals should be added as descriptors to the resulting cloud

keepDensities (default: 0)

• whether the point densities should be added as descriptors to the resulting cloud

keepEigenValues (default: 0)

• whether the eigen values should be added as descriptors to the resulting cloud

keepEigenVectors (default: 0)

• whether the eigen vectors should be added as descriptors to the resulting cloud

keepMatchedIds (default: 0)

• whethen the identifiers of matches points should be added as descriptors to the resulting cloud

UniformizeDensityDataPointsFilter

Subsampling. Reduce the points number of a certain ration while trying to uniformize the density of the point cloud.

ratio (default: 0.5, min: 0.0000001, max: 0.9999999)

• targeted reduction ratio

nbBin (default: 10, min: 1, max: 2147483647)

• number of bin used to estimate the probability distribution of the density.

Matcher

KDTreeMatcher

This matcher matches a point from the reading to its closest neighbors in the reference.

knn (default: 1, min: 1, max: 2147483647)

• number of nearest neighbors to consider it the reference

epsilon (default: 0, min: 0, max: inf)

• approximation to use for the nearest-neighbor search

searchType (default: 1, min: 0, max: 2)

• Nabo search type. 0: brute force, check distance to every point in the data (very slow), 1: kd-tree with linear heap, good for small knn (~up to 30) and 2: kd-tree with tree heap, good for large knn (~from 30)

maxDist (default: inf, min: 0, max: inf)

• maximum distance to consider for neighbors

NullMatcher

Does nothing, returns no match.

• no parameters

FeatureOutlierFilter

MaxDistOutlierFilter

This filter considers as outlier links whose norms are above a fix threshold.

maxDist (default: 1, min: 0.0000001, max: inf)

• threshold distance

MedianDistOutlierFilter

This filter considers as outlier links whose norms are above the median link norms times a factor. Based on [2].

factor (default: 3, min: 0.0000001, max: inf)

• points farther away factor * median will be considered outliers.

MinDistOutlierFilter

This filter considers as outlier links whose norms are below a threshold.

minDist (default: 1, min: 0.0000001, max: inf)

• threshold distance

NullFeatureOutlierFilter

Does nothing.

• no parameters

TrimmedDistOutlierFilter

Hard rejection threshold using quantile. This filter considers as inlier a certain percentage of the links with the smallest norms. Based on [3].

ratio (default: 0.85, min: 0.0000001, max: 0.9999999)

• percentage to keep

VarTrimmedDistOutlierFilter

Hard rejection threshold using quantile and variable ratio. Based on [4].

minRatio (default: 0.05, min: 0.0000001, max: 1)

• min ratio

maxRatio (default: 0.99, min: 0.0000001, max: 1)

• max ratio

lambda (default: 0.95)

• lambda (part of the term that balance the rmsd: 1/ratio^lambda

DescriptorOutlierFilter

ErrorMinimizer

IdentityErrorMinimizer

Does nothing.

• no parameters

PointToPlaneErrorMinimizer

Point-to-plane error (or point-to-line in 2D). Based on [5]

• no parameters

PointToPointErrorMinimizer

Point-to-point error. Based on SVD decomposition. Based on [6].

• no parameters

TransformationChecker

BoundTransformationChecker

This checker stops the ICP loop with an exception when the transformation values exceed bounds.

maxRotationNorm (default: 1, min: 0, max: inf)

• rotation bound

maxTranslationNorm (default: 1, min: 0, max: inf)

• translation bound

CounterTransformationChecker

This checker stops the ICP loop after a certain number of iterations.

maxIterationCount (default: 40, min: 0, max: 2147483647)

• maximum number of iterations

DifferentialTransformationChecker

This checker stops the ICP loop when the relative motions (i.e. abs(currentIter - lastIter)) of rotation and translation components are below a fix thresholds. This allows to stop the iteration when the point cloud is stabilized. Smoothing can be applied to avoid oscillations. Inspired by [3].

minDiffRotErr (default: 0.001, min: 0., max: 6.2831854)

• threshold for rotation error (radian)

minDiffTransErr (default: 0.001, min: 0., max: inf)

• threshold for translation error

smoothLength (default: 3, min: 0, max: 2147483647)

• number of iterations over which to average the differencial error

Inspector

NullInspector

Does nothing.

• no parameters

VTKFileInspector

Dump the different steps into VTK files.

baseFileName (default: point-matcher-output)

• base file name for the VTK files

dumpPerfOnExit (default: 0)

• dump performance statistics to stderr on exit

Logger

FileLogger

Log using std::stream.

infoFileName (default: /dev/stdout)

• name of the file to output infos to

warningFileName (default: /dev/stderr)

• name of the file to output warnings to

displayLocation (default: 0)

• display the location of message in source code

NullLogger

Does not log anything.

• no parameters

Bibliography

• [1] - Registration and integration of multiple range images for 3-D model construction. Masuda, T. and Sakaue, K. and Yokoya, N. In Pattern Recognition, 1996., Proceedings of the 13th International Conference on. 879--883. 1996. DOI: 10.1109/ICPR.1996.546150. full text.

• [2] - Simultaneous Localization and Mapping with Active Stereo Vision. Diebel, J. and Reutersward, K. and Thrun, S. and Davis, J. and Gupta, R. In Intelligent Robots and Systems, 2004. (IROS 2004). Proceedings. 2004 IEEE/RSJ International Conference on. 3436--3443. 2004. DOI: 10.1109/IROS.2004.1389948. full text.

• [3] - The Trimmed Iterative Closest Point Algorithm. Chetverikov, D. and Svirko, D. and Stepanov, D. and Krsek, P. In Pattern Recognition, 2002. Proceedings. 16th International Conference on. 545--548. 2002. DOI: 10.1109/ICPR.2002.1047997. full text.

• [4] - Outlier robust ICP for minimizing fractional RMSD. Phillips, J.M. and Liu, R. and Tomasi, C. In 3-D Digital Imaging and Modeling, 2007. 3DIM '07. Sixth International Conference on. 427--434. 2007. DOI: 10.1109/3DIM.2007.39. full text.

• [5] - Object modeling by registration of multiple range images. Chen, Y. and Medioni, G. In Robotics and Automation, 1991. Proceedings., 1991 IEEE International Conference on. 2724--2729. 1991. DOI: 10.1109/ROBOT.1991.132043. full text.

• [6] - A Method for Registration of 3-D Shapes. Besl, P.J. and McKay, H.D. In Pattern Analysis and Machine Intelligence, IEEE Transactions. 239--256. 1992. DOI: 10.1109/34.121791. full text.

Limitations

This library solely performs point-cloud registration, it does not incorporate data fusion based on IMU or odometry. This can be done through other nodes such as robot_pose_ekf.

Report a Bug

Please report bugs and request features using the github page.