pcl_utils.cpp
Go to the documentation of this file.
00001 /*
00002  * pcl_utils.cpp
00003  *
00004  *  Created on: Sep 26, 2012
00005  *      Author: kidson
00006  */
00007 
00008 #include "rgbd_registration/pcl_utils.h"
00009 #include "rgbd_registration/parameter_server.h"
00010 
00011 #include <pcl/io/pcd_io.h>
00012 #include <pcl/point_types.h>
00013 #include <pcl/features/normal_3d_omp.h>
00014 #include <pcl/features/normal_3d.h>
00015 #include <pcl/filters/filter.h>
00016 #include <pcl/common/transforms.h>
00017 
00018 #include <ros/console.h>
00019 
00020 void writePCDToFile (const std::string& fileName, const PointCloudConstPtr cloud_ptr)
00021 {
00022   pcl::PCDWriter writer;
00023   ROS_INFO_STREAM("[pcl_utils] Writing point cloud " << fileName << " to file");
00024   writer.write (fileName, *cloud_ptr, false);
00025 }
00026 
00027 void writePCDToFile (const std::string& fileName, const PointCloudConstPtr cloud_ptr,
00028     const std::vector<int>& indices)
00029 {
00030   pcl::PCDWriter writer;
00031   ROS_INFO_STREAM("[pcl_utils] Writing point cloud " << fileName << " to file");
00032   writer.write (fileName, *cloud_ptr, indices, false);
00033 }
00034 
00035 void transformAndWriteToFile (const PointCloudConstPtr cloud_in, const Eigen::Matrix4f& trafo)
00036 {
00037   PointCloudPtr cloud_out (new PointCloud);
00038   pcl::transformPointCloud (*cloud_in, *cloud_out, trafo);
00039   writePCDToFile ("transformed_cloud.pcd", cloud_out);
00040 }
00041 
00042 void transformAndWriteToFile (const PointCloudConstPtr cloud_in, const std::vector<int>& indices,
00043     const Eigen::Matrix4f& trafo)
00044 {
00045   PointCloudPtr cloud_out (new PointCloud);
00046   pcl::transformPointCloud (*cloud_in, *cloud_out, trafo);
00047   writePCDToFile ("transformed_cloud_features.pcd", cloud_out, indices);
00048 }
00049 
00050 // ----------- TODO: Template ----------------------
00051 void writePCDToFile (const std::string& fileName, const PointCloudNormalsConstPtr cloud_ptr)
00052 {
00053   pcl::PCDWriter writer;
00054   ROS_INFO_STREAM("[pcl_utils] Writing point cloud " << fileName << " to file");
00055   writer.write (fileName, *cloud_ptr, false);
00056 }
00057 
00058 void writePCDToFile (const std::string& fileName, const PointCloudNormalsConstPtr cloud_ptr,
00059     const std::vector<int>& indices)
00060 {
00061   pcl::PCDWriter writer;
00062   ROS_INFO_STREAM("[pcl_utils] Writing point cloud " << fileName << " to file");
00063   writer.write (fileName, *cloud_ptr, indices, false);
00064 }
00065 
00066 void transformAndWriteToFile (const PointCloudNormalsConstPtr cloud_in, const Eigen::Matrix4f& trafo)
00067 {
00068   PointCloudNormalsPtr cloud_out (new PointCloudNormals);
00069   pcl::transformPointCloud (*cloud_in, *cloud_out, trafo);
00070   writePCDToFile ("transformed_cloud.pcd", cloud_out);
00071 }
00072 
00073 void transformAndWriteToFile (const PointCloudNormalsConstPtr cloud_in, const std::vector<int>& indices,
00074     const Eigen::Matrix4f& trafo)
00075 {
00076   PointCloudNormalsPtr cloud_out (new PointCloudNormals);
00077   pcl::transformPointCloud (*cloud_in, *cloud_out, trafo);
00078   writePCDToFile ("transformed_cloud_features.pcd", cloud_out, indices);
00079 }
00080 
00081 void calculatePointCloudNormals (const PointCloudConstPtr input_cloud_ptr,
00082     PointCloudNormalsPtr output_cloud_ptr)
00083 {
00084   PointCloudPtr filtered (new PointCloud);
00085   pcl::NormalEstimation<PointType, PointNormal>::Ptr normal_estimator_ptr;
00086   if (ParameterServer::instance ()->get<bool> ("use_openmp_normal_calculation"))
00087     normal_estimator_ptr.reset (new pcl::NormalEstimationOMP<PointType, PointNormal>);
00088   else
00089     normal_estimator_ptr.reset (new pcl::NormalEstimation<PointType, PointNormal>);
00090   normal_estimator_ptr->setInputCloud (input_cloud_ptr);
00091   pcl::search::KdTree<PointType>::Ptr tree (new pcl::search::KdTree<PointType> ());
00092   normal_estimator_ptr->setSearchMethod (tree);
00093   normal_estimator_ptr->setRadiusSearch (0.1);
00094   normal_estimator_ptr->compute (*output_cloud_ptr);
00095   pcl::copyPointCloud (*input_cloud_ptr, *output_cloud_ptr);
00096   removePointNormalsWithNaNs (output_cloud_ptr);
00097 }
00098 
00099 void removePointNormalsWithNaNs (const PointCloudNormalsPtr input_cloud_ptr)
00100 {
00101   for (std::vector<PointNormal, Eigen::aligned_allocator<PointNormal> >::iterator itr =
00102       input_cloud_ptr->points.begin (); itr != input_cloud_ptr->points.end (); itr++)
00103     if ( (itr->x != itr->x) || (itr->y != itr->y) || (itr->z != itr->z)
00104         || (itr->normal_x != itr->normal_x) || (itr->normal_y != itr->normal_y)
00105         || (itr->normal_z != itr->normal_z))
00106     {
00107       ROS_DEBUG_STREAM("point has a NaN! idx" << itr - input_cloud_ptr->points.begin());
00108       ROS_DEBUG_STREAM(
00109           "x[" << itr->x << "] y[" << itr->y << "] z[" << itr->z<< "] xn[" << itr->normal_x << "] yn[" << itr->normal_y<< "] zn[" << itr->normal_z<< "]");
00110       input_cloud_ptr->points.erase (itr);
00111       itr--;
00112     }
00113   input_cloud_ptr->resize (input_cloud_ptr->points.size ());
00114   input_cloud_ptr->width = input_cloud_ptr->points.size ();
00115   input_cloud_ptr->height = 1;
00116 }
00117 
00118 void checkforNaNs (const PointCloudNormalsConstPtr input_cloud_ptr)
00119 {
00120   for (uint i = 0; i < input_cloud_ptr->points.size (); i++)
00121     if ( (input_cloud_ptr->points[i].x != input_cloud_ptr->points[i].x)
00122         || (input_cloud_ptr->points[i].y != input_cloud_ptr->points[i].y)
00123         || (input_cloud_ptr->points[i].z != input_cloud_ptr->points[i].z)
00124         || (input_cloud_ptr->points[i].normal_x != input_cloud_ptr->points[i].normal_x)
00125         || (input_cloud_ptr->points[i].normal_y != input_cloud_ptr->points[i].normal_y)
00126         || (input_cloud_ptr->points[i].normal_z != input_cloud_ptr->points[i].normal_z))
00127     {
00128       ROS_ERROR_STREAM("point has a NaN! idx" << i);
00129       ROS_ERROR_STREAM(
00130           "x[" << input_cloud_ptr->points[i].x << "] y[" << input_cloud_ptr->points[i].y<< "] z[" << input_cloud_ptr->points[i].z<< "] xn[" << input_cloud_ptr->points[i].normal_x<< "] yn[" << input_cloud_ptr->points[i].normal_y<< "] zn[" << input_cloud_ptr->points[i].normal_z<< "]");
00131     }
00132 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


rgbd_registration
Author(s): Ross Kidson
autogenerated on Sun Oct 6 2013 12:00:40