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