feature_map_alg.cpp

Go to the documentation of this file.

#include "feature_map_alg.h"

FeatureMapAlgorithm::FeatureMapAlgorithm(void) :
    last_eval_cluster_max_score(0.0),
    last_eval_cluster_min_score(0.0)
{ }

FeatureMapAlgorithm::~FeatureMapAlgorithm(void)
{
}

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

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

float FeatureMapAlgorithm::eval_clusters(const std::vector<float> & holy_vect, bool suppress_nonselected)
{
  // sqr dist: dot(A-B, A-B)
  float *vect = new float[holy_vect.size()];
  int dim = holy_vect.size();
  memcpy(vect,&holy_vect[0],sizeof(float)*dim);
  Eigen::MatrixXf vect_adapt = Eigen::Map<Eigen::MatrixXf>(&vect[0],1,dim);
  //std::cout<<vect_adapt<<std::endl;
  vect_adapt = vect_adapt/vect_adapt.sum();
  if(isnan(vect_adapt(0)) || vect_adapt(0)==0)
    return 0;

  Eigen::MatrixXf vect_full  = this->classifiers_ - vect_adapt.replicate(this->classifiers_.rows(),1);
  Eigen::MatrixXf dists      = vect_full*vect_full.transpose();
  //std::cout << dists.rows() << " " << dists.cols() << std::endl;
  //std::cout << dists << std::endl;
  if(!suppress_nonselected)
  {
    int i=this->sel_vector_;
    delete [] vect;
    return 1.0/sqrt(dists(i,i));   //inverse to have higher==better
  }

  //suppress not selected points
  int sel_center=0;
  float min_val=dists(0,0);
  for(int i=1;i<this->classifiers_.rows();i++)
  {
    //vect_adapt = this->classifiers_.row(i) - vect_adapt;
    //std::cout<<vect_adapt<<std::endl;
    //vect_adapt = vect_adapt.cwiseProduct(vect_adapt);
    //float val=vect_adapt.norm();
    //std::cout<<dists(i,i)<<" "<<std::endl;
    if(min_val>dists(i,i))
    {
      sel_center=i;
      min_val=dists(i,i);
    }
  }
  //std::cout<<sel_center<<":"<<min_val<<"  ";
  delete [] vect;
  if(sel_center==this->sel_vector_)
    return 1.0/sqrt(min_val); //inverse to have higher == better
  return 0.;
}

float FeatureMapAlgorithm::eval_linear(const std::vector<float> & vect)
{
  return 0;
}

float FeatureMapAlgorithm::eval_logistic(const std::vector<float> & vect)
{

  return 0;
}


// FeatureMapAlgorithm Public API

void FeatureMapAlgorithm::compute_feature_response_map(const normal_descriptor_node::ndesc_pc &ndesc_pc_msg, pcl::PointCloud<pcl::PointXYZRGB> &hmap)
{
  //process descriptors
  hmap.width = ndesc_pc_msg.width;
  hmap.height = ndesc_pc_msg.height;
  hmap.points.resize(hmap.width*hmap.height);
  // reset the max score
  this->last_eval_cluster_max_score = 0.0;
  this->last_eval_cluster_min_score = 0.0;
  
  //TODO reset vector more efficiently
  for(int i=0; i<hmap.points.size(); i++)
  {
    hmap.points[i].x=std::numeric_limits<float>::quiet_NaN();
    hmap.points[i].y=std::numeric_limits<float>::quiet_NaN();
    hmap.points[i].z=std::numeric_limits<float>::quiet_NaN();
    hmap.points[i].rgb=std::numeric_limits<float>::quiet_NaN();
  }
  ROS_INFO("size %d %d ndesc %d", hmap.width, hmap.height, ndesc_pc_msg.descriptors.size());
  for(uint i=0; i<ndesc_pc_msg.descriptors.size(); i++)
  {
    int idx            = ndesc_pc_msg.descriptors[i].u + ndesc_pc_msg.descriptors[i].v*hmap.width;
    hmap.points[idx].x = ndesc_pc_msg.descriptors[i].point3d.x;
    hmap.points[idx].y = ndesc_pc_msg.descriptors[i].point3d.y;
    hmap.points[idx].z = ndesc_pc_msg.descriptors[i].point3d.z;
    
    switch(this->filter_method_)
    {
    case 0:
      hmap.points[idx].rgb = this->eval_clusters(ndesc_pc_msg.descriptors[i].descriptor, false);
      // Update max and min if needed
      if (fabs(hmap.points[idx].rgb) > this->last_eval_cluster_max_score) {
          this->last_eval_cluster_max_score = fabs(hmap.points[idx].rgb);
      }
      // Min is only update to something diferent than 0.0
      if (fabs(hmap.points[idx].rgb) > 0.0)
          if ((this->last_eval_cluster_min_score == 0.0) || 
             (hmap.points[idx].rgb < this->last_eval_cluster_min_score))
                this->last_eval_cluster_min_score = fabs(hmap.points[idx].rgb);
      break;
    case 1:
      hmap.points[idx].rgb = this->eval_clusters(ndesc_pc_msg.descriptors[i].descriptor, true);
      // Update max and min if needed
      if (fabs(hmap.points[idx].rgb) > this->last_eval_cluster_max_score) {
          this->last_eval_cluster_max_score = fabs(hmap.points[idx].rgb);
      }
      // Min is only update to something diferent than 0.0
      if (fabs(hmap.points[idx].rgb) > 0.0)
          if ((this->last_eval_cluster_min_score == 0.0) || 
             (hmap.points[idx].rgb < this->last_eval_cluster_min_score))
                this->last_eval_cluster_min_score = fabs(hmap.points[idx].rgb);
      break;
    case 2:
      ROS_ERROR("OPTION NOT IMPLEMENTED YET!");
      exit(-1);
      hmap.points[idx].rgb = this->eval_linear(ndesc_pc_msg.descriptors[i].descriptor);
      break;
    case 3:
      ROS_ERROR("OPTION NOT IMPLEMENTED YET!");
      exit(-1);
      hmap.points[idx].rgb = this->eval_logistic(ndesc_pc_msg.descriptors[i].descriptor);
      break;
    default:
      ROS_ERROR("iri_feature_map Unknown filter method");
    }
  }
  ROS_INFO("Exiting feature map computer.");
}
    
void FeatureMapAlgorithm::update_params(const iri_publish_params::classifier_update &params)
{
  ROS_INFO("iri_feature_map: Updating parameters. %d vectors of size %d. Selected %d, method %d", params.update_params.size(), params.update_params[0].params.size(), params.selected_centroid, params.filter_method);
  this->sel_vector_ = params.selected_centroid;
  if(this->sel_vector_ >= params.update_params[0].params.size())
    ROS_ERROR("iri_feature_map: Selected vector outside range");

  this->filter_method_ = params.filter_method;
  this->classifiers_.resize(params.update_params.size(), params.update_params[0].params.size());
  for(int j=0;j<params.update_params.size();j++){
    for(int i=0;i<params.update_params[0].params.size();i++)
    {
      //init all vectors in matrix
      this->classifiers_(j,i) = params.update_params[j].params[i];
    }
  }
  ROS_INFO("iri_feature_map: Update successful!");
}

void
FeatureMapAlgorithm::update_selected_vector(const int selected_vector)
{
    sel_vector_ = selected_vector;
}

float
FeatureMapAlgorithm::get_last_cluster_max_score()
{
    return this->last_eval_cluster_max_score;
}

float
FeatureMapAlgorithm::get_last_cluster_min_score()
{
    return this->last_eval_cluster_min_score;
}