ClearpathPointCloudTools.cpp
Go to the documentation of this file.
00001 #include "ClearpathPointCloudTools.h"
00002 
00003 #include "pcl/filters/passthrough.h"
00004 
00005 #include <cstdlib>
00006 
00007 void ClearpathPointCloudTools::PassthroughPointCloud(PointCloud* cloud, PointCloud* result, double dist)
00008 {
00009     pcl::PassThrough<PointType> pass;
00010     PointCloud cloud_filtered;
00011 
00012     pass.setInputCloud (boost::make_shared<const PointCloud > (*cloud));
00013     pass.setFilterFieldName ("z");
00014     pass.setFilterLimits (0.0, dist);
00015     pass.filter (cloud_filtered);
00016 
00017     PointCloud final;
00018     final.height = 1;
00019     for (int i = 0; i < cloud_filtered.points.size(); i++)
00020     {
00021         if ( cloud_filtered.points[i].x == cloud_filtered.points[i].x &&
00022              cloud_filtered.points[i].y == cloud_filtered.points[i].y &&
00023              cloud_filtered.points[i].z == cloud_filtered.points[i].z )
00024         {    
00025             final.points.push_back(cloud_filtered.points[i]);
00026         }
00027     }
00028     final.width = final.points.size();
00029     final.header.frame_id = cloud->header.frame_id;
00030 
00031     (*result) = final;
00032 }
00033 
00034 void ClearpathPointCloudTools::PassthroughPointCloudImage(PointCloud* cloud, PointCloud* result, double dist)
00035 {
00036     PointCloud final;
00037     final = *cloud;
00038     for (int i = 0; i < final.points.size(); i++)
00039     {
00040         if (final.points[i].z == final.points[i].z) // check that it is not nan
00041         {
00042             if (final.points[i].z < 0.0 || final.points[i].z > dist)
00043             {
00044                 final.points[i].x = NAN;
00045                 final.points[i].y = NAN;
00046                 final.points[i].z = NAN;
00047             }
00048         }
00049     }
00050     *result = final;
00051 }
00052 
00053 void ClearpathPointCloudTools::SkimPointCloudImage(PointCloud* cloud, PointCloud* result, unsigned int num_of_times)
00054 {
00055     unsigned int skip = 2*num_of_times;
00056 
00057     PointCloud cloud_filtered;
00058     cloud_filtered = *cloud;
00059     cloud_filtered.width = cloud->width/(skip); 
00060     cloud_filtered.height = cloud->height/(skip); 
00061     cloud_filtered.points.resize(cloud_filtered.width*cloud_filtered.height); 
00062 
00063     int w = cloud_filtered.width;
00064     int h = cloud_filtered.height;
00065     for (int i = 0; i < h; i++)
00066     {
00067         for (int j = 0; j < w; j++)
00068         {
00069             cloud_filtered.points[i*w+j].x = cloud->points[skip*i*cloud->width+skip*j].x;
00070             cloud_filtered.points[i*w+j].y = cloud->points[skip*i*cloud->width+skip*j].y;
00071             cloud_filtered.points[i*w+j].z = cloud->points[skip*i*cloud->width+skip*j].z;
00072             cloud_filtered.points[i*w+j].rgb = cloud->points[skip*i*cloud->width+skip*j].rgb;
00073         }
00074     }
00075     (*result) = cloud_filtered;
00076 }
00077 


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