Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
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
41namespace 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>
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, pcl::Indices & 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);
78
79 pcl::Indices 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
PointCloud represents the base class in PCL for storing collections of 3D points.
shared_ptr< const PointCloud< PointT > > ConstPtr
shared_ptr< PointCloud< PointT > > Ptr
Class to reason about occlusions.
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:144
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)
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