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