ROS PCL: Help with Moving Least Squares filter

Question

I'm trying to implement a simple ROS node to perform Moving Least Squares filtering on a sensor_msgs/PointCloud2 topic.

I'm following this PCL tutorial, which uses the pcl/surface/mls.h file.

My code is at this GitHub page, but replicated below;

#include <ros/ros.h>

// PCL specific includes
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/surface/mls.h>


/**
 * Simple class to allow appling a Moving Least Squares smoothing filter
 */
class MovingLeastSquares {
private:
    double _search_radius;

public:
    MovingLeastSquares(double search_radius = 0.03)
        : _search_radius(search_radius)
    {
        // Pass
    };

    ros::Subscriber sub;
    ros::Publisher pub;
    void cloudCallback (const sensor_msgs::PointCloud2ConstPtr& cloud_msg);
};


/**
 * Callback that performs the Point Cloud downsapling
 */
void MovingLeastSquares::cloudCallback (const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
    // Container for original & filtered data
    pcl::PCLPointCloud2 cloud;

    // Convert to PCL data type
    pcl_conversions::toPCL(*cloud_msg, cloud);

    // Convert to dumbcloud
    pcl::PointCloud<pcl::PointXYZ>::Ptr dumb_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
    //pcl::MsgFieldMap field_map;
    //pcl::createMapping<pcl::PointXYZ>(cloud_msg->fields, field_map);
    //pcl::fromPCLPointCloud2<pcl::PointXYZ>(cloud, *dumb_cloud);
    pcl::fromPCLPointCloud2<pcl::PointXYZ>(cloud, *dumb_cloud);

    // Create a KD-Tree
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);

    // Output has the PointNormal type in order to store the normals calculated by MLS
    pcl::PointCloud<pcl::PointNormal> mls_points;

    // Init object (second point type is for the normals, even if unused)
    pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal> mls;

    mls.setComputeNormals (true);

    // Set parameters
    mls.setInputCloud (dumb_cloud);
    mls.setPolynomialFit (true);
    mls.setSearchMethod (tree);
    mls.setSearchRadius (_search_radius);

    // Reconstruct
    mls.process (mls_points);

    // Convert from dumbcloud to cloud
    pcl::PCLPointCloud2 cloud_filtered;
    pcl::toPCLPointCloud2(mls_points, cloud_filtered);

    // Convert to ROS data type
    sensor_msgs::PointCloud2 output;
    pcl_conversions::moveFromPCL(cloud_filtered, output);

    // Publish the data
    pub.publish (output);
}


/**
 * Main
 */
int main (int argc, char** argv)
{
    // Initialize ROS
    ros::init (argc, argv, "pcl_mls");
    ros::NodeHandle nh("~");

    // Read optional leaf_size argument
    double search_radius = 0.03;
    if (nh.hasParam("search_radius"))
    {
        nh.getParam("search_radius", search_radius);
        ROS_INFO("Using %0.4f as search radius", search_radius);
    }

    // Create our filter
    MovingLeastSquares MovingLeastSquaresObj(search_radius);
    const boost::function< void(const sensor_msgs::PointCloud2ConstPtr &)> boundCloudCallback = boost::bind(&MovingLeastSquares::cloudCallback, &MovingLeastSquaresObj, _1);

    // Create a ROS subscriber for the input point cloud
    MovingLeastSquaresObj.sub = nh.subscribe<sensor_msgs::PointCloud2> ("/input", 10, boundCloudCallback);

    // Create a ROS publisher for the output point cloud
    MovingLeastSquaresObj.pub = nh.advertise<sensor_msgs::PointCloud2> ("/output", 10);

    // Spin
    ros::spin ();
}

This compiles fine for me, but I don't get any points coming through the output topic. Can anyone tell me what I'm doing wrong? I suspect it is something to do with the conversions at line 46 or line 74.

Thank you!

Answer 1

Update: I got it working. Thanks for the feedback everyone.

To address some of the comments:

• I wasn't aware that 'filtering' has a specific meaning in the context of PCL. My code wasn't trying to implement an actual filter.
• I was able to compile and run the PCL tutorial no problems
• Sure enough, my problem was in the conversion from ROS message types to PCL point cloud types. I was essentially missing a call to pcl_conversions::toPCL().

The working code can be seen here.