40 #include <pcl/common/transforms.h>
41 #include <pcl/common/io.h>
46 namespace occlusion_reasoning
53 template<
typename ModelT,
typename SceneT>
77 float cx = (
static_cast<float> (organized_cloud->
width) / 2.f - 0.5f);
78 float cy = (
static_cast<float> (organized_cloud->
height) / 2.f - 0.5f);
81 std::vector<int> indices_to_keep;
82 indices_to_keep.resize (to_be_filtered->
points.size ());
85 for (std::size_t i = 0; i < to_be_filtered->
points.size (); i++)
87 float x = to_be_filtered->
points[i].x;
88 float y = to_be_filtered->
points[i].y;
89 float z = to_be_filtered->
points[i].z;
90 int u =
static_cast<int> (f * x / z + cx);
91 int v =
static_cast<int> (f * y / z + cy);
94 if ((u >=
static_cast<int> (organized_cloud->
width)) || (v >=
static_cast<int> (organized_cloud->
height)) || (u < 0) || (v < 0))
98 if (!std::isfinite (organized_cloud->
at (u, v).x) || !std::isfinite (organized_cloud->
at (u, v).y)
99 || !std::isfinite (organized_cloud->
at (u, v).z))
102 float z_oc = organized_cloud->
at (u, v).z;
105 if ((z - z_oc) > threshold)
108 indices_to_keep[keep] =
static_cast<int> (i);
112 indices_to_keep.resize (keep);
119 float threshold,
bool check_invalid_depth =
true)
121 float cx = (
static_cast<float> (organized_cloud->
width) / 2.f - 0.5f);
122 float cy = (
static_cast<float> (organized_cloud->
height) / 2.f - 0.5f);
125 std::vector<int> indices_to_keep;
126 indices_to_keep.resize (to_be_filtered->
points.size ());
129 for (std::size_t i = 0; i < to_be_filtered->
points.size (); i++)
131 float x = to_be_filtered->
points[i].x;
132 float y = to_be_filtered->
points[i].y;
133 float z = to_be_filtered->
points[i].z;
134 int u =
static_cast<int> (f * x / z + cx);
135 int v =
static_cast<int> (f * y / z + cy);
138 if ((u >=
static_cast<int> (organized_cloud->
width)) || (v >=
static_cast<int> (organized_cloud->
height)) || (u < 0) || (v < 0))
142 if (check_invalid_depth)
144 if (!std::isfinite (organized_cloud->
at (u, v).x) || !std::isfinite (organized_cloud->
at (u, v).y)
145 || !std::isfinite (organized_cloud->
at (u, v).z))
149 float z_oc = organized_cloud->
at (u, v).z;
152 if ((z - z_oc) > threshold)
155 indices_to_keep[keep] =
static_cast<int> (i);
159 indices_to_keep.resize (keep);
166 float threshold,
bool check_invalid_depth =
true)
168 float cx = (
static_cast<float> (organized_cloud->
width) / 2.f - 0.5f);
169 float cy = (
static_cast<float> (organized_cloud->
height) / 2.f - 0.5f);
172 std::vector<int> indices_to_keep;
173 indices_to_keep.resize (to_be_filtered->
points.size ());
176 for (std::size_t i = 0; i < to_be_filtered->
points.size (); i++)
178 float x = to_be_filtered->
points[i].x;
179 float y = to_be_filtered->
points[i].y;
180 float z = to_be_filtered->
points[i].z;
181 int u =
static_cast<int> (f * x / z + cx);
182 int v =
static_cast<int> (f * y / z + cy);
185 if ((u >=
static_cast<int> (organized_cloud->
width)) || (v >=
static_cast<int> (organized_cloud->
height)) || (u < 0) || (v < 0))
189 if (check_invalid_depth)
191 if (!std::isfinite (organized_cloud->
at (u, v).x) || !std::isfinite (organized_cloud->
at (u, v).y)
192 || !std::isfinite (organized_cloud->
at (u, v).z))
196 float z_oc = organized_cloud->
at (u, v).z;
199 if ((z - z_oc) > threshold)
201 indices_to_keep[keep] =
static_cast<int> (i);
206 indices_to_keep.resize (keep);
213 #ifdef PCL_NO_PRECOMPILE
214 #include <pcl/recognition/impl/hv/occlusion_reasoning.hpp>