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>;
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.