40 #include <pcl/common/transforms.h>
41 #include <pcl/common/io.h>
46 namespace occlusion_reasoning
53 template<
typename ModelT,
typename SceneT>
77 float cx = (
static_cast<float> (organized_cloud->
width) / 2.f - 0.5f);
78 float cy = (
static_cast<float> (organized_cloud->
height) / 2.f - 0.5f);
81 std::vector<int> indices_to_keep;
82 indices_to_keep.resize (to_be_filtered->
points.size ());
85 for (std::size_t i = 0; i < to_be_filtered->
points.size (); i++)
87 float x = to_be_filtered->
points[i].x;
88 float y = to_be_filtered->
points[i].y;
89 float z = to_be_filtered->
points[i].z;
90 int u =
static_cast<int> (f * x / z + cx);
91 int v =
static_cast<int> (f * y / z + cy);
94 if ((u >=
static_cast<int> (organized_cloud->
width)) || (v >=
static_cast<int> (organized_cloud->
height)) || (u < 0) || (v < 0))
98 if (!std::isfinite (organized_cloud->
at (u, v).x) || !std::isfinite (organized_cloud->
at (u, v).y)
99 || !std::isfinite (organized_cloud->
at (u, v).z))
102 float z_oc = organized_cloud->
at (u, v).z;
105 if ((z - z_oc) > threshold)
108 indices_to_keep[keep] =
static_cast<int> (i);
112 indices_to_keep.resize (keep);
119 float threshold,
bool check_invalid_depth =
true)
121 float cx = (
static_cast<float> (organized_cloud->
width) / 2.f - 0.5f);
122 float cy = (
static_cast<float> (organized_cloud->
height) / 2.f - 0.5f);
125 std::vector<int> indices_to_keep;
126 indices_to_keep.resize (to_be_filtered->
points.size ());
129 for (std::size_t i = 0; i < to_be_filtered->
points.size (); i++)
131 float x = to_be_filtered->
points[i].x;
132 float y = to_be_filtered->
points[i].y;
133 float z = to_be_filtered->
points[i].z;
134 int u =
static_cast<int> (f * x / z + cx);
135 int v =
static_cast<int> (f * y / z + cy);
138 if ((u >=
static_cast<int> (organized_cloud->
width)) || (v >=
static_cast<int> (organized_cloud->
height)) || (u < 0) || (v < 0))
142 if (check_invalid_depth)
144 if (!std::isfinite (organized_cloud->
at (u, v).x) || !std::isfinite (organized_cloud->
at (u, v).y)
145 || !std::isfinite (organized_cloud->
at (u, v).z))
149 float z_oc = organized_cloud->
at (u, v).z;
152 if ((z - z_oc) > threshold)
155 indices_to_keep[keep] =
static_cast<int> (i);
159 indices_to_keep.resize (keep);
166 float threshold,
bool check_invalid_depth =
true)
168 float cx = (
static_cast<float> (organized_cloud->
width) / 2.f - 0.5f);
169 float cy = (
static_cast<float> (organized_cloud->
height) / 2.f - 0.5f);
172 std::vector<int> indices_to_keep;
173 indices_to_keep.resize (to_be_filtered->
points.size ());
176 for (std::size_t i = 0; i < to_be_filtered->
points.size (); i++)
178 float x = to_be_filtered->
points[i].x;
179 float y = to_be_filtered->
points[i].y;
180 float z = to_be_filtered->
points[i].z;
181 int u =
static_cast<int> (f * x / z + cx);
182 int v =
static_cast<int> (f * y / z + cy);
185 if ((u >=
static_cast<int> (organized_cloud->
width)) || (v >=
static_cast<int> (organized_cloud->
height)) || (u < 0) || (v < 0))
189 if (check_invalid_depth)
191 if (!std::isfinite (organized_cloud->
at (u, v).x) || !std::isfinite (organized_cloud->
at (u, v).y)
192 || !std::isfinite (organized_cloud->
at (u, v).z))
196 float z_oc = organized_cloud->
at (u, v).z;
199 if ((z - z_oc) > threshold)
201 indices_to_keep[keep] =
static_cast<int> (i);
206 indices_to_keep.resize (keep);
213 #ifdef PCL_NO_PRECOMPILE
214 #include <pcl/recognition/impl/hv/occlusion_reasoning.hpp>
const PointT & at(int column, int row) const
Obtain the point given by the (column, row) coordinates.
std::uint32_t width
The point cloud width (if organized as an image-structure).
std::uint32_t height
The point cloud height (if organized as an image-structure).
shared_ptr< PointCloud< PointT > > Ptr
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
shared_ptr< const PointCloud< PointT > > ConstPtr
Class to reason about occlusions.
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)
Define standard C methods and C++ classes that are common to all methods.
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)
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)