Point Cloud Library (PCL)  1.14.1-dev
pyramid.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2012-, Open Perception, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id$
37  *
38  */
39 
40 
41 #ifndef PCL_FILTERS_IMPL_PYRAMID_HPP
42 #define PCL_FILTERS_IMPL_PYRAMID_HPP
43 
44 #include <pcl/common/distances.h>
45 #include <pcl/filters/pyramid.h>
46 #include <pcl/console/print.h>
47 #include <pcl/point_types.h>
48 
49 namespace pcl
50 {
51 namespace filters
52 {
53 template <typename PointT> bool
54 Pyramid<PointT>::initCompute ()
55 {
56  if (!input_->isOrganized ())
57  {
58  PCL_ERROR ("[pcl::filters::%s::initCompute] Number of levels should be at least 2!\n", getClassName ().c_str ());
59  return (false);
60  }
61 
62  if (levels_ < 2)
63  {
64  PCL_ERROR ("[pcl::filters::%s::initCompute] Number of levels should be at least 2!\n", getClassName ().c_str ());
65  return (false);
66  }
67 
68  // std::size_t ratio (std::pow (2, levels_));
69  // std::size_t last_width = input_->width / ratio;
70  // std::size_t last_height = input_->height / ratio;
71 
72  if (levels_ > 4)
73  {
74  PCL_ERROR ("[pcl::filters::%s::initCompute] Number of levels should not exceed 4!\n", getClassName ().c_str ());
75  return (false);
76  }
77 
78  if (large_)
79  {
80  Eigen::VectorXf k (5);
81  k << 1.f/16.f, 1.f/4.f, 3.f/8.f, 1.f/4.f, 1.f/16.f;
82  kernel_ = k * k.transpose ();
83  if (threshold_ != std::numeric_limits<float>::infinity ())
84  threshold_ *= 2 * threshold_;
85 
86  }
87  else
88  {
89  Eigen::VectorXf k (3);
90  k << 1.f/4.f, 1.f/2.f, 1.f/4.f;
91  kernel_ = k * k.transpose ();
92  if (threshold_ != std::numeric_limits<float>::infinity ())
93  threshold_ *= threshold_;
94  }
95 
96  return (true);
97 }
98 
99 template <typename PointT> void
100 Pyramid<PointT>::compute (std::vector<PointCloudPtr>& output)
101 {
102  std::cout << "compute" << std::endl;
103  if (!initCompute ())
104  {
105  PCL_ERROR ("[pcl::%s::compute] initCompute failed!\n", getClassName ().c_str ());
106  return;
107  }
108 
109  int kernel_rows = static_cast<int> (kernel_.rows ());
110  int kernel_cols = static_cast<int> (kernel_.cols ());
111  int kernel_center_x = kernel_cols / 2;
112  int kernel_center_y = kernel_rows / 2;
113 
114  output.resize (levels_ + 1);
115  output[0].reset (new pcl::PointCloud<PointT>);
116  *(output[0]) = *input_;
117 
118  if (input_->is_dense)
119  {
120  for (int l = 1; l <= levels_; ++l)
121  {
122  output[l].reset (new pcl::PointCloud<PointT> (output[l-1]->width/2, output[l-1]->height/2));
123  const PointCloud<PointT> &previous = *output[l-1];
124  PointCloud<PointT> &next = *output[l];
125 #pragma omp parallel for \
126  default(none) \
127  shared(next) \
128  num_threads(threads_)
129  for(int i=0; i < next.height; ++i)
130  {
131  for(int j=0; j < next.width; ++j)
132  {
133  for(int m=0; m < kernel_rows; ++m)
134  {
135  int mm = kernel_rows - 1 - m;
136  for(int n=0; n < kernel_cols; ++n)
137  {
138  int nn = kernel_cols - 1 - n;
139 
140  int ii = 2*i + (m - kernel_center_y);
141  int jj = 2*j + (n - kernel_center_x);
142 
143  if (ii < 0) ii = 0;
144  if (ii >= previous.height) ii = previous.height - 1;
145  if (jj < 0) jj = 0;
146  if (jj >= previous.width) jj = previous.width - 1;
147  next.at (j,i) += previous.at (jj,ii) * kernel_ (mm,nn);
148  }
149  }
150  }
151  }
152  }
153  }
154  else
155  {
156  for (int l = 1; l <= levels_; ++l)
157  {
158  output[l].reset (new pcl::PointCloud<PointT> (output[l-1]->width/2, output[l-1]->height/2));
159  const PointCloud<PointT> &previous = *output[l-1];
160  PointCloud<PointT> &next = *output[l];
161 #pragma omp parallel for \
162  default(none) \
163  shared(next) \
164  num_threads(threads_)
165  for(int i=0; i < next.height; ++i)
166  {
167  for(int j=0; j < next.width; ++j)
168  {
169  float weight = 0;
170  for(int m=0; m < kernel_rows; ++m)
171  {
172  int mm = kernel_rows - 1 - m;
173  for(int n=0; n < kernel_cols; ++n)
174  {
175  int nn = kernel_cols - 1 - n;
176  int ii = 2*i + (m - kernel_center_y);
177  int jj = 2*j + (n - kernel_center_x);
178  if (ii < 0) ii = 0;
179  if (ii >= previous.height) ii = previous.height - 1;
180  if (jj < 0) jj = 0;
181  if (jj >= previous.width) jj = previous.width - 1;
182  if (!isFinite (previous.at (jj,ii)))
183  continue;
184  if (pcl::squaredEuclideanDistance (previous.at (2*j,2*i), previous.at (jj,ii)) < threshold_)
185  {
186  next.at (j,i) += previous.at (jj,ii).x * kernel_ (mm,nn);
187  weight+= kernel_ (mm,nn);
188  }
189  }
190  }
191  if (weight == 0)
192  nullify (next.at (j,i));
193  else
194  {
195  weight = 1.f/weight;
196  next.at (j,i)*= weight;
197  }
198  }
199  }
200  }
201  }
202 }
203 
204 template <> void
206 
207 template <> void
209 
210 template<> void
212 {
213  p.r = 0; p.g = 0; p.b = 0;
214 }
215 
216 template <> void
218 
219 } // namespace filters
220 } // namespace pcl
221 
222 #endif
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
const PointT & at(int column, int row) const
Obtain the point given by the (column, row) coordinates.
Definition: point_cloud.h:262
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:400
Pyramid constructs a multi-scale representation of an organised point cloud.
Definition: pyramid.h:63
void compute(std::vector< PointCloudPtr > &output)
compute the pyramid levels.
Definition: pyramid.hpp:100
typename PointCloud< PointT >::Ptr PointCloudPtr
Definition: pyramid.h:65
Define standard C methods to do distance calculations.
Defines all the PCL implemented PointT point type structures.
float squaredEuclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the squared euclidean distance between the two given points.
Definition: distances.h:182
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Definition: point_tests.h:55
A structure representing RGB color information.