39#ifndef PCL_WORLD_MODEL_IMPL_HPP_
40#define PCL_WORLD_MODEL_IMPL_HPP_
42#include <pcl/gpu/kinfu_large_scale/world_model.h>
43#include <pcl/common/transforms.h>
45template <
typename Po
intT>
49 PCL_DEBUG(
"Adding new cloud. Current world contains %zu points.\n",
50 static_cast<std::size_t
>(world_->size()));
52 PCL_DEBUG(
"New slice contains %zu points.\n",
57 PCL_DEBUG(
"World now contains %zu points.\n",
58 static_cast<std::size_t
>(world_->size()));
61template <
typename Po
intT>
114 condrem.setKeepOrganized (
false);
121 Eigen::Affine3f transformation;
122 transformation.translation ()[0] =
newOriginX;
123 transformation.translation ()[1] =
newOriginY;
124 transformation.translation ()[2] =
newOriginZ;
126 transformation.linear ().setIdentity ();
134template <
typename Po
intT>
139 if(world_->points.empty ())
141 PCL_INFO(
"The world is empty, returning nothing\n");
145 PCL_INFO(
"Getting world as cubes. World contains %zu points.\n",
146 static_cast<std::size_t
>(world_->size()));
149 world_->is_dense =
false;
153 PCL_INFO(
"World contains %zu points after nan removal.\n",
154 static_cast<std::size_t
>(world_->size()));
160 PCL_ERROR (
"Size of the cube must be positive and non null (%f given). Setting it to 3.0 meters.\n",
cubeSide);
164 std::cout <<
"cube size is set to " <<
cubeSide << std::endl;
170 PCL_ERROR (
"Overlap ratio must be positive or null (%f given). Setting it to 0.0 procent.\n",
overlap);
175 PCL_ERROR (
"Overlap ratio must be less or equal to 1.0 (%f given). Setting it to 10 procent.\n",
overlap);
184 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);
220 condrem.setInputCloud (world_);
221 condrem.setKeepOrganized(
false);
226 if(!
box->points.empty ())
228 Eigen::Vector3f transform;
235 PCL_INFO (
"Extracted cube was empty, skipping this one.\n");
237 origin.z += cubeSide * step_increment;
239 origin.y += cubeSide * step_increment;
241 origin.x += cubeSide * step_increment;
253 std::cout <<
"returning " << cubes.
size() <<
" cubes" << std::endl;
256template <
typename Po
intT>
260 std::vector<pcl::PCLPointField> fields;
262 float my_nan = std::numeric_limits<float>::quiet_NaN ();
264 for (
int rii = 0; rii < static_cast<int> (indices->size ()); ++rii)
266 std::uint8_t* pt_data =
reinterpret_cast<std::uint8_t*
> (&(*cloud)[(*indices)[rii]]);
267 for (
const auto &field : fields)
268 memcpy (pt_data + field.offset, &my_nan, sizeof (float));
273template <
typename Po
intT>
412#define PCL_INSTANTIATE_WorldModel(T) template class PCL_EXPORTS pcl::kinfuLS::WorldModel<T>;
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
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::Matrix< Scalar, 4, 4 > &transform, bool copy_all_fields)
Apply a rigid transform defined by a 4x4 matrix.
void removeNaNFromPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, Indices &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.