Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
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
51namespace 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>
63 {
64 public:
65
66 using Ptr = shared_ptr<WorldModel<PointT> >;
67 using ConstPtr = shared_ptr<const WorldModel<PointT> >;
68
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.
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
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.
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.
void cleanWorldFromNans()
Remove points with nan values from the world.
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.
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.
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
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
shared_ptr< const Indices > IndicesConstPtr
Definition pcl_base.h:59