3
$\begingroup$

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!

$\endgroup$
  • $\begingroup$ Welcome to Robotics, aaronsnowell. What troubleshooting steps have you done so far? You said you were following a tutorial, but the tutorial (1) doesn't use a filter class, (2) doesn't use ROS, and (3) gets the data from a file. If I were you, I would try first to replicate the tutorial as-is, then try it using the filter class, then try with ROS - build up the layers. Everything in the code you posted looks okay to me (with little/weak skills in C++ and ROS), but I can't see what generates or reads the data. Are you passing in data one point at time? All the points? $\endgroup$ – Chuck Jan 25 '18 at 16:28
  • $\begingroup$ Please edit your question to include any troubleshooting steps you've done so far, how you're generating data that's getting passed to the filter, how that's getting pushed to the filter, and how you're collecting the output. $\endgroup$ – Chuck Jan 25 '18 at 16:30
  • $\begingroup$ Thanks for the suggestions. I'll try some more troubleshooting and update my answer :) $\endgroup$ – aaronsnoswell Jan 26 '18 at 23:54
2
$\begingroup$

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.

| improve this answer | |
$\endgroup$
  • $\begingroup$ Glad you were able to find a solution! If you could, please take a moment to accept your own answer by clicking the check mark to the left. This marks the question as solved in the system, which will help future visitors find answers to their similar problems in less time. $\endgroup$ – Chuck Jan 29 '18 at 14:43

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.