Smoother.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 
00003 #include <iostream>
00004 #include <pcl_ros/point_cloud.h>
00005 #include <pcl/point_types.h>
00006 #include "pcl/io/pcd_io.h"
00007 
00008 #include <pcl/kdtree/kdtree_flann.h>
00009 #include <pcl/surface/mls.h>
00010 
00011 typedef pcl::PointXYZRGB PointType;
00012 typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
00013 
00014 ros::Publisher pub_cloud;
00015 
00016 PointCloud::Ptr temp(new PointCloud);
00017 
00018 //***********************
00019 //       Functions
00020 //***********************
00021 
00022 void callback(const PointCloud::ConstPtr& msg)
00023 {
00024     ROS_INFO("Beginning Moving Least Squares on %i Points", msg->points.size());
00025 
00026     // Make a copy of the point cloud
00027     pcl::copyPointCloud(*msg, *temp);
00028 
00029     // Create a KD-Tree
00030 #if defined PCL_MAJOR_VERSION && defined PCL_MINOR_VERSION && PCL_MAJOR_VERSION == 1 && PCL_MINOR_VERSION >= 5
00031     pcl::search::KdTree<PointType>::Ptr tree (new pcl::search::KdTree<PointType>);
00032 #else
00033     // support for PCL version 1.1.1 in electric
00034     pcl::KdTree<PointType>::Ptr tree (new pcl::KdTreeFLANN<PointType>);
00035 #endif
00036     tree->setInputCloud (temp);
00037 
00038     // Output has the same type as the input one, it will be only smoothed
00039     PointCloud mls_points;
00040 
00041     // Init object (second point type is for the normals, even if unused)
00042     pcl::MovingLeastSquares<PointType, pcl::Normal> mls;
00043 
00044     // Optionally, a pointer to a cloud can be provided, to be set by MLS
00045     pcl::PointCloud<pcl::Normal>::Ptr mls_normals (new pcl::PointCloud<pcl::Normal> ());
00046     mls.setOutputNormals (mls_normals);
00047 
00048     // Set parameters
00049     mls.setInputCloud (temp);
00050     mls.setPolynomialFit (true);
00051     mls.setSearchMethod (tree);
00052     mls.setSearchRadius (0.05);
00053 
00054     // Reconstruct
00055     mls.reconstruct(mls_points);
00056 
00057     ROS_INFO("Done MLS");
00058 
00059     pub_cloud.publish(mls_points);
00060 }
00061 
00062 
00063 int main(int argc, char** argv)
00064 {
00065     ros::init (argc, argv, "smooth_default");
00066     
00067     // Get Parameters
00068     ros::NodeHandle nh = ros::NodeHandle(ros::this_node::getName());
00069     std::string cloud_in, cloud_out;
00070     nh.param<std::string>("cloud_in", cloud_in, "smooth_cloud_in");
00071     nh.param<std::string>("cloud_out", cloud_out, "smooth_cloud_out");
00072 
00073     // Subscribe
00074     ros::NodeHandle nx;
00075 
00076     ros::Subscriber cloud_sub = nx.subscribe(cloud_in, 1, callback);
00077     pub_cloud = nx.advertise<PointCloud> (cloud_out, 1);
00078 
00079     ros::spin();
00080 
00081     return (0);
00082 }
00083 
00084 
00085 


clearpath_tools
Author(s): Sean Anderson
autogenerated on Sun Oct 5 2014 22:52:30