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; }