$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 "household_objects_database/database_view.h"
#include "household_objects_database/database_vfh.h"
#include "household_objects_database/objects_database.h"
#include "household_objects_database/database_helper_classes.h"
#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 vfh_recognizer_db
{
  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;
      bool binary;
      int offset;
      r.readHeader (ss.str (), cloud, origin, orientation, version, binary, offset);

      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];
      }
  }
}
#endif // VFH_CLUSTER_CLASSIFIER_COMMON_IO_H_

---

vfh_recognizer_db
Author(s): Aitor Aldoma
autogenerated on Tue Mar 5 15:26:29 2013