finddd_alg.h

Go to the documentation of this file.

// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
// Author 
// All rights reserved.
//
// This file is part of iri-ros-pkg
// iri-ros-pkg is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.
// 
// IMPORTANT NOTE: This code has been generated through a script from the 
// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
// of the scripts. ROS topics can be easly add by using those scripts. Please
// refer to the IRI wiki page for more information:
// http://wikiri.upc.es/index.php/Robotics_Lab

#ifndef _finddd_alg_h_
#define _finddd_alg_h_

#include <iri_finddd/FindddConfig.h>
#include "mutex.h"
#include <sensor_msgs/PointCloud2.h>
#include <iri_perception_msgs/DescriptorSet.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/ros/conversions.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/integral_image_normal.h>
#include <cmath>
#include <vector>
#include <sstream>
#include <time.h>
#include <iostream>
#include <fstream>

//only form saving normals
#include <pcl/io/pcd_io.h>

template <class T>
inline std::string to_string (const T& t)
{
  std::stringstream ss;
  ss << t;
  return ss.str();
}

//include finddd_alg main library

class FindddAlgorithm
{
  protected:
    CMutex alg_mutex_;

    // private attributes and methods
    uint nt_;

    // [descriptor attributes]
    std::vector<float> orient_bin_centers_x_;
    std::vector<float> orient_bin_centers_y_;
    std::vector<float> orient_bin_centers_z_;
    std::vector<int> desc_compute_positionsX_;
    std::vector<int> desc_compute_positionsY_;
    float radius_inf_;
    uint desc_num_orient_bins_;
    uint desc_num_side_spatial_bins_;
    uint desc_num_total_bins_;
    uint desc_patch_radius_;
    std::string centroids_file_;
    std::string positions_file_;
    uint normalize_desc_;
    uint use_soft_voting_;

    // [descriptor sampling attributes]
    uint sample_each_;
    uint margin_;

  public:
    typedef iri_finddd::FindddConfig Config;

    Config config_;

    FindddAlgorithm(void);

    void lock(void) { alg_mutex_.enter(); };

    void unlock(void) { alg_mutex_.exit(); };

    bool try_enter(void) { return alg_mutex_.try_enter(); };

    void config_update(Config& new_cfg, uint32_t level=0);

    // here define all finddd_alg interface methods to retrieve and set
    // the driver parameters

    inline int compute_total_bins()
    {
      //this->desc_num_total_bins_ = this->desc_num_orient_bins_ * this->desc_num_orient_bins_ * 
      //this->desc_num_side_spatial_bins_ * this->desc_num_side_spatial_bins_;
      this->desc_num_total_bins_ = this->desc_num_side_spatial_bins_ * this->desc_num_side_spatial_bins_ * this->orient_bin_centers_x_.size();
      
      return this->desc_num_total_bins_;
    }

    inline int set_num_side_spatial_bins(int nb)
    {
      this->desc_num_side_spatial_bins_ = nb;
      this->compute_total_bins();
      return this->desc_num_side_spatial_bins_;
    }


    inline int set_desc_patch_radius(int r)
    {
      this->desc_patch_radius_ = r;
      this->margin_ = this->desc_patch_radius_;
      return this->desc_patch_radius_;
    }

    inline int set_sample_each(int s)
    {
      this->sample_each_ = s;
      return this->sample_each_;
    }

    inline void set_centroids_file(std::string new_centroids_file)
    {
      this->centroids_file_ = new_centroids_file;
      this->update_centroids();
    }

    inline void update_centroids()
    {
      std::ifstream ifs(this->centroids_file_.c_str(), std::ifstream::in);
      if(!ifs.is_open())
      {
        ROS_ERROR("FINDDD: Incorrect number of centers loaded from file %s", this->centroids_file_.c_str());
      }
      
      this->orient_bin_centers_x_.clear();
      this->orient_bin_centers_y_.clear();
      this->orient_bin_centers_z_.clear();

      while (ifs.good()) //trusting the file...
      {
        float x,y,z;
        ifs >> x;
        ifs >> y;
        ifs >> z;
        //ROS_INFO("%f %f %f", x,y,z);
        if(ifs.good())
        {
          this->orient_bin_centers_x_.push_back(x);
          this->orient_bin_centers_y_.push_back(y);
          this->orient_bin_centers_z_.push_back(z);
        }
      }
      ifs.close();
      this->desc_num_orient_bins_ = this->orient_bin_centers_x_.size();
      ROS_INFO("Loaded %d centers\n", this->desc_num_orient_bins_);
      this->compute_total_bins();
      this->compute_radius_inf();
    }

    inline void set_positions_file(std::string new_positions_file)
    {
      this->positions_file_ = new_positions_file;
      
      this->desc_compute_positionsX_.clear();
      this->desc_compute_positionsY_.clear();

      if(this->positions_file_=="")
      {
        ROS_INFO("Generating positions with 'sample_each' param.");
        return;
      }
      
      std::ifstream ifs(this->positions_file_.c_str(), std::ifstream::in);
      if(!ifs.is_open())
      {
        ROS_ERROR("FINDDD: Incorrect descriptor positions file %s", this->positions_file_.c_str());
      }

      while (ifs.good()) //trusting the file...
      {
        int x,y;
        ifs >> x;
        ifs >> y;
        this->desc_compute_positionsX_.push_back(x);
        this->desc_compute_positionsY_.push_back(y);
      }
      ifs.close();
      ROS_INFO("Loaded %d positions\n", this->desc_compute_positionsX_.size());
    }

    inline void compute_radius_inf()
    {
      //finding minimum distance between point zero and the other points
      float x = this->orient_bin_centers_x_[0];
      float y = this->orient_bin_centers_y_[0];
      float z = this->orient_bin_centers_z_[0];
      this->radius_inf_ = 9999999999;
      for(int i=1; i<this->orient_bin_centers_x_.size(); i++)
      {
        float dist = sqrt(pow(x-this->orient_bin_centers_x_[i],2) +
                          pow(y-this->orient_bin_centers_y_[i],2) +
                          pow(z-this->orient_bin_centers_z_[i],2));
        if(dist<this->radius_inf_)
          this->radius_inf_ = dist;
      }
      ROS_INFO("Using ratio_inf_ = %f", this->radius_inf_);
    }

    //With interpolation in votes
    void compute_ndescs_integral_spatial_interpolation(pcl::PointCloud<pcl::PointXYZ>& cloud, iri_perception_msgs::DescriptorSet &descriptor_set);

    ~FindddAlgorithm(void);
};

#endif