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 %zu points.\n",
49 static_cast<std::size_t
>(world_->size()));
51 PCL_DEBUG(
"New slice contains %zu points.\n",
52 static_cast<std::size_t
>(new_cloud->size()));
54 *world_ += *new_cloud;
56 PCL_DEBUG(
"World now contains %zu points.\n",
57 static_cast<std::size_t
>(world_->size()));
60 template <
typename Po
intT>
64 double newOriginX = previous_origin_x + offset_x;
65 double newOriginY = previous_origin_y + offset_y;
66 double newOriginZ = previous_origin_z + offset_z;
67 double newLimitX = newOriginX + volume_x;
68 double newLimitY = newOriginY + volume_y;
69 double newLimitZ = newOriginZ + volume_z;
89 condremAND.
filter (*newCube);
111 condrem.setCondition (range_condOR);
112 condrem.setInputCloud (newCube);
113 condrem.setKeepOrganized (
false);
115 condrem.filter (existing_slice);
120 Eigen::Affine3f transformation;
121 transformation.translation ()[0] = newOriginX;
122 transformation.translation ()[1] = newOriginY;
123 transformation.translation ()[2] = newOriginZ;
125 transformation.linear ().setIdentity ();
133 template <
typename Po
intT>
138 if(world_->points.empty ())
140 PCL_INFO(
"The world is empty, returning nothing\n");
144 PCL_INFO(
"Getting world as cubes. World contains %zu points.\n",
145 static_cast<std::size_t
>(world_->size()));
148 world_->is_dense =
false;
149 std::vector<int> indices;
152 PCL_INFO(
"World contains %zu points after nan removal.\n",
153 static_cast<std::size_t
>(world_->size()));
156 double cubeSide = size;
157 if (cubeSide <= 0.0f)
159 PCL_ERROR (
"Size of the cube must be positive and non null (%f given). Setting it to 3.0 meters.\n", cubeSide);
163 std::cout <<
"cube size is set to " << cubeSide << std::endl;
166 double step_increment = 1.0f - overlap;
169 PCL_ERROR (
"Overlap ratio must be positive or null (%f given). Setting it to 0.0 procent.\n", overlap);
170 step_increment = 1.0f;
174 PCL_ERROR (
"Overlap ratio must be less or equal to 1.0 (%f given). Setting it to 10 procent.\n", overlap);
175 step_increment = 0.1f;
183 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);
192 while (origin.x < max.x)
195 while (origin.y < max.y)
198 while (origin.z < max.z)
201 PCL_INFO (
"Extracting cube at: [%f, %f, %f].\n", origin.x, origin.y, origin.z);
225 if(!box->points.empty ())
227 Eigen::Vector3f transform;
228 transform[0] = origin.x, transform[1] = origin.y, transform[2] = origin.z;
229 transforms.push_back(transform);
230 cubes.push_back(box);
234 PCL_INFO (
"Extracted cube was empty, skipping this one.\n");
236 origin.z += cubeSide * step_increment;
238 origin.y += cubeSide * step_increment;
240 origin.x += cubeSide * step_increment;
252 std::cout <<
"returning " << cubes.size() <<
" cubes" << std::endl;
255 template <
typename Po
intT>
259 std::vector<pcl::PCLPointField> fields;
261 float my_nan = std::numeric_limits<float>::quiet_NaN ();
263 for (
int rii = 0; rii < static_cast<int> (indices->size ()); ++rii)
266 for (
const auto &field : fields)
267 memcpy (pt_data + field.offset, &my_nan, sizeof (
float));
272 template <
typename Po
intT>
281 double previous_origin_x = origin_x;
282 double previous_limit_x = origin_x + size_x - 1;
283 double new_origin_x = origin_x + offset_x;
284 double new_limit_x = previous_limit_x + offset_x;
286 double previous_origin_y = origin_y;
287 double previous_limit_y = origin_y + size_y - 1;
288 double new_origin_y = origin_y + offset_y;
289 double new_limit_y = previous_limit_y + offset_y;
291 double previous_origin_z = origin_z;
292 double previous_limit_z = origin_z + size_z - 1;
293 double new_origin_z = origin_z + offset_z;
294 double new_limit_z = previous_limit_z + offset_z;
297 double lower_limit_x, upper_limit_x;
300 lower_limit_x = previous_origin_x;
301 upper_limit_x = new_origin_x;
305 lower_limit_x = new_limit_x;
306 upper_limit_x = previous_limit_x;
326 condrem_x.
filter (*slice);
330 setIndicesAsNans(world_, indices_x);
335 double lower_limit_y, upper_limit_y;
338 lower_limit_y = previous_origin_y;
339 upper_limit_y = new_origin_y;
343 lower_limit_y = new_limit_y;
344 upper_limit_y = previous_limit_y;
364 condrem_y.
filter (*slice);
368 setIndicesAsNans(world_, indices_y);
372 double lower_limit_z, upper_limit_z;
375 lower_limit_z = previous_origin_z;
376 upper_limit_z = new_origin_z;
380 lower_limit_z = new_limit_z;
381 upper_limit_z = previous_limit_z;
401 condrem_z.
filter (*slice);
405 setIndicesAsNans(world_, indices_z);
411 #define PCL_INSTANTIATE_WorldModel(T) template class PCL_EXPORTS pcl::kinfuLS::WorldModel<T>;
413 #endif // PCL_WORLD_MODEL_IMPL_HPP_