$search

vfh_recognition.h

Go to the documentation of this file.

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "sensor_msgs/PointCloud2.h"
#include <flann/io/hdf5.h>
#include "boost/filesystem/operations.hpp"
#include "boost/filesystem/path.hpp"
#include <boost/lexical_cast.hpp>
#include <boost/algorithm/string/replace.hpp>
#include <boost/algorithm/string.hpp>
#include <fstream>
#include <iostream>
#include <algorithm>
#include <pcl/common/common.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/vfh.h>
#include <pcl/features/cvfh.h>
#include <pcl/ros/conversions.h>
#include <pcl/io/pcd_io.h>
#include <pcl/segmentation/extract_clusters.h>
#include <flann/flann.h>
#include <vector>
#include <pcl/registration/transforms.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/registration/registration.h>
#include <pcl/registration/icp.h>
#include <pcl_ros/point_cloud.h>
#include <ros/message_traits.h>
#include <ros/serialization.h>
#include "geometry_msgs/Pose.h"
#include "tf/transform_datatypes.h"
#include "LinearMath/btTransform.h"
#include <string>
#include <Eigen/Geometry>

#include <math.h>
#include <limits>

#include "cv.h"

#define PI 3.14159265

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

namespace vfh_recognition
{

  typedef std::pair<std::string, std::vector<float> > vfh_model_db;
  typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
  typedef pcl::PointCloud<pcl::PointNormal> PointCloudAndNormals;
  typedef std::pair<int, float> index_score;
  const int nr_bins = 90;

  inline bool
  ICPredicate (const index_score& d1, const index_score& d2)
  {
    return d1.second < d2.second;
  }

  template<template<class> class Distance>
  class VFHRecognizer
  {

  typedef Distance<float> DistT;

  protected:
    std::vector<vfh_model_db> models_;
    flann::Matrix<float> data_;
    int knn_;
    flann::Index<DistT> * index_;
    std::vector<int> roll_rotation_angles_;

    std::vector<Eigen::Vector4f,Eigen::aligned_allocator< Eigen::Vector4f > > centroid_results_;
    std::vector<Eigen::Vector4f,Eigen::aligned_allocator< Eigen::Vector4f > > centroid_inputs_;
    std::vector<Eigen::Matrix4f,Eigen::aligned_allocator< Eigen::Matrix4f > > icp_transformations_;

    /*std::vector < Eigen::Matrix4f > icp_transformations_;
    std::vector < Eigen::Vector4f > centroid_inputs_;
    std::vector < Eigen::Vector4f > centroid_results_;*/

    bool apply_voxel_grid_;
    bool apply_radius_removal_;
    bool perform_icp_;
    int icp_iterations_;
    bool use_old_vfh_; //true - use old vfh false - use new vfh
    bool normalize_vfh_bins_;
    bool use_cluster_centroids_;

    bool
    loadFileList (std::vector<vfh_model_db> &models, const std::string &filename);


    void
    saveFileList (const std::vector<vfh_model_db> &models, const std::string &filename);

    void
    nearestKSearch (flann::Index<DistT> * index, const vfh_model_db &model, int k,
                    flann::Matrix<int> &indices, flann::Matrix<float> &distances);

    void
    filterNormalsWithHighCurvature(pcl::PointCloud<pcl::PointNormal> & cloud, std::vector<int> & indices_out, std::vector<int> & indices_in, float threshold);

    void
    computeTransformToZAxes(Eigen::Vector4f & centroid, Eigen::Affine3f &  transform);

    void
    computeRollTransform(Eigen::Vector4f & centroidInput, Eigen::Vector4f & centroidResult, double roll_angle, Eigen::Affine3f & final_trans);

    virtual bool
    getPointCloudFromId (pcl::PointCloud<pcl::PointNormal> & cloud, std::string id)=0;

    virtual bool
    getVFHRollOrientationFromId (pcl::PointCloud<pcl::VFHSignature308> & vfh_orientation_signature, std::string id)=0;

    virtual bool
    getVFHRollOrientationFromIdThroughView (pcl::PointCloud<pcl::VFHSignature308> & vfh_orientation_signature, std::string id)=0;

    virtual bool
    getVFHCentroidFromVFHid (std::vector<float> & centroid, std::string id)=0;

    virtual bool
    getViewCentroidFromVFHid (std::vector<float> & centroid, std::string id)=0;

    virtual bool
    getPoseFromId(std::string id, geometry_msgs::Pose & pose)=0;

    virtual std::string
    getModelId (std::string id)=0;

    virtual int
    getVFHId (std::string id)=0;

  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    pcl::PointCloud<pcl::PointNormal> input_filtered;
    pcl::PointCloud<pcl::VFHSignature308> vfh_histogram_processed;
    int view_id_being_processed_; //temporal thing
    std::vector<float> roll_histogram_errors_;
    std::vector<float> roll_histogram_errors_flipped_;
    std::vector < float > fitness_scores_;
    bool fill_size_component_;

    int
    computeAngleRollOrientationFrequency (cv::Mat & hist_fft, cv::Mat & hist_fft_input, int nr_bins_,
                                          Eigen::Vector4f centroid_view, Eigen::Vector4f centroid_input,
                                          const pcl::PointCloud<pcl::PointNormal> & view, const pcl::PointCloud<
                                          pcl::PointNormal> & input, int jj, pcl::PointCloud<pcl::VFHSignature308> & hist_view, pcl::PointCloud<pcl::VFHSignature308> & hist_input);

    virtual bool
    getVFHHistogramFromVFHId (pcl::PointCloud<pcl::VFHSignature308> & vfh_descriptor, std::string vfh_id)=0;

    void
    getRollRotationAngles(std::vector<int> * roll_angles);

    void
    getCentroids(std::vector<Eigen::Vector4f,Eigen::aligned_allocator< Eigen::Vector4f > > * centroids);

    void
    getCentroidsResults(std::vector<Eigen::Vector4f,Eigen::aligned_allocator< Eigen::Vector4f > > * centroids);

    void
    getICPTransformations( std::vector<Eigen::Matrix4f,Eigen::aligned_allocator< Eigen::Matrix4f > > * icp_transformations);

    bool
    computeVFH (pcl::PointCloud<pcl::PointXYZ>::Ptr input, pcl::PointCloud<pcl::PointNormal>::Ptr cloud_normals,
                std::vector<pcl::PointCloud<pcl::VFHSignature308>,Eigen::aligned_allocator< pcl::PointCloud<pcl::VFHSignature308> > > & vfh_signatures,
                std::vector<Eigen::Vector3f> & centroids_dominant_orientations);

    bool
    computeNormals (pcl::PointCloud<pcl::PointXYZ>::Ptr input,
                    pcl::PointCloud<pcl::PointNormal>::Ptr cloud_normals);

    void
    computeVFHRollOrientation (pcl::PointCloud<pcl::PointNormal>::Ptr input,
                               pcl::PointCloud<pcl::VFHSignature308>::Ptr vfh_signature,
                               Eigen::Vector3f & centroid, cv::Mat & frequency_domain);

    bool
    detect_ (const sensor_msgs::PointCloud2ConstPtr& msg, int nModels, std::vector<std::string>& model_ids, std::vector<
        geometry_msgs::Pose> & poses, std::vector<float>& confidences, std::vector<std::string>& vfh_ids, bool use_fitness_score = true);

    void
    setApplyVoxelGrid(bool apply) {
      apply_voxel_grid_ = apply;
    };

    void
    setApplyRadiusRemoval(bool apply) {
      apply_radius_removal_ = apply;
    };

    void
    setApplyICP(bool apply) {
      perform_icp_ = apply;
    };

    void
    setICPIterations(int iter) {
      icp_iterations_ = iter;
    };

    void setUseOldVFH(bool use) {
      this->use_old_vfh_ = use;
    };

    void setNormalizeVFHBins(bool normalize) {
      this->normalize_vfh_bins_ = normalize;
    };

    void setUseClusterCentroids(bool use) {
      use_cluster_centroids_ = use;
    }
    ~VFHRecognizer();

    VFHRecognizer();
  };
}

---

vfh_recognition
Author(s): Aitor Aldoma
autogenerated on Tue Mar 5 15:13:29 2013