Point Cloud Library (PCL)  1.8.1-dev
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
data_source.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage, Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  * Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
35  */
36 
37 
38 #ifndef PCL_GPU_FEATURES_TEST_DATA_SOURCE_HPP_
39 #define PCL_GPU_FEATURES_TEST_DATA_SOURCE_HPP_
40 
41 #include<string>
42 
43 #include <pcl/point_types.h>
44 #include <pcl/point_cloud.h>
45 #include <pcl/io/pcd_io.h>
46 #include <pcl/common/common.h>
47 #include <pcl/features/normal_3d.h>
48 #include <pcl/visualization/cloud_viewer.h>
49 #include <pcl/gpu/containers/kernel_containers.h>
50 #include <pcl/search/search.h>
51 
52 #include <Eigen/StdVector>
53 
54 #if defined (_WIN32) || defined(_WIN64)
55  EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(pcl::PointXYZ)
56  EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(pcl::Normal)
57 #endif
58 
59 #include <algorithm>
60 
61 namespace pcl
62 {
63  namespace gpu
64  {
65  struct DataSource
66  {
67  const static int k = 32;
68  const static int max_elements = 500;
69 
73 
76  float radius;
77 
78  std::vector< std::vector<int> > neighbors_all;
79  std::vector<int> sizes;
81 
82  DataSource(const std::string& file = "d:/office_chair_model.pcd")
83  : cloud(new PointCloud<PointXYZ>()), surface(new PointCloud<PointXYZ>()), indices( new std::vector<int>() ),
85  {
86  PCDReader pcd;
87  pcd.read(file, *cloud);
88 
89  PointXYZ minp, maxp;
90  pcl::getMinMax3D(*cloud, minp, maxp);
91  float sz = (maxp.x - minp.x + maxp.y - minp.y + maxp.z - minp.z) / 3;
92  radius = sz / 15;
93  }
94 
96  {
97  size_t cloud_size = cloud->points.size();
98  for(size_t i = 0; i < cloud_size; ++i)
99  {
100  PointXYZ& p = cloud->points[i];
101 
102  int r = std::max(1, std::min(255, static_cast<int>((double(rand())/RAND_MAX)*255)));
103  int g = std::max(1, std::min(255, static_cast<int>((double(rand())/RAND_MAX)*255)));
104  int b = std::max(1, std::min(255, static_cast<int>((double(rand())/RAND_MAX)*255)));
105 
106  *reinterpret_cast<int*>(&p.data[3]) = (b << 16) + (g << 8) + r;
107  }
108  }
109 
111  {
113  ne.setInputCloud (cloud);
115  ne.setKSearch (k);
116  //ne.setRadiusSearch (radius);
117 
118  ne.compute (*normals);
119  }
120 
121  void runCloudViewer() const
122  {
123  pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
124  viewer.showCloud (cloud);
125  while (!viewer.wasStopped ()) {}
126  }
127 
129  {
131  kdtree->setInputCloud(cloud);
132 
133  size_t cloud_size = cloud->points.size();
134 
135  std::vector<float> dists;
136  neighbors_all.resize(cloud_size);
137  for(size_t i = 0; i < cloud_size; ++i)
138  {
139  kdtree->nearestKSearch(cloud->points[i], k, neighbors_all[i], dists);
140  sizes.push_back((int)neighbors_all[i].size());
141  }
142  max_nn_size = *max_element(sizes.begin(), sizes.end());
143  }
144 
145  void findRadiusNeghbors(float radius = -1)
146  {
147  radius = radius == -1 ? this->radius : radius;
148 
150  kdtree->setInputCloud(cloud);
151 
152  size_t cloud_size = cloud->points.size();
153 
154  std::vector<float> dists;
155  neighbors_all.resize(cloud_size);
156  for(size_t i = 0; i < cloud_size; ++i)
157  {
158  kdtree->radiusSearch(cloud->points[i], radius, neighbors_all[i], dists);
159  sizes.push_back((int)neighbors_all[i].size());
160  }
161  max_nn_size = *max_element(sizes.begin(), sizes.end());
162  }
163 
164  void getNeghborsArray(std::vector<int>& data)
165  {
166  data.resize(max_nn_size * neighbors_all.size());
167  pcl::gpu::PtrStep<int> ps(&data[0], max_nn_size * sizeof(int));
168  for(size_t i = 0; i < neighbors_all.size(); ++i)
169  copy(neighbors_all[i].begin(), neighbors_all[i].end(), ps.ptr(i));
170  }
171 
173  {
174  surface->points.clear();
175  for(size_t i = 0; i < cloud->points.size(); i+= 10)
176  surface->points.push_back(cloud->points[i]);
177  surface->width = surface->points.size();
178  surface->height = 1;
179 
180  if (!normals->points.empty())
181  {
182  normals_surface->points.clear();
183  for(size_t i = 0; i < normals->points.size(); i+= 10)
184  normals_surface->points.push_back(normals->points[i]);
185 
186  normals_surface->width = surface->points.size();
187  normals_surface->height = 1;
188  }
189  }
190 
191  void generateIndices(size_t step = 100)
192  {
193  indices->clear();
194  for(size_t i = 0; i < cloud->points.size(); i += step)
195  indices->push_back(i);
196  }
197 
199  {
200  PointXYZ operator()(const Normal& n) const
201  {
202  PointXYZ xyz;
203  xyz.x = n.normal[0];
204  xyz.y = n.normal[1];
205  xyz.z = n.normal[2];
206  return xyz;
207  }
208  };
209  };
210  }
211 }
212 
213 #endif /* PCL_GPU_FEATURES_TEST_DATA_SOURCE_HPP_ */
int radiusSearch(const PointT &point, double radius, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const
Search for all the nearest neighbors of the query point in a given radius.
A point structure representing normal coordinates and the surface curvature estimate.
KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
Definition: kdtree_flann.h:69
void generateIndices(size_t step=100)
bool wasStopped(int millis_to_wait=1)
Check if the gui was quit, you should quit also.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:62
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr())
Provide a pointer to the input dataset.
static const int k
Definition: data_source.hpp:67
boost::shared_ptr< std::vector< int > > IndicesPtr
Definition: pcl_base.h:60
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
int nearestKSearch(const PointT &point, int k, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances) const
Search for k-nearest neighbors for the given query point.
void getNeghborsArray(std::vector< int > &data)
PointCloud< Normal >::Ptr normals
Definition: data_source.hpp:74
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
DataSource(const std::string &file="d:/office_chair_model.pcd")
Definition: data_source.hpp:82
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point...
Definition: normal_3d.h:242
PointXYZ operator()(const Normal &n) const
std::vector< std::vector< int > > neighbors_all
Definition: data_source.hpp:78
boost::shared_ptr< KdTreeFLANN< PointT, Dist > > Ptr
Definition: kdtree_flann.h:89
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
void findRadiusNeghbors(float radius=-1)
static const int max_elements
Definition: data_source.hpp:68
boost::shared_ptr< KdTree< PointT, Tree > > Ptr
Definition: kdtree.h:79
A point structure representing Euclidean xyz coordinates.
void showCloud(const ColorCloud::ConstPtr &cloud, const std::string &cloudname="cloud")
Show a cloud, with an optional key for multiple clouds.
PointCloud< PointXYZ >::Ptr cloud
Definition: data_source.hpp:70
void getMinMax3D(const pcl::PointCloud< PointT > &cloud, PointT &min_pt, PointT &max_pt)
Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud...
Definition: common.hpp:228
PointCloud< Normal >::Ptr normals_surface
Definition: data_source.hpp:75
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: normal_3d.h:333
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Simple point cloud visualization class.
Definition: cloud_viewer.h:52
void setSearchMethod(const KdTreePtr &tree)
Provide a pointer to the search object.
Definition: feature.h:166
void runCloudViewer() const
Point Cloud Data (PCD) file format reader.
Definition: pcd_io.h:52
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Definition: feature.hpp:189
void setKSearch(int k)
Set the number of k nearest neighbors to use for the feature estimation.
Definition: feature.h:186
__PCL_GPU_HOST_DEVICE__ T * ptr(int y=0)
PointCloud< PointXYZ >::Ptr surface
Definition: data_source.hpp:71
std::vector< int > sizes
Definition: data_source.hpp:79
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
int read(const std::string &file_name, pcl::PCLPointCloud2 &cloud, Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &pcd_version, const int offset=0)
Read a point cloud data from a PCD file and store it into a pcl/PCLPointCloud2.