Point Cloud Library (PCL)  1.11.0
world_model.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, 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 Willow Garage, Inc. 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  * Author: Raphael Favier, Technical University Eindhoven, (r.mysurname <aT> tue.nl)
37  */
38 
39 #ifndef PCL_WORLD_MODEL_IMPL_HPP_
40 #define PCL_WORLD_MODEL_IMPL_HPP_
41 
42 #include <pcl/gpu/kinfu_large_scale/world_model.h>
43 
44 template <typename PointT>
45 void
47 {
48  PCL_DEBUG ("Adding new cloud. Current world contains %d points.\n", world_->points.size ());
49 
50  PCL_DEBUG ("New slice contains %d points.\n", new_cloud->points.size ());
51 
52  *world_ += *new_cloud;
53 
54  PCL_DEBUG ("World now contains %d points.\n", world_->points.size ());
55 }
56 
57 
58 template <typename PointT>
59 void
60 pcl::kinfuLS::WorldModel<PointT>::getExistingData(const double previous_origin_x, const double previous_origin_y, const double previous_origin_z, const double offset_x, const double offset_y, const double offset_z, const double volume_x, const double volume_y, const double volume_z, pcl::PointCloud<PointT> &existing_slice)
61 {
62  double newOriginX = previous_origin_x + offset_x;
63  double newOriginY = previous_origin_y + offset_y;
64  double newOriginZ = previous_origin_z + offset_z;
65  double newLimitX = newOriginX + volume_x;
66  double newLimitY = newOriginY + volume_y;
67  double newLimitZ = newOriginZ + volume_z;
68 
69  // filter points in the space of the new cube
71  // condition
72  ConditionAndPtr range_condAND (new pcl::ConditionAnd<PointT> ());
73  range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, newOriginX)));
74  range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, newLimitX)));
75  range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, newOriginY)));
76  range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, newLimitY)));
77  range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, newOriginZ)));
78  range_condAND->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, newLimitZ)));
79 
80  // build the filter
81  pcl::ConditionalRemoval<PointT> condremAND (true);
82  condremAND.setCondition (range_condAND);
83  condremAND.setInputCloud (world_);
84  condremAND.setKeepOrganized (false);
85 
86  // apply filter
87  condremAND.filter (*newCube);
88 
89  // filter points that belong to the new slice
90  ConditionOrPtr range_condOR (new pcl::ConditionOr<PointT> ());
91 
92  if(offset_x >= 0)
93  range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, previous_origin_x + volume_x - 1.0 )));
94  else
95  range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, previous_origin_x )));
96 
97  if(offset_y >= 0)
98  range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, previous_origin_y + volume_y - 1.0 )));
99  else
100  range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, previous_origin_y )));
101 
102  if(offset_z >= 0)
103  range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, previous_origin_z + volume_z - 1.0 )));
104  else
105  range_condOR->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, previous_origin_z )));
106 
107  // build the filter
108  pcl::ConditionalRemoval<PointT> condrem (true);
109  condrem.setCondition (range_condOR);
110  condrem.setInputCloud (newCube);
111  condrem.setKeepOrganized (false);
112  // apply filter
113  condrem.filter (existing_slice);
114 
115  if(!existing_slice.points.empty ())
116  {
117  //transform the slice in new cube coordinates
118  Eigen::Affine3f transformation;
119  transformation.translation ()[0] = newOriginX;
120  transformation.translation ()[1] = newOriginY;
121  transformation.translation ()[2] = newOriginZ;
122 
123  transformation.linear ().setIdentity ();
124 
125  transformPointCloud (existing_slice, existing_slice, transformation.inverse ());
126 
127  }
128 }
129 
130 
131 template <typename PointT>
132 void
133 pcl::kinfuLS::WorldModel<PointT>::getWorldAsCubes (const double size, std::vector<typename WorldModel<PointT>::PointCloudPtr> &cubes, std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &transforms, double overlap)
134 {
135 
136  if(world_->points.empty ())
137  {
138  PCL_INFO("The world is empty, returning nothing\n");
139  return;
140  }
141 
142  PCL_INFO ("Getting world as cubes. World contains %d points.\n", world_->points.size ());
143 
144  // remove nans from world cloud
145  world_->is_dense = false;
146  std::vector<int> indices;
147  pcl::removeNaNFromPointCloud ( *world_, *world_, indices);
148 
149  PCL_INFO ("World contains %d points after nan removal.\n", world_->points.size ());
150 
151 
152  // check cube size value
153  double cubeSide = size;
154  if (cubeSide <= 0.0f)
155  {
156  PCL_ERROR ("Size of the cube must be positive and non null (%f given). Setting it to 3.0 meters.\n", cubeSide);
157  cubeSide = 512.0f;
158  }
159 
160  std::cout << "cube size is set to " << cubeSide << std::endl;
161 
162  // check overlap value
163  double step_increment = 1.0f - overlap;
164  if (overlap < 0.0)
165  {
166  PCL_ERROR ("Overlap ratio must be positive or null (%f given). Setting it to 0.0 procent.\n", overlap);
167  step_increment = 1.0f;
168  }
169  if (overlap > 1.0)
170  {
171  PCL_ERROR ("Overlap ratio must be less or equal to 1.0 (%f given). Setting it to 10 procent.\n", overlap);
172  step_increment = 0.1f;
173  }
174 
175 
176  // get world's bounding values on XYZ
177  PointT min, max;
178  pcl::getMinMax3D(*world_, min, max);
179 
180  PCL_INFO ("Bounding box for the world: \n\t [%f - %f] \n\t [%f - %f] \n\t [%f - %f] \n", min.x, max.x, min.y, max.y, min.z, max.z);
181 
182  PointT origin = min;
183 
184  // clear returned vectors
185  cubes.clear();
186  transforms.clear();
187 
188  // iterate with box filter
189  while (origin.x < max.x)
190  {
191  origin.y = min.y;
192  while (origin.y < max.y)
193  {
194  origin.z = min.z;
195  while (origin.z < max.z)
196  {
197  // extract cube here
198  PCL_INFO ("Extracting cube at: [%f, %f, %f].\n", origin.x, origin.y, origin.z);
199 
200  // pointcloud for current cube.
201  PointCloudPtr box (new pcl::PointCloud<PointT>);
202 
203 
204  // set conditional filter
205  ConditionAndPtr range_cond (new pcl::ConditionAnd<PointT> ());
206  range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, origin.x)));
207  range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, origin.x + cubeSide)));
208  range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, origin.y)));
209  range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, origin.y + cubeSide)));
210  range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, origin.z)));
211  range_cond->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, origin.z + cubeSide)));
212 
213  // build the filter
215  condrem.setCondition (range_cond);
216  condrem.setInputCloud (world_);
217  condrem.setKeepOrganized(false);
218  // apply filter
219  condrem.filter (*box);
220 
221  // also push transform along with points.
222  if(!box->points.empty ())
223  {
224  Eigen::Vector3f transform;
225  transform[0] = origin.x, transform[1] = origin.y, transform[2] = origin.z;
226  transforms.push_back(transform);
227  cubes.push_back(box);
228  }
229  else
230  {
231  PCL_INFO ("Extracted cube was empty, skipping this one.\n");
232  }
233  origin.z += cubeSide * step_increment;
234  }
235  origin.y += cubeSide * step_increment;
236  }
237  origin.x += cubeSide * step_increment;
238  }
239 
240 
241  /* for(int c = 0 ; c < cubes.size() ; ++c)
242  {
243  std::stringstream name;
244  name << "cloud" << c+1 << ".pcd";
245  pcl::io::savePCDFileASCII(name.str(), *(cubes[c]));
246 
247  }*/
248 
249  std::cout << "returning " << cubes.size() << " cubes" << std::endl;
250 
251 }
252 
253 template <typename PointT>
254 inline void
255 pcl::kinfuLS::WorldModel<PointT>::setIndicesAsNans (PointCloudPtr cloud, IndicesConstPtr indices)
256 {
257  std::vector<pcl::PCLPointField> fields;
258  pcl::for_each_type<FieldList> (pcl::detail::FieldAdder<PointT> (fields));
259  float my_nan = std::numeric_limits<float>::quiet_NaN ();
260 
261  for (int rii = 0; rii < static_cast<int> (indices->size ()); ++rii) // rii = removed indices iterator
262  {
263  std::uint8_t* pt_data = reinterpret_cast<std::uint8_t*> (&cloud->points[(*indices)[rii]]);
264  for (const auto &field : fields)
265  memcpy (pt_data + field.offset, &my_nan, sizeof (float));
266  }
267 }
268 
269 
270 template <typename PointT>
271 void
272 pcl::kinfuLS::WorldModel<PointT>::setSliceAsNans (const double origin_x, const double origin_y, const double origin_z, const double offset_x, const double offset_y, const double offset_z, const int size_x, const int size_y, const int size_z)
273 {
274  // PCL_DEBUG ("IN SETSLICE AS NANS\n");
275 
277 
278  // prepare filter limits on all dimensions
279  double previous_origin_x = origin_x;
280  double previous_limit_x = origin_x + size_x - 1;
281  double new_origin_x = origin_x + offset_x;
282  double new_limit_x = previous_limit_x + offset_x;
283 
284  double previous_origin_y = origin_y;
285  double previous_limit_y = origin_y + size_y - 1;
286  double new_origin_y = origin_y + offset_y;
287  double new_limit_y = previous_limit_y + offset_y;
288 
289  double previous_origin_z = origin_z;
290  double previous_limit_z = origin_z + size_z - 1;
291  double new_origin_z = origin_z + offset_z;
292  double new_limit_z = previous_limit_z + offset_z;
293 
294  // get points of slice on X (we actually set a negative filter and set the ouliers (so, our slice points) to nan)
295  double lower_limit_x, upper_limit_x;
296  if(offset_x >=0)
297  {
298  lower_limit_x = previous_origin_x;
299  upper_limit_x = new_origin_x;
300  }
301  else
302  {
303  lower_limit_x = new_limit_x;
304  upper_limit_x = previous_limit_x;
305  }
306 
307  // PCL_DEBUG ("Limit X: [%f - %f]\n", lower_limit_x, upper_limit_x);
308 
309  ConditionOrPtr range_cond_OR_x (new pcl::ConditionOr<PointT> ());
310  range_cond_OR_x->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, upper_limit_x ))); // filtered dimension
311  range_cond_OR_x->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, lower_limit_x ))); // filtered dimension
312 
313  range_cond_OR_x->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, previous_limit_y)));
314  range_cond_OR_x->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, previous_origin_y )));
315 
316  range_cond_OR_x->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, previous_limit_z)));
317  range_cond_OR_x->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, previous_origin_z )));
318 
319  pcl::ConditionalRemoval<PointT> condrem_x (true);
320  condrem_x.setCondition (range_cond_OR_x);
321  condrem_x.setInputCloud (world_);
322  condrem_x.setKeepOrganized (false);
323  // apply filter
324  condrem_x.filter (*slice);
325  IndicesConstPtr indices_x = condrem_x.getRemovedIndices ();
326 
327  //set outliers (so our slice points) to nan
328  setIndicesAsNans(world_, indices_x);
329 
330  // PCL_DEBUG("%d points set to nan on X\n", indices_x->size ());
331 
332  // get points of slice on Y (we actually set a negative filter and set the ouliers (so, our slice points) to nan)
333  double lower_limit_y, upper_limit_y;
334  if(offset_y >=0)
335  {
336  lower_limit_y = previous_origin_y;
337  upper_limit_y = new_origin_y;
338  }
339  else
340  {
341  lower_limit_y = new_limit_y;
342  upper_limit_y = previous_limit_y;
343  }
344 
345  // PCL_DEBUG ("Limit Y: [%f - %f]\n", lower_limit_y, upper_limit_y);
346 
347  ConditionOrPtr range_cond_OR_y (new pcl::ConditionOr<PointT> ());
348  range_cond_OR_y->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, previous_limit_x )));
349  range_cond_OR_y->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, previous_origin_x )));
350 
351  range_cond_OR_y->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, upper_limit_y))); // filtered dimension
352  range_cond_OR_y->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, lower_limit_y ))); // filtered dimension
353 
354  range_cond_OR_y->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, previous_limit_z)));
355  range_cond_OR_y->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, previous_origin_z )));
356 
357  pcl::ConditionalRemoval<PointT> condrem_y (true);
358  condrem_y.setCondition (range_cond_OR_y);
359  condrem_y.setInputCloud (world_);
360  condrem_y.setKeepOrganized (false);
361  // apply filter
362  condrem_y.filter (*slice);
363  IndicesConstPtr indices_y = condrem_y.getRemovedIndices ();
364 
365  //set outliers (so our slice points) to nan
366  setIndicesAsNans(world_, indices_y);
367  // PCL_DEBUG ("%d points set to nan on Y\n", indices_y->size ());
368 
369  // get points of slice on Z (we actually set a negative filter and set the ouliers (so, our slice points) to nan)
370  double lower_limit_z, upper_limit_z;
371  if(offset_z >=0)
372  {
373  lower_limit_z = previous_origin_z;
374  upper_limit_z = new_origin_z;
375  }
376  else
377  {
378  lower_limit_z = new_limit_z;
379  upper_limit_z = previous_limit_z;
380  }
381 
382  // PCL_DEBUG ("Limit Z: [%f - %f]\n", lower_limit_z, upper_limit_z);
383 
384  ConditionOrPtr range_cond_OR_z (new pcl::ConditionOr<PointT> ());
385  range_cond_OR_z->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::GE, previous_limit_x )));
386  range_cond_OR_z->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("x", pcl::ComparisonOps::LT, previous_origin_x )));
387 
388  range_cond_OR_z->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::GE, previous_limit_y)));
389  range_cond_OR_z->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("y", pcl::ComparisonOps::LT, previous_origin_y )));
390 
391  range_cond_OR_z->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::GE, upper_limit_z))); // filtered dimension
392  range_cond_OR_z->addComparison (FieldComparisonConstPtr (new pcl::FieldComparison<PointT> ("z", pcl::ComparisonOps::LT, lower_limit_z ))); // filtered dimension
393 
394  pcl::ConditionalRemoval<PointT> condrem_z (true);
395  condrem_z.setCondition (range_cond_OR_z);
396  condrem_z.setInputCloud (world_);
397  condrem_z.setKeepOrganized (false);
398  // apply filter
399  condrem_z.filter (*slice);
400  IndicesConstPtr indices_z = condrem_z.getRemovedIndices ();
401 
402  //set outliers (so our slice points) to nan
403  setIndicesAsNans(world_, indices_z);
404  // PCL_DEBUG("%d points set to nan on Z\n", indices_z->size ());
405 
406 
407 }
408 
409 #define PCL_INSTANTIATE_WorldModel(T) template class PCL_EXPORTS pcl::kinfuLS::WorldModel<T>;
410 
411 #endif // PCL_WORLD_MODEL_IMPL_HPP_
ConditionalRemoval filters data that satisfies certain conditions.
void setCondition(ConditionBasePtr condition)
Set the condition that the filter will use.
void setKeepOrganized(bool val)
Set whether the filtered points should be kept and set to the value given through setUserFilterValue ...
The field-based specialization of the comparison object.
void filter(PointCloud &output)
Calls the filtering method and returns the filtered dataset in output.
Definition: filter.h:124
IndicesConstPtr const getRemovedIndices() const
Get the point indices being removed.
Definition: filter.h:106
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: pcl_base.hpp:65
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:180
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
WorldModel maintains a 3D point cloud that can be queried and updated via helper functions.
Definition: world_model.h:64
typename PointCloud::Ptr PointCloudPtr
Definition: world_model.h:71
void setSliceAsNans(const double origin_x, const double origin_y, const double origin_z, const double offset_x, const double offset_y, const double offset_z, const int size_x, const int size_y, const int size_z)
Give nan values to the slice of the world.
void getWorldAsCubes(double size, std::vector< PointCloudPtr > &cubes, std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > &transforms, double overlap=0.0)
Returns the world as two vectors of cubes of size "size" (pointclouds) and transforms.
typename pcl::FieldComparison< PointT >::ConstPtr FieldComparisonConstPtr
Definition: world_model.h:76
void getExistingData(const double previous_origin_x, const double previous_origin_y, const double previous_origin_z, const double offset_x, const double offset_y, const double offset_z, const double volume_x, const double volume_y, const double volume_z, pcl::PointCloud< PointT > &existing_slice)
Retrieve existing data from the world model, after a shift.
Definition: world_model.hpp:60
typename pcl::ConditionAnd< PointT >::Ptr ConditionAndPtr
Definition: world_model.h:74
void addSlice(const PointCloudPtr new_cloud)
Append a new point cloud (slice) to the world.
Definition: world_model.hpp:46
typename pcl::ConditionOr< PointT >::Ptr ConditionOrPtr
Definition: world_model.h:75
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:242
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform, bool copy_all_fields)
Apply an affine transform defined by an Eigen Transform.
Definition: transforms.hpp:221
void removeNaNFromPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, std::vector< int > &index)
Removes points with x, y, or z equal to NaN.
Definition: filter.hpp:46
shared_ptr< const Indices > IndicesConstPtr
Definition: pcl_base.h:62
std::uint8_t uint8_t
Definition: types.h:54
A point structure representing Euclidean xyz coordinates, and the RGB color.