simplification_remove_unused_vertices.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Dirk Holz, University of Bonn.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: simplification_remove_unused_vertices.cpp 4917 2012-03-05 17:36:10Z rusu $
00035  *
00036  */
00037 
00038 #include <pcl/surface/simplification_remove_unused_vertices.h>
00039 
00040 #include <cstring>
00041 #include <vector>
00042 #include <iostream>
00043 #include <stdio.h>
00044 
00045 void
00046 pcl::surface::SimplificationRemoveUnusedVertices::simplify(const pcl::PolygonMesh& input, pcl::PolygonMesh& output, std::vector<int>& indices)
00047 {
00048   if (input.polygons.size () == 0)
00049     return;
00050 
00051   unsigned int nr_points = input.cloud.width * input.cloud.height;
00052 
00053   std::vector<int> new_indices (nr_points, -1);
00054   indices.clear ();
00055   indices.reserve (nr_points);
00056 
00057   // mark all points in triangles as being used
00058   for (size_t polygon = 0; polygon < input.polygons.size (); ++polygon)
00059     for (size_t point = 0; point < input.polygons[polygon].vertices.size (); ++point)
00060       if (new_indices[ input.polygons[polygon].vertices[point] ] == -1 )
00061       {
00062         new_indices[input.polygons[polygon].vertices[point]] = static_cast<int> (indices.size ());
00063         indices.push_back (input.polygons[polygon].vertices[point]);
00064       }
00065 
00066   // in case all points are used , do nothing and return input mesh
00067   if (indices.size () == nr_points)
00068   {
00069     output = input;
00070     return;
00071   }
00072 
00073   // copy cloud information
00074   output.header = input.header;
00075   output.cloud.data.clear ();
00076   output.cloud.header = input.cloud.header;
00077   output.cloud.fields = input.cloud.fields;
00078   output.cloud.row_step = input.cloud.row_step;
00079   output.cloud.point_step = input.cloud.point_step;
00080   output.cloud.is_bigendian = input.cloud.is_bigendian;
00081   output.cloud.height = 1; // cloud is no longer organized
00082   output.cloud.width = static_cast<int> (indices.size ());
00083   output.cloud.row_step = output.cloud.point_step * output.cloud.width;
00084   output.cloud.data.resize (output.cloud.width * output.cloud.height * output.cloud.point_step);
00085   output.cloud.is_dense = false;
00086   output.polygons.clear ();
00087 
00088   // copy (only!) used points
00089   for (size_t i = 0; i < indices.size (); ++i)
00090     memcpy (&output.cloud.data[i * output.cloud.point_step], &input.cloud.data[indices[i] * output.cloud.point_step], output.cloud.point_step);
00091 
00092   // copy mesh information (and update indices)
00093   output.polygons.reserve (input.polygons.size ());
00094   for (size_t polygon = 0; polygon < input.polygons.size (); ++polygon)
00095   {
00096     pcl::Vertices corrected_polygon;
00097     corrected_polygon.vertices.resize (input.polygons[polygon].vertices.size ());
00098     for (size_t point = 0; point < input.polygons[polygon].vertices.size(); ++point)
00099       corrected_polygon.vertices[point] = new_indices[input.polygons[polygon].vertices[point]];
00100     output.polygons.push_back (corrected_polygon);
00101   }
00102 }
00103 


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:17:58