Point Cloud Library (PCL)  1.14.1-dev
world_model.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * Author: Raphael Favier, Technical University Eindhoven, (r.mysurname <aT> tue.nl)
37  */
38 
39 #pragma once
40 
41 #include <pcl/common/impl/common.hpp>
42 #include <pcl/filters/extract_indices.h>
43 #include <pcl/filters/filter_indices.h>
44 #include <pcl/filters/conditional_removal.h>
45 #include <pcl/point_types.h>
46 #include <pcl/io/pcd_io.h>
47 //#include <pcl/gpu/kinfu_large_scale/tsdf_buffer.h>
48 //#include <boost/graph/buffer_concepts.hpp>
49 
50 
51 namespace pcl
52 {
53  namespace kinfuLS
54  {
55  /** \brief WorldModel maintains a 3D point cloud that can be queried and updated via helper functions.\n
56  * The world is represented as a point cloud.\n
57  * When new points are added to the world, we replace old ones by the newest ones.
58  * This is achieved by setting old points to nan (for speed)
59  * \author Raphael Favier
60  */
61  template <typename PointT>
62  class WorldModel
63  {
64  public:
65 
66  using Ptr = shared_ptr<WorldModel<PointT> >;
67  using ConstPtr = shared_ptr<const WorldModel<PointT> >;
68 
70  using PointCloudPtr = typename PointCloud::Ptr;
72 
76 
77  using FieldList = typename pcl::traits::fieldList<PointT>::type;
78 
79  /** \brief Default constructor for the WorldModel.
80  */
82  world_ (new PointCloud)
83  {
84  world_->is_dense = false;
85  }
86 
87  /** \brief Clear the world.
88  */
89  void reset()
90  {
91  if(!world_->points.empty ())
92  {
93  PCL_WARN("Clearing world model\n");
94  world_->points.clear ();
95  }
96  }
97 
98  /** \brief Append a new point cloud (slice) to the world.
99  * \param[in] new_cloud the point cloud to add to the world
100  */
101  void addSlice (const PointCloudPtr new_cloud);
102 
103 
104  /** \brief Retrieve existing data from the world model, after a shift
105  * \param[in] previous_origin_x global origin of the cube on X axis, before the shift
106  * \param[in] previous_origin_y global origin of the cube on Y axis, before the shift
107  * \param[in] previous_origin_z global origin of the cube on Z axis, before the shift
108  * \param[in] offset_x shift on X, in indices
109  * \param[in] offset_y shift on Y, in indices
110  * \param[in] offset_z shift on Z, in indices
111  * \param[in] volume_x size of the cube, X axis, in indices
112  * \param[in] volume_y size of the cube, Y axis, in indices
113  * \param[in] volume_z size of the cube, Z axis, in indices
114  * \param[out] existing_slice the extracted point cloud representing the slice
115  */
116  void getExistingData(const double previous_origin_x, const double previous_origin_y, const double previous_origin_z,
117  const double offset_x, const double offset_y, const double offset_z,
118  const double volume_x, const double volume_y, const double volume_z, pcl::PointCloud<PointT> &existing_slice);
119 
120  /** \brief Give nan values to the slice of the world
121  * \param[in] origin_x global origin of the cube on X axis, before the shift
122  * \param[in] origin_y global origin of the cube on Y axis, before the shift
123  * \param[in] origin_z global origin of the cube on Z axis, before the shift
124  * \param[in] offset_x shift on X, in indices
125  * \param[in] offset_y shift on Y, in indices
126  * \param[in] offset_z shift on Z, in indices
127  * \param[in] size_x size of the cube, X axis, in indices
128  * \param[in] size_y size of the cube, Y axis, in indices
129  * \param[in] size_z size of the cube, Z axis, in indices
130  */
131  void setSliceAsNans (const double origin_x, const double origin_y, const double origin_z,
132  const double offset_x, const double offset_y, const double offset_z,
133  const int size_x, const int size_y, const int size_z);
134 
135  /** \brief Remove points with nan values from the world.
136  */
138  {
139  world_->is_dense = false;
140  pcl::Indices indices;
141  pcl::removeNaNFromPointCloud (*world_, *world_, indices);
142  }
143 
144  /** \brief Returns the world as a point cloud.
145  */
147  {
148  return (world_);
149  }
150 
151  /** \brief Returns the number of points contained in the world.
152  */
153  std::size_t getWorldSize ()
154  {
155  return (world_->size () );
156  }
157 
158  /** \brief Returns the world as two vectors of cubes of size "size" (pointclouds) and transforms
159  * \param[in] size the size of a 3D cube.
160  * \param[out] cubes a vector of point clouds representing each cube (in their original world coordinates).
161  * \param[out] transforms a vector containing the xyz position of each cube in world coordinates.
162  * \param[in] overlap optional overlap (in percent) between each cube (useful to create overlapped meshes).
163  */
164  void getWorldAsCubes (double size, std::vector<PointCloudPtr> &cubes, std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &transforms, double overlap = 0.0);
165 
166 
167  private:
168 
169  /** \brief cloud containing our world */
170  PointCloudPtr world_;
171 
172  /** \brief set the points which index is in the indices vector to nan
173  * \param[in] cloud the cloud that contains the point to be set to nan
174  * \param[in] indices the vector of indices to set to nan
175  */
176  inline void setIndicesAsNans (PointCloudPtr cloud, IndicesConstPtr indices);
177 
178  };
179  }
180 }
shared_ptr< ConditionAnd< PointT > > Ptr
shared_ptr< ConditionOr< PointT > > Ptr
shared_ptr< const FieldComparison< PointT > > ConstPtr
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
WorldModel maintains a 3D point cloud that can be queried and updated via helper functions.
Definition: world_model.h:63
typename PointCloud::Ptr PointCloudPtr
Definition: world_model.h:70
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: world_model.h:71
shared_ptr< WorldModel< PointT > > Ptr
Definition: world_model.h:66
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
Definition: world_model.h:75
std::size_t getWorldSize()
Returns the number of points contained in the world.
Definition: world_model.h:153
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.
Definition: world_model.hpp:63
void cleanWorldFromNans()
Remove points with nan values from the world.
Definition: world_model.h:137
typename pcl::ConditionAnd< PointT >::Ptr ConditionAndPtr
Definition: world_model.h:73
void reset()
Clear the world.
Definition: world_model.h:89
void addSlice(const PointCloudPtr new_cloud)
Append a new point cloud (slice) to the world.
Definition: world_model.hpp:47
WorldModel()
Default constructor for the WorldModel.
Definition: world_model.h:81
typename pcl::traits::fieldList< PointT >::type FieldList
Definition: world_model.h:77
typename pcl::ConditionOr< PointT >::Ptr ConditionOrPtr
Definition: world_model.h:74
PointCloudPtr getWorld()
Returns the world as a point cloud.
Definition: world_model.h:146
shared_ptr< const WorldModel< PointT > > ConstPtr
Definition: world_model.h:67
Defines all the PCL implemented PointT point type structures.
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.
Definition: filter.hpp:46
shared_ptr< const Indices > IndicesConstPtr
Definition: pcl_base.h:59
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133