io.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
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: io.cpp 6161 2012-07-05 17:37:29Z rusu $
00035  *
00036  */
00037 #include <pcl/visualization/common/io.h>
00038 #include <Eigen/Geometry>
00039 #include <pcl/io/pcd_io.h>
00040 
00042 void
00043 pcl::visualization::getCorrespondingPointCloud (vtkPolyData *src, 
00044                                                 const pcl::PointCloud<pcl::PointXYZ> &tgt, 
00045                                                 std::vector<int> &indices)
00046 {
00047   // Iterate through the points and copy the data in a pcl::PointCloud
00048   pcl::PointCloud<pcl::PointXYZ> cloud;
00049   cloud.height = 1; cloud.width = static_cast<uint32_t> (src->GetNumberOfPoints ());
00050   cloud.is_dense = false;
00051   cloud.points.resize (cloud.width * cloud.height);
00052   for (int i = 0; i < src->GetNumberOfPoints (); i++)
00053   {
00054     double p[3];
00055     src->GetPoint (i, p);
00056     cloud.points[i].x = static_cast<float> (p[0]); 
00057     cloud.points[i].y = static_cast<float> (p[1]); 
00058     cloud.points[i].z = static_cast<float> (p[2]);
00059   }
00060 
00061   // Compute a kd-tree for tgt
00062   pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
00063   boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ> > tgt_ptr (new pcl::PointCloud<pcl::PointXYZ> (tgt));
00064   kdtree.setInputCloud (tgt_ptr);
00065 
00066   std::vector<int> nn_indices (1);
00067   std::vector<float> nn_dists (1);
00068   // For each point on screen, find its correspondent in the target
00069   for (size_t i = 0; i < cloud.points.size (); ++i)
00070   {
00071     kdtree.nearestKSearch (cloud.points[i], 1, nn_indices, nn_dists);
00072     indices.push_back (nn_indices[0]);
00073   }
00074   // Sort and remove duplicate indices
00075   std::sort (indices.begin (), indices.end ());
00076   indices.erase (std::unique (indices.begin (), indices.end ()), indices.end ()); 
00077 }
00078 
00080 bool 
00081 pcl::visualization::savePointData (vtkPolyData* data, const std::string &out_file, const boost::shared_ptr<CloudActorMap> &actors)
00082 {
00083   // Clean the data (no duplicates!)
00084   vtkSmartPointer<vtkCleanPolyData> cleaner = vtkSmartPointer<vtkCleanPolyData>::New ();
00085   cleaner->SetTolerance (0.0);
00086   cleaner->SetInput (data);
00087   cleaner->ConvertLinesToPointsOff ();
00088   cleaner->ConvertPolysToLinesOff ();
00089   cleaner->ConvertStripsToPolysOff ();
00090   cleaner->PointMergingOn ();
00091   cleaner->Update ();
00092 
00093   // If we pruned any points, print the number of points pruned to screen
00094   if (cleaner->GetOutput ()->GetNumberOfPoints () != data->GetNumberOfPoints ())
00095   {
00096     int nr_pts_pruned = static_cast<int> (data->GetNumberOfPoints () - cleaner->GetOutput ()->GetNumberOfPoints ());
00097     pcl::console::print_highlight ("Number of points pruned: "); pcl::console::print_value ("%d\n", nr_pts_pruned);
00098   }
00099 
00100   // Attempting to load all Point Cloud data input files (using the actor name)...
00101   CloudActorMap::iterator it;
00102   int i = 1;
00103   for (it = actors->begin (); it != actors->end (); ++it)
00104   {
00105     std::string file_name = (*it).first;
00106 
00107     // Is there a ".pcd" in the name? If no, then do not attempt to load this actor
00108     std::string::size_type position;
00109     if ((position = file_name.find (".pcd")) == std::string::npos)
00110       continue;
00111 
00112     // Strip the ".pcd-X"
00113     file_name = file_name.substr (0, position) + ".pcd";
00114 
00115     pcl::console::print_debug ("  Load: %s ... ", file_name.c_str ());
00116     // Assume the name of the actor is the name of the file
00117     sensor_msgs::PointCloud2 cloud;
00118     if (pcl::io::loadPCDFile (file_name, cloud) == -1)
00119     {
00120       pcl::console::print_error (stdout, "[failed]\n");
00121       return (false);
00122     }
00123     else
00124       pcl::console::print_debug ("[success]\n");
00125  
00126     pcl::PointCloud<pcl::PointXYZ> cloud_xyz;
00127     pcl::fromROSMsg (cloud, cloud_xyz);
00128     // Get the corresponding indices that we need to save from this point cloud
00129     std::vector<int> indices;
00130     getCorrespondingPointCloud (cleaner->GetOutput (), cloud_xyz, indices);
00131 
00132     // Copy the indices and save the file
00133     sensor_msgs::PointCloud2 cloud_out;
00134     pcl::copyPointCloud (cloud, indices, cloud_out);
00135     std::stringstream ss;
00136     ss << out_file << i++ << ".pcd";
00137     pcl::console::print_debug ("  Save: %s ... ", ss.str ().c_str ());
00138     if (pcl::io::savePCDFile (ss.str (), cloud_out, Eigen::Vector4f::Zero (),
00139                               Eigen::Quaternionf::Identity (), true) == -1)
00140     {
00141       pcl::console::print_error (stdout, "[failed]\n");
00142       return (false);
00143     }
00144     else
00145       pcl::console::print_debug ("[success]\n");
00146   }
00147 
00148   return (true);
00149 }


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:15:30