Point Cloud Library (PCL)  1.9.1-dev
occlusion_reasoning.hpp
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 #ifndef PCL_RECOGNITION_OCCLUSION_REASONING_HPP_
38 #define PCL_RECOGNITION_OCCLUSION_REASONING_HPP_
39 
40 #include <pcl/recognition/hv/occlusion_reasoning.h>
41 
42 ///////////////////////////////////////////////////////////////////////////////////////////
43 template<typename ModelT, typename SceneT>
45  f_ (f), cx_ (resx), cy_ (resy), depth_ (nullptr)
46 {
47 }
48 
49 ///////////////////////////////////////////////////////////////////////////////////////////
50 template<typename ModelT, typename SceneT>
52  f_ (), cx_ (), cy_ (), depth_ (nullptr)
53 {
54 }
55 
56 ///////////////////////////////////////////////////////////////////////////////////////////
57 template<typename ModelT, typename SceneT>
59 {
60  delete[] depth_;
61 }
62 
63 ///////////////////////////////////////////////////////////////////////////////////////////
64 template<typename ModelT, typename SceneT> void
66  typename pcl::PointCloud<ModelT>::Ptr & filtered, float thres)
67 {
68  std::vector<int> indices_to_keep;
69  filter(model, indices_to_keep, thres);
70  pcl::copyPointCloud (*model, indices_to_keep, *filtered);
71 }
72 
73 ///////////////////////////////////////////////////////////////////////////////////////////
74 template<typename ModelT, typename SceneT> void
76  std::vector<int> & indices_to_keep, float thres)
77 {
78 
79  float cx, cy;
80  cx = static_cast<float> (cx_) / 2.f - 0.5f;
81  cy = static_cast<float> (cy_) / 2.f - 0.5f;
82 
83  indices_to_keep.resize (model->points.size ());
84  int keep = 0;
85  for (size_t i = 0; i < model->points.size (); i++)
86  {
87  float x = model->points[i].x;
88  float y = model->points[i].y;
89  float z = model->points[i].z;
90  int u = static_cast<int> (f_ * x / z + cx);
91  int v = static_cast<int> (f_ * y / z + cy);
92 
93  if (u >= cx_ || v >= cy_ || u < 0 || v < 0)
94  continue;
95 
96  //Check if point depth (distance to camera) is greater than the (u,v) meaning that the point is not visible
97  if ((z - thres) > depth_[u * cy_ + v] || !std::isfinite(depth_[u * cy_ + v]))
98  continue;
99 
100  indices_to_keep[keep] = static_cast<int> (i);
101  keep++;
102  }
103 
104  indices_to_keep.resize (keep);
105 }
106 
107 ///////////////////////////////////////////////////////////////////////////////////////////
108 template<typename ModelT, typename SceneT> void
110  bool smooth, int wsize)
111 {
112  float cx, cy;
113  cx = static_cast<float> (cx_) / 2.f - 0.5f;
114  cy = static_cast<float> (cy_) / 2.f - 0.5f;
115 
116  //compute the focal length
117  if (compute_focal)
118  {
119 
120  float max_u, max_v, min_u, min_v;
121  max_u = max_v = std::numeric_limits<float>::max () * -1;
122  min_u = min_v = std::numeric_limits<float>::max ();
123 
124  for (size_t i = 0; i < scene->points.size (); i++)
125  {
126  float b_x = scene->points[i].x / scene->points[i].z;
127  if (b_x > max_u)
128  max_u = b_x;
129  if (b_x < min_u)
130  min_u = b_x;
131 
132  float b_y = scene->points[i].y / scene->points[i].z;
133  if (b_y > max_v)
134  max_v = b_y;
135  if (b_y < min_v)
136  min_v = b_y;
137  }
138 
139  float maxC = std::max (std::max (std::abs (max_u), std::abs (max_v)), std::max (std::abs (min_u), std::abs (min_v)));
140  f_ = (cx) / maxC;
141  }
142 
143  depth_ = new float[cx_ * cy_];
144  for (int i = 0; i < (cx_ * cy_); i++)
145  depth_[i] = std::numeric_limits<float>::quiet_NaN ();
146 
147  for (size_t i = 0; i < scene->points.size (); i++)
148  {
149  float x = scene->points[i].x;
150  float y = scene->points[i].y;
151  float z = scene->points[i].z;
152  int u = static_cast<int> (f_ * x / z + cx);
153  int v = static_cast<int> (f_ * y / z + cy);
154 
155  if (u >= cx_ || v >= cy_ || u < 0 || v < 0)
156  continue;
157 
158  if ((z < depth_[u * cy_ + v]) || (!std::isfinite(depth_[u * cy_ + v])))
159  depth_[u * cx_ + v] = z;
160  }
161 
162  if (smooth)
163  {
164  //Dilate and smooth the depth map
165  int ws = wsize;
166  int ws2 = int (std::floor (static_cast<float> (ws) / 2.f));
167  float * depth_smooth = new float[cx_ * cy_];
168  for (int i = 0; i < (cx_ * cy_); i++)
169  depth_smooth[i] = std::numeric_limits<float>::quiet_NaN ();
170 
171  for (int u = ws2; u < (cx_ - ws2); u++)
172  {
173  for (int v = ws2; v < (cy_ - ws2); v++)
174  {
175  float min = std::numeric_limits<float>::max ();
176  for (int j = (u - ws2); j <= (u + ws2); j++)
177  {
178  for (int i = (v - ws2); i <= (v + ws2); i++)
179  {
180  if (std::isfinite(depth_[j * cx_ + i]) && (depth_[j * cx_ + i] < min))
181  {
182  min = depth_[j * cx_ + i];
183  }
184  }
185  }
186 
187  if (min < (std::numeric_limits<float>::max () - 0.1))
188  {
189  depth_smooth[u * cx_ + v] = min;
190  }
191  }
192  }
193 
194  memcpy (depth_, depth_smooth, sizeof(float) * cx_ * cy_);
195  delete[] depth_smooth;
196  }
197 }
198 
199 #endif // PCL_RECOGNITION_OCCLUSION_REASONING_HPP_
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:423
PCL_EXPORTS void copyPointCloud(const pcl::PCLPointCloud2 &cloud_in, const std::vector< int > &indices, pcl::PCLPointCloud2 &cloud_out)
Extract the indices of a given point cloud as a new point cloud.
void computeDepthMap(typename pcl::PointCloud< SceneT >::ConstPtr &scene, bool compute_focal=false, bool smooth=false, int wsize=3)
void filter(typename pcl::PointCloud< ModelT >::ConstPtr &model, typename pcl::PointCloud< ModelT >::Ptr &filtered, float thres=0.01)
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:441
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:442