39 #include <pcl/common/io.h>
44 namespace occlusion_reasoning
51 template<
typename ModelT,
typename SceneT>
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);
79 std::vector<int> indices_to_keep;
80 indices_to_keep.resize (to_be_filtered->
size ());
83 for (std::size_t i = 0; i < to_be_filtered->
size (); i++)
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);
92 if ((u >=
static_cast<int> (organized_cloud->
width)) || (v >=
static_cast<int> (organized_cloud->
height)) || (u < 0) || (v < 0))
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))
100 float z_oc = organized_cloud->
at (u, v).z;
103 if ((z - z_oc) > threshold)
106 indices_to_keep[keep] =
static_cast<int> (i);
110 indices_to_keep.resize (keep);
117 float threshold,
bool check_invalid_depth =
true)
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);
123 std::vector<int> indices_to_keep;
124 indices_to_keep.resize (to_be_filtered->
size ());
127 for (std::size_t i = 0; i < to_be_filtered->
size (); i++)
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);
136 if ((u >=
static_cast<int> (organized_cloud->
width)) || (v >=
static_cast<int> (organized_cloud->
height)) || (u < 0) || (v < 0))
140 if (check_invalid_depth)
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))
147 float z_oc = organized_cloud->
at (u, v).z;
150 if ((z - z_oc) > threshold)
153 indices_to_keep[keep] =
static_cast<int> (i);
157 indices_to_keep.resize (keep);
164 float threshold,
bool check_invalid_depth =
true)
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);
170 std::vector<int> indices_to_keep;
171 indices_to_keep.resize (to_be_filtered->
size ());
174 for (std::size_t i = 0; i < to_be_filtered->
size (); i++)
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);
183 if ((u >=
static_cast<int> (organized_cloud->
width)) || (v >=
static_cast<int> (organized_cloud->
height)) || (u < 0) || (v < 0))
187 if (check_invalid_depth)
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))
194 float z_oc = organized_cloud->
at (u, v).z;
197 if ((z - z_oc) > threshold)
199 indices_to_keep[keep] =
static_cast<int> (i);
204 indices_to_keep.resize (keep);
211 #ifdef PCL_NO_PRECOMPILE
212 #include <pcl/recognition/impl/hv/occlusion_reasoning.hpp>