void
pcl::surface::SimplificationRemoveUnusedVertices::simplify(const pcl::PolygonMesh& input, pcl::PolygonMesh& output, std::vector<int>& indices)
{
if (input.polygons.size () == 0)
return;
unsigned int nr_points = input.cloud.width * input.cloud.height;
std::vector<int> new_indices (nr_points, -1);
indices.clear ();
indices.reserve (nr_points);
// mark all points in triangles as being used
for (size_t polygon = 0; polygon < input.polygons.size (); ++polygon)
for (size_t point = 0; point < input.polygons[polygon].vertices.size (); ++point)
if (new_indices[ input.polygons[polygon].vertices[point] ] == -1 )
{
new_indices[input.polygons[polygon].vertices[point]] = static_cast<int> (indices.size ());
indices.push_back (input.polygons[polygon].vertices[point]);
}
// in case all points are used , do nothing and return input mesh
if (indices.size () == nr_points)
{
output = input;
return;
}
// copy cloud information
output.header = input.header;
output.cloud.data.clear ();
output.cloud.header = input.cloud.header;
output.cloud.fields = input.cloud.fields;
output.cloud.row_step = input.cloud.row_step;
output.cloud.point_step = input.cloud.point_step;
output.cloud.is_bigendian = input.cloud.is_bigendian;
output.cloud.height = 1; // cloud is no longer organized
output.cloud.width = static_cast<int> (indices.size ());
output.cloud.row_step = output.cloud.point_step * output.cloud.width;
output.cloud.data.resize (output.cloud.width * output.cloud.height * output.cloud.point_step);
output.cloud.is_dense = false;
output.polygons.clear ();
// copy (only!) used points
for (size_t i = 0; i < indices.size (); ++i)
memcpy (&output.cloud.data[i * output.cloud.point_step], &input.cloud.data[indices[i] * output.cloud.point_step], output.cloud.point_step);
// copy mesh information (and update indices)
output.polygons.reserve (input.polygons.size ());
for (size_t polygon = 0; polygon < input.polygons.size (); ++polygon)
{
pcl::Vertices corrected_polygon;
corrected_polygon.vertices.resize (input.polygons[polygon].vertices.size ());
for (size_t point = 0; point < input.polygons[polygon].vertices.size(); ++point)
corrected_polygon.vertices[point] = new_indices[input.polygons[polygon].vertices[point]];
output.polygons.push_back (corrected_polygon);
}
}
void
PCLOpenCVConverter<PointT>::computeOrganizedCloud()
{
CHECK( !cloud_->isOrganized() && cam_ && cam_->getWidth()>0 && cam_->getHeight()>0 );
ZBufferingParameter zBufParams;
zBufParams.do_smoothing_ = true;
zBufParams.smoothing_radius_ = 2;
ZBuffering<PointT> zbuf (cam_, zBufParams);
typename pcl::PointCloud<PointT>::Ptr organized_cloud (new pcl::PointCloud<PointT>);
zbuf.renderPointCloud( *cloud_, *organized_cloud);
for(PointT &p:organized_cloud->points)
{
if (!pcl::isFinite(p))
{
p.r = background_color_(0);
p.g = background_color_(1);
p.b = background_color_(2);
}
}
index_map_ = zbuf.getIndexMap();
if(!indices_.empty())
{
size_t width = cam_->getWidth();
size_t height = cam_->getHeight();
boost::dynamic_bitset<> fg_mask = createMaskFromIndices(indices_, cloud_->points.size());
std::vector<int> new_indices(indices_.size());
size_t kept=0;
for(size_t u=0; u<width; u++)
{
for(size_t v=0; v<height; v++)
{
int orig_idx = index_map_(v,u);
if(orig_idx >= 0 && fg_mask[orig_idx] )
{
new_indices[kept++] = v*width + u;
}
}
}
new_indices.resize(kept);
indices_.swap(new_indices);
}
cloud_ = organized_cloud;
}
