37 #ifndef PCL_RECOGNITION_OCCLUSION_REASONING_HPP_
38 #define PCL_RECOGNITION_OCCLUSION_REASONING_HPP_
40 #include <pcl/recognition/hv/occlusion_reasoning.h>
43 template<
typename ModelT,
typename SceneT>
45 f_ (f), cx_ (resx), cy_ (resy), depth_ (NULL)
50 template<
typename ModelT,
typename SceneT>
52 f_ (), cx_ (), cy_ (), depth_ (NULL)
57 template<
typename ModelT,
typename SceneT>
65 template<
typename ModelT,
typename SceneT>
void
69 std::vector<int> indices_to_keep;
70 filter(model, indices_to_keep, thres);
75 template<
typename ModelT,
typename SceneT>
void
77 std::vector<int> & indices_to_keep,
float thres)
81 cx =
static_cast<float> (cx_) / 2.f - 0.5f;
82 cy =
static_cast<float> (cy_) / 2.f - 0.5f;
84 indices_to_keep.resize (model->
points.size ());
86 for (
size_t i = 0; i < model->
points.size (); i++)
88 float x = model->
points[i].x;
89 float y = model->
points[i].y;
90 float z = model->
points[i].z;
91 int u =
static_cast<int> (f_ * x / z + cx);
92 int v =
static_cast<int> (f_ * y / z + cy);
94 if (u >= cx_ || v >= cy_ || u < 0 || v < 0)
98 if ((z - thres) > depth_[u * cy_ + v] || !pcl_isfinite(depth_[u * cy_ + v]))
101 indices_to_keep[keep] =
static_cast<int> (i);
105 indices_to_keep.resize (keep);
109 template<
typename ModelT,
typename SceneT>
void
111 bool smooth,
int wsize)
114 cx =
static_cast<float> (cx_) / 2.f - 0.5f;
115 cy =
static_cast<float> (cy_) / 2.f - 0.5f;
121 float max_u, max_v, min_u, min_v;
122 max_u = max_v = std::numeric_limits<float>::max () * -1;
123 min_u = min_v = std::numeric_limits<float>::max ();
125 for (
size_t i = 0; i < scene->
points.size (); i++)
140 float maxC = std::max (std::max (std::abs (max_u), std::abs (max_v)), std::max (std::abs (min_u), std::abs (min_v)));
144 depth_ =
new float[cx_ * cy_];
145 for (
int i = 0; i < (cx_ * cy_); i++)
146 depth_[i] = std::numeric_limits<float>::quiet_NaN ();
148 for (
size_t i = 0; i < scene->
points.size (); i++)
150 float x = scene->
points[i].x;
151 float y = scene->
points[i].y;
152 float z = scene->
points[i].z;
153 int u =
static_cast<int> (f_ * x / z + cx);
154 int v =
static_cast<int> (f_ * y / z + cy);
156 if (u >= cx_ || v >= cy_ || u < 0 || v < 0)
159 if ((z < depth_[u * cy_ + v]) || (!pcl_isfinite(depth_[u * cy_ + v])))
160 depth_[u * cx_ + v] = z;
167 int ws2 = 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 ();
172 for (
int u = ws2; u < (cx_ - ws2); u++)
174 for (
int v = ws2; v < (cy_ - ws2); v++)
176 float min = std::numeric_limits<float>::max ();
177 for (
int j = (u - ws2); j <= (u + ws2); j++)
179 for (
int i = (v - ws2); i <= (v + ws2); i++)
181 if (pcl_isfinite(depth_[j * cx_ + i]) && (depth_[j * cx_ + i] < min))
183 min = depth_[j * cx_ + i];
188 if (min < (std::numeric_limits<float>::max () - 0.1))
190 depth_smooth[u * cx_ + v] = min;
195 memcpy (depth_, depth_smooth,
sizeof(
float) * cx_ * cy_);
196 delete[] depth_smooth;
200 #endif // PCL_RECOGNITION_OCCLUSION_REASONING_HPP_