Only released in EOL distros:   fuerte groovy

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.

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

Transformation checkers can be chained.

Inspector

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

Installation

Currently Diamondback and Electric are supported.

Dependencies

Ubuntu

Install libpointmatcher-dev and its dependencies from my PPA.

Others

First, you have to compile libnabo and libpointmatcher. Make sure you link them against Eigen3. Note that you need to compile both libraries with the CMAKE_BUILD_TYPE sets to RelWithDebInfo to optimize speed performance.

modular_cloud_matcher

Get the ethzasl_mapping stack:

git clone git://github.com/ethz-asl/ros-mapping.git ethzasl_mapping

Make sure that it is included in your ROS_PACKAGE_PATH:

export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:`pwd`/ethzasl_mapping

Then, to compile this package, execute:

rosmake modular_cloud_matcher

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.

Arguments

By default, this node sends tf, but it can send StampedTransform representing the transformation matrix between the most recent one and the previous. To do so, run:

rosrun modular_cloud_matcher cloud_matcher_node --senddeltapose

In that mode, the dela pose will be sent on the topic /openni_delta_pose, excepted if the parameter ~deltaPoseTopic states otherwise.

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.

Subscribed Topics

/camera/rgb/points (sensor_msgs/PointCloud2)

• You can change the name of this topic using the ~cloudTopic parameter. See the Parameters (Generic) section for more details.

Parameters

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

• When the argument -topic is used, the node will listen to the topic whose name is defined by this parameter and will expect a sensor_msgs/PointCloud2.

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

~deltaPoseTopic (string, default: "/openni_delta_pose", only in diamondback)

• Topic on which to send delta pose, if the node was started with the --senddeltapose argument.

Node: cloud_matcher_service

Arguments

If started without any argument, the node will provide a service waiting for two sensor_msgs/PointCloud2 messages (the first is the reference and the second the reading) and will output a geometry_msgs/Transform message once the registration is completed. You can execute the node in this mode with the following command:

rosrun modular_cloud_matcher cloud_matcher_node

You can also use a mode in which the point clouds are streamed through a topic and the node will perform the registration continuously using the most recent point cloud as reading and the previous one as reference. The type of topic messages must be sensor_msgs/PointCloud2. You can execute the node in this mode with the following command:

rosrun modular_cloud_matcher cloud_matcher_node -topic

This mode effectively implements a real-time tracker in 3D pose. }}}}

rosrun modular_cloud_matcher cloud_matcher_node

rosrun modular_cloud_matcher cloud_matcher_node --senddeltapose

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

As this node is a modular ICP chain, there are many parameters. These allow to configure the chain. Some combinations 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.

Generic

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

• When the argument -topic is used, the node will listen to the topic whose name is defined by this parameter and will expect a sensor_msgs/PointCloud2.

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

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

~deltaPoseTopic (string, default: "/openni_delta_pose", only in diamondback)

• Topic on which to send delta pose, if the node was started with the --senddeltapose argument.

Related to data filtering

~readingDataPointsFilterCount (unsigned, default: 1)

• and

~referenceDataPointsFilterCount (unsigned, default: 1)

• Number of data-point filters that the node will apply to the reading and reference point clouds. This will create a vector of filters, and the node will apply each filter sequentially. To configure a specific filter, you have to access its parameters under the paths ~readingDataPointsFilter/<id> and ~referenceDataPointsFilter/<id>, where <id> is the number of the filter in the vector, with <id> lies in the intervals [0;~readingDataPointsFilterCount[ and [0;~referenceDataPointsFilterCount[.

~readingDataPointsFilter/<id>/name (string, default: "FixstepSamplingDataPointsFilter")

• and

~referenceDataPointsFilter/<id>/name (string, default: "SamplingSurfaceNormalDataPointsFilter")

• The name of the filter, one of:

  • SurfaceNormalDataPointsFilter

  • SamplingSurfaceNormalDataPointsFilter

  • RandomSamplingDataPointsFilter

  • FixstepSamplingDataPointsFilter

  Each of these filters can have additional parameters, defined in ~readingDataPointsFilter/<id>/<p> and ~referenceDataPointsFilter/<id>/<p> where <p> is the name of the parameter, as described in the following sections.

SurfaceNormalDataPointsFilter

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

This filter is always applied before the ICP loop.

knn, (unsigned, default: 10)

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

epsilon (double (positive), default: 0.)

• approximation to use for the nearest-neighbor search

keepNormals (bool, default: true)

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

keepDensities (bool, default: false)

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

keepEigenValues (bool, default: false)

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

keepEigenVectors (bool, default: false)

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

SamplingSurfaceNormalDataPointsFilter

This filter decomposes the point-cloud space in boxes, by recursively splitting the cloud in two on 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 k 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.

This filter is always applied before the ICP loop.

k (unsigned, default: 20)

• limit over which a box is splitted in two

averageExistingDescriptors (bool, default: true)

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

keepNormals (bool, default: true)

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

keepDensities (bool, default: false)

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

keepEigenValues (bool, default: false)

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

keepEigenVectors (bool, default: false)

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

RandomSamplingDataPointsFilter

This filter reduces the size of the point cloud by randomly dropping points.

prob (double (in range [0;1]), default: 0.5)

• probability to keep a point, one over decimation factor

enablePreFilter (bool, default: true)

• whether this filter be applied before the ICP loop

enableStepFilter (bool, default: false)

• whether this filter be applied inside the ICP loop

FixstepSamplingDataPointsFilter

This filter reduces the size of the point cloud by only keeping one point over step ones.

step (unsigned, default: 20)

• number of point to skip, decimation factor

enablePreFilter (bool, default: true)

• whether this filter be applied before the ICP loop

enableStepFilter (bool, default: false)

• whether this filter be applied inside the ICP loop

Related to matching

~matcher/name (string, default: "KDTreeMatcher")

• The name of the matcher, one of:

  • KDTreeMatcher

  Each of these matchers can have additional parameters, defined in ~matcher/<p> where <p> is the name of the parameter, as described in the following sections.

KDTreeMatcher

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

knn (unsigned, default: 1)

• number of nearest neighbors to consider it the reference

epsilon (double (positive), default: 0.)

• approximation to use for the nearest-neighbor search

Related to outlier filtering

~featureOutlierFilterCount (unsigned, default: 1)

• Number of feature outlier filters that the node will apply. This will create a vector of filters, and the node will apply each filter sequentially. To configure a specific filter, you have to access its parameters under the path ~featureOutlierFilter/<id>, where <id> is the number of the filter in the vector, with <id> lies in the interval [0;~featureOutlierFilterCount[.

~featureOutlierFilter/<id>/name (string, default: "MedianDistOutlierFilter")

• The name of the filter, one of:

  • MaxDistOutlierFilter

  • MedianDistOutlierFilter

  • TrimmedDistOutlierFilter

  Each of these filters can have additional parameters, defined in ~featureOutlierFilter/<id>/<p> where <p> is the name of the parameter, as described in the following sections.

MaxDistOutlierFilter

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

maxDist (double, default: 1.0)

• threshold distance

MedianDistOutlierFilter

This filter considers as outlier links whose norms are above the median link norms times a factor.

factor (double, default: 3.0)

• factor

TrimmedDistOutlierFilter

This filter considers as inlier links with the smallest norms.

factor (double, default: 0.9)

• percentage to keep

Related to error minimization

~errorMinimizer/name (string, default: "PointToPlaneErrorMinimizer")

• The name of the error minimizer, one of:

  • PointToPointErrorMinimizer

  • PointToPlaneErrorMinimizer

PointToPointErrorMinimizer

Minimize the point-to-point error.

PointToPlaneErrorMinimizer

Minimize the point-to-plane error. This requires normals in the reference, if they are not available, the node will abort.

Related to transformation checking

~transformationCheckerCount (unsigned, default: 1)

• Number of transformation checkers that the node will apply. This will create a vector of checkers, and the node will apply each checker sequentially. To configure a specific checker, you have to access its parameters under the path ~transformationChecker/<id>, where <id> is the number of the checker in the vector, with <id> lies in the interval [0;~transformationCheckerCount[.

~transformationChecker/<id>/name (string, default: "ErrorTransformationChecker")

• The name of the checker, one of:

  • CounterTransformationChecker

  • ErrorTransformationChecker

  • BoundTransformationChecker

  Each of these checkers can have additional parameters, defined in ~transformationChecker/<id>/<p> where <p> is the name of the parameter, as described in the following sections.

CounterTransformationChecker

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

maxIterationCount (unsigned, default: 60)

• maximum number of iterations

ErrorTransformationChecker

This checker stops the ICP loop when the average transformation errors are below thresholds.

minRotErrorDelta (double (positive), default: 0.01)

• threshold for rotation error (radian)

minTransErrorDelta (double (positive), default: 0.01)

• threshold for translation error

tail (unsigned, default: 3)

• number of iterations over which to average error

BoundTransformationChecker

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

maxRotationNorm (double (positive), default: 1.)

• rotation bound

maxTranslationNorm (double (positive), default: 1.)

• translation bound

Related to inspection

~inspector/name (string, default: "Inspector")

• The name of the inspector, one of:

  • Inspector

  • VTKFileInspector

  Each of these inspectors can have additional parameters, defined in ~inspector/<p> where <p> is the name of the parameter, as described in the following sections.

Inspector

Does nothing.

VTKFileInspector

Dump the different steps into VTK files.

baseFileName (string, default: "msa-output")

• base file name for the VTK files

Node: cloud_matcher_service

This package provides a single node: cloud_matcher_service. This node is only available for diamondback, on cturtle, the service is an option to the cloud_matcher_node.

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

Known Issues

It is possible that you see the following error reported by Eigen:

cloud_matcher_node: /opt/ros/cturtle/stacks/geometry/eigen/include/Eigen/src/Core/MatrixStorage.h:44: Eigen::ei_matrix_array<T, Size, MatrixOptions, Align>::ei_matrix_array() [with T = double, int Size = 4, int MatrixOptions = 2, bool Align = true]: Assertion `(reinterpret_cast<size_t>(array) & 0xf) == 0 && "this assertion is explained here: http://eigen.tuxfamily.org/dox/UnalignedArrayAssert.html **** READ THIS WEB PAGE !!! ****"' failed.

If so, you can restart the node and it should eventually work.

We did not find a way to reproduce the problem every time and it appeared after we resolved last-minute issues with 32 and 64-bits architectures. Given the short time for the release of the library, we delay the resolution of that bug. This resolution will be our main priority once we finish the basic documentation.

Meanwhile, if you find a way to reproduce the problem every time, please send us an email so we can fix the bug more quickly.

Report a Bug

Please report bugs and request features using the github page.