This tutorial explains how a Moving Least Squares (MLS) surface reconstruction method can be used to smooth and resample noisy data. Please see an example in the video below:
Some of the data irregularities (caused by small distance measurement errors) are very hard to remove using statistical analysis. To create complete models, glossy surfaces as well as occlusions in the data must be accounted for. In situations where additional scans are impossible to acquire, a solution is to use a resampling algorithm, which attempts to recreate the missing parts of the surface by higher order polynomial interpolations between the surrounding data points. By performing resampling, these small errors can be corrected and the “double walls” artifacts resulted from registering multiple scans together can be smoothed.
On the left side of the figure above, we see the effect or estimating surface normals in a dataset comprised of two registered point clouds together. Due to alignment errors, the resultant normals are noisy. On the right side we see the effects of surface normal estimation in the same dataset after it has been smoothed with a Moving Least Squares algorithm. Plotting the curvatures at each point as a measure of the eigenvalue relationship before and after resampling, we obtain:
To approximate the surface defined by a local neighborhood of points p1, p2 ... pk at a point q we use a bivariate polynomial height function defined on a on a robustly computed reference plane.
First, create a file, let’s say, resampling.cpp in your favorite editor, and place the following inside it:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 | #include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/surface/mls.h>
int
main (int argc, char** argv)
{
// Load input file into a PointCloud<T> with an appropriate type
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());
sensor_msgs::PointCloud2 cloud_blob;
// Load bun0.pcd -- should be available with the PCL archive in test
pcl::io::loadPCDFile ("bun0.pcd", cloud_blob);
pcl::fromROSMsg (cloud_blob, *cloud);
// Create a KD-Tree
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (cloud);
// Output has the same type as the input one, it will be only smoothed
pcl::PointCloud<pcl::PointXYZ> mls_points;
// Init object (second point type is for the normals, even if unused)
pcl::MovingLeastSquares<pcl::PointXYZ, pcl::Normal> mls;
// Optionally, a pointer to a cloud can be provided, to be set by MLS
pcl::PointCloud<pcl::Normal>::Ptr mls_normals (new pcl::PointCloud<pcl::Normal> ());
mls.setOutputNormals (mls_normals);
// Set parameters
mls.setInputCloud (cloud);
mls.setPolynomialFit (true);
mls.setSearchMethod (tree);
mls.setSearchRadius (0.03);
// Reconstruct
mls.reconstruct (mls_points);
// Concatenate fields for saving
pcl::PointCloud<pcl::PointNormal> mls_cloud;
pcl::concatenateFields (mls_points, *mls_normals, mls_cloud);
// Save output
pcl::io::savePCDFile ("bun0-mls.pcd", mls_cloud);
}
|
You should be able to find the input file at pcl/test/bun0.pcd.
Now, let’s break down the code piece by piece.
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (cloud);
as the example PCD has only XYZ coordinates, we load it into a PointCloud<PointXYZ>. These fields are mandatory for the method, other ones are allowed and will be preserved.
// Init object (second point type is for the normals, even if unused)
pcl::MovingLeastSquares<pcl::PointXYZ, pcl::Normal> mls;
if normal estimation is not required, this step can be skipped.
mls.setInputCloud (cloud);
mls.setPolynomialFit (true);
the first template type is used for the input and output cloud. Only the XYZ dimensions of the input are smoothed in the output.
mls.setSearchRadius (0.03);
// Reconstruct
mls.reconstruct (mls_points);
// Concatenate fields for saving
polynomial fitting could be disabled for speeding up smoothing. Please consult the code API (constructor and setter) for default values and additional parameters to control the smoothing process.
// Save output
pcl::io::savePCDFile ("bun0-mls.pcd", mls_cloud);
if the normals and the original dimensions need to be in the same cloud, the fields have to be concatenated.
Add the following lines to your CMakeLists.txt file:
1 2 3 4 5 6 7 8 9 10 11 12 | cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
project(resampling)
find_package(PCL 1.3 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
add_executable (resampling resampling.cpp)
target_link_libraries (resampling ${PCL_LIBRARIES})
|
After you have made the executable, you can run it. Simply do:
$ ./resampling
You can view the smoothed cloud for example by executing:
$ pcd_viewer bun0-mls.pcd