Point Cloud Library (PCL)  1.8.1-dev
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.size () != 0)
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.size () == 0)
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.
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.size() > 0)
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, skiping 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
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  uint8_t* pt_data = reinterpret_cast<uint8_t*> (&cloud->points[(*indices)[rii]]);
264  for (int fi = 0; fi < static_cast<int> (fields.size ()); ++fi) // fi = field iterator
265  memcpy (pt_data + fields[fi].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_
void setKeepOrganized(bool val)
Set whether the filtered points should be kept and set to the value given through setUserFilterValue ...
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
void setCondition(ConditionBasePtr condition)
Set the condition that the filter will use.
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)
Retreive existing data from the world model, after a shift.
Definition: world_model.hpp:60
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.
pcl::ConditionOr< PointT >::Ptr ConditionOrPtr
Definition: world_model.h:76
void filter(PointCloud &output)
Calls the filtering method and returns the filtered dataset in output.
Definition: filter.h:132
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
ConditionalRemoval filters data that satisfies certain conditions.
void addSlice(const PointCloudPtr new_cloud)
Append a new point cloud (slice) to the world.
Definition: world_model.hpp:46
IndicesConstPtr const getRemovedIndices()
Get the point indices being removed.
Definition: filter.h:114
PointCloud::Ptr PointCloudPtr
Definition: world_model.h:72
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=true)
Apply an affine transform defined by an Eigen Transform.
Definition: transforms.hpp:42
The field-based specialization of the comparison object.
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
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.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: pcl_base.hpp:66
boost::shared_ptr< const std::vector< int > > IndicesConstPtr
Definition: pcl_base.h:61
pcl::ConditionAnd< PointT >::Ptr ConditionAndPtr
Definition: world_model.h:75
WorldModel maintains a 3D point cloud that can be queried and updated via helper functions.
Definition: world_model.h:64
A point structure representing Euclidean xyz coordinates, and the RGB color.
pcl::FieldComparison< PointT >::ConstPtr FieldComparisonConstPtr
Definition: world_model.h:77