mean_shift.cpp

mean-shift. pointer sample

/*! \file
  \verbatim
  
    Copyright (c) 2007, Sylvain Paris and Fr閐o Durand

    Permission is hereby granted, free of charge, to any person
    obtaining a copy of this software and associated documentation
    files (the "Software"), to deal in the Software without
    restriction, including without limitation the rights to use, copy,
    modify, merge, publish, distribute, sublicense, and/or sell copies
    of the Software, and to permit persons to whom the Software is
    furnished to do so, subject to the following conditions:

    The above copyright notice and this permission notice shall be
    included in all copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
    EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
    MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
    NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
    HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
    WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
    DEALINGS IN THE SOFTWARE.

  \endverbatim
*/



#include <iomanip>
#include <iostream>
#include <limits>
#include <sstream>


#include "array_tools.h"
#include "chrono.h"
#include "color_tools.h"
#include "geom_tools.h"
#include "matrix_tools.h"
#include "msg_stream.h"
#include "ppm_tools.h"


#include "config.h"
#include "type_definition.h"


using namespace std; 
 
size_type n_bins;

size_type padding;

realN_type         min_coord;
input_feature_type sigma;
intN_type          size;

real_type n_substeps;
size_type n_iterations;

saddle_list_type  saddle;
cluster_list_type cluster;
count_list_type cluster_size;

size_type width;
size_type height;

label_type boundary_offset = numeric_limits<label_type>::max() / 2;

#ifdef MOVIE_MODE
size_type depth;
#endif


#ifdef USE_PCA_REDUCTION

pca_matrix_type      pca_matrix;
back_pca_matrix_type back_pca;
input_feature_type   pca_mean;

#endif


bool verbose = true;


Chrono init_chrono("init",false);
Chrono creation_chrono("creation",false);
Chrono filter_chrono("filter",false);
Chrono sort_chrono("sort",false);
Chrono safe_labels_chrono("node labels",false);
#ifndef LABEL_ON_THE_FLY
Chrono prune_chrono("prune",false);
#endif
Chrono pixel_chrono("pixel labels",false);





template<typename Vector>
inline realN_type point_from_index(const Vector& index){

  realN_type res;

  for(size_type n=0;n<space_dimension;n++){
    res[n] = (index[n] + min_coord[n] - padding) * sigma[n];
  }

  return res;
}


template<typename Vector> // kind of hack, allows for a (N+1)-vector or a N-vector
inline intN_type index_of_real_index(const Vector& r){
  
  intN_type index;

  for(size_type n=0;n<space_dimension;n++){    
    index[n] = static_cast<size_type>(r[n] + 0.5);
  }
  return index;
}




template<typename Vector> // kind of hack, allows for a (N+1)-vector or a N-vector
inline realN_type real_index_of_feature(const Vector& p){
  
  realN_type index;

  for(size_type n=0;n<space_dimension;n++){    
    index[n] = p[n] - min_coord[n] + padding;
  }
  return index;
}





template<typename Vector> // kind of hack, allows for a (N+1)-vector or a N-vector
inline intN_type index_of_feature(const Vector& p){
  
  return index_of_real_index(real_index_of_feature(p));
}




inline void input_feature_from_pixel(const size_type           x,
				     const size_type           y,
#ifdef MOVIE_MODE
				     const size_type           f,
#endif
				     const input_data_type&    image,
				     input_feature_type* const feat){
  
  input_feature_type& feature  = *feat;

  feature[0] = x;
  feature[1] = y;
  
#ifdef IMAGE_MODE
#ifdef GRAY_MODE
  feature[2] = image(PIXEL);
#endif

#ifdef COLOR_MODE
  const vector_color_type& clr = image(PIXEL);
  feature[2] = clr[0];
  feature[3] = clr[1];
  feature[4] = clr[2];
#endif

#ifdef TEXTURE_MODE
  const vector_texture_type& txt = image(PIXEL);
  for(size_type n = 0;n < R_DIMENSION;++n){
    feature[2+n] = txt[n];
  }
#endif

#endif // END  OF image_mode
  
#ifdef MOVIE_MODE
  feature[2] = f;
#ifdef GRAY_MODE
  feature[3] = image(PIXEL);
#endif

#ifdef COLOR_MODE
  const vector_color_type& clr = image(PIXEL);
  feature[3] = clr[0];
  feature[4] = clr[1];
  feature[5] = clr[2];
#endif
#endif // END  OF movie_mode   

  for(size_type n = 0;n < INPUT_FEATURE_DIMENSION;++n){
    feature[n] /= sigma[n];
  }  
}


inline void reduce_input_feature(const input_feature_type& input,
				 feature_type* const feat){
  
  feature_type& feature = *feat;

#ifdef USE_PCA_REDUCTION

  feature = pca_matrix * (input - pca_mean);
  
#else

  feature = input;

  Message::warning
  <<"reduce_input_feature: should not be here with PCA reduction disabled"
  <<Message::done;
  
#endif // END OF use_pca_reduction
}



void init(const input_data_type&    image,
	  const real_type           space_sigma,
#ifdef MOVIE_MODE
	  const real_type           time_sigma,
#endif
	  const real_type           range_sigma,
	  const real_type           sampling_factor,
	  const real_type           n_sub,
	  feature_array_type* const f_image){
  
  // Request memory space
  // ######################################

  feature_array_type& feature_image = *f_image;
  feature_image.resize(SIZES);

#ifdef USE_PCA_REDUCTION

  input_feature_array_type input_feature_array(SIZES);
  
#else
    
  feature_array_type& input_feature_array = feature_image;

#endif // END OF use_pca_reduction


  
  init_chrono.start();
  
  // Set parameters
  // ######################################
  
  n_substeps = n_sub;
  
  n_iterations =
    static_cast<size_type>(n_substeps / (sampling_factor * sampling_factor));

  padding = n_iterations / 2 + 1; 
//   padding = n_iterations / 2 + 2; 


  // Set sigmas
  // ######################################
  
  sigma[0] = space_sigma;
  sigma[1] = space_sigma;

#ifdef IMAGE_MODE
  sigma[2] = range_sigma;

#ifdef COLOR_MODE
  sigma[3] = range_sigma;
  sigma[4] = range_sigma;
#endif

#ifdef TEXTURE_MODE
  for(size_type n = 1;n < R_DIMENSION;++n){
    sigma[2+n] = range_sigma;
  }
#endif
  
#endif

#ifdef MOVIE_MODE
  
  sigma[2] = time_sigma;
  sigma[3] = range_sigma;

#ifdef COLOR_MODE
  sigma[4] = range_sigma;
  sigma[5] = range_sigma;
#endif
#endif

  sigma *= sampling_factor;


  
  // Create feature image
  // ######################################

  
  FOR_ALL_PIXELS {
    
    input_feature_type& input_feature = input_feature_array(PIXEL);
    input_feature_from_pixel(PIXEL,image,&input_feature);
   
  } // END OF for_all_pixels


  
#ifdef USE_PCA_REDUCTION

  // Compute PCA if needed
  // ######################################
  
  vector<input_feature_type> eigen_vector;
  vector<real_type>          eigen_value;

  Matrix_tools::PCA(input_feature_array.begin(),
		    input_feature_array.end(),
 		    &pca_mean,&eigen_vector,&eigen_value,11);

  for(size_type n = 0;n < FEATURE_DIMENSION;++n){

    const size_type m = INPUT_FEATURE_DIMENSION - n - 1;
    
    input_feature_type& v = eigen_vector[m];

    for(size_type i = 0;i < INPUT_FEATURE_DIMENSION;++i){
      pca_matrix(n,i) = v[i];
    }
  }
  
  back_pca = pca_matrix.transpose();
  
  // Compute reduced coordinates
  // ######################################
  
  FOR_ALL_PIXELS {
    GET_FEATURE(input_feature_array(PIXEL));
    feature_image(PIXEL) = feature;
  }
  
  
#endif


  // Compute space size
  // ######################################
  
  realN_type max_coord;
  Geom_tools::get_bounding_box(feature_image.begin(),
			       feature_image.end(),
			       &min_coord,&max_coord);
  
  size = index_of_feature(max_coord);

  for(size_type n = 0;n < space_dimension;++n){
    size[n] += padding + 1;
  }

  init_chrono.stop();
  
  
}





void init_relabel(label_list_type* const relabel){

  const size_type n = cluster.size();
  relabel->resize(n);
  for(size_type i = 0;i < n;i++){
    (*relabel)[i] = i;
  }
}





inline real_type color_square_distance(
#if (ARRAY_DENSITY == ADAPTIVE)
  const SR_domain_type::dense_type& SR_domain,  
#else
  const SR_domain_type& SR_domain,
#endif
  const saddle_type&    saddle){

#if defined(USE_PCA_REDUCTION) || defined(DENSITY_ONLY)

  const SR_value_type& s1 = SR_domain[cluster[saddle.minor].summit_index];
  const SR_value_type& s2 = SR_domain[cluster[saddle.major].summit_index];

  const real_type d1 = DENSITY(s1);
  const real_type d2 = DENSITY(s2);
  
  feature_type f1,f2;

  for(size_type i = 0;i < FEATURE_DIMENSION;i++){
    f1[i] = s1[i];
    f2[i] = s2[i];
  }

    
  const input_feature_type v1 = back_pca * f1;
  const input_feature_type v2 = back_pca * f2;
  
  
#else

  const SR_value_type& v1 = SR_domain[cluster[saddle.minor].summit_index];
  const SR_value_type& v2 = SR_domain[cluster[saddle.major].summit_index];

  const real_type d1 = DENSITY(v1);
  const real_type d2 = DENSITY(v2);

#endif
  
  real_type d = 0;
  
  for(size_type i = S_DIMENSION;i < INPUT_FEATURE_DIMENSION;i++){
    const real_type r = v1[i] / d1 - v2[i] / d2;
    d += r * r;
  }

  return d;

}




inline real_type persistence(
#if (ARRAY_DENSITY == ADAPTIVE)
  const SR_domain_type::dense_type& SR_domain,  
#else
  const SR_domain_type& SR_domain,
#endif
  const saddle_type&    saddle){

  const real_type topological =
#ifdef LOG_DENSITY
    log(cluster[saddle.minor].summit_density) - log(DENSITY(saddle.value));
#else
    cluster[saddle.minor].summit_density - DENSITY(saddle.value);
#endif

#ifdef COLOR_BIAS
  const real_type color = sqrt(color_square_distance(SR_domain,saddle));
  
  return topological * color;
#else
  return topological;
#endif
}






void update_saddle(
#if (ARRAY_DENSITY == ADAPTIVE)
  const SR_domain_type::dense_type& SR_domain,
#else  
  const SR_domain_type&  SR_domain,
#endif
  const label_list_type& relabel,
  saddle_type* const     saddle){

  label_type& M = saddle->major;
  label_type& m = saddle->minor;
  
  M = get_label(M,relabel);
  m = get_label(m,relabel);

  if (cluster[M] < cluster[m]) swap(M,m);

  saddle->persistence = (m == M)
    ? numeric_limits<real_type>::max()
    : persistence(SR_domain,*saddle);
}





void prune_persistence(const SR_domain_type&  SR_dom,
		       const real_type        threshold,
		       label_list_type* const relabel){

#if (ARRAY_DENSITY == ADAPTIVE)
  
  SR_domain_type::dense_type& SR_domain = *(SR_dom.dense);
  
#else
  
  const SR_domain_type& SR_domain = SR_dom;
  
#endif

#ifdef OUTPUT_HIERARCHY
  ofstream out_hierarchy("hierarchy.txt");
#endif
  
  bool collapsed = true;

  while(collapsed){

    collapsed = false;
    
    for(saddle_list_type::iterator
	  s     = saddle.begin(),
	  s_end = saddle.end();
	(s != s_end);s++){
      
      update_saddle(SR_domain,*relabel,&(*s));      
    }
    
    const saddle_type& s = *min_element(saddle.begin(),saddle.end());
    
    if (s.persistence < threshold){
      (*relabel)[s.minor] = s.major;
      collapsed = true;
      
#ifdef OUTPUT_HIERARCHY
      out_hierarchy << s.minor << "\t->\t" << s.major << '\n';
#endif
    }
  }
  
#ifdef OUTPUT_HIERARCHY
  out_hierarchy.flush();
  out_hierarchy.close();
#endif
}




real_type get_persistence_of_pair(const label_type l1,
				  const label_type l2){

  for(saddle_list_type::iterator
	s     = saddle.begin(),
	s_end = saddle.end();
      (s != s_end);s++){

    if (((s->major == l1) && (s->minor == l2)) ||
	((s->major == l2) && (s->minor == l1))){
      return s->persistence;
    }
  }
  
  return -1;
 
}



void fill_SR_domain(const feature_array_type&   feature_image,
		    SR_domain_type* const       SR_dom,
		    bool_SR_domain_type* const  occ){

  // Request memory space
  // ######################################
  
#if (ARRAY_DENSITY == ADAPTIVE)
  
  SR_dom->sparse = new SR_domain_type::sparse_type;
  SR_dom->dense = NULL;
  SR_domain_type::sparse_type& SR_domain = *(SR_dom->sparse);

  occ->sparse = new bool_SR_domain_type::sparse_type;
  bool_SR_domain_type::sparse_type& occupancy = *(occ->sparse);
  
#else
  
  SR_domain_type&      SR_domain = *SR_dom;
  bool_SR_domain_type& occupancy = *occ;
  
#endif

  SR_domain.resize(size);  
  occupancy.resize(size);


  
  creation_chrono.start();


  
  // Create random LUT
  // ######################################  

#ifdef RANDOM_OFFSET_FILLING

  const size_type n_offsets = 151;
  vector<real_type> random_offset(n_offsets);

  for(size_type n = 0;n < n_offsets;++n){
    random_offset[n] = Math_tools::random(-0.5,0.5);
  }

  size_type offset = 0;
  
#endif


  
  // Fill in space
  // ######################################
 
  SR_value_type value;
  DENSITY(value) = 1.0;

#ifdef DENSITY_ONLY

  FOR_ALL_PIXELS {  
    const intN_type i = index_of_feature(feature_image(PIXEL));
    SR_domain[i] += value;
    occupancy[i]  = true;
  }
  
#else
  
  realN_type q;

  FOR_ALL_PIXELS {  
    q = real_index_of_feature(feature_image(PIXEL));
	
    for(size_type i = 0;i<space_dimension;i++){
      
#ifdef RANDOM_OFFSET_FILLING
      value[i] = q[i] + random_offset[offset];
      offset   = (offset + 1) % n_offsets;
#else
      value[i] = q[i];
#endif
    }
      
    const intN_type i = index_of_real_index(q);
 
#ifdef LINEARLY_INTERPOLATED_FILLING
    Math_tools::write_through_Nlinear_interpolation(value,&SR_domain,q);
#else
    SR_domain[i] += value;
#endif
    
    occupancy[i] = true;
  }

#endif

  creation_chrono.stop();
   
}



void create_filter_mask(const size_type    n_iter,
			vector<real_type>* const m){

  const real_type delta = 0.5 / n_substeps;

  const size_type size = 2 * n_iter + 1;
  
  vector<real_type>& mask = *m;
  mask.resize(size);
  mask[n_iterations] = 1.0;

  vector<real_type> buffer(mask);
  
  for(size_type iter = 0;iter < n_iter;iter++){
    
    for(size_type
	  n     = n_iter - iter,
	  n_end = n_iter + iter;
	n <= n_end;n++){

      const real_type v = mask[n] * delta;
      
      buffer[n-1] += v;
      buffer[n]   += -2.0 * v;
      buffer[n+1] += v;