37 #ifndef PCL_RECOGNITION_OCCLUSION_REASONING_H_ 38 #define PCL_RECOGNITION_OCCLUSION_REASONING_H_ 41 #include <pcl/common/transforms.h> 42 #include <pcl/common/io.h> 47 namespace occlusion_reasoning
54 template<
typename ModelT,
typename SceneT>
78 float cx = (static_cast<float> (organized_cloud->
width) / 2.f - 0.5f);
79 float cy = (static_cast<float> (organized_cloud->
height) / 2.f - 0.5f);
82 std::vector<int> indices_to_keep;
83 indices_to_keep.resize (to_be_filtered->
points.size ());
86 for (
size_t i = 0; i < to_be_filtered->
points.size (); i++)
88 float x = to_be_filtered->
points[i].x;
89 float y = to_be_filtered->
points[i].y;
90 float z = to_be_filtered->
points[i].z;
91 int u = static_cast<int> (f * x / z + cx);
92 int v = static_cast<int> (f * y / z + cy);
95 if ((u >= static_cast<int> (organized_cloud->
width)) || (v >= static_cast<int> (organized_cloud->
height)) || (u < 0) || (v < 0))
99 if (!pcl_isfinite (organized_cloud->
at (u, v).x) || !pcl_isfinite (organized_cloud->
at (u, v).y)
100 || !pcl_isfinite (organized_cloud->
at (u, v).z))
103 float z_oc = organized_cloud->
at (u, v).z;
106 if ((z - z_oc) > threshold)
109 indices_to_keep[keep] = static_cast<int> (i);
113 indices_to_keep.resize (keep);
120 float threshold,
bool check_invalid_depth =
true)
122 float cx = (static_cast<float> (organized_cloud->
width) / 2.f - 0.5f);
123 float cy = (static_cast<float> (organized_cloud->
height) / 2.f - 0.5f);
126 std::vector<int> indices_to_keep;
127 indices_to_keep.resize (to_be_filtered->
points.size ());
130 for (
size_t i = 0; i < to_be_filtered->
points.size (); i++)
132 float x = to_be_filtered->
points[i].x;
133 float y = to_be_filtered->
points[i].y;
134 float z = to_be_filtered->
points[i].z;
135 int u = static_cast<int> (f * x / z + cx);
136 int v = static_cast<int> (f * y / z + cy);
139 if ((u >= static_cast<int> (organized_cloud->
width)) || (v >= static_cast<int> (organized_cloud->
height)) || (u < 0) || (v < 0))
143 if (check_invalid_depth)
145 if (!pcl_isfinite (organized_cloud->
at (u, v).x) || !pcl_isfinite (organized_cloud->
at (u, v).y)
146 || !pcl_isfinite (organized_cloud->
at (u, v).z))
150 float z_oc = organized_cloud->
at (u, v).z;
153 if ((z - z_oc) > threshold)
156 indices_to_keep[keep] = static_cast<int> (i);
160 indices_to_keep.resize (keep);
167 float threshold,
bool check_invalid_depth =
true)
169 float cx = (static_cast<float> (organized_cloud->
width) / 2.f - 0.5f);
170 float cy = (static_cast<float> (organized_cloud->
height) / 2.f - 0.5f);
173 std::vector<int> indices_to_keep;
174 indices_to_keep.resize (to_be_filtered->
points.size ());
177 for (
size_t i = 0; i < to_be_filtered->
points.size (); i++)
179 float x = to_be_filtered->
points[i].x;
180 float y = to_be_filtered->
points[i].y;
181 float z = to_be_filtered->
points[i].z;
182 int u = static_cast<int> (f * x / z + cx);
183 int v = static_cast<int> (f * y / z + cy);
186 if ((u >= static_cast<int> (organized_cloud->
width)) || (v >= static_cast<int> (organized_cloud->
height)) || (u < 0) || (v < 0))
190 if (check_invalid_depth)
192 if (!pcl_isfinite (organized_cloud->
at (u, v).x) || !pcl_isfinite (organized_cloud->
at (u, v).y)
193 || !pcl_isfinite (organized_cloud->
at (u, v).z))
197 float z_oc = organized_cloud->
at (u, v).z;
200 if ((z - z_oc) > threshold)
202 indices_to_keep[keep] = static_cast<int> (i);
207 indices_to_keep.resize (keep);
214 #ifdef PCL_NO_PRECOMPILE 215 #include <pcl/recognition/impl/hv/occlusion_reasoning.hpp> std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
pcl::PointCloud< ModelT >::Ptr getOccludedCloud(typename pcl::PointCloud< SceneT >::Ptr &organized_cloud, typename pcl::PointCloud< ModelT >::Ptr &to_be_filtered, float f, float threshold, bool check_invalid_depth=true)
Class to reason about occlusions.
PCL_EXPORTS void copyPointCloud(const pcl::PCLPointCloud2 &cloud_in, const std::vector< int > &indices, pcl::PCLPointCloud2 &cloud_out)
Extract the indices of a given point cloud as a new point cloud.
void computeDepthMap(typename pcl::PointCloud< SceneT >::ConstPtr &scene, bool compute_focal=false, bool smooth=false, int wsize=3)
const PointT & at(int column, int row) const
Obtain the point given by the (column, row) coordinates.
uint32_t height
The point cloud height (if organized as an image-structure).
void filter(typename pcl::PointCloud< ModelT >::ConstPtr &model, typename pcl::PointCloud< ModelT >::Ptr &filtered, float thres=0.01)
boost::shared_ptr< PointCloud< PointT > > Ptr
Define standard C methods and C++ classes that are common to all methods.
uint32_t width
The point cloud width (if organized as an image-structure).
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
pcl::PointCloud< ModelT >::Ptr filter(typename pcl::PointCloud< SceneT >::ConstPtr &organized_cloud, typename pcl::PointCloud< ModelT >::ConstPtr &to_be_filtered, float f, float threshold)