Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
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#include <algorithm>
43
44///////////////////////////////////////////////////////////////////////////////////////////
45template<typename ModelT, typename SceneT>
47 f_ (f), cx_ (resx), cy_ (resy), depth_ (nullptr)
48{
49}
50
51///////////////////////////////////////////////////////////////////////////////////////////
52template<typename ModelT, typename SceneT>
54 f_ (), cx_ (), cy_ (), depth_ (nullptr)
55{
56}
57
58///////////////////////////////////////////////////////////////////////////////////////////
59template<typename ModelT, typename SceneT>
64
65///////////////////////////////////////////////////////////////////////////////////////////
66template<typename ModelT, typename SceneT> void
68 typename pcl::PointCloud<ModelT>::Ptr & filtered, float thres)
69{
70 pcl::Indices indices_to_keep;
71 filter(model, indices_to_keep, thres);
72 pcl::copyPointCloud (*model, indices_to_keep, *filtered);
73}
74
75///////////////////////////////////////////////////////////////////////////////////////////
76template<typename ModelT, typename SceneT> void
78 pcl::Indices & indices_to_keep, float thres)
79{
80
81 float cx, cy;
82 cx = static_cast<float> (cx_) / 2.f - 0.5f;
83 cy = static_cast<float> (cy_) / 2.f - 0.5f;
84
85 indices_to_keep.resize (model->size ());
86 int keep = 0;
87 for (std::size_t i = 0; i < model->size (); i++)
88 {
89 float x = (*model)[i].x;
90 float y = (*model)[i].y;
91 float z = (*model)[i].z;
92 int u = static_cast<int> (f_ * x / z + cx);
93 int v = static_cast<int> (f_ * y / z + cy);
94
95 if (u >= cx_ || v >= cy_ || u < 0 || v < 0)
96 continue;
97
98 //Check if point depth (distance to camera) is greater than the (u,v) meaning that the point is not visible
99 if ((z - thres) > depth_[u * cy_ + v] || !std::isfinite(depth_[u * cy_ + v]))
100 continue;
101
102 indices_to_keep[keep] = static_cast<int> (i);
103 keep++;
104 }
105
106 indices_to_keep.resize (keep);
107}
108
109///////////////////////////////////////////////////////////////////////////////////////////
110template<typename ModelT, typename SceneT> void
112 bool smooth, int wsize)
113{
114 float cx, cy;
115 cx = static_cast<float> (cx_) / 2.f - 0.5f;
116 cy = static_cast<float> (cy_) / 2.f - 0.5f;
117
118 //compute the focal length
119 if (compute_focal)
120 {
121
122 float max_u, max_v, min_u, min_v;
123 max_u = max_v = std::numeric_limits<float>::max () * -1;
124 min_u = min_v = std::numeric_limits<float>::max ();
125
126 for (const auto& point: *scene)
127 {
128 float b_x = point.x / point.z;
129 if (b_x > max_u)
130 max_u = b_x;
131 if (b_x < min_u)
132 min_u = b_x;
133
134 float b_y = point.y / point.z;
135 if (b_y > max_v)
136 max_v = b_y;
137 if (b_y < min_v)
138 min_v = b_y;
139 }
140
141 float maxC = std::max (std::max (std::abs (max_u), std::abs (max_v)), std::max (std::abs (min_u), std::abs (min_v)));
142 f_ = (cx) / maxC;
143 }
144
145 depth_ = new float[cx_ * cy_];
146 std::fill_n(depth_, cx * cy, std::numeric_limits<float>::quiet_NaN());
147
148 for (const auto& point: *scene)
149 {
150 const float& x = point.x;
151 const float& y = point.y;
152 const float& z = point.z;
153 const int u = static_cast<int> (f_ * x / z + cx);
154 const 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]) || (!std::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 = static_cast<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 (std::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 std::copy(depth_smooth, depth_smooth + cx_ * cy_, depth_);
196 delete[] depth_smooth;
197 }
198}
199
200#endif // PCL_RECOGNITION_OCCLUSION_REASONING_HPP_
std::size_t size() const
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
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)
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:142
pcl::PointCloud< ModelT >::Ptr filter(typename pcl::PointCloud< SceneT >::ConstPtr &organized_cloud, typename pcl::PointCloud< ModelT >::ConstPtr &to_be_filtered, float f, float threshold)
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133