Point Cloud Library (PCL)  1.8.0
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 #ifndef PCL_WORLD_MODEL_H_
40 #define PCL_WORLD_MODEL_H_
41 
42 #include <pcl/common/impl/common.hpp>
43 #include <pcl/octree/octree.h>
44 #include <pcl/octree/octree_impl.h>
45 #include <pcl/filters/extract_indices.h>
46 #include <pcl/filters/filter_indices.h>
47 #include <pcl/filters/crop_box.h>
48 #include <pcl/filters/conditional_removal.h>
49 #include <pcl/point_types.h>
50 #include <pcl/io/pcd_io.h>
51 //#include <pcl/gpu/kinfu_large_scale/tsdf_buffer.h>
52 //#include <boost/graph/buffer_concepts.hpp>
53 
54 
55 namespace pcl
56 {
57  namespace kinfuLS
58  {
59  /** \brief WorldModel maintains a 3D point cloud that can be queried and updated via helper functions.\n
60  * The world is represented as a point cloud.\n
61  * When new points are added to the world, we replace old ones by the newest ones.
62  * This is acheived by setting old points to nan (for speed)
63  * \author Raphael Favier
64  */
65  template <typename PointT>
66  class WorldModel
67  {
68  public:
69 
70  typedef boost::shared_ptr<WorldModel<PointT> > Ptr;
71  typedef boost::shared_ptr<const WorldModel<PointT> > ConstPtr;
72 
74  typedef typename PointCloud::Ptr PointCloudPtr;
76 
80 
82 
83  /** \brief Default constructor for the WorldModel.
84  */
86  world_ (new PointCloud)
87  {
88  world_->is_dense = false;
89  }
90 
91  /** \brief Clear the world.
92  */
93  void reset()
94  {
95  if(world_->points.size () != 0)
96  {
97  PCL_WARN("Clearing world model\n");
98  world_->points.clear ();
99  }
100  }
101 
102  /** \brief Append a new point cloud (slice) to the world.
103  * \param[in] new_cloud the point cloud to add to the world
104  */
105  void addSlice (const PointCloudPtr new_cloud);
106 
107 
108  /** \brief Retreive existing data from the world model, after a shift
109  * \param[in] previous_origin_x global origin of the cube on X axis, before the shift
110  * \param[in] previous_origin_y global origin of the cube on Y axis, before the shift
111  * \param[in] previous_origin_z global origin of the cube on Z axis, before the shift
112  * \param[in] offset_x shift on X, in indices
113  * \param[in] offset_y shift on Y, in indices
114  * \param[in] offset_z shift on Z, in indices
115  * \param[in] volume_x size of the cube, X axis, in indices
116  * \param[in] volume_y size of the cube, Y axis, in indices
117  * \param[in] volume_z size of the cube, Z axis, in indices
118  * \param[out] existing_slice the extracted point cloud representing the slice
119  */
120  void getExistingData(const double previous_origin_x, const double previous_origin_y, const double previous_origin_z,
121  const double offset_x, const double offset_y, const double offset_z,
122  const double volume_x, const double volume_y, const double volume_z, pcl::PointCloud<PointT> &existing_slice);
123 
124  /** \brief Give nan values to the slice of the world
125  * \param[in] origin_x global origin of the cube on X axis, before the shift
126  * \param[in] origin_y global origin of the cube on Y axis, before the shift
127  * \param[in] origin_z global origin of the cube on Z axis, before the shift
128  * \param[in] offset_x shift on X, in indices
129  * \param[in] offset_y shift on Y, in indices
130  * \param[in] offset_z shift on Z, in indices
131  * \param[in] size_x size of the cube, X axis, in indices
132  * \param[in] size_y size of the cube, Y axis, in indices
133  * \param[in] size_z size of the cube, Z axis, in indices
134  */
135  void setSliceAsNans (const double origin_x, const double origin_y, const double origin_z,
136  const double offset_x, const double offset_y, const double offset_z,
137  const int size_x, const int size_y, const int size_z);
138 
139  /** \brief Remove points with nan values from the world.
140  */
142  {
143  world_->is_dense = false;
144  std::vector<int> indices;
145  pcl::removeNaNFromPointCloud (*world_, *world_, indices);
146  }
147 
148  /** \brief Returns the world as a point cloud.
149  */
150  PointCloudPtr getWorld ()
151  {
152  return (world_);
153  }
154 
155  /** \brief Returns the number of points contained in the world.
156  */
157  size_t getWorldSize ()
158  {
159  return (world_->points.size () );
160  }
161 
162  /** \brief Returns the world as two vectors of cubes of size "size" (pointclouds) and transforms
163  * \param[in] size the size of a 3D cube.
164  * \param[out] cubes a vector of point clouds representing each cube (in their original world coordinates).
165  * \param[out] transforms a vector containing the xyz position of each cube in world coordinates.
166  * \param[in] overlap optional overlap (in percent) between each cube (usefull to create overlapped meshes).
167  */
168  void getWorldAsCubes (double size, std::vector<PointCloudPtr> &cubes, std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &transforms, double overlap = 0.0);
169 
170 
171  private:
172 
173  /** \brief cloud containing our world */
174  PointCloudPtr world_;
175 
176  /** \brief set the points which index is in the indices vector to nan
177  * \param[in] cloud the cloud that contains the point to be set to nan
178  * \param[in] indices the vector of indices to set to nan
179  */
180  inline void setIndicesAsNans (PointCloudPtr cloud, IndicesConstPtr indices);
181 
182  };
183  }
184 }
185 
186 #endif // PCL_WORLD_MODEL_H_
void reset()
Clear the world.
Definition: world_model.h:93
pcl::traits::fieldList< PointT >::type FieldList
Definition: world_model.h:81
boost::shared_ptr< ConditionOr< PointT > > Ptr
void cleanWorldFromNans()
Remove points with nan values from the world.
Definition: world_model.h:141
boost::shared_ptr< ConditionAnd< PointT > > Ptr
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.
Definition: filter.hpp:46
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)
Retreive existing data from the world model, after a shift.
Definition: world_model.hpp:60
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.
pcl::ConditionOr< PointT >::Ptr ConditionOrPtr
Definition: world_model.h:78
WorldModel()
Default constructor for the WorldModel.
Definition: world_model.h:85
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
PointCloudPtr getWorld()
Returns the world as a point cloud.
Definition: world_model.h:150
void addSlice(const PointCloudPtr new_cloud)
Append a new point cloud (slice) to the world.
Definition: world_model.hpp:46
size_t getWorldSize()
Returns the number of points contained in the world.
Definition: world_model.h:157
PointCloud::Ptr PointCloudPtr
Definition: world_model.h:74
Defines all the PCL implemented PointT point type structures.
boost::shared_ptr< WorldModel< PointT > > Ptr
Definition: world_model.h:70
boost::shared_ptr< const FieldComparison< PointT > > ConstPtr
boost::shared_ptr< const PointCloud< pcl::pcl::PointXYZI > > ConstPtr
Definition: point_cloud.h:429
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.
pcl::PointCloud< PointT > PointCloud
Definition: world_model.h:73
boost::shared_ptr< const std::vector< int > > IndicesConstPtr
Definition: pcl_base.h:61
pcl::ConditionAnd< PointT >::Ptr ConditionAndPtr
Definition: world_model.h:77
WorldModel maintains a 3D point cloud that can be queried and updated via helper functions.
Definition: world_model.h:66
PointCloud::ConstPtr PointCloudConstPtr
Definition: world_model.h:75
boost::shared_ptr< const WorldModel< PointT > > ConstPtr
Definition: world_model.h:71
pcl::FieldComparison< PointT >::ConstPtr FieldComparisonConstPtr
Definition: world_model.h:79