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