data_source.hpp
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
*/
#ifndef PCL_GPU_FEATURES_TEST_DATA_SOURCE_HPP_
#define PCL_GPU_FEATURES_TEST_DATA_SOURCE_HPP_
#include<string>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/common.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/gpu/containers/kernel_containers.h>
#include <pcl/search/search.h>
#include <Eigen/StdVector>
#if defined (_WIN32) || defined(_WIN64)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(pcl::PointXYZ)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(pcl::Normal)
#endif
#include <algorithm>
namespace pcl
{
namespace gpu
{
struct DataSource
{
const static int k = 32;
const static int max_elements = 500;
PointCloud<PointXYZ>::Ptr cloud;
PointCloud<PointXYZ>::Ptr surface;
IndicesPtr indices;
PointCloud<Normal>::Ptr normals;
PointCloud<Normal>::Ptr normals_surface;
float radius;
std::vector< std::vector<int> > neighbors_all;
std::vector<int> sizes;
int max_nn_size;
DataSource(const std::string& file = "d:/office_chair_model.pcd")
: cloud(new PointCloud<PointXYZ>()), surface(new PointCloud<PointXYZ>()), indices( new std::vector<int>() ),
normals(new PointCloud<Normal>()), normals_surface(new PointCloud<Normal>())
{
PCDReader pcd;
pcd.read(file, *cloud);
PointXYZ minp, maxp;
pcl::getMinMax3D(*cloud, minp, maxp);
float sz = (maxp.x - minp.x + maxp.y - minp.y + maxp.z - minp.z) / 3;
radius = sz / 15;
}
void generateColor()
{
for (auto& p: *cloud)
{
int r = std::max(1, std::min(255, static_cast<int>((double(rand())/RAND_MAX)*255)));
int g = std::max(1, std::min(255, static_cast<int>((double(rand())/RAND_MAX)*255)));
int b = std::max(1, std::min(255, static_cast<int>((double(rand())/RAND_MAX)*255)));
*reinterpret_cast<int*>(&p.data[3]) = (b << 16) + (g << 8) + r;
}
}
void estimateNormals()
{
pcl::NormalEstimation<PointXYZ, Normal> ne;
ne.setInputCloud (cloud);
ne.setSearchMethod (pcl::search::KdTree<PointXYZ>::Ptr (new pcl::search::KdTree<PointXYZ>));
ne.setKSearch (k);
//ne.setRadiusSearch (radius);
ne.compute (*normals);
}
void runCloudViewer() const
{
pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
viewer.showCloud (cloud);
while (!viewer.wasStopped ()) {}
}
void findKNNeghbors()
{
KdTreeFLANN<PointXYZ>::Ptr kdtree(new KdTreeFLANN<PointXYZ>);
kdtree->setInputCloud(cloud);
const auto cloud_size = cloud->size();
std::vector<float> dists;
neighbors_all.resize(cloud_size);
for(std::size_t i = 0; i < cloud_size; ++i)
{
kdtree->nearestKSearch((*cloud)[i], k, neighbors_all[i], dists);
sizes.push_back((int)neighbors_all[i].size());
}
max_nn_size = *max_element(sizes.begin(), sizes.end());
}
void findRadiusNeghbors(float radius = -1)
{
radius = radius == -1 ? this->radius : radius;
KdTreeFLANN<PointXYZ>::Ptr kdtree(new KdTreeFLANN<PointXYZ>);
kdtree->setInputCloud(cloud);
const auto cloud_size = cloud->size();
std::vector<float> dists;
neighbors_all.resize(cloud_size);
for(std::size_t i = 0; i < cloud_size; ++i)
{
kdtree->radiusSearch((*cloud)[i], radius, neighbors_all[i], dists);
sizes.push_back((int)neighbors_all[i].size());
}
max_nn_size = *max_element(sizes.begin(), sizes.end());
}
void getNeghborsArray(std::vector<int>& data)
{
data.resize(max_nn_size * neighbors_all.size());
pcl::gpu::PtrStep<int> ps(&data[0], max_nn_size * sizeof(int));
for(std::size_t i = 0; i < neighbors_all.size(); ++i)
copy(neighbors_all[i].begin(), neighbors_all[i].end(), ps.ptr(i));
}
void generateSurface()
{
surface->clear();
for(std::size_t i = 0; i < cloud->size(); i+= 10)
surface->push_back((*cloud)[i]);
surface->width = surface->size();
surface->height = 1;
if (!normals->empty())
{
normals_surface->clear();
for(std::size_t i = 0; i < normals->size(); i+= 10)
normals_surface->push_back((*normals)[i]);
normals_surface->width = surface->size();
normals_surface->height = 1;
}
}
void generateIndices(std::size_t step = 100)
{
indices->clear();
for(std::size_t i = 0; i < cloud->size(); i += step)
indices->push_back(i);
}
struct Normal2PointXYZ
{
PointXYZ operator()(const Normal& n) const
{
PointXYZ xyz;
xyz.x = n.normal[0];
xyz.y = n.normal[1];
xyz.z = n.normal[2];
return xyz;
}
};
};
}
}
#endif /* PCL_GPU_FEATURES_TEST_DATA_SOURCE_HPP_ */
pcl::Feature::setKSearch
void setKSearch(int k)
Set the number of k nearest neighbors to use for the feature estimation.
Definition: feature.h:187
pcl::Feature::setSearchMethod
void setSearchMethod(const KdTreePtr &tree)
Provide a pointer to the search object.
Definition: feature.h:167
pcl::Feature::compute
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Definition: feature.hpp:194
pcl::KdTreeFLANN
KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
Definition: kdtree_flann.h:132
pcl::KdTreeFLANN::Ptr
shared_ptr< KdTreeFLANN< PointT, Dist > > Ptr
Definition: kdtree_flann.h:151
pcl::KdTreeFLANN::nearestKSearch
int nearestKSearch(const PointT &point, unsigned int k, Indices &k_indices, std::vector< float > &k_sqr_distances) const override
Search for k-nearest neighbors for the given query point.
Definition: kdtree_flann.hpp:233
pcl::KdTreeFLANN::radiusSearch
int radiusSearch(const PointT &point, double radius, Indices &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const override
Search for all the nearest neighbors of the query point in a given radius.
Definition: kdtree_flann.hpp:367
pcl::KdTreeFLANN::setInputCloud
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr()) override
Provide a pointer to the input dataset.
Definition: kdtree_flann.hpp:102
pcl::NormalEstimation
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point.
Definition: normal_3d.h:244
pcl::NormalEstimation::setInputCloud
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
Definition: normal_3d.h:332
pcl::PCDReader
Point Cloud Data (PCD) file format reader.
Definition: pcd_io.h:55
pcl::PCDReader::read
int read(const std::string &file_name, pcl::PCLPointCloud2 &cloud, Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &pcd_version, const int offset=0) override
Read a point cloud data from a PCD file and store it into a pcl/PCLPointCloud2.
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
pcl::search::KdTree
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:62
pcl::search::KdTree::Ptr
shared_ptr< KdTree< PointT, Tree > > Ptr
Definition: kdtree.h:75
pcl::visualization::CloudViewer
Simple point cloud visualization class.
Definition: cloud_viewer.h:53
pcl::visualization::CloudViewer::wasStopped
bool wasStopped(int millis_to_wait=1)
Check if the gui was quit, you should quit also.
pcl::visualization::CloudViewer::showCloud
void showCloud(const ColorCloud::ConstPtr &cloud, const std::string &cloudname="cloud")
Show a cloud, with an optional key for multiple clouds.
common.h
Define standard C methods and C++ classes that are common to all methods.
point_types.h
Defines all the PCL implemented PointT point type structures.
pcl::getMinMax3D
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:295
pcl
Definition: convolution.h:46
pcl::IndicesPtr
shared_ptr< Indices > IndicesPtr
Definition: pcl_base.h:58
pcl::Normal
A point structure representing normal coordinates and the surface curvature estimate.
Definition: point_types.hpp:900
pcl::PointXYZ
A point structure representing Euclidean xyz coordinates.
Definition: point_types.hpp:347
pcl::gpu::DataSource::Normal2PointXYZ
Definition: data_source.hpp:196
pcl::gpu::DataSource::Normal2PointXYZ::operator()
PointXYZ operator()(const Normal &n) const
Definition: data_source.hpp:197
pcl::gpu::DataSource
Definition: data_source.hpp:66
pcl::gpu::DataSource::generateSurface
void generateSurface()
Definition: data_source.hpp:169
pcl::gpu::DataSource::normals_surface
PointCloud< Normal >::Ptr normals_surface
Definition: data_source.hpp:75
pcl::gpu::DataSource::normals
PointCloud< Normal >::Ptr normals
Definition: data_source.hpp:74
pcl::gpu::DataSource::radius
float radius
Definition: data_source.hpp:76
pcl::gpu::DataSource::max_elements
static const int max_elements
Definition: data_source.hpp:68
pcl::gpu::DataSource::generateColor
void generateColor()
Definition: data_source.hpp:95
pcl::gpu::DataSource::surface
PointCloud< PointXYZ >::Ptr surface
Definition: data_source.hpp:71
pcl::gpu::DataSource::DataSource
DataSource(const std::string &file="d:/office_chair_model.pcd")
Definition: data_source.hpp:82
pcl::gpu::DataSource::k
static const int k
Definition: data_source.hpp:67
pcl::gpu::DataSource::neighbors_all
std::vector< std::vector< int > > neighbors_all
Definition: data_source.hpp:78
pcl::gpu::DataSource::indices
IndicesPtr indices
Definition: data_source.hpp:72
pcl::gpu::DataSource::getNeghborsArray
void getNeghborsArray(std::vector< int > &data)
Definition: data_source.hpp:161
pcl::gpu::DataSource::findKNNeghbors
void findKNNeghbors()
Definition: data_source.hpp:125
pcl::gpu::DataSource::generateIndices
void generateIndices(std::size_t step=100)
Definition: data_source.hpp:188
pcl::gpu::DataSource::sizes
std::vector< int > sizes
Definition: data_source.hpp:79
pcl::gpu::DataSource::findRadiusNeghbors
void findRadiusNeghbors(float radius=-1)
Definition: data_source.hpp:142
pcl::gpu::DataSource::runCloudViewer
void runCloudViewer() const
Definition: data_source.hpp:118
pcl::gpu::DataSource::cloud
PointCloud< PointXYZ >::Ptr cloud
Definition: data_source.hpp:70
pcl::gpu::DataSource::estimateNormals
void estimateNormals()
Definition: data_source.hpp:107
pcl::gpu::DataSource::max_nn_size
int max_nn_size
Definition: data_source.hpp:80
pcl::gpu::PtrStep
Definition: kernel_containers.h:85
pcl::gpu::PtrStep::ptr
__PCL_GPU_HOST_DEVICE__ T * ptr(int y=0)
Definition: kernel_containers.h:97