common.h

Go to the documentation of this file.

#include <pcl/point_types.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/features/normal_3d.h>
#include <geometry_msgs/Pose.h>
#include <pcl/registration/registration.h>


#ifndef COLLECT_FOOT_COMMON_H
#define COLLECT_FOOT_COMMON_H

typedef pcl::PointXYZRGB PointT;

template <class P>
void quickViz(boost::shared_ptr<const pcl::PointCloud<P> > cloud,
              const std::string& title = std::string("Cloud"))
{
  pcl::visualization::PCLVisualizer viewer(title);
  viewer.addPointCloud<P>(cloud);
  viewer.spin();
}


template <class P, class N>
void quickViz(boost::shared_ptr<const pcl::PointCloud<P> > cloud,
              boost::shared_ptr<const pcl::PointCloud<N> > normals,
              const std::string& title = std::string("Cloud"))
{
  pcl::visualization::PCLVisualizer viewer(title);
  viewer.addPointCloud<P>(cloud, "cloud");
  viewer.addPointCloudNormals<P, N>(cloud, normals, 1, 0.01, "normals");
  viewer.spin();
}

// TODO: what if input & output point to the same object?!?
template <class C>
void extractIndices(const typename C::ConstPtr& input, pcl::PointIndices::ConstPtr indices,
                    C& output, bool positive=true)
{
  pcl::ExtractIndices<typename C::PointType> extractor;
  extractor.setNegative(!positive);
  extractor.setInputCloud(input);
  extractor.setIndices(indices);
  extractor.filter(output);
}
template <class C>
void extractIndices(const typename C::Ptr& input, const pcl::PointIndices& indices,
                    C& output, bool positive=true)
{
  pcl::PointIndices::ConstPtr indices_ptr(new pcl::PointIndices(indices));
  extractIndices(input, indices_ptr, output, positive);
}

template <class P>
typename pcl::PointCloud<P>::Ptr loadPCDFile(const std::string &filename)
{
  typename pcl::PointCloud<P>::Ptr c(new pcl::PointCloud<P>);
  pcl::io::loadPCDFile<P>(filename, *c);
  return c;
}


template <class P>
typename pcl::PointCloud<P>::Ptr downsample(typename pcl::PointCloud<P>::ConstPtr cloud,
                                            double voxel_grid_size)
{
  typename pcl::PointCloud<P>::Ptr result(new pcl::PointCloud<P>);
  pcl::VoxelGrid<P> vox_grid;
  vox_grid.setInputCloud(cloud);
  vox_grid.setLeafSize(voxel_grid_size, voxel_grid_size, voxel_grid_size);
  vox_grid.filter(*result);
  return result;
}


template <class P, class N>
typename pcl::PointCloud<N>::Ptr compute_surface_normals (typename pcl::PointCloud<P>::ConstPtr points, 
                                                          float normal_radius)
{
  typename pcl::PointCloud<N>::Ptr result(new pcl::PointCloud<N>);
  typename pcl::NormalEstimation<P, N> norm_est;

  // Use a FLANN-based KdTree to perform neighborhood searches
  norm_est.setSearchMethod (typename pcl::search::Search<P>::Ptr(new pcl::search::KdTree<P>));

  // Specify the size of the local neighborhood to use when computing the surface normals
  norm_est.setRadiusSearch (normal_radius);

  // Set the input points
  norm_est.setInputCloud (points);

  // Estimate the surface normals and store the result in "normals_out"
  norm_est.compute(*result);
  return result;
}



template <class P>
void transform(typename pcl::PointCloud<P>::Ptr cloud_in, typename pcl::PointCloud<P>::Ptr cloud_out, const geometry_msgs::Pose& p)
{
  Eigen::Quaternionf q(p.orientation.w, p.orientation.x, p.orientation.y, p.orientation.z);
  Eigen::Translation3f t(p.position.x, p.position.y, p.position.z);
  Eigen::Affine3f trans = t * q;
  pcl::transformPointCloud(*cloud_in, *cloud_out, trans);
}


template <class P>
void transform_inverse(typename pcl::PointCloud<P>::Ptr cloud_in, typename pcl::PointCloud<P>::Ptr cloud_out, const geometry_msgs::Pose& p)
{
  Eigen::Quaternionf q(p.orientation.w, p.orientation.x, p.orientation.y, p.orientation.z);
  Eigen::Translation3f t(p.position.x, p.position.y, p.position.z);
  Eigen::Affine3f trans = t * q;
  pcl::transformPointCloud(*cloud_in, *cloud_out, trans.inverse());
}




#endif