$search

common_io.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2010, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * $Id: common_io.h 48493 2011-02-01 03:49:28Z aaldoma $
 *
 */

#ifndef VFH_CLUSTER_CLASSIFIER_COMMON_IO_H_
#define VFH_CLUSTER_CLASSIFIER_COMMON_IO_H_

#include <ros/message_traits.h>
#include <ros/serialization.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <boost/filesystem.hpp>
#include <fstream>
#include <sstream>
#include <pcl_ros/point_cloud.h>
#include <flann/flann.h>
#include <flann/io/hdf5.h>
#include <fstream>
#include <iostream>
#include <limits>

namespace ser = ros::serialization;
namespace fs = boost::filesystem;

namespace vfh_recognizer_fs
{
  typedef std::pair<std::string, std::vector<float> > vfh_model;
  typedef std::pair<std::string, std::vector<float> > vfh_model_db;


  /*inline bool
  loadHist (const boost::filesystem::path &path, vfh_model &vfh)
  {
    using namespace std;
    using namespace pcl;

    int vfh_idx;
    // Load the file as a PCD
    stringstream ss;
    ss << path;
    try
    {
      sensor_msgs::PointCloud2 cloud;
      int version;
      Eigen::Vector4f origin;
      Eigen::Quaternionf orientation;
      PCDReader r;
      r.readHeader (ss.str (), cloud, origin, orientation, version);

      vfh_idx = getFieldIndex (cloud, "vfh");
      if (vfh_idx == -1)
        return (false);
      if ((int)cloud.width * cloud.height != 1)
        return (false);
    }
    catch (pcl::InvalidConversionException e)
    {
      return (false);
    }

    // Treat the VFH signature as a single Point Cloud
    PointCloud < VFHSignature308 > point;
    io::loadPCDFile (ss.str (), point);
    vfh.second.resize (308);

    vector < sensor_msgs::PointField > fields;
    getFieldIndex (point, "vfh", fields);

    for (size_t i = 0; i < fields[vfh_idx].count; ++i)
    {
      vfh.second[i] = point.points[0].histogram[i];
    }
    vfh.first = ss.str ();
    return (true);
  }*/


  /*inline bool
  loadHistPlain (const boost::filesystem::path &path, vfh_model &vfh)
  {
    std::ifstream fs;
    std::string line, file_name;
    std::stringstream ss;
    ss << path;
    file_name = ss.str ();

    // Open file
    fs.open (file_name.c_str ());
    if (!fs.is_open () || fs.fail ())
    {
      fprintf (stderr, "Couldn't open %s for reading!\n", file_name.c_str ());
      return (false);
    }

    // Get the number of lines (points) this histogram file holds
    int nr_samples = 0;
    while ((!fs.eof ()) && (getline (fs, line)))
      nr_samples++;

    vfh.second.resize (nr_samples);

    fs.clear ();
    fs.seekg (0, std::ios_base::beg);
    for (int i = 0; i < nr_samples; ++i)
    {
      getline (fs, line);
      if (line == "")
        continue;

      vfh.second[i] = atof (line.c_str ());
    }
    vfh.first = file_name;
    // Close file
    fs.close ();
    return (true);
  }*/

  inline void
  convertToFLANN (const std::vector<vfh_model_db> &models, flann::Matrix<float> &data)
  {
    data.rows = models.size ();
    data.cols = models[0].second.size (); // number of histogram bins
    data.data = (float*)malloc (data.rows * data.cols * sizeof(float));

    for (size_t i = 0; i < data.rows; ++i)
      for (size_t j = 0; j < data.cols; ++j)
      {
        data.data[i * data.cols + j] = models[i].second[j];
      }
  }

  inline std::string
  getModelId(fs::path path) {
    //split path by _ and get the last number
    std::string path_str = path.string();
    std::vector<std::string> strs;
    boost::split(strs, path_str, boost::is_any_of("/"));

    std::string filename = strs[strs.size() - 1];
    boost::replace_last (filename, "vfh_", "");
    boost::replace_last (filename, ".pcd", "");

    return filename;
  }

  inline bool
  buildTreeFromFiles (std::vector<vfh_model_db> & models, flann::Matrix<float> & data, std::string vfh_directory)
  {

    fs::path full_path = vfh_directory;

    if (!fs::exists (full_path))
    {
#if (!defined BOOST_FILESYSTEM_VERSION) || (BOOST_FILESYSTEM_VERSION<=2)
      std::cout << "\nNot found: " << full_path.native_file_string () << std::endl;
#else
      std::cout << "\nNot found: " << full_path.string () << std::endl;
#endif
      return 1;
    }

    if (fs::is_directory (full_path))
    {
#if (!defined BOOST_FILESYSTEM_VERSION) || (BOOST_FILESYSTEM_VERSION<=2)
      std::cout << "\nIn directory: " << full_path.native_directory_string () << "lolo\n\n";
#else
      std::cout << "\nIn directory: " << full_path.string () << "lolo\n\n";
#endif

      fs::directory_iterator end_iter;
      for (fs::directory_iterator dir_itr (full_path); dir_itr != end_iter; ++dir_itr)
      {
#if (!defined BOOST_FILESYSTEM_VERSION) || (BOOST_FILESYSTEM_VERSION<=2)
        std::cout << "lolo" << (*dir_itr).string() << std::endl;
#else
        std::cout << "lolo" << (*dir_itr).path().string() << std::endl;
#endif
        if (!fs::is_directory(*dir_itr)) {
          //vfh pcd..
          vfh_model_db vfh;
          std::cout << "lolo" << getModelId(*dir_itr) << std::endl;
          vfh.first = getModelId(*dir_itr);
          vfh.second.resize (308);
          pcl::PointCloud<pcl::VFHSignature308>::Ptr vfh_signaturePtr (new pcl::PointCloud<pcl::VFHSignature308> ());
#if (!defined BOOST_FILESYSTEM_VERSION) || (BOOST_FILESYSTEM_VERSION<=2)
          pcl::io::loadPCDFile((*dir_itr).string(), *vfh_signaturePtr);
#else
          pcl::io::loadPCDFile((*dir_itr).path().string(), *vfh_signaturePtr);
#endif
          memcpy (&vfh.second[0], &vfh_signaturePtr->points[0].histogram[0], 308 * sizeof(float));
          models.push_back (vfh);
        }
      }
    }

    std::cout << "models size:" << models.size() << std::endl;

    convertToFLANN (models, data);
    return true;
  }
}
#endif // VFH_CLUSTER_CLASSIFIER_COMMON_IO_H_

---

vfh_recognizer_fs
Author(s): Aitor Aldoma
autogenerated on Tue Mar 5 15:52:15 2013