simple_object_capture: simple_object_capture: common.h Source File

00001 #include <pcl/point_types.h>
00002 #include <pcl/filters/extract_indices.h>
00003 #include <pcl/filters/voxel_grid.h>
00004 #include <pcl/io/pcd_io.h>
00005 #include <pcl/visualization/cloud_viewer.h>
00006 #include <pcl/kdtree/kdtree_flann.h>
00007 #include <pcl/kdtree/kdtree.h>
00008 #include <pcl/features/normal_3d.h>
00009 #include <geometry_msgs/Pose.h>
00010 #include <pcl/registration/registration.h>
00011 
00012 
00013 #ifndef COLLECT_FOOT_COMMON_H
00014 #define COLLECT_FOOT_COMMON_H
00015 
00016 typedef pcl::PointXYZRGB PointT;
00017 
00018 template <class P>
00019 void quickViz(boost::shared_ptr<const pcl::PointCloud<P> > cloud,
00020               const std::string& title = std::string("Cloud"))
00021 {
00022   pcl::visualization::PCLVisualizer viewer(title);
00023   viewer.addPointCloud<P>(cloud);
00024   viewer.spin();
00025 }
00026 
00027 
00028 template <class P, class N>
00029 void quickViz(boost::shared_ptr<const pcl::PointCloud<P> > cloud,
00030               boost::shared_ptr<const pcl::PointCloud<N> > normals,
00031               const std::string& title = std::string("Cloud"))
00032 {
00033   pcl::visualization::PCLVisualizer viewer(title);
00034   viewer.addPointCloud<P>(cloud, "cloud");
00035   viewer.addPointCloudNormals<P, N>(cloud, normals, 1, 0.01, "normals");
00036   viewer.spin();
00037 }
00038 
00039 // TODO: what if input & output point to the same object?!?
00040 template <class C>
00041 void extractIndices(const typename C::ConstPtr& input, pcl::PointIndices::ConstPtr indices,
00042                     C& output, bool positive=true)
00043 {
00044   pcl::ExtractIndices<typename C::PointType> extractor;
00045   extractor.setNegative(!positive);
00046   extractor.setInputCloud(input);
00047   extractor.setIndices(indices);
00048   extractor.filter(output);
00049 }
00050 template <class C>
00051 void extractIndices(const typename C::Ptr& input, const pcl::PointIndices& indices,
00052                     C& output, bool positive=true)
00053 {
00054   pcl::PointIndices::ConstPtr indices_ptr(new pcl::PointIndices(indices));
00055   extractIndices(input, indices_ptr, output, positive);
00056 }
00057 
00058 template <class P>
00059 typename pcl::PointCloud<P>::Ptr loadPCDFile(const std::string &filename)
00060 {
00061   typename pcl::PointCloud<P>::Ptr c(new pcl::PointCloud<P>);
00062   pcl::io::loadPCDFile<P>(filename, *c);
00063   return c;
00064 }
00065 
00066 
00067 template <class P>
00068 typename pcl::PointCloud<P>::Ptr downsample(typename pcl::PointCloud<P>::ConstPtr cloud,
00069                                             double voxel_grid_size)
00070 {
00071   typename pcl::PointCloud<P>::Ptr result(new pcl::PointCloud<P>);
00072   pcl::VoxelGrid<P> vox_grid;
00073   vox_grid.setInputCloud(cloud);
00074   vox_grid.setLeafSize(voxel_grid_size, voxel_grid_size, voxel_grid_size);
00075   vox_grid.filter(*result);
00076   return result;
00077 }
00078 
00079 
00080 template <class P, class N>
00081 typename pcl::PointCloud<N>::Ptr compute_surface_normals (typename pcl::PointCloud<P>::ConstPtr points, 
00082                                                           float normal_radius)
00083 {
00084   typename pcl::PointCloud<N>::Ptr result(new pcl::PointCloud<N>);
00085   typename pcl::NormalEstimation<P, N> norm_est;
00086 
00087   // Use a FLANN-based KdTree to perform neighborhood searches
00088   norm_est.setSearchMethod (typename pcl::search::Search<P>::Ptr(new pcl::search::KdTree<P>));
00089 
00090   // Specify the size of the local neighborhood to use when computing the surface normals
00091   norm_est.setRadiusSearch (normal_radius);
00092 
00093   // Set the input points
00094   norm_est.setInputCloud (points);
00095 
00096   // Estimate the surface normals and store the result in "normals_out"
00097   norm_est.compute(*result);
00098   return result;
00099 }
00100 
00101 
00102 
00103 template <class P>
00104 void transform(typename pcl::PointCloud<P>::Ptr cloud_in, typename pcl::PointCloud<P>::Ptr cloud_out, const geometry_msgs::Pose& p)
00105 {
00106   Eigen::Quaternionf q(p.orientation.w, p.orientation.x, p.orientation.y, p.orientation.z);
00107   Eigen::Translation3f t(p.position.x, p.position.y, p.position.z);
00108   Eigen::Affine3f trans = t * q;
00109   pcl::transformPointCloud(*cloud_in, *cloud_out, trans);
00110 }
00111 
00112 
00113 template <class P>
00114 void transform_inverse(typename pcl::PointCloud<P>::Ptr cloud_in, typename pcl::PointCloud<P>::Ptr cloud_out, const geometry_msgs::Pose& p)
00115 {
00116   Eigen::Quaternionf q(p.orientation.w, p.orientation.x, p.orientation.y, p.orientation.z);
00117   Eigen::Translation3f t(p.position.x, p.position.y, p.position.z);
00118   Eigen::Affine3f trans = t * q;
00119   pcl::transformPointCloud(*cloud_in, *cloud_out, trans.inverse());
00120 }
00121 
00122 
00123 
00124 
00125 #endif