39 #ifndef PCL_WORLD_MODEL_IMPL_HPP_
40 #define PCL_WORLD_MODEL_IMPL_HPP_
42 #include <pcl/gpu/kinfu_large_scale/world_model.h>
44 template <
typename Po
intT>
48 PCL_DEBUG (
"Adding new cloud. Current world contains %d points.\n", world_->points.size ());
50 PCL_DEBUG (
"New slice contains %d points.\n", new_cloud->points.size ());
52 *world_ += *new_cloud;
54 PCL_DEBUG (
"World now contains %d points.\n", world_->points.size ());
58 template <
typename Po
intT>
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;
87 condremAND.
filter (*newCube);
113 condrem.
filter (existing_slice);
115 if(!existing_slice.
points.empty ())
118 Eigen::Affine3f transformation;
119 transformation.translation ()[0] = newOriginX;
120 transformation.translation ()[1] = newOriginY;
121 transformation.translation ()[2] = newOriginZ;
123 transformation.linear ().setIdentity ();
131 template <
typename Po
intT>
136 if(world_->points.empty ())
138 PCL_INFO(
"The world is empty, returning nothing\n");
142 PCL_INFO (
"Getting world as cubes. World contains %d points.\n", world_->points.size ());
145 world_->is_dense =
false;
146 std::vector<int> indices;
149 PCL_INFO (
"World contains %d points after nan removal.\n", world_->points.size ());
153 double cubeSide = size;
154 if (cubeSide <= 0.0f)
156 PCL_ERROR (
"Size of the cube must be positive and non null (%f given). Setting it to 3.0 meters.\n", cubeSide);
160 std::cout <<
"cube size is set to " << cubeSide << std::endl;
163 double step_increment = 1.0f - overlap;
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;
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;
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);
189 while (origin.x < max.x)
192 while (origin.y < max.y)
195 while (origin.z < max.z)
198 PCL_INFO (
"Extracting cube at: [%f, %f, %f].\n", origin.x, origin.y, origin.z);
222 if(!box->points.empty ())
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);
231 PCL_INFO (
"Extracted cube was empty, skipping this one.\n");
233 origin.z += cubeSide * step_increment;
235 origin.y += cubeSide * step_increment;
237 origin.x += cubeSide * step_increment;
249 std::cout <<
"returning " << cubes.size() <<
" cubes" << std::endl;
253 template <
typename Po
intT>
257 std::vector<pcl::PCLPointField> fields;
259 float my_nan = std::numeric_limits<float>::quiet_NaN ();
261 for (
int rii = 0; rii < static_cast<int> (indices->size ()); ++rii)
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));
270 template <
typename Po
intT>
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;
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;
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;
295 double lower_limit_x, upper_limit_x;
298 lower_limit_x = previous_origin_x;
299 upper_limit_x = new_origin_x;
303 lower_limit_x = new_limit_x;
304 upper_limit_x = previous_limit_x;
324 condrem_x.
filter (*slice);
328 setIndicesAsNans(world_, indices_x);
333 double lower_limit_y, upper_limit_y;
336 lower_limit_y = previous_origin_y;
337 upper_limit_y = new_origin_y;
341 lower_limit_y = new_limit_y;
342 upper_limit_y = previous_limit_y;
362 condrem_y.
filter (*slice);
366 setIndicesAsNans(world_, indices_y);
370 double lower_limit_z, upper_limit_z;
373 lower_limit_z = previous_origin_z;
374 upper_limit_z = new_origin_z;
378 lower_limit_z = new_limit_z;
379 upper_limit_z = previous_limit_z;
399 condrem_z.
filter (*slice);
403 setIndicesAsNans(world_, indices_z);
409 #define PCL_INSTANTIATE_WorldModel(T) template class PCL_EXPORTS pcl::kinfuLS::WorldModel<T>;
411 #endif // PCL_WORLD_MODEL_IMPL_HPP_