finddd_alg.cpp

Go to the documentation of this file.

#include "finddd_alg.h"

#define PI 3.1415926535

//helper fun
inline 
std::vector<float> get_bin_float_empty(std::vector<float> &bin_centers_phi, std::vector<float> &bin_centers_theta)
{
  int nb_phi=bin_centers_phi.size();
  std::vector<float> val_bins;
  val_bins.resize(nb_phi,0);
  return val_bins;
}

inline 
std::vector<float> get_bin_float_interp_xyz(float x, float y, float z, std::vector<float> &bin_centers_x, std::vector<float> &bin_centers_y, std::vector<float> &bin_centers_z, float radius_inf)
{
  int nb_bins=bin_centers_x.size();
  std::vector<float> val_bins;
  val_bins.resize(nb_bins,0);

  //radius_inf now determined from loaded centroids
  //float radius_inf = 0.4; //0.39 is dist among bins in 54 bin case.

  for(int ith=0; ith<nb_bins; ith++)
  {
    float dx=(x-bin_centers_x[ith]);
    float dy=(y-bin_centers_y[ith]);
    float dz=(z-bin_centers_z[ith]);
    float dist = sqrt(dx*dx+dy*dy+dz*dz);
    float vote = std::max(1-dist/radius_inf, (float)0.0);
    //From D Lowe, IJCV 04: Therefore, trilinear interpolation is used to
    //distribute the value of each gradient sample into adjacent histogram
    //bins. In other words, each entry into a bin is multiplied by a weight
    //of 1 − d for each dimension, where d is the distance of the sample
    //from the central value of the bin as measured in units of the
    //histogram bin spacing.
    
    val_bins[ith] = vote;
  }
  return val_bins;
}

inline 
std::vector<float> get_bin_float_xyz(float x, float y, float z, std::vector<float> &bin_centers_x, std::vector<float> &bin_centers_y, std::vector<float> &bin_centers_z)
{
  int nb_bins=bin_centers_x.size();
  std::vector<float> val_bins;
  val_bins.resize(nb_bins,0);

  int best_match=0;
  float dist_best_match=999999999999;

  for(int ith=0; ith<nb_bins; ith++)
  {
    float dx=(x-bin_centers_x[ith]);
    float dy=(y-bin_centers_y[ith]);
    float dz=(z-bin_centers_z[ith]);
    float dist = sqrt(dx*dx+dy*dy+dz*dz);
    if(dist < dist_best_match)
    {
      best_match = ith;
      dist_best_match = dist;
    }
  }
  val_bins[best_match] = 1;

  return val_bins;
}

void compute_normals_integral(pcl::PointCloud<pcl::Normal> &pcl_normals, pcl::PointCloud<pcl::PointXYZ>& cloud)
{
  //#include <pcl/features/integral_image_normal.h>
  //compute normal
  pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
  normal_estimator.setNormalEstimationMethod (normal_estimator.AVERAGE_3D_GRADIENT);
  normal_estimator.setMaxDepthChangeFactor(0.02f);
  normal_estimator.setNormalSmoothingSize(10.0f);
  normal_estimator.setInputCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZ> > (cloud));
  normal_estimator.compute (pcl_normals);
  //pcl::io::savePCDFileASCII("/tmp/cloud_pointnormals.pcd", pcl_normals);
}

void compute_normals(pcl::PointCloud<pcl::Normal> &pcl_normals, pcl::PointCloud<pcl::PointXYZ>& cloud)
{
  //compute normal
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
  normal_estimator.setInputCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZ> > (cloud));
  normal_estimator.setKSearch (400);
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
  //pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr tree = boost::make_shared<pcl::KdTreeFLANN<pcl::PointXYZ> > ();
  normal_estimator.setSearchMethod (tree);
  normal_estimator.compute (pcl_normals);
}

class IIf
{
public:
  std::vector<float*> ii_;
  unsigned int bins_;
  unsigned int width_;
  unsigned int height_;

  IIf(unsigned int bins, unsigned int width, unsigned int height):bins_(bins),width_(width),height_(height){
    ii_.resize(bins);
    for(unsigned int i=0;i<bins;i++)
      ii_[i]=new float[width*height];
  }
  ~IIf(){
    for(unsigned int i=0;i<this->bins_;i++)
      delete [] ii_[i];
  }
  void construct(std::vector<std::vector<float> > &im){
    //0,0
    unsigned int u=0;
    unsigned int v=0;
    for(unsigned int b=0;b<this->bins_;b++)
    {
      //printf("%d %d %d\n",u,v,b);fflush(0);
      ii_[b][this->width_*v+u] = im[this->width_*v+u][b];
    }

    //n,0
    v=0;
    for(u=1;u<this->width_;u++)
    {
      for(unsigned int b=0;b<this->bins_;b++)
      {
        //printf("%d %d %d\n",u,v,b);fflush(0);
        ii_[b][this->width_*v+u] =                   \
          ii_[b][this->width_*v+(u-1)] +             \
          im[this->width_*v+u][b];
        //printf("%d ",im[this->width_*v+u]);
      }
    }
    //printf("0,n\n");fflush(0);
    //0,n
    u=0;
    for(v=1;v<this->height_;v++)
    {
      for(unsigned int b=0;b<this->bins_;b++)
      {
        //printf("%d %d %d\n",u,v,b);fflush(0);
        ii_[b][this->width_*v+u] =                   \
          ii_[b][this->width_*(v-1)+u] +             \
          im[this->width_*v+u][b];
        //printf("%d ",im[this->width_*v+u]);
      }
    }
    //n,n
    for(v=1;v<this->height_;v++)
    {
      for(u=1;u<this->width_;u++)
      {
        for(unsigned int b=0;b<this->bins_;b++)
        {
          //printf("%d %d %d\n",u,v,b);fflush(0);
          ii_[b][this->width_*v+u] =                 \
            ii_[b][this->width_*(v-1)+u] +           \
            ii_[b][this->width_*v+(u-1)] -           \
            ii_[b][this->width_*(v-1)+(u-1)] +       \
            im[this->width_*v+u][b];
      
          //printf("%d ",im[this->width_*v+u]);
        }
      }
    }
    // FILE *pf=fopen("/tmp/aaaa.txt","w");
    // for(unsigned int b=0;b<this->bins_;b++)
    // {
    //   for(v=0;v<this->height_;v++)
    //   {
    //  for(u=0;u<this->width_;u++)
    //  {
    //    fprintf(pf,"%f ",ii_[b][this->width_*v+u]);
    //  }
    //  fprintf(pf,"\n");
    //   }
    // }
    // fclose(pf);
  }
  std::vector<float> get_sub_rect(unsigned int tu, unsigned int tv, unsigned int bu, unsigned int bv)
  {
    std::vector<float> ret(this->bins_);
    for(unsigned int b=0;b<this->bins_;b++)
    {
      ret[b] = ii_[b][this->width_*tv+tu] + ii_[b][this->width_*bv+bu] - ii_[b][this->width_*tv+bu] - ii_[b][this->width_*bv+tu];
      //ROS_INFO("(%i) %f + %f - %f - %f = %f", b, ii_[b][this->width_*tv+tu], ii_[b][this->width_*bv+bu], ii_[b][this->width_*tv+bu], ii_[b][this->width_*bv+tu], ret[b]);
      if(ret[b]<0){
        //ROS_ERROR("wtfwtf %f\n", ret[b]);
        ret[b]=0;
      }
    }

    return ret;
  }
};



FindddAlgorithm::FindddAlgorithm(void)
{
  //TODO check for new parameters!!!
  //TODO add the list of bin centers
  //TODO remove useless parameters

  //default values
  this->desc_num_side_spatial_bins_ = 1;
  this->desc_patch_radius_=15;  
  
  //patch radius
  this->set_desc_patch_radius(15);

  //Spatial bins (side)
  this->set_num_side_spatial_bins(1);

  //Sample each (num) pixels
  this->sample_each_ = 1;
  this->desc_compute_positionsX_.clear();
  this->desc_compute_positionsY_.clear();
  //this->centroids_file_ = "conf/xyz_centers13.txt";
  //this->update_centroids();
  this->set_positions_file("");
}

FindddAlgorithm::~FindddAlgorithm(void)
{
}

void FindddAlgorithm::config_update(Config& new_cfg, uint32_t level)
{
  this->lock();

  //TODO check what parameters need to be here
  if(this->config_.num_spatial_bins != new_cfg.num_spatial_bins)
  {
    this->set_num_side_spatial_bins(new_cfg.num_spatial_bins);
  }
  if(this->config_.desc_patch_radius != new_cfg.desc_patch_radius)
  {
    this->set_desc_patch_radius(new_cfg.desc_patch_radius);
  }
  if(this->config_.sample_each != new_cfg.sample_each)
  {
    this->set_sample_each(new_cfg.sample_each);
  }
  if(this->config_.centroids_file.compare(new_cfg.centroids_file)!=0)
  {
    this->set_centroids_file(new_cfg.centroids_file);
    this->update_centroids();
  }
  if(this->config_.positions_file.compare(new_cfg.positions_file)!=0)
  {
    this->set_positions_file(new_cfg.positions_file);
  }
  if(this->config_.normalize_desc != new_cfg.normalize_desc) // * 0 - No normalization
  {                                                           // * 1 - L1
    this->normalize_desc_ = new_cfg.normalize_desc;           // * 2 - L2
  }                                                           // * 3 - sqrt + L2
  if(this->config_.use_soft_voting != new_cfg.use_soft_voting)
  {
    this->use_soft_voting_ = new_cfg.use_soft_voting;
  }

  // save the current configuration
  this->config_=new_cfg;
  
  this->unlock();
}

// FindddAlgorithm Public API

void FindddAlgorithm::compute_ndescs_integral_spatial_interpolation(pcl::PointCloud<pcl::PointXYZ>& cloud, iri_perception_msgs::DescriptorSet &descriptor_set)
{

  clock_t start=clock();
  ROS_INFO("II with interpolation method.\n");

  ROS_INFO("PointCloud received");

  //compute normal
  pcl::PointCloud<pcl::Normal> pcl_normals; 
  compute_normals_integral(pcl_normals, cloud);
  //save normals file
  //pcl::PointCloud<pcl::PointNormal> pcl_pointnormals;
  //pcl::concatenateFields(cloud, pcl_normals, pcl_pointnormals); 
  //pcl::io::savePCDFileASCII("/tmp/cloud_pointnormals.pcd", pcl_pointnormals);
  
  //get spherical coordinates
  //pcl::PointCloud<pcl::PointXY> pcl_spherical; //(ab)using PointXY for holding spherical angles
  //compute_spherical_coords(pcl_normals, pcl_spherical);

  //compute descriptors
  descriptor_set.descriptors.clear();
  descriptor_set.len    = this->desc_num_total_bins_;
  descriptor_set.width  = pcl_normals.width;
  descriptor_set.height = pcl_normals.height;
  descriptor_set.num_orient_bins = this->orient_bin_centers_x_.size();
  descriptor_set.num_spa_bins = this->desc_num_side_spatial_bins_;


  //Compile all normal orientations and add them to the II
  std::vector<std::vector<float> > binned(cloud.width*cloud.height);

  // TODO this would be more compact with function pointers!!
  if(this->use_soft_voting_)
  {
    for(uint v=0;v<cloud.height;v++)
    {
      for(uint u=0;u<cloud.width;u++)
      {
        if(isnan(pcl_normals(u,v).normal_x) || isnan(pcl_normals(u,v).normal_y) || isnan(pcl_normals(u,v).normal_z))
          binned[u+cloud.width*v]=get_bin_float_empty(this->orient_bin_centers_x_, this->orient_bin_centers_y_); // empty vector
        else
        {
          binned[u+cloud.width*v] = 
            get_bin_float_interp_xyz(
              pcl_normals(u,v).normal_x, pcl_normals(u,v).normal_y, pcl_normals(u,v).normal_z, 
              this->orient_bin_centers_x_, this->orient_bin_centers_y_, this->orient_bin_centers_z_,
              this->radius_inf_);
        }
      }
    }
  }
  else
  {
    for(uint v=0;v<cloud.height;v++)
    {
      for(uint u=0;u<cloud.width;u++)
      {
        if(isnan(pcl_normals(u,v).normal_x) || isnan(pcl_normals(u,v).normal_y) || isnan(pcl_normals(u,v).normal_z))
          binned[u+cloud.width*v]=get_bin_float_empty(this->orient_bin_centers_x_, this->orient_bin_centers_y_); // empty vector
        else
        {
          binned[u+cloud.width*v] = 
            get_bin_float_xyz(
              pcl_normals(u,v).normal_x, pcl_normals(u,v).normal_y, pcl_normals(u,v).normal_z, 
              this->orient_bin_centers_x_, this->orient_bin_centers_y_, this->orient_bin_centers_z_);
        }
      }
    }
  }

  //Build II
  IIf intim(this->desc_num_orient_bins_, cloud.width, cloud.height);
  intim.construct(binned);
  ROS_INFO("built II\n");

  unsigned int spatial_bin_size    = (2*this->desc_patch_radius_+1)/this->desc_num_side_spatial_bins_;
  unsigned int spatial_bin_orients = this->desc_num_orient_bins_;

  //If no positions given, generate standard positions
  if(this->desc_compute_positionsX_.size()==0)
  {
    ROS_INFO("Generating default positions");
    for(unsigned int v=this->desc_patch_radius_;v<cloud.height-this->desc_patch_radius_-1;v+=this->sample_each_)
    {
      for(unsigned int u=this->desc_patch_radius_;u<cloud.width-this->desc_patch_radius_-1;u+=this->sample_each_)
      {      
        this->desc_compute_positionsX_.push_back(u);
        this->desc_compute_positionsY_.push_back(v);
      }
    }
  }
  
  //ROS_INFO("p;oasdpoasdifaposdf");

  //Compute descriptors
  for(unsigned int i_pos=0; i_pos<this->desc_compute_positionsX_.size();i_pos++)
  {
    unsigned int u=this->desc_compute_positionsX_[i_pos];
    unsigned int v=this->desc_compute_positionsY_[i_pos];
    //ROS_INFO("i_pos %d",i_pos);

    if( v<this->desc_patch_radius_ || 
        u<this->desc_patch_radius_ || 
        u>=cloud.width-this->desc_patch_radius_-1 || 
        v>=cloud.height-this->desc_patch_radius_-1)
    {
      //ROS_INFO("skipping %d %d",u,v);
      continue;
    }
    //ROS_INFO("computing %d %d",u,v);
    //ROS_INFO("Computing base descriptor");
    iri_perception_msgs::Descriptor desc;
    //insert point3D
    desc.point3d.x=cloud(u,v).x;
    desc.point3d.y=cloud(u,v).y;
    desc.point3d.z=cloud(u,v).z;
    
    //insert point2D
    desc.u=u;
    desc.v=v;
    
    desc.descriptor.resize(this->desc_num_total_bins_,0);
    float total_votes=0; // for normalization
    
    unsigned int absolute_top  = v-this->desc_patch_radius_;
    //int absolute_bottom = v+this->desc_patch_radius_;
    unsigned int absolute_left = u-this->desc_patch_radius_;
    //int absolute_right = u+this->desc_patch_radius_;
    //ROS_INFO("atop aleft %d %d",absolute_top,absolute_left);
    
    for(unsigned int k=0;k<this->desc_num_side_spatial_bins_; k++)
    {
      unsigned int bound_left  = absolute_left + k*spatial_bin_size;
      unsigned int bound_right = absolute_left + (k+1)*spatial_bin_size;
      for(unsigned int l=0; l<this->desc_num_side_spatial_bins_; l++)
      {
        unsigned int bound_top    = absolute_top + l*spatial_bin_size;
        unsigned int bound_bottom = absolute_top + (l+1)*spatial_bin_size;

        //ROS_INFO("(l,t,r,b)(%d,%d,%d,%d)",bound_left, bound_top, bound_right, bound_bottom);
        std::vector<float> tmp = intim.get_sub_rect(bound_left, bound_top, bound_right, bound_bottom);
        for(unsigned int i=0;i<spatial_bin_orients;i++)
        {
          unsigned int offset_full_rows = l*this->desc_num_side_spatial_bins_*spatial_bin_orients;
          unsigned int offset_this_row  = k*spatial_bin_orients;//swaped k and l, Arnau 4/4/12
          desc.descriptor[offset_full_rows + offset_this_row + i]=tmp[i];
          total_votes+=tmp[i];
        }
      }
    }
 
    if(this->normalize_desc_ != 0)
    {
      float sum=0;
      for(int iii=0; iii<desc.descriptor.size(); iii++)
      {
        //printf("%f ",desc.descriptor[iii]);
        if((this->normalize_desc_==1) || (this->normalize_desc_==3)) sum+=desc.descriptor[iii];
        else if(this->normalize_desc_==2) sum+=desc.descriptor[iii]*desc.descriptor[iii];
      }
      if(this->normalize_desc_==2) sum=sqrt(sum);
      for(int iii=0; iii<desc.descriptor.size(); iii++)
      {
        if((this->normalize_desc_==1) || (this->normalize_desc_==2)) desc.descriptor[iii] /= sum;
        else if(this->normalize_desc_==3) desc.descriptor[iii] = sqrt(desc.descriptor[iii])/sqrt(sum);
      }
      //printf("\n");
    }
    descriptor_set.descriptors.push_back(desc);
    descriptor_set.num++;
  }

  ROS_INFO("Computed %d descriptors, in %f s", descriptor_set.num, (clock()-start)/(float)CLOCKS_PER_SEC);
}