39 #ifndef PCL_WORLD_MODEL_IMPL_HPP_
40 #define PCL_WORLD_MODEL_IMPL_HPP_
42 #include <pcl/gpu/kinfu_large_scale/world_model.h>
43 #include <pcl/common/transforms.h>
45 template <
typename Po
intT>
49 PCL_DEBUG(
"Adding new cloud. Current world contains %zu points.\n",
50 static_cast<std::size_t
>(world_->size()));
52 PCL_DEBUG(
"New slice contains %zu points.\n",
53 static_cast<std::size_t
>(new_cloud->size()));
55 *world_ += *new_cloud;
57 PCL_DEBUG(
"World now contains %zu points.\n",
58 static_cast<std::size_t
>(world_->size()));
61 template <
typename Po
intT>
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;
90 condremAND.
filter (*newCube);
116 condrem.
filter (existing_slice);
118 if(!existing_slice.
empty ())
121 Eigen::Affine3f transformation;
122 transformation.translation ()[0] = newOriginX;
123 transformation.translation ()[1] = newOriginY;
124 transformation.translation ()[2] = newOriginZ;
126 transformation.linear ().setIdentity ();
134 template <
typename Po
intT>
139 if(world_->points.empty ())
141 PCL_INFO(
"The world is empty, returning nothing\n");
145 PCL_INFO(
"Getting world as cubes. World contains %zu points.\n",
146 static_cast<std::size_t
>(world_->size()));
149 world_->is_dense =
false;
150 std::vector<int> indices;
153 PCL_INFO(
"World contains %zu points after nan removal.\n",
154 static_cast<std::size_t
>(world_->size()));
157 double cubeSide = size;
158 if (cubeSide <= 0.0f)
160 PCL_ERROR (
"Size of the cube must be positive and non null (%f given). Setting it to 3.0 meters.\n", cubeSide);
164 std::cout <<
"cube size is set to " << cubeSide << std::endl;
167 double step_increment = 1.0f - overlap;
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;
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;
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);
193 while (origin.x < max.x)
196 while (origin.y < max.y)
199 while (origin.z < max.z)
202 PCL_INFO (
"Extracting cube at: [%f, %f, %f].\n", origin.x, origin.y, origin.z);
226 if(!box->points.empty ())
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);
235 PCL_INFO (
"Extracted cube was empty, skipping this one.\n");
237 origin.z += cubeSide * step_increment;
239 origin.y += cubeSide * step_increment;
241 origin.x += cubeSide * step_increment;
253 std::cout <<
"returning " << cubes.size() <<
" cubes" << std::endl;
256 template <
typename Po
intT>
260 std::vector<pcl::PCLPointField> fields;
262 float my_nan = std::numeric_limits<float>::quiet_NaN ();
264 for (
int rii = 0; rii < static_cast<int> (indices->size ()); ++rii)
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));
273 template <
typename Po
intT>
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;
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;
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;
298 double lower_limit_x, upper_limit_x;
301 lower_limit_x = previous_origin_x;
302 upper_limit_x = new_origin_x;
306 lower_limit_x = new_limit_x;
307 upper_limit_x = previous_limit_x;
327 condrem_x.
filter (*slice);
331 setIndicesAsNans(world_, indices_x);
336 double lower_limit_y, upper_limit_y;
339 lower_limit_y = previous_origin_y;
340 upper_limit_y = new_origin_y;
344 lower_limit_y = new_limit_y;
345 upper_limit_y = previous_limit_y;
365 condrem_y.
filter (*slice);
369 setIndicesAsNans(world_, indices_y);
373 double lower_limit_z, upper_limit_z;
376 lower_limit_z = previous_origin_z;
377 upper_limit_z = new_origin_z;
381 lower_limit_z = new_limit_z;
382 upper_limit_z = previous_limit_z;
402 condrem_z.
filter (*slice);
406 setIndicesAsNans(world_, indices_z);
412 #define PCL_INSTANTIATE_WorldModel(T) template class PCL_EXPORTS pcl::kinfuLS::WorldModel<T>;
414 #endif // PCL_WORLD_MODEL_IMPL_HPP_