Point Cloud Library (PCL)  1.11.1-dev
occlusion_reasoning.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 
37 #pragma once
38 
39 #include <pcl/common/io.h>
40 
41 namespace pcl
42 {
43 
44  namespace occlusion_reasoning
45  {
46  /**
47  * \brief Class to reason about occlusions
48  * \author Aitor Aldoma
49  */
50 
51  template<typename ModelT, typename SceneT>
52  class ZBuffering
53  {
54  private:
55  float f_;
56  int cx_, cy_;
57  float * depth_;
58 
59  public:
60 
61  ZBuffering ();
62  ZBuffering (int resx, int resy, float f);
63  ~ZBuffering ();
64  void
65  computeDepthMap (typename pcl::PointCloud<SceneT>::ConstPtr & scene, bool compute_focal = false, bool smooth = false, int wsize = 3);
66  void
67  filter (typename pcl::PointCloud<ModelT>::ConstPtr & model, typename pcl::PointCloud<ModelT>::Ptr & filtered, float thres = 0.01);
68  void filter (typename pcl::PointCloud<ModelT>::ConstPtr & model, std::vector<int> & indices, float thres = 0.01);
69  };
70 
71  template<typename ModelT, typename SceneT> typename pcl::PointCloud<ModelT>::Ptr
72  filter (typename pcl::PointCloud<SceneT>::ConstPtr & organized_cloud, typename pcl::PointCloud<ModelT>::ConstPtr & to_be_filtered, float f,
73  float threshold)
74  {
75  float cx = (static_cast<float> (organized_cloud->width) / 2.f - 0.5f);
76  float cy = (static_cast<float> (organized_cloud->height) / 2.f - 0.5f);
77  typename pcl::PointCloud<ModelT>::Ptr filtered (new pcl::PointCloud<ModelT> ());
78 
79  std::vector<int> indices_to_keep;
80  indices_to_keep.resize (to_be_filtered->size ());
81 
82  int keep = 0;
83  for (std::size_t i = 0; i < to_be_filtered->size (); i++)
84  {
85  float x = (*to_be_filtered)[i].x;
86  float y = (*to_be_filtered)[i].y;
87  float z = (*to_be_filtered)[i].z;
88  int u = static_cast<int> (f * x / z + cx);
89  int v = static_cast<int> (f * y / z + cy);
90 
91  //Not out of bounds
92  if ((u >= static_cast<int> (organized_cloud->width)) || (v >= static_cast<int> (organized_cloud->height)) || (u < 0) || (v < 0))
93  continue;
94 
95  //Check for invalid depth
96  if (!std::isfinite (organized_cloud->at (u, v).x) || !std::isfinite (organized_cloud->at (u, v).y)
97  || !std::isfinite (organized_cloud->at (u, v).z))
98  continue;
99 
100  float z_oc = organized_cloud->at (u, v).z;
101 
102  //Check if point depth (distance to camera) is greater than the (u,v)
103  if ((z - z_oc) > threshold)
104  continue;
105 
106  indices_to_keep[keep] = static_cast<int> (i);
107  keep++;
108  }
109 
110  indices_to_keep.resize (keep);
111  pcl::copyPointCloud (*to_be_filtered, indices_to_keep, *filtered);
112  return filtered;
113  }
114 
115  template<typename ModelT, typename SceneT> typename pcl::PointCloud<ModelT>::Ptr
116  filter (typename pcl::PointCloud<SceneT>::Ptr & organized_cloud, typename pcl::PointCloud<ModelT>::Ptr & to_be_filtered, float f,
117  float threshold, bool check_invalid_depth = true)
118  {
119  float cx = (static_cast<float> (organized_cloud->width) / 2.f - 0.5f);
120  float cy = (static_cast<float> (organized_cloud->height) / 2.f - 0.5f);
121  typename pcl::PointCloud<ModelT>::Ptr filtered (new pcl::PointCloud<ModelT> ());
122 
123  std::vector<int> indices_to_keep;
124  indices_to_keep.resize (to_be_filtered->size ());
125 
126  int keep = 0;
127  for (std::size_t i = 0; i < to_be_filtered->size (); i++)
128  {
129  float x = (*to_be_filtered)[i].x;
130  float y = (*to_be_filtered)[i].y;
131  float z = (*to_be_filtered)[i].z;
132  int u = static_cast<int> (f * x / z + cx);
133  int v = static_cast<int> (f * y / z + cy);
134 
135  //Not out of bounds
136  if ((u >= static_cast<int> (organized_cloud->width)) || (v >= static_cast<int> (organized_cloud->height)) || (u < 0) || (v < 0))
137  continue;
138 
139  //Check for invalid depth
140  if (check_invalid_depth)
141  {
142  if (!std::isfinite (organized_cloud->at (u, v).x) || !std::isfinite (organized_cloud->at (u, v).y)
143  || !std::isfinite (organized_cloud->at (u, v).z))
144  continue;
145  }
146 
147  float z_oc = organized_cloud->at (u, v).z;
148 
149  //Check if point depth (distance to camera) is greater than the (u,v)
150  if ((z - z_oc) > threshold)
151  continue;
152 
153  indices_to_keep[keep] = static_cast<int> (i);
154  keep++;
155  }
156 
157  indices_to_keep.resize (keep);
158  pcl::copyPointCloud (*to_be_filtered, indices_to_keep, *filtered);
159  return filtered;
160  }
161 
162  template<typename ModelT, typename SceneT> typename pcl::PointCloud<ModelT>::Ptr
163  getOccludedCloud (typename pcl::PointCloud<SceneT>::Ptr & organized_cloud, typename pcl::PointCloud<ModelT>::Ptr & to_be_filtered, float f,
164  float threshold, bool check_invalid_depth = true)
165  {
166  float cx = (static_cast<float> (organized_cloud->width) / 2.f - 0.5f);
167  float cy = (static_cast<float> (organized_cloud->height) / 2.f - 0.5f);
168  typename pcl::PointCloud<ModelT>::Ptr filtered (new pcl::PointCloud<ModelT> ());
169 
170  std::vector<int> indices_to_keep;
171  indices_to_keep.resize (to_be_filtered->size ());
172 
173  int keep = 0;
174  for (std::size_t i = 0; i < to_be_filtered->size (); i++)
175  {
176  float x = (*to_be_filtered)[i].x;
177  float y = (*to_be_filtered)[i].y;
178  float z = (*to_be_filtered)[i].z;
179  int u = static_cast<int> (f * x / z + cx);
180  int v = static_cast<int> (f * y / z + cy);
181 
182  //Out of bounds
183  if ((u >= static_cast<int> (organized_cloud->width)) || (v >= static_cast<int> (organized_cloud->height)) || (u < 0) || (v < 0))
184  continue;
185 
186  //Check for invalid depth
187  if (check_invalid_depth)
188  {
189  if (!std::isfinite (organized_cloud->at (u, v).x) || !std::isfinite (organized_cloud->at (u, v).y)
190  || !std::isfinite (organized_cloud->at (u, v).z))
191  continue;
192  }
193 
194  float z_oc = organized_cloud->at (u, v).z;
195 
196  //Check if point depth (distance to camera) is greater than the (u,v)
197  if ((z - z_oc) > threshold)
198  {
199  indices_to_keep[keep] = static_cast<int> (i);
200  keep++;
201  }
202  }
203 
204  indices_to_keep.resize (keep);
205  pcl::copyPointCloud (*to_be_filtered, indices_to_keep, *filtered);
206  return filtered;
207  }
208  }
209 }
210 
211 #ifdef PCL_NO_PRECOMPILE
212 #include <pcl/recognition/impl/hv/occlusion_reasoning.hpp>
213 #endif
pcl
Definition: convolution.h:46
pcl::occlusion_reasoning::ZBuffering::~ZBuffering
~ZBuffering()
Definition: occlusion_reasoning.hpp:60
pcl::PointCloud::height
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:394
pcl::occlusion_reasoning::getOccludedCloud
pcl::PointCloud< ModelT >::Ptr getOccludedCloud(typename pcl::PointCloud< SceneT >::Ptr &organized_cloud, typename pcl::PointCloud< ModelT >::Ptr &to_be_filtered, float f, float threshold, bool check_invalid_depth=true)
Definition: occlusion_reasoning.h:163
pcl::PointCloud< ModelT >
pcl::copyPointCloud
void copyPointCloud(const pcl::PointCloud< PointInT > &cloud_in, pcl::PointCloud< PointOutT > &cloud_out)
Copy all the fields from a given point cloud into a new point cloud.
Definition: io.hpp:122
pcl::PointCloud::at
const PointT & at(int column, int row) const
Obtain the point given by the (column, row) coordinates.
Definition: point_cloud.h:262
pcl::occlusion_reasoning::ZBuffering::ZBuffering
ZBuffering()
Definition: occlusion_reasoning.hpp:53
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:392
pcl::PointCloud::size
std::size_t size() const
Definition: point_cloud.h:437
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:407
pcl::occlusion_reasoning::ZBuffering::computeDepthMap
void computeDepthMap(typename pcl::PointCloud< SceneT >::ConstPtr &scene, bool compute_focal=false, bool smooth=false, int wsize=3)
Definition: occlusion_reasoning.hpp:111
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:408
pcl::occlusion_reasoning::filter
pcl::PointCloud< ModelT >::Ptr filter(typename pcl::PointCloud< SceneT >::ConstPtr &organized_cloud, typename pcl::PointCloud< ModelT >::ConstPtr &to_be_filtered, float f, float threshold)
Definition: occlusion_reasoning.h:72
pcl::occlusion_reasoning::ZBuffering::filter
void filter(typename pcl::PointCloud< ModelT >::ConstPtr &model, typename pcl::PointCloud< ModelT >::Ptr &filtered, float thres=0.01)
Definition: occlusion_reasoning.hpp:67
pcl::occlusion_reasoning::ZBuffering
Class to reason about occlusions.
Definition: occlusion_reasoning.h:52