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_ (nullptr)
50 template<
typename ModelT,
typename SceneT>
52 f_ (), cx_ (), cy_ (), depth_ (nullptr)
57 template<
typename ModelT,
typename SceneT>
64 template<
typename ModelT,
typename SceneT>
void
68 std::vector<int> indices_to_keep;
69 filter(model, indices_to_keep, thres);
74 template<
typename ModelT,
typename SceneT>
void
76 std::vector<int> & indices_to_keep,
float thres)
80 cx =
static_cast<float> (cx_) / 2.f - 0.5f;
81 cy =
static_cast<float> (cy_) / 2.f - 0.5f;
83 indices_to_keep.resize (model->
points.size ());
85 for (std::size_t i = 0; i < model->
points.size (); i++)
87 float x = model->
points[i].x;
88 float y = model->
points[i].y;
89 float z = model->
points[i].z;
90 int u =
static_cast<int> (f_ * x / z + cx);
91 int v =
static_cast<int> (f_ * y / z + cy);
93 if (u >= cx_ || v >= cy_ || u < 0 || v < 0)
97 if ((z - thres) > depth_[u * cy_ + v] || !std::isfinite(depth_[u * cy_ + v]))
100 indices_to_keep[keep] =
static_cast<int> (i);
104 indices_to_keep.resize (keep);
108 template<
typename ModelT,
typename SceneT>
void
110 bool smooth,
int wsize)
113 cx =
static_cast<float> (cx_) / 2.f - 0.5f;
114 cy =
static_cast<float> (cy_) / 2.f - 0.5f;
120 float max_u, max_v, min_u, min_v;
121 max_u = max_v = std::numeric_limits<float>::max () * -1;
122 min_u = min_v = std::numeric_limits<float>::max ();
124 for (std::size_t i = 0; i < scene->
points.size (); i++)
139 float maxC = std::max (std::max (std::abs (max_u), std::abs (max_v)), std::max (std::abs (min_u), std::abs (min_v)));
143 depth_ =
new float[cx_ * cy_];
144 for (
int i = 0; i < (cx_ * cy_); i++)
145 depth_[i] = std::numeric_limits<float>::quiet_NaN ();
147 for (std::size_t i = 0; i < scene->
points.size (); i++)
149 float x = scene->
points[i].x;
150 float y = scene->
points[i].y;
151 float z = scene->
points[i].z;
152 int u =
static_cast<int> (f_ * x / z + cx);
153 int v =
static_cast<int> (f_ * y / z + cy);
155 if (u >= cx_ || v >= cy_ || u < 0 || v < 0)
158 if ((z < depth_[u * cy_ + v]) || (!std::isfinite(depth_[u * cy_ + v])))
159 depth_[u * cx_ + v] = z;
166 int ws2 = int (std::floor (static_cast<float> (ws) / 2.f));
167 float * depth_smooth =
new float[cx_ * cy_];
168 for (
int i = 0; i < (cx_ * cy_); i++)
169 depth_smooth[i] = std::numeric_limits<float>::quiet_NaN ();
171 for (
int u = ws2; u < (cx_ - ws2); u++)
173 for (
int v = ws2; v < (cy_ - ws2); v++)
175 float min = std::numeric_limits<float>::max ();
176 for (
int j = (u - ws2); j <= (u + ws2); j++)
178 for (
int i = (v - ws2); i <= (v + ws2); i++)
180 if (std::isfinite(depth_[j * cx_ + i]) && (depth_[j * cx_ + i] < min))
182 min = depth_[j * cx_ + i];
187 if (min < (std::numeric_limits<float>::max () - 0.1))
189 depth_smooth[u * cx_ + v] = min;
194 memcpy (depth_, depth_smooth,
sizeof(
float) * cx_ * cy_);
195 delete[] depth_smooth;
199 #endif // PCL_RECOGNITION_OCCLUSION_REASONING_HPP_
void computeDepthMap(typename pcl::PointCloud< SceneT >::ConstPtr &scene, bool compute_focal=false, bool smooth=false, int wsize=3)
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void filter(typename pcl::PointCloud< ModelT >::ConstPtr &model, typename pcl::PointCloud< ModelT >::Ptr &filtered, float thres=0.01)
shared_ptr< PointCloud< PointT > > Ptr
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.
shared_ptr< const PointCloud< PointT > > ConstPtr