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);
109 condrem.setCondition (range_condOR);
110 condrem.setInputCloud (newCube);
111 condrem.setKeepOrganized (
false);
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)
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>;
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.
IndicesConstPtr const getRemovedIndices() const
Get the point indices being removed.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
PointCloud represents the base class in PCL for storing collections of 3D points.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
WorldModel maintains a 3D point cloud that can be queried and updated via helper functions.
typename PointCloud::Ptr PointCloudPtr
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
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
void addSlice(const PointCloudPtr new_cloud)
Append a new point cloud (slice) to the world.
typename pcl::ConditionOr< PointT >::Ptr ConditionOrPtr
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.
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform, bool copy_all_fields)
Apply an affine transform defined by an Eigen Transform.
void removeNaNFromPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, std::vector< int > &index)
Removes points with x, y, or z equal to NaN.
shared_ptr< const Indices > IndicesConstPtr
A point structure representing Euclidean xyz coordinates, and the RGB color.