$search

vfh_recognition.cpp

Go to the documentation of this file.

#include "vfh_recognition/vfh_recognition.h"
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/segmentation/extract_clusters.h>
#include <fstream>
#include <iostream>
#include <pcl/segmentation/segment_differences.h>

#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>
#include <sys/time.h>
#include <stdio.h>
#include <unistd.h>
#include <utility>

#include <pcl/common/time.h>
#include <algorithm>
#include <Eigen/StdVector>

namespace vfh_recognition
{

  typedef struct
  {
    bool
    operator() (std::pair<double, size_t> const& a, std::pair<double, size_t> const& b)
    {
      return a.first > b.first;
    }
  } peaks_ordering;

  typedef pcl::KdTree<pcl::PointNormal>::Ptr KdTreePtr;

  template<template<class > class Distance>
    VFHRecognizer<Distance>::VFHRecognizer ()
    {
      index_ = NULL;
      apply_voxel_grid_ = true;
      apply_radius_removal_ = true;
      icp_iterations_ = 0;
      perform_icp_ = false;
      this->use_old_vfh_ = false;
      normalize_vfh_bins_ = true;
      use_cluster_centroids_ = false;
      view_id_being_processed_ = -1;
      fill_size_component_ = false;
    }

  template<template<class > class Distance>
    VFHRecognizer<Distance>::~VFHRecognizer ()
    {
      //free memory...
      delete index_;
      free (data_.data);
    }

  template<template<class > class Distance>
    void
    VFHRecognizer<Distance>::computeVFHRollOrientation (
                                                        pcl::PointCloud<pcl::PointNormal>::Ptr input,
                                                        pcl::PointCloud<pcl::VFHSignature308>::Ptr vfh_orientation_signature,
                                                        Eigen::Vector3f & centroid, cv::Mat & frequency_domain)
    {
      Eigen::Vector3f plane_normal;
      plane_normal[0] = -centroid[0];
      plane_normal[1] = -centroid[1];
      plane_normal[2] = -centroid[2];
      Eigen::Vector3f z_vector = Eigen::Vector3f::UnitZ ();
      plane_normal.normalize ();
      Eigen::Vector3f axis = plane_normal.cross (z_vector);
      double rotation = -asin (axis.norm ());
      axis.normalize ();

      unsigned int nbins = nr_bins;
      int bin_angle = 360 / nbins;
      Eigen::VectorXf orientation_hist;
      orientation_hist.setZero (nbins);

      Eigen::Affine3f transformPC (Eigen::AngleAxisf (rotation, axis));

      pcl::PointCloud < pcl::PointNormal > grid;
      pcl::transformPointCloudWithNormals (*input, grid, transformPC);

      cv::Mat hist = cv::Mat_<float>::zeros (nbins, 1);

      double sum_w = 0, w = 0;
      int bin = 0;
      for (size_t i = 0; i < grid.points.size (); ++i)
      {
        bin = (int)(((atan2 (grid.points[i].normal_y, grid.points[i].normal_x) + PI) * 180 / PI) / bin_angle) % nbins;
        w
            = sqrt (
                    grid.points[i].normal_y * grid.points[i].normal_y + grid.points[i].normal_x
                        * grid.points[i].normal_x);
        //w = grid.points[i].normal_y * grid.points[i].normal_y + grid.points[i].normal_x * grid.points[i].normal_x;
        orientation_hist[bin] += w;
        sum_w += w;
        hist.at<float> (bin) += w;
      }

      for (size_t i = 0; i < nbins; ++i)
      {
        orientation_hist[i] /= sum_w;
        hist.at<float> (i) /= sum_w;
      }

      cv::dft (hist, frequency_domain);

      //transform to vfh_signature
      vfh_orientation_signature->points.resize (1);
      vfh_orientation_signature->width = 1;
      vfh_orientation_signature->height = 1;

      //no sliding window...
      for (size_t i = 0; i < nbins; ++i)
      {
        vfh_orientation_signature->points[0].histogram[i] = orientation_hist[i];
      }

      //sliding window implementation
      /*int wsize = 5;
       double sum_wsize = 0;
       //bootstrap window
       for (int i = -wsize / 2; i <= wsize / 2; ++i)
       {
       sum_wsize += orientation_hist[(i + nbins) % nbins];
       }
       vfh_orientation_signature->points[0].histogram[0] = sum_wsize / double (wsize);

       for (size_t i = 1; i < nbins; ++i)
       {
       int idx = (int)i;
       sum_wsize += orientation_hist[((idx - wsize / 2 - 1) + nbins) % nbins] * -1 + orientation_hist[(idx + wsize / 2
       + nbins) % nbins];
       vfh_orientation_signature->points[0].histogram[i] = sum_wsize / double (wsize);
       }

       struct timeval start;
       gettimeofday (&start, NULL);*/

      /*std::stringstream path_vfh_orientation;
       path_vfh_orientation << "/u/aaldoma/data/vfhs_roll_orientation/vfh_orientation_input_inside:" << start.tv_usec << ".pcd";
       //std::cout << path_vfh_orientation.str() << std::endl;
       pcl::io::savePCDFileBinary (path_vfh_orientation.str(), *vfh_orientation_signature);

       std::stringstream path_vfh_point_cloud;
       path_vfh_point_cloud << "/tmp/vfh_orientation_point_cloud:" << start.tv_usec << ".pcd";
       //std::cout << path_vfh_point_cloud.str() << std::endl;
       pcl::io::savePCDFileBinary (path_vfh_point_cloud.str(), grid);*/
    }

  template<template<class > class Distance>
    void
    VFHRecognizer<Distance>::filterNormalsWithHighCurvature (pcl::PointCloud<pcl::PointNormal> & cloud,
                                                             std::vector<int> & indices_out,
                                                             std::vector<int> & indices_in, float threshold)
    {
      indices_out.resize (cloud.points.size ());
      indices_in.resize (cloud.points.size ());

      size_t in, out;
      in = out = 0;

      for (size_t i = 0; i < cloud.points.size (); i++)
      {
        if (cloud.points[i].curvature > threshold)
        {
          indices_out[out] = i;
          out++;
        }
        else
        {
          indices_in[in] = i;
          in++;
        }
      }

      indices_out.resize (out);
      indices_in.resize (in);
    }

  template<template<class > class Distance>
    bool
    VFHRecognizer<Distance>::computeNormals (pcl::PointCloud<pcl::PointXYZ>::Ptr input,
                                             pcl::PointCloud<pcl::PointNormal>::Ptr cloud_normals)
    {
      typedef pcl::NormalEstimation<pcl::PointNormal, pcl::PointNormal> NormalEstimation;
      NormalEstimation n3d;

      pcl::PointCloud<pcl::PointXYZ>::Ptr grid (new pcl::PointCloud<pcl::PointXYZ> ());
      double leafSize = 0.005;

      if (apply_voxel_grid_)
      {
        pcl::VoxelGrid < pcl::PointXYZ > grid_;
        grid_.setInputCloud (input);
        grid_.setLeafSize (leafSize, leafSize, leafSize);
        grid_.filter (*grid);
      }
      else
      {
        ROS_WARN ("Do not apply voxel gridding");
        copyPointCloud (*input, *grid);
      }

      if (apply_radius_removal_)
      {
        //in synthetic views the render grazes some parts of the objects that are barely seen from that VP
        //thus creating a very sparse set of points that causes the normals to be very noisy!!
        //remove these points
        pcl::PointCloud < pcl::PointXYZ > gridFiltered;
        pcl::RadiusOutlierRemoval < pcl::PointXYZ > sor;
        sor.setInputCloud (grid);
        sor.setRadiusSearch (leafSize * 3);
        sor.setMinNeighborsInRadius (10);
        sor.filter (gridFiltered);
        copyPointCloud (gridFiltered, *cloud_normals);
      }
      else
      {
        copyPointCloud (*grid, *cloud_normals);
        ROS_WARN ("Do not apply radius removal");
      }

      if (cloud_normals->points.size () == 0)
      {
        ROS_WARN ("Cloud has no points after voxel grid and filtering, wont be able to compute normals or VFH!");
        return false;
      }

      KdTreePtr normals_tree = boost::make_shared<pcl::KdTreeFLANN<pcl::PointNormal> > (false);
      normals_tree->setInputCloud (cloud_normals);
      n3d.setRadiusSearch (leafSize * 3);
      n3d.setSearchMethod (normals_tree);
      n3d.setInputCloud (cloud_normals);
      n3d.compute (*cloud_normals);

      //check nans...
      int j = 0;
      for (size_t i = 0; i < cloud_normals->points.size (); ++i)
      {
        if (!pcl_isfinite (cloud_normals->points[i].normal_x) || !pcl_isfinite (cloud_normals->points[i].normal_y)
            || !pcl_isfinite (cloud_normals->points[i].normal_z))
          continue;
        cloud_normals->points[j] = cloud_normals->points[i];
        j++;
      }

      cloud_normals->points.resize (j);
      cloud_normals->width = j;
      cloud_normals->height = 1;
      return true;
    }

  template<template<class > class Distance>
    bool
    VFHRecognizer<Distance>::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)
    {
      computeNormals (input, cloud_normals);

      //std::stringstream normals_path;
      //normals_path << "/u/aaldoma/data/clusters/normals.pcd";
      //std::cout << normals_path.str() << std::endl;
      //pcl::io::savePCDFileASCII (normals_path.str(), *cloud_normals);

      if (!(this->use_old_vfh_))
      {
        ROS_WARN ("USING CVFH...");
        typedef pcl::CVFHEstimation<pcl::PointNormal, pcl::PointNormal, pcl::VFHSignature308> CVFHEstimator;
        pcl::PointCloud < pcl::VFHSignature308 > cvfh_signatures;

        CVFHEstimator cvfh;
        pcl::KdTree<pcl::PointNormal>::Ptr cvfh_tree = boost::make_shared<pcl::KdTreeFLANN<pcl::PointNormal> > ();
        //cvfh_tree->setInputCloud (cloud_normals);
        cvfh.setSearchMethod (cvfh_tree);
        cvfh.setInputCloud (cloud_normals);
        cvfh.setInputNormals (cloud_normals);
        cvfh.compute (cvfh_signatures);

        for (size_t i = 0; i < cvfh_signatures.points.size (); i++)
        {
          pcl::PointCloud < pcl::VFHSignature308 > vfh_signature;
          vfh_signature.points.resize (1);
          vfh_signature.width = vfh_signature.height = 1;
          for (int d = 0; d < 308; ++d)
            vfh_signature.points[0].histogram[d] = cvfh_signatures.points[i].histogram[d];

          vfh_signatures.push_back (vfh_signature);

          /*std::stringstream path_vfh_point_cloud;
           path_vfh_point_cloud << "/u/aaldoma/data/vfhs/vfh.pcd";
           std::cout << path_vfh_point_cloud.str() << std::endl;
           pcl::io::savePCDFileBinary (path_vfh_point_cloud.str(), vfh_signature);*/
        }

        cvfh.getCentroidClusters (centroids_dominant_orientations);
      }
      else
      {

        ROS_WARN ("USING VFH...");
        typedef pcl::VFHEstimation<pcl::PointNormal, pcl::PointNormal, pcl::VFHSignature308> VFHEstimator;
        VFHEstimator vfh;

        Eigen::Vector4f avg_centroid;
        pcl::compute3DCentroid (*cloud_normals, avg_centroid);
        Eigen::Vector3f cloud_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);

        pcl::KdTree<pcl::PointNormal>::Ptr vfh_tree = boost::make_shared<pcl::KdTreeFLANN<pcl::PointNormal> > ();
        vfh_tree->setInputCloud (cloud_normals);
        vfh.setSearchMethod (vfh_tree);
        vfh.setInputCloud (cloud_normals);
        vfh.setInputNormals (cloud_normals);
        vfh.setUseGivenCentroid (false);
        vfh.setFillSizeComponent (true);
        vfh.setNormalizeDistance (true);
        vfh.setUseGivenNormal (false);
        vfh.setNormalizeDistance (true);
        vfh.setNormalizeBins (true);

        pcl::PointCloud < pcl::VFHSignature308 > vfh_signature;
        vfh.compute (vfh_signature);
        vfh_signatures.push_back (vfh_signature);

        //std::stringstream path_vfh_orientation;
        //path_vfh_orientation << "/u/aaldoma/data/vfhs/vfh_input_oldVFH.pcd";
        //std::cout << "Saving VFH histogram to:" << path_vfh_orientation.str () << std::endl;
        //pcl::io::savePCDFileASCII (path_vfh_orientation.str (), vfh_signature);

        centroids_dominant_orientations.push_back (cloud_centroid);
      }

      return true;
    }

  template<template<class > class Distance>
    bool
    VFHRecognizer<Distance>::loadFileList (std::vector<vfh_model_db> &models, const std::string &filename)
    {
      std::ifstream fs;
      fs.open (filename.c_str ());
      if (!fs.is_open () || fs.fail ())
        return (false);

      std::string line;
      while (!fs.eof ())
      {
        getline (fs, line);
        if (line.empty ())
          continue;
        vfh_model_db m;
        m.first = std::string (line.c_str ());
        models.push_back (m);
      }
      fs.close ();
      return (true);
    }

  template<template<class > class Distance>
    void
    VFHRecognizer<Distance>::saveFileList (const std::vector<vfh_model_db> &models, const std::string &filename)
    {
      std::ofstream fs;
      fs.open (filename.c_str ());
      for (size_t i = 0; i < models.size (); ++i)
        fs << models[i].first << "\n";
      fs.close ();
    }

  template<template<class > class Distance>
    void
    VFHRecognizer<Distance>::nearestKSearch (flann::Index<DistT> * index, const vfh_model_db &model, int k,
                                             flann::Matrix<int> &indices, flann::Matrix<float> &distances)
    {
      flann::Matrix<float> p = flann::Matrix<float> (new float[model.second.size ()], 1, model.second.size ());
      memcpy (&p.data[0], &model.second[0], p.cols * p.rows * sizeof(float));

      indices = flann::Matrix<int> (new int[k], 1, k);
      distances = flann::Matrix<float> (new float[k], 1, k);
      index->knnSearch (p, indices, distances, k, flann::SearchParams (512));
      p.free ();
    }

  template<template<class > class Distance>
    void
    VFHRecognizer<Distance>::getRollRotationAngles (std::vector<int> * roll_angles)
    {
      for (size_t i = 0; i < roll_rotation_angles_.size (); i++)
      {
        roll_angles->push_back (roll_rotation_angles_.at (i));
      }
    }

  template<template<class > class Distance>
    void
    VFHRecognizer<Distance>::getICPTransformations (std::vector<Eigen::Matrix4f,Eigen::aligned_allocator< Eigen::Matrix4f > > * icp_transformations)
    {
      for (size_t i = 0; i < icp_transformations_.size (); i++)
      {
        icp_transformations->push_back (icp_transformations_.at (i));
      }
    }

  template<template<class > class Distance>
    void
    VFHRecognizer<Distance>::getCentroids (std::vector<Eigen::Vector4f,Eigen::aligned_allocator< Eigen::Vector4f > > * centroids)
    {
      for (size_t i = 0; i < centroid_inputs_.size (); i++)
      {
        centroids->push_back (centroid_inputs_.at (i));
      }
    }

  template<template<class > class Distance>
    void
    VFHRecognizer<Distance>::getCentroidsResults (std::vector<Eigen::Vector4f,Eigen::aligned_allocator< Eigen::Vector4f > > * centroids)
    {
      for (size_t i = 0; i < centroid_results_.size (); i++)
      {
        centroids->push_back (centroid_results_.at (i));
      }
    }

  template<template<class > class Distance>
    int
    VFHRecognizer<Distance>::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)
    {

      for (size_t i = 2; i < (size_t)nr_bins_; i += 2)
      {
        hist_fft.at<float> (i) = -hist_fft.at<float> (i);
      }

      //int nr_bins_after_padding = 90;
      int nr_bins_after_padding = 180;
      int peak_distance = 4;
      cv::Mat multAB = cv::Mat_<float>::zeros (nr_bins_after_padding, 1);

      float a, b, c, d;

      multAB.at<float> (0) = hist_fft.at<float> (0) * hist_fft_input.at<float> (0);

      size_t cutoff = 5;
      cutoff = nr_bins_ - 1;
      for (size_t i = 1; i < cutoff; i += 2)
      {
        a = hist_fft.at<float> (i);
        b = hist_fft.at<float> (i + 1);
        c = hist_fft_input.at<float> (i);
        d = hist_fft_input.at<float> (i + 1);
        multAB.at<float> (i) = a * c - b * d;
        multAB.at<float> (i + 1) = b * c + a * d;

        float tmp = sqrt (
                          multAB.at<float> (i) * multAB.at<float> (i) + multAB.at<float> (i + 1)
                              * multAB.at<float> (i + 1));

        multAB.at<float> (i) /= tmp;
        multAB.at<float> (i + 1) /= tmp;
      }

      multAB.at<float> (nr_bins_ - 1) = hist_fft.at<float> (nr_bins_ - 1) * hist_fft_input.at<float> (nr_bins_ - 1);

      cv::Mat invAB;
      cv::idft (multAB, invAB, cv::DFT_SCALE);

      std::vector < std::pair<double, int> > scored_peaks (nr_bins_after_padding);

      pcl::PointCloud < pcl::VFHSignature308 > cross_power;
      cross_power.points.resize (1);
      cross_power.width = cross_power.height = 1;

      for (size_t i = 0; i < (size_t)nr_bins_after_padding; i++)
      {
        scored_peaks[i] = std::make_pair<double, size_t> (invAB.at<float> (i), (int)i);
        cross_power.points[0].histogram[i] = invAB.at<float> (i);
      }

      /*std::stringstream path_vfh_orientation;
       path_vfh_orientation << "/u/aaldoma/data/vfhs_roll_orientation/cross_input_" << jj << ".pcd";
       std::cout << path_vfh_orientation.str() << std::endl;
       pcl::io::savePCDFileBinary (path_vfh_orientation.str(), cross_power);*/

      std::sort (scored_peaks.begin (), scored_peaks.end (), peaks_ordering ());
      std::vector<int> peaks;

      // we look at the upper 20% quantile
      for (size_t i = 0; i < (size_t) (0.2 * nr_bins_after_padding); i++)
      {
        if (scored_peaks[i].first >= scored_peaks[0].first * 0.8)
        {
          bool insert = true;

          for (size_t j = 0; j < peaks.size (); j++)
          { //check inserted peaks, first pick always inserted
            //std::cout << peaks[j] << " " << scored_peaks[i].second << " " << fabs ((int)peaks[j] - (int)scored_peaks[i].second) << std::endl;
            if (fabs (peaks[j] - scored_peaks[i].second) <= peak_distance ||
                fabs ( peaks[j] - (scored_peaks[i].second - nr_bins_after_padding)) <= peak_distance)
            {
              insert = false;
              break;
            }
          }

          if (insert) {
            std::cout << "Inserted:" << (float)scored_peaks[i].first << " " << scored_peaks[i].second << std::endl;
            peaks.push_back (scored_peaks[i].second);
          }
        }
      }

      int max_idx = 0;
      int NUM_PEAKS_CHECKED = 0;
      pcl::PointCloud<pcl::PointNormal>::Ptr input_ptr (new pcl::PointCloud<pcl::PointNormal> (input));
      double max_score = std::numeric_limits<double>::min ();

      //PCLVisualizer vis ("roll orientation");

      for (size_t i = 0; i < peaks.size (); i++)
      {
        pcl::PointCloud < pcl::PointNormal > cloud_xyz;
        cloud_xyz = view;

        pcl::PointCloud<pcl::PointNormal>::Ptr gridTransformed (new pcl::PointCloud<pcl::PointNormal> ());

        double current_angle = peaks[i] * (360 / nr_bins_after_padding);
        Eigen::Affine3f final_trans;
        computeRollTransform (centroid_input, centroid_view, current_angle, final_trans);

        pcl::transformPointCloudWithNormals (cloud_xyz, *gridTransformed, final_trans);

        //translate result point cloud so centroids match...
        Eigen::Vector3f centr (centroid_view[0], centroid_view[1], centroid_view[2]);
        centr = final_trans * centr;
        Eigen::Vector4f diff = Eigen::Vector4f::Zero ();
        diff[0] = -centroid_input[0] + centr[0];
        diff[1] = -centroid_input[1] + centr[1];
        diff[2] = -centroid_input[2] + centr[2];

        cloud_xyz = *gridTransformed;

        pcl::PointCloud<pcl::PointNormal>::Ptr cloud_xyz_demean (new pcl::PointCloud<pcl::PointNormal> ());

        pcl::demeanPointCloud (cloud_xyz, diff, *cloud_xyz_demean);

        pcl::SegmentDifferences < pcl::PointNormal > seg;
        typedef pcl::KdTree<pcl::PointNormal>::Ptr KdTreePtr;
        KdTreePtr normals_tree = boost::make_shared<pcl::KdTreeFLANN<pcl::PointNormal> > (true);
        seg.setDistanceThreshold (0.005 * 0.005); //1cm
        seg.setSearchMethod (normals_tree);
        seg.setInputCloud (input_ptr);
        seg.setTargetCloud (cloud_xyz_demean);

        pcl::PointCloud < pcl::PointNormal > difference;
        seg.segment (difference);

        //int smallest_pointcloud = std::min(input.points.size(),cloud_xyz_demean->points.size());
        double fscore = input.points.size () - difference.points.size (); //number of inliers

        //std::cout << "angle checked as peak:" <<  peaks[i] * (360 / nr_bins_after_padding) << " fscore:" << fscore << std::endl;

        if (fscore > max_score)
        {
          max_score = fscore;
          max_idx = peaks[i];
        }

        NUM_PEAKS_CHECKED++;
      }

      std::cout << "NUM_PEAKS_CHECKED:" << NUM_PEAKS_CHECKED << " " << max_idx << std::endl;
      return max_idx * (360 / nr_bins_after_padding);
    }

  template<template<class > class Distance>
    void
    VFHRecognizer<Distance>::computeTransformToZAxes (Eigen::Vector4f & centroid, Eigen::Affine3f & transform)
    {
      Eigen::Vector3f plane_normal;
      plane_normal[0] = -centroid[0];
      plane_normal[1] = -centroid[1];
      plane_normal[2] = -centroid[2];
      Eigen::Vector3f z_vector = Eigen::Vector3f::UnitZ ();
      plane_normal.normalize ();
      Eigen::Vector3f axis = plane_normal.cross (z_vector);
      double rotation = -asin (axis.norm ());
      axis.normalize ();
      transform = Eigen::Affine3f (Eigen::AngleAxisf (rotation, axis));
    }

  template<template<class > class Distance>
    void
    VFHRecognizer<Distance>::computeRollTransform (Eigen::Vector4f & centroidInput, Eigen::Vector4f & centroidResult,
                                                   double roll_angle, Eigen::Affine3f & final_trans)
    {
      Eigen::Affine3f transformInputToZ;
      computeTransformToZAxes (centroidInput, transformInputToZ); //use current cluster centroid
      //std::cout << transformInputToZ.matrix() << std::endl;
      transformInputToZ = transformInputToZ.inverse ();

      Eigen::Affine3f transformRoll (Eigen::AngleAxisf (-(roll_angle * M_PI / 180), Eigen::Vector3f::UnitZ ()));

      Eigen::Affine3f transformDBResultToZ;
      computeTransformToZAxes (centroidResult, transformDBResultToZ); //use current cluster centroid

      final_trans = transformInputToZ * transformRoll * transformDBResultToZ;
    }

  template<template<class > class Distance>
    bool
    VFHRecognizer<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, std::vector<std::string>& vfh_ids,
                                      bool use_fitness_score)
    {

      //reset stuff from one detection to the following
      roll_rotation_angles_.clear ();
      icp_transformations_.clear ();
      centroid_inputs_.clear ();
      centroid_results_.clear ();
      roll_histogram_errors_.clear ();
      roll_histogram_errors_flipped_.clear ();
      fitness_scores_.clear ();

      PointCloudAndNormals::Ptr cloud_normals (new PointCloudAndNormals ());
      pcl::PointCloud<pcl::VFHSignature308>::Ptr vfh_signature (new pcl::PointCloud<pcl::VFHSignature308> ());

      PointCloud::Ptr cloud (new PointCloud ());

      pcl::fromROSMsg (*msg, *cloud);

      //compute CVFH for the point cloud
      std::vector<pcl::PointCloud<pcl::VFHSignature308>,Eigen::aligned_allocator< pcl::PointCloud<pcl::VFHSignature308> > > vfh_signatures;
      std::vector < Eigen::Vector3f > centroids_dominant_orientations;

      computeVFH (cloud, cloud_normals, vfh_signatures, centroids_dominant_orientations); //centroids of the input clusters...

      //TODO: Deactivate this...
      pcl::copyPointCloud (vfh_signatures[0], vfh_histogram_processed);
      pcl::copyPointCloud (*cloud_normals, input_filtered);

      //allocate space for roll histograms and for input centroids
      std::vector < pcl::PointCloud<pcl::VFHSignature308>::Ptr > orientation_histogram_rolls_input; //(vfh_signatures.size (),NULL);
      std::vector < cv::Mat > frequency_domain_hists_input;
      orientation_histogram_rolls_input.resize (vfh_signatures.size ());
      frequency_domain_hists_input.resize (vfh_signatures.size ());
      std::vector < Eigen::Vector3f > centroids_input;
      centroids_input.resize (vfh_signatures.size ());

      Eigen::Vector4f centroidInput;
      pcl::PointCloud<pcl::PointNormal>::Ptr cloud_xyz_demean_input (new pcl::PointCloud<pcl::PointNormal> ());

      pcl::compute3DCentroid (*cloud_normals, centroidInput);
      pcl::demeanPointCloud (*cloud_normals, centroidInput, *cloud_xyz_demean_input);

      if (use_cluster_centroids_)
      {
        for (size_t jj = 0; jj < vfh_signatures.size (); jj++)
        {
          //for each VFH, compute the orientation histogram
          //using the cluster centroids
          pcl::PointCloud<pcl::VFHSignature308>::Ptr
                                                     vfh_o_signatureInput (new pcl::PointCloud<pcl::VFHSignature308> ());
          Eigen::Vector3f ci3f (centroids_dominant_orientations[jj][0], centroids_dominant_orientations[jj][1],
                                centroids_dominant_orientations[jj][2]);

          cv::Mat frequency_domain_input;
          computeVFHRollOrientation (cloud_normals, vfh_o_signatureInput, ci3f, frequency_domain_input);
          orientation_histogram_rolls_input[jj] = vfh_o_signatureInput;
          frequency_domain_hists_input[jj] = frequency_domain_input;
          centroids_input[jj] = ci3f;

          /*std::stringstream path_vfh_orientation;
           path_vfh_orientation << "/u/aaldoma/data/vfhs_roll_orientation/vfh_orientation_input_" << jj << ".pcd";
           std::cout << path_vfh_orientation.str() << std::endl;
           pcl::io::savePCDFileBinary (path_vfh_orientation.str(), *vfh_o_signatureInput);*/
        }
      }
      else
      {
        //compute one orientation histogram for the whole view
        //using the centroid of the whole view
        pcl::PointCloud<pcl::VFHSignature308>::Ptr vfh_o_signatureInput (new pcl::PointCloud<pcl::VFHSignature308> ());
        Eigen::Vector3f ci3f (centroidInput[0], centroidInput[1], centroidInput[2]);
        cv::Mat frequency_domain_input;
        computeVFHRollOrientation (cloud_normals, vfh_o_signatureInput, ci3f, frequency_domain_input);
        orientation_histogram_rolls_input[0] = vfh_o_signatureInput;
        frequency_domain_hists_input[0] = frequency_domain_input;
        centroids_input[0] = ci3f;
      }

      double fscore = 0;
      std::vector<index_score> index_scores;
      std::vector<int> model_indices;
      std::vector<int> roll_input_indices;
      std::vector<int> input_centroid_indices;
      std::vector<float> roll_histogram_errors;
      std::vector<float> roll_histogram_errors_flipped;
      int counter = 0;

      //search for all different dominant orientations
      for (size_t jj = 0; jj < vfh_signatures.size (); jj++)
      {
        float* hist = vfh_signatures[jj].points[0].histogram;
        std::vector<float> std_hist (hist, hist + 308);
        vfh_model_db histogram ("", std_hist);

        //size_t k = nModels*5;
        //size_t k = 15;
        size_t k = nModels;
        flann::Matrix<int> indices;
        flann::Matrix<float> distances;

        nearestKSearch (index_, histogram, k, indices, distances);

        for (size_t i = 0; i < k; ++i, ++counter)
        {
          std::string vfh_id = models_.at (indices[0][i]).first;
          model_indices.push_back (indices[0][i]);

          fscore = distances[0][i];
          index_score is;
          is.first = counter;
          is.second = fscore;
          index_scores.push_back (is);
          if (use_cluster_centroids_)
          {
            roll_input_indices.push_back (jj);
            input_centroid_indices.push_back (jj);
          }
          else
          {
            roll_input_indices.push_back (0); //we will have just one of them
            input_centroid_indices.push_back (0);
          }
        }
      }

      //at this point, the VFHs have been searched and concatenated into
      //an unsorted list (index_scores)
      //sort the list by index score as we might have performed more than one search
      std::sort (index_scores.begin (), index_scores.end (), ICPredicate);

      std::vector<index_score> index_scores_short;
      std::vector<Eigen::Vector4f,Eigen::aligned_allocator< Eigen::Vector4f > > centroids;
      std::vector<Eigen::Vector4f,Eigen::aligned_allocator< Eigen::Vector4f > > centroidsInput;
      std::vector<Eigen::Matrix4f,Eigen::aligned_allocator< Eigen::Matrix4f > > icp_transformations;
      //std::vector < Eigen::Vector4f > centroids;
      //std::vector < Eigen::Vector4f > centroidsInput;
      //std::vector < Eigen::Matrix4f > icp_transformations;
      std::vector<int> roll_rotation_angles;
      std::vector<int> model_indices_short; //points to the vfh_ids in models

      //std::cout << "Number of total models:" << nModels << " size of index_scores:" << index_scores.size() << std::endl;
      counter = 0;

      {
        pcl::ScopeTime t ("-------- fitness score, ICP and so on...");

        //process nModels, computing roll orientation and ICP as needed
        for (size_t i = 0; i < (size_t)nModels; ++i)
        {
          //model_indices[index_scores.at (i).first] points to models_, first gives vfh_id
          //we want model_indices_short to do the same
          std::string vfh_id = models_.at (model_indices[index_scores.at (i).first]).first;

          std::cout << "vfh_id:" << vfh_id << " " << index_scores.at (i).first << " "
              << model_indices[index_scores.at (i).first] << " " << i << std::endl;

          model_indices_short.push_back (model_indices[index_scores.at (i).first]);

          pcl::PointCloud < pcl::VFHSignature308 > vfh_orientation_signature;

          //get VFH Roll orientation
          if (use_cluster_centroids_)
          {
            //get the roll orientation for the VFH_id
            getVFHRollOrientationFromId (vfh_orientation_signature, vfh_id);

            /*std::stringstream path_vfh_orientation;
             path_vfh_orientation << "/u/aaldoma/data/vfhs_roll_orientation/vfh_orientation_view_" << i << ".pcd";
             std::cout << path_vfh_orientation.str() << std::endl;
             pcl::io::savePCDFileBinary (path_vfh_orientation.str(), vfh_orientation_signature);*/

          }
          else
          {
            //get the roll orientation for the view associated with vfh_id
            getVFHRollOrientationFromIdThroughView (vfh_orientation_signature, vfh_id);
          }

          //Minimize square errors between histograms (vfh_orientation_signature and vfh_orientation_signatureInput)
          //vfh_orientation_signatureInput does not change
          double error = -1.0;
          double error_flipped = -1.0;
          int angle;

          //pcl::PointCloud<pcl::VFHSignature308>::Ptr vfh_o_signatureInput_ptr;
          //vfh_o_signatureInput_ptr = orientation_histogram_rolls_input[roll_input_indices[index_scores.at (i).first]]; //access the correct roll histogram
          //std::cout << "INDEX TO ROLL HISTOGRAM:" << roll_input_indices[index_scores.at (i).first] << std::endl;
          //angle = computeAngleRollOrientation (vfh_orientation_signature, vfh_o_signatureInput, nr_bins, &error);

          pcl::PointCloud<pcl::PointNormal>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointNormal> ());
          getPointCloudFromId (*cloud_xyz, vfh_id);

          if (cloud_xyz->points.size () == 0)
            break;

          pcl::PointCloud<pcl::VFHSignature308>::Ptr
                                                     vfh_o_signature_trash (
                                                                            new pcl::PointCloud<pcl::VFHSignature308> ());

          std::vector<float> centroid_vfh_db (3, 0);
          getVFHCentroidFromVFHid (centroid_vfh_db, vfh_id); //centroids of the clusters in the database

          Eigen::Vector3f ci3f (centroid_vfh_db[0], centroid_vfh_db[1], centroid_vfh_db[2]);
          cv::Mat frequency_domain_view;
          computeVFHRollOrientation (cloud_xyz, vfh_o_signature_trash, ci3f, frequency_domain_view);

          /*std::stringstream path_vfh_orientation;
           path_vfh_orientation << "/u/aaldoma/data/vfhs_roll_orientation/vfh_orientation_view_" << i << ".pcd";
           std::cout << path_vfh_orientation.str() << std::endl;
           pcl::io::savePCDFileBinary (path_vfh_orientation.str(), *vfh_o_signature_trash);*/

          /*std::stringstream path_vfh_orientation;
           path_vfh_orientation << "/u/aaldoma/data/vfhs_roll_orientation/vfh_orientation_frequency_domain:" << vfh_id << ".pcd";
           //std::cout << path_vfh_orientation.str() << std::endl;
           pcl::io::savePCDFileBinary (path_vfh_orientation.str(), *vfh_o_signature_trash);*/

          //TODO: dirty stuff
          pcl::PointCloud < pcl::PointNormal > cloud_xyz_1;
          getPointCloudFromId (cloud_xyz_1, vfh_id);

          std::cout << "Points in pointcloud:" << cloud_xyz_1.size () << std::endl;

          std::vector<float> centroid_vfh_db_1 (3, 0);
          getVFHCentroidFromVFHid (centroid_vfh_db_1, vfh_id); //centroids of the clusters in the database
          Eigen::Vector4f
                          centroidClusterInput (
                                                centroids_dominant_orientations[roll_input_indices[index_scores.at (i).first]][0],
                                                centroids_dominant_orientations[roll_input_indices[index_scores.at (i).first]][1],
                                                centroids_dominant_orientations[roll_input_indices[index_scores.at (i).first]][2],
                                                0);

          Eigen::Vector4f centroid_view (centroid_vfh_db_1[0], centroid_vfh_db_1[1], centroid_vfh_db_1[2], 0);

          pcl::PointCloud<pcl::VFHSignature308>::Ptr vfh_o_signatureInput_ptr;
          vfh_o_signatureInput_ptr = orientation_histogram_rolls_input[roll_input_indices[index_scores.at (i).first]]; //access the correct roll histogram
          angle
              = computeAngleRollOrientationFrequency (
                                                      frequency_domain_hists_input[roll_input_indices[index_scores.at (
                                                                                                                       i).first]],
                                                      frequency_domain_view, nr_bins, centroid_view,
                                                      centroidClusterInput, cloud_xyz_1, *cloud_normals, i,
                                                      *vfh_o_signature_trash, *vfh_o_signatureInput_ptr);

          roll_rotation_angles.push_back (angle);
          roll_histogram_errors.push_back (error);
          roll_histogram_errors_flipped.push_back (error_flipped);

          if (use_cluster_centroids_)
          {
            getVFHCentroidFromVFHid (centroid_vfh_db, vfh_id); //centroids of the clusters in the database
            Eigen::Vector4f
                            centroidClusterInput (
                                                  centroids_dominant_orientations[roll_input_indices[index_scores.at (i).first]][0],
                                                  centroids_dominant_orientations[roll_input_indices[index_scores.at (i).first]][1],
                                                  centroids_dominant_orientations[roll_input_indices[index_scores.at (i).first]][2],
                                                  0);

            centroidsInput.push_back (centroidClusterInput);
          }
          else
          {
            getViewCentroidFromVFHid (centroid_vfh_db, vfh_id); //centroids of the view...
            centroidsInput.push_back (centroidInput);
          }

          Eigen::Vector4f centroid_vfh (centroid_vfh_db[0], centroid_vfh_db[1], centroid_vfh_db[2], 0);
          centroids.push_back (centroid_vfh); //get centroid from the VFH (cluster being used) or the view in the database...

          if (use_fitness_score || perform_icp_)
          {
            pcl::PointCloud < pcl::PointNormal > cloud_xyz;
            getPointCloudFromId (cloud_xyz, vfh_id);

            if (cloud_xyz.points.size () == 0)
              break;

            pcl::PointCloud<pcl::PointNormal>::Ptr gridTransformed (new pcl::PointCloud<pcl::PointNormal> ());

            Eigen::Affine3f final_trans;
            computeRollTransform (centroidsInput[counter], centroids[counter], roll_rotation_angles.at (counter),
                                  final_trans);

            //pcl::transformPointCloudWithNormals (cloud_xyz, *gridTransformed, transform);
            pcl::transformPointCloudWithNormals (cloud_xyz, *gridTransformed, final_trans);

            //translate result point cloud so centroids match...
            Eigen::Vector3f centr (centroids[counter][0], centroids[counter][1], centroids[counter][2]);
            centr = final_trans * centr;
            Eigen::Vector4f diff = Eigen::Vector4f::Zero ();
            diff[0] = -centroidsInput[counter][0] + centr[0];
            diff[1] = -centroidsInput[counter][1] + centr[1];
            diff[2] = -centroidsInput[counter][2] + centr[2];

            cloud_xyz = *gridTransformed;

            pcl::PointCloud<pcl::PointNormal>::Ptr cloud_xyz_demean (new pcl::PointCloud<pcl::PointNormal> ());
            pcl::demeanPointCloud (cloud_xyz, diff, *cloud_xyz_demean);

            //Compute fitness score, we want the transformation from model to input
            if (perform_icp_)
            {

              std::stringstream path_vfh_orientation;
              path_vfh_orientation << "/home/aitor/cloud_xyz_demean.pcd";
              pcl::io::savePCDFileASCII (path_vfh_orientation.str(), *cloud_xyz_demean);

              std::stringstream path_2;
              path_2 << "/home/aitor/cloud_normals.pcd";
              pcl::io::savePCDFileASCII (path_2.str(), *cloud_normals);

              std::cout << "Computing ICP" << std::endl;
              pcl::IterativeClosestPoint < pcl::PointNormal, pcl::PointNormal > reg;
              reg.setInputCloud (cloud_xyz_demean); //model aligned after VFH
              reg.setInputTarget (cloud_normals); //cluster in the camera coordinate system

              //Perform ICP and save the final transformation, transformation will be appended later
              //after sorting...
              reg.setMaximumIterations (icp_iterations_);
              reg.setMaxCorrespondenceDistance (0.02);
              pcl::PointCloud<pcl::PointNormal>::Ptr output_ (new pcl::PointCloud<pcl::PointNormal> ());
              reg.align (*output_);

              Eigen::Matrix4f icp_trans = reg.getFinalTransformation ();
              icp_transformations.push_back (icp_trans);

              if (use_fitness_score)
              {
                pcl::SegmentDifferences < pcl::PointNormal > seg;
                typedef pcl::KdTree<pcl::PointNormal>::Ptr KdTreePtr;
                KdTreePtr normals_tree = boost::make_shared<pcl::KdTreeFLANN<pcl::PointNormal> > (true);
                seg.setDistanceThreshold (0.005 * 0.005); //1cm
                seg.setSearchMethod (normals_tree);
                seg.setInputCloud (cloud_normals);
                //seg.setTargetCloud(cloud_xyz_demean);
                seg.setTargetCloud (output_);

                pcl::PointCloud < pcl::PointNormal > difference;
                seg.segment (difference);
                //fscore = 1.0 / (cloud_normals->points.size() - difference.points.size()); //number of inliers
                std::cout << "difference size:" << difference.points.size () << " " << cloud_normals->points.size ()
                    << std::endl;
                fscore = difference.points.size ();
              }
              else
              {
                fscore = index_scores.at (i).second;
              }
            }
            else
            {
              pcl::SegmentDifferences < pcl::PointNormal > seg;
              typedef pcl::KdTree<pcl::PointNormal>::Ptr KdTreePtr;
              KdTreePtr normals_tree = boost::make_shared<pcl::KdTreeFLANN<pcl::PointNormal> > (true);
              seg.setDistanceThreshold (0.01 * 0.01); //1cm
              seg.setSearchMethod (normals_tree);
              seg.setInputCloud (cloud_normals);
              seg.setTargetCloud (cloud_xyz_demean);

              pcl::PointCloud < pcl::PointNormal > difference;
              seg.segment (difference);

              fscore = difference.points.size ();
            }
          }
          else
          {
            std::cout << "NOT USING FITNESS SCORE OR ICP" << std::endl;
            fscore = index_scores.at (i).second;
          }

          std::cout << "fscore:" << fscore << std::endl;
          index_score is;
          is.first = counter;
          is.second = fscore;
          index_scores_short.push_back (is);
          counter++;
        }
      } //end of time scope, final step...

      std::sort (index_scores_short.begin (), index_scores_short.end (), ICPredicate);

      size_t result_size = (std::min) ((size_t) (nModels), index_scores_short.size ());
      //otherwise its already sorted
      poses.resize (result_size);
      model_ids.resize (result_size);
      vfh_ids.resize (result_size);
      confidences.resize (result_size);
      roll_rotation_angles_.resize (result_size);
      roll_histogram_errors_.resize (result_size);
      roll_histogram_errors_flipped_.resize (result_size);

      centroid_inputs_.resize (result_size);
      centroid_results_.resize (result_size);

      if (perform_icp_)
        icp_transformations_.resize (result_size);

      {
        pcl::ScopeTime t ("-------- computes poses, final step");
        //Populate output structures
        for (size_t i = 0; i < result_size; ++i)
        {
          std::string vfh_id = models_.at (model_indices_short[index_scores_short.at (i).first]).first;
          geometry_msgs::Pose p;
          getPoseFromId (vfh_id, p);
          Eigen::Quaternion<double> pq = Eigen::Quaternion<double> (p.orientation.w, p.orientation.x, p.orientation.y,
                                                                    p.orientation.z);

          //transform quaternion to matrix...
          Eigen::Matrix3d poseToRot = pq.toRotationMatrix ();
          Eigen::Matrix4f homMatrix = Eigen::Matrix4f ();
          homMatrix.setIdentity (4, 4);
          homMatrix.block (0, 0, 3, 3) = poseToRot.cast<float> ();

          //roll
          Eigen::Affine3f rollToRot;
          computeRollTransform (centroidsInput[index_scores_short.at (i).first],
                                centroids[index_scores_short.at (i).first],
                                roll_rotation_angles.at (index_scores_short.at (i).first), rollToRot);
          Eigen::Matrix4f rollHomMatrix = Eigen::Matrix4f ();
          rollHomMatrix.setIdentity (4, 4);
          rollHomMatrix = rollToRot.matrix ();
          //std::cout << rollHomMatrix << std::endl;
          //rollHomMatrix.block (0, 0, 4, 3) = rollToRot.matrix();

          //translation matrix (1) - translates the mesh to the view
          Eigen::Matrix4f translation1;
          translation1.setIdentity (4, 4);
          translation1 (0, 3) = p.position.x;
          translation1 (1, 3) = p.position.y;
          translation1 (2, 3) = p.position.z;

          Eigen::Vector4f cInput = centroidsInput[index_scores_short.at (i).first];

          //translation matrix (2) - center the view and the input
          Eigen::Vector4f centroidView = centroids[index_scores_short.at (i).first]; //contains the centroid of the rotated point cloud
          Eigen::Matrix4f translation2;
          translation2.setIdentity (4, 4);

          Eigen::Vector3f centr (centroidView[0], centroidView[1], centroidView[2]);
          centr = rollToRot * centr;

          translation2 (0, 3) = -centr[0] + cInput[0];
          translation2 (1, 3) = -centr[1] + cInput[1];
          translation2 (2, 3) = -centr[2] + cInput[2];

          //std::cout << "CentroidView:" << centroidView << std::endl;
          //std::cout << "CentroidInput:" << cInput << std::endl;

          Eigen::Matrix4f resultHom = Eigen::Matrix4f ();
          Eigen::Matrix4f resultHom_1 = Eigen::Matrix4f ();
          resultHom_1.setIdentity (4, 4);

          if (perform_icp_)
          //if (perform_icp_ && (i < ICP_SIZE))
          {
            //append transformation
            //Eigen::Matrix4f icp_trans = icp_transformations.at (i);
            Eigen::Matrix4f icp_trans = icp_transformations.at (index_scores_short.at (i).first);
            //RESULT = TRANSLATION2 * (ROLL * (TRANSLATION1 * POSE_MATRIX)) * [p]
            resultHom = icp_trans * translation2 * (rollHomMatrix * translation1 * homMatrix); //result
          }
          else
          {
            //RESULT = TRANSLATION2 * (ROLL * (TRANSLATION1 * POSE_MATRIX)) * [p]
            resultHom = translation2 * (rollHomMatrix * translation1 * homMatrix); //result
            //resultHom = translation2 * (translation1 * homMatrix); //result
          }

          //transform resultHom to pose
          p.position.x = resultHom (0, 3);
          p.position.y = resultHom (1, 3);
          p.position.z = resultHom (2, 3);

          Eigen::Matrix3f rotFinal = resultHom.block (0, 0, 3, 3);
          Eigen::Quaternion<float> final = Eigen::Quaternion<float> (rotFinal);
          p.orientation.x = final.x ();
          p.orientation.y = final.y ();
          p.orientation.z = final.z ();
          p.orientation.w = final.w ();

          poses[i] = p;
          model_ids[i] = getModelId (vfh_id);
          vfh_ids[i] = vfh_id;
          confidences[i] = index_scores_short.at (i).second;
          roll_rotation_angles_[i] = roll_rotation_angles.at (index_scores_short.at (i).first);
          roll_histogram_errors_[i] = roll_histogram_errors.at (index_scores_short.at (i).first);
          roll_histogram_errors_flipped_[i] = roll_histogram_errors_flipped.at (index_scores_short.at (i).first);

          centroid_inputs_[i] = cInput;
          centroid_results_[i] = centroidView;

          if (perform_icp_)
            icp_transformations_[i] = icp_transformations.at (index_scores_short.at (i).first);
        }
      }

      return true;

    }

  template class VFHRecognizer<flann::ChiSquareDistance> ;
  template class VFHRecognizer<flann::L2> ;
  template class VFHRecognizer<flann::HistIntersectionDistance> ;
  template class VFHRecognizer<flann::HistIntersectionUnionDistance> ;
}

---

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