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);
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);