37#ifndef PCL_RECOGNITION_OCCLUSION_REASONING_HPP_
38#define PCL_RECOGNITION_OCCLUSION_REASONING_HPP_
40#include <pcl/recognition/hv/occlusion_reasoning.h>
45template<
typename ModelT,
typename SceneT>
52template<
typename ModelT,
typename SceneT>
54 f_ (), cx_ (), cy_ (), depth_ (
nullptr)
59template<
typename ModelT,
typename SceneT>
66template<
typename ModelT,
typename SceneT>
void
76template<
typename ModelT,
typename SceneT>
void
82 cx =
static_cast<float> (cx_) / 2.f - 0.5f;
83 cy =
static_cast<float> (cy_) / 2.f - 0.5f;
87 for (std::size_t i = 0; i <
model->
size (); i++)
89 float x = (*model)[i].x;
90 float y = (*model)[i].y;
91 float z = (*model)[i].z;
92 int u =
static_cast<int> (f_ * x / z + cx);
93 int v =
static_cast<int> (f_ * y / z + cy);
95 if (u >= cx_ || v >= cy_ || u < 0 || v < 0)
99 if ((z -
thres) > depth_[u * cy_ + v] || !std::isfinite(depth_[u * cy_ + v]))
110template<
typename ModelT,
typename SceneT>
void
112 bool smooth,
int wsize)
115 cx =
static_cast<float> (cx_) / 2.f - 0.5f;
116 cy =
static_cast<float> (cy_) / 2.f - 0.5f;
123 max_u =
max_v = std::numeric_limits<float>::max () * -1;
124 min_u =
min_v = std::numeric_limits<float>::max ();
126 for (
const auto& point: *
scene)
128 float b_x = point.x / point.z;
134 float b_y = point.y / point.z;
141 float maxC = std::max (std::max (std::abs (
max_u), std::abs (
max_v)), std::max (std::abs (
min_u), std::abs (
min_v)));
145 depth_ =
new float[cx_ * cy_];
146 std::fill_n(depth_, cx * cy, std::numeric_limits<float>::quiet_NaN());
148 for (
const auto& point: *
scene)
150 const float& x = point.x;
151 const float& y = point.y;
152 const float& z = point.z;
153 const int u =
static_cast<int> (f_ * x / z + cx);
154 const 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]) || (!std::isfinite(depth_[u * cy_ + v])))
160 depth_[u * cx_ + v] = z;
167 int ws2 =
int (std::floor (
static_cast<float> (
ws) / 2.f));
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 (std::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))
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
void computeDepthMap(typename pcl::PointCloud< SceneT >::ConstPtr &scene, bool compute_focal=false, bool smooth=false, int wsize=3)
void filter(typename pcl::PointCloud< ModelT >::ConstPtr &model, typename pcl::PointCloud< ModelT >::Ptr &filtered, float thres=0.01)
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.
pcl::PointCloud< ModelT >::Ptr filter(typename pcl::PointCloud< SceneT >::ConstPtr &organized_cloud, typename pcl::PointCloud< ModelT >::ConstPtr &to_be_filtered, float f, float threshold)
IndicesAllocator<> Indices
Type used for indices in PCL.