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