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);
115 condrem.
filter (existing_slice);
117 if(!existing_slice.
points.empty ())
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_
typename pcl::ConditionAnd< pcl::PointXYZI >::Ptr ConditionAndPtr
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void setKeepOrganized(bool val)
Set whether the filtered points should be kept and set to the value given through setUserFilterValue ...
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.
void setCondition(ConditionBasePtr condition)
Set the condition that the filter will use.
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.
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.
void filter(PointCloud &output)
Calls the filtering method and returns the filtered dataset in output.
shared_ptr< const Indices > IndicesConstPtr
ConditionalRemoval filters data that satisfies certain conditions.
typename pcl::ConditionOr< pcl::PointXYZI >::Ptr ConditionOrPtr
void addSlice(const PointCloudPtr new_cloud)
Append a new point cloud (slice) to the world.
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.
typename PointCloud::Ptr PointCloudPtr
The field-based specialization of the comparison object.
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...
IndicesConstPtr const getRemovedIndices() const
Get the point indices being removed.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
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.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
WorldModel maintains a 3D point cloud that can be queried and updated via helper functions.
A point structure representing Euclidean xyz coordinates, and the RGB color.
typename pcl::FieldComparison< pcl::PointXYZI >::ConstPtr FieldComparisonConstPtr