$search

vfh_recognition.cpp

Go to the documentation of this file.

#include "vfh_recognizer_fs/vfh_recognition.h"
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <fstream>
#include <iostream>

namespace vfh_recognizer_fs
{
  template<template<class > class Distance>
    bool
    VFHRecognizerFS<Distance>::getPointCloudFromId (pcl::PointCloud<pcl::PointNormal> & cloud, std::string id)
    {
      /*std::stringstream view_file;
      view_file << root_dir << "views/view_" << id << ".pcd";
      pcl::io::loadPCDFile (view_file.str (), cloud);*/

    //get the vfh - view map
    size_t last_found;
    last_found=id.find_last_of("_");

    std::string model_id = id.substr(0,last_found);

    std::stringstream vfh_view_map;
    vfh_view_map << root_dir << "vfh_views_map/vfh_view_" << id << ".txt";

    std::ifstream in;
    in.open (vfh_view_map.str ().c_str (), std::ifstream::in);

    char linebuf[256];
    in.getline (linebuf, 256);
    std::string line (linebuf);

    std::vector < std::string > strs;
    boost::split (strs, line, boost::is_any_of (" "));

    std::stringstream view_file;
    view_file << root_dir << "views/view_" << model_id << "_" << strs[0] << ".pcd";
    std::cout << "Going to get point cloud" << view_file.str() << std::endl;
    pcl::io::loadPCDFile(view_file.str(), cloud);

      return (true);
    }

  template<template<class > class Distance>
    bool
    VFHRecognizerFS<Distance>::getVFHHistogramFromVFHId (pcl::PointCloud<pcl::VFHSignature308> & vfh_descriptor,
                                                         std::string vfh_id)
    {

      std::stringstream vfh_file;
      vfh_file << root_dir << "vfhs/vfh_" << vfh_id << ".pcd";
      pcl::io::loadPCDFile (vfh_file.str (), vfh_descriptor);
      return (true);
    }

  template<template<class > class Distance>
    bool
    VFHRecognizerFS<Distance>::getVFHRollOrientationFromId (
                                                            pcl::PointCloud<pcl::VFHSignature308> & vfh_orientation_signature,
                                                            std::string id)
    {
      std::stringstream vfh_roll_file;
      vfh_roll_file << root_dir << "vfhs_roll_orientation/vfh_orientation_" << id << ".pcd";
      pcl::io::loadPCDFile (vfh_roll_file.str (), vfh_orientation_signature);
      return (true);
    }

  template<template<class > class Distance>
    bool
    VFHRecognizerFS<Distance>::getVFHRollOrientationFromIdThroughView (
                                                                       pcl::PointCloud<pcl::VFHSignature308> & vfh_orientation_signature,
                                                                       std::string vfh_id)
    {
      return false;
    }

  template<template<class > class Distance>
    bool
    VFHRecognizerFS<Distance>::getVFHCentroidFromVFHid (std::vector<float> & centroid, std::string id)
    {
      std::stringstream centroid_file;
      centroid_file << root_dir << "centroids/centroid_" << id << ".txt";
      std::ifstream in;
      in.open (centroid_file.str ().c_str (), std::ifstream::in);

      char linebuf[256];
      in.getline (linebuf, 256);
      std::string line (linebuf);

      std::cout << line << std::endl;

      std::vector < std::string > strs;
      boost::split (strs, line, boost::is_any_of (" "));

      centroid[0] = atof (strs[0].c_str ());
      centroid[1] = atof (strs[1].c_str ());
      centroid[2] = atof (strs[2].c_str ());
      return false;
    }

  template<template<class > class Distance>
    bool
    VFHRecognizerFS<Distance>::getViewCentroidFromVFHid (std::vector<float> & centroid, std::string id)
    {
      return false;
    }

  template<template<class > class Distance>
    bool
    VFHRecognizerFS<Distance>::getPoseFromId (std::string id, geometry_msgs::Pose & pose)
    {

      //read from file
      std::stringstream transform_file;
      transform_file << root_dir << "transforms/transform_" << id << ".txt";
      std::ifstream in;
      in.open (transform_file.str ().c_str (), std::ifstream::in);

      char linebuf[256];
      in.getline (linebuf, 256);
      std::string line (linebuf);

      std::cout << line << std::endl;

      std::vector < std::string > strs;
      boost::split (strs, line, boost::is_any_of (" "));

      pose.position.x = atof (strs[0].c_str ());
      pose.position.y = atof (strs[1].c_str ());
      pose.position.z = atof (strs[2].c_str ());

      pose.orientation.x = atof (strs[3].c_str ());
      pose.orientation.y = atof (strs[4].c_str ());
      pose.orientation.z = atof (strs[5].c_str ());
      pose.orientation.w = atof (strs[6].c_str ());

      in.close ();
      return true;
    }

  template<template<class > class Distance>
    std::string
    VFHRecognizerFS<Distance>::getModelId (std::string id)
    {
      //TODO: should split id and get the model_id
      return id;
    }

  template<template<class > class Distance>
    int
    VFHRecognizerFS<Distance>::getVFHId (std::string id)
    {
      return atoi (id.c_str ());
    }
  ;

  /*template <template<class> class Distance>
   int VFHRecognizerFS< Distance >::initializeFromViews(std::vector<boost::shared_ptr<household_objects_database::DatabaseView> > & views) {
   int minid = vfh_recognizer_db::buildTreeFromViews (models_, data_, views);
   index_ = new flann::Index<DistT> (data_, flann::LinearIndexParams ());
   index_->buildIndex ();
   return minid;
   }*/

  template<template<class > class Distance>
    bool
    VFHRecognizerFS<Distance>::initialize (bool reloadFiles, bf::path dpath, std::string root_dir, int linear)
    {
      bf::path training_data_h5_file_name = dpath / "training_data.h5";
      bf::path training_data_list_file_name = dpath / "training_data.list";
      bf::path index_filename = dpath / "kdtree.idx";

      this->root_dir = root_dir;

      if (reloadFiles || !boost::filesystem::exists (training_data_h5_file_name)
          || !boost::filesystem::exists (training_data_list_file_name) || !boost::filesystem::exists (index_filename))
      {
        //read training data from DB and build kdtree
        std::cout << "Calling tree build from files" << std::endl;
        std::string vfhs_dir = root_dir + "vfhs";
        vfh_recognizer_fs::buildTreeFromFiles (models_, data_, vfhs_dir);
        std::cout << "Tree build from DB:" << models_.size () << " " << data_.rows << std::endl;
        if (linear == 0)
        {
          index_ = new flann::Index<DistT> (data_, flann::LinearIndexParams ());
        }
        else
        {
          index_ = new flann::Index<DistT> (data_, flann::KDTreeIndexParams (4));
        }
        index_->buildIndex ();

        if (reloadFiles)
        {
          //files could exists, try to delete them
          if (boost::filesystem::exists (training_data_h5_file_name))
            boost::filesystem::remove (training_data_h5_file_name);

          if (boost::filesystem::exists (training_data_list_file_name))
            boost::filesystem::remove (training_data_list_file_name);

          if (boost::filesystem::exists (index_filename))
            boost::filesystem::remove (index_filename);
        }

        if (!boost::filesystem::exists (dpath))
        {
          ROS_WARN ("%s does not exist. Not able to save training_data!", dpath.string ().c_str ());
        }
        else
        {
          //SAVE the kdtree + training_list
          flann::save_to_file (data_, training_data_h5_file_name.string ().c_str (), "training_data");
          saveFileList (models_, training_data_list_file_name.string ().c_str ());
          index_->save (index_filename.string ().c_str ());
        }
      }
      else
      {
        loadFileList (models_, training_data_list_file_name.string ());
        flann::load_from_file (data_, training_data_h5_file_name.string (), "training_data");
        std::cout << "Training data found. Loaded" << (int)data_.rows << "VFH models from:" <<
                         training_data_h5_file_name.string () << training_data_list_file_name.string () << std::endl;

        index_ = new flann::Index<DistT> (data_, flann::SavedIndexParams (index_filename.string ().c_str ()));
        index_->buildIndex ();
      }

      return true;
    }

  template<template<class > class Distance>
    bool
    VFHRecognizerFS<Distance>::detect (const sensor_msgs::PointCloud2ConstPtr& msg, int nModels,
                                       std::vector<std::string>& model_ids, std::vector<geometry_msgs::Pose> & poses,
                                       std::vector<float>& confidences, bool use_fitness_score)
    {

      std::vector < std::string > vfh_ids;
      this->detect_ (msg, nModels, model_ids, poses, confidences, vfh_ids, use_fitness_score);

      return true;
    }

  template class VFHRecognizerFS<flann::ChiSquareDistance> ;
  //template class VFHRecognizerFS<flann::L1> ;
  //template class VFHRecognizerFS<flann::L2> ;
  //template class VFHRecognizerFS<flann::L2_Simple> ;
  template class VFHRecognizerFS<flann::HistIntersectionDistance> ;
  template class VFHRecognizerFS<flann::HistIntersectionUnionDistance> ;
//template class VFHRecognizerFS<flann::KL_Divergence> ;
}

---

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