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)
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
