Point Cloud Library (PCL) 1.12.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
82
83 // build the filter
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
94
95 if(offset_x >= 0)
97 else
99
100 if(offset_y >= 0)
102 else
104
105 if(offset_z >= 0)
107 else
109
110 // build the filter
112 condrem.setCondition (range_condOR);
113 condrem.setInputCloud (newCube);
114 condrem.setKeepOrganized (false);
115 // apply filter
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;
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
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
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
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)
299 if(offset_x >=0)
300 {
303 }
304 else
305 {
308 }
309
310 // PCL_DEBUG ("Limit X: [%f - %f]\n", lower_limit_x, upper_limit_x);
311
315
318
321
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)
337 if(offset_y >=0)
338 {
341 }
342 else
343 {
346 }
347
348 // PCL_DEBUG ("Limit Y: [%f - %f]\n", lower_limit_y, upper_limit_y);
349
353
356
359
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)
374 if(offset_z >=0)
375 {
378 }
379 else
380 {
383 }
384
385 // PCL_DEBUG ("Limit Z: [%f - %f]\n", lower_limit_z, upper_limit_z);
386
390
393
396
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_
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
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
A point structure representing Euclidean xyz coordinates, and the RGB color.