Go to the documentation of this file.00001
00002
00003
00004
00005
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
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 }