array_tools.h

mean-shift. pointer sample

#ifndef __ARRAY_TOOLS__
#define __ARRAY_TOOLS__

#include <cmath>

#include <algorithm>

#include "array.h"
#include "math_tools.h"
#include "msg_stream.h"

namespace Array_tools{

  typedef unsigned int size_type;


  inline size_type mirrored_coordinate(const int c,
				       const size_type bound);
  
  template<typename Array>
  void mirror_neighborhood(const Array&    source,
			   const size_type x_ref,
			   const size_type y_ref,
			   const size_type x_radius,
			   const size_type y_radius,
			   Array* const    result);


  template<typename Array>
  void truncated_neighborhood(const Array&     source,
			      const size_type  x_ref,
			      const size_type  y_ref,
			      const size_type  x_radius,
			      const size_type  y_radius,
			      Array* const     result,
			      size_type* const x_offset,
			      size_type* const y_offset);


  
  template<typename Array>
  void window(const Array&     source,
	      const size_type  x_corner,
	      const size_type  y_corner,
	      const size_type  w_width,
	      const size_type  w_height,
	      Array* const     result);





  
  
  //! \p res = \p op1 + \p op2
  template<typename Array>
  void add(const Array& op1,
	   const Array& op2,
	   Array* const res);


  
  //! \p res += \p op
  template<typename Array>
  void add(const Array& op,
	   Array* const res);

  
  //! \p res = \p op + \p lambda
  template<typename Array,typename Scalar>
  void add(const Array& op,
	   const Scalar lambda,
	   Array* const res);

  
  //! \p res = \p op1 - \p op2
  template<typename Array>
  void sub(const Array& op1,
	   const Array& op2,
	   Array* const res);


  //! \p res = \p op - \p lambda
  template<typename Array,typename Scalar>
  void sub(const Array& op,
	   const Scalar lambda,
	   Array* const res);
  
  //! \p res = \p op1 / \p op2
  template<typename Array>
  void mul(const Array& op1,
	   const Array& op2,
	   Array* const res);

  
  //! \p res = \p op * \p lambda
  template<typename Array,typename Scalar>
  void mul(const Array& op,
	   const Scalar lambda,
	   Array* const res);

  
  //! \p res = \p op1 / \p op2
  template<typename Array>
  void div(const Array& op1,
	   const Array& op2,
	   Array* const res,
	   const typename Array::value_type epsilon = 1e-10);

  
  //! \p res = \p op / \p lambda
  template<typename Array,typename Scalar>
  void div(const Array& op,
	   const Scalar lambda,
	   Array* const res);

  
  //! \p res = min(\p op1,\p op2)
  template<typename Array>
  void min(const Array& op1,
	   const Array& op2,
	   Array* const res);
  
  //! \p res = max(\p op1,\p op2)
  template<typename Array>
  void max(const Array& op1,
	   const Array& op2,
	   Array* const res);

  
  //! \p res = log(\p op)
  template<typename Array>
  void log(const Array& op,
	   Array* const res);

  
  //! \p res = clamp(\p m,\p M,\p op)
  template<typename Array,typename Scalar>
  void clamp(const Array& op,
	     const Scalar m,
	     const Scalar M,
	     Array* const res);

  
  //! \p res = pow(\p op,\p lambda)
  template<typename Array,typename Scalar>
  void pow(const Array& op,
	   const Scalar lambda,
	   Array* const res);

/*
  ###########################################################
  ###########################################################
  ###########################################################
  ##############                               ##############
  ##############  I M P L E M E N T A T I O N  ##############
  ##############                               ##############
  ###########################################################
  ###########################################################
  ###########################################################
*/



  //! \p bound is never reached. The order is ...,bound-2,bound-1,bound-2,...
  size_type mirrored_coordinate(const int c,
				const size_type bound){

    size_type r = (c>0) ? c : (-c);
    r = (r>=bound) ? (2*bound - 2 - r) : r;

    return r;
  }
  
  //! The (x_ref,y_ref) point is located at (x_radius,y_radius) in the resulting array.
  template<typename Array>
  void mirror_neighborhood(const Array&    source,
			   const size_type x_ref,
			   const size_type y_ref,
			   const size_type x_radius,
			   const size_type y_radius,
			   Array* const    result){
    
    const size_type width  = source.x_size();
    const size_type height = source.y_size();

    result->resize(2*x_radius+1,2*y_radius+1);
    
    size_type tx = 0;
    
    for(int x = static_cast<int>(x_ref) - static_cast<int>(x_radius);
	x <= static_cast<int>(x_ref) + static_cast<int>(x_radius); x++){
      
      size_type ty = 0;
      
      const size_type sx = mirrored_coordinate(x,width);
      
      for(int y = static_cast<int>(y_ref) - static_cast<int>(y_radius);
	  y <= static_cast<int>(y_ref) + static_cast<int>(y_radius); y++){
	
	const size_type sy = mirrored_coordinate(y,height);
	
	(*result)(tx,ty) = source(sx,sy);
	
	ty++;
      }
      
      tx++;
    }

  }



  
  //! In the resulting array, (x_ref,y_ref) is located at (x_offset,y_offset).
  template<typename Array>
  void truncated_neighborhood(const Array&     source,
			      const size_type  x_ref,
			      const size_type  y_ref,
			      const size_type  x_radius,
			      const size_type  y_radius,
			      Array* const     result,
			      size_type* const x_offset,
			      size_type* const y_offset){

    using namespace Math_tools;
  
    const size_type width  = source.x_size();
    const size_type height = source.y_size();
    
    const size_type x_begin = Math_tools::clamp<int>(0,width-1,static_cast<int>(x_ref)-x_radius);
    const size_type x_end   = Math_tools::clamp<int>(0,width-1,static_cast<int>(x_ref)+x_radius);
    
    const size_type y_begin = Math_tools::clamp<int>(0,height-1,static_cast<int>(y_ref)-y_radius);
    const size_type y_end   = Math_tools::clamp<int>(0,height-1,static_cast<int>(y_ref)+y_radius);

    *x_offset = x_ref - x_begin;
    *y_offset = y_ref - y_begin;

    const size_type res_width  = x_end - x_begin + 1;
    const size_type res_height = y_end - y_begin + 1;
    result->resize(res_width,res_height);

    size_type x = 0;
    for(size_type i=x_begin;i<=x_end;i++){

      size_type y = 0;      
      for(size_type j=y_begin;j<=y_end;j++){

	(*result)(x,y) = source(i,j);
	y++;
      }

      x++;
    }
  }


  template<typename Array>
  void window(const Array&     source,
	      const size_type  x_corner,
	      const size_type  y_corner,
	      const size_type  w_width,
	      const size_type  w_height,
	      Array* const     result){

    const size_type width  = source.x_size();
    const size_type height = source.y_size();

    if((x_corner+w_width>width)||
       (y_corner+w_height>height)){

      Message::error<<"Array_tools::window : window does not fit the source array\n"
		    <<"corner: "<<x_corner<<"x"<<y_corner
		    <<"\tsize: "<<w_width<<"x"<<w_height<<Message::done;
    }

    result->resize(w_width,w_height);

    for(size_type x=0;x<w_width;x++){
      for(size_type y=0;y<w_height;y++){
	(*result)(x,y) = source(x+x_corner,y+y_corner);
      }
    }

  }





  
  template<typename Array>
  void add(const Array& op1,
	   const Array& op2,
	   Array* const res){

    typedef typename Array::const_iterator const_iterator;
    typedef typename Array::iterator       iterator;

    res->resize(op1.width(),op1.height());
    
    const_iterator i     = op1.begin();
    const_iterator i_end = op1.end();
    const_iterator j     = op2.begin();
    iterator       r     = res->begin();
    
    for(;i!=i_end;i++,j++,r++){
      *r = (*i) + (*j);
    }
  }



  
  template<typename Array>
  void add(const Array& op,
	   Array* const res){

    typedef typename Array::const_iterator const_iterator;
    typedef typename Array::iterator       iterator;

    res->resize(op.width(),op.height());

    const_iterator i     = op.begin();
    const_iterator i_end = op.end();
    iterator       r     = res->begin();
    
    for(;i!=i_end;i++,r++){
      *r += (*i);
    }

  }



  template<typename Array,typename Scalar>
  void add(const Array& op,
	   const Scalar lambda,
	   Array* const res){

    typedef typename Array::const_iterator const_iterator;
    typedef typename Array::iterator       iterator;
    
    res->resize(op.width(),op.height());

    const_iterator i     = op.begin();
    const_iterator i_end = op.end();
    iterator       r     = res->begin();
    
    for(;i!=i_end;i++,r++){
      *r = *i + lambda;
    }

  }


  
  template<typename Array>
  void sub(const Array& op1,
	   const Array& op2,
	   Array* const res){

    typedef typename Array::const_iterator const_iterator;
    typedef typename Array::iterator       iterator;

    res->resize(op1.width(),op1.height());

    const_iterator i     = op1.begin();
    const_iterator i_end = op1.end();
    const_iterator j     = op2.begin();
    iterator       r     = res->begin();
    
    for(;i!=i_end;i++,j++,r++){
      *r = (*i) - (*j);
    }
  }



  template<typename Array,typename Scalar>
  void sub(const Array& op,
	   const Scalar lambda,
	   Array* const res){

    typedef typename Array::const_iterator const_iterator;
    typedef typename Array::iterator       iterator;
    
    res->resize(op.width(),op.height());

    const_iterator i     = op.begin();
    const_iterator i_end = op.end();
    iterator       r     = res->begin();
    
    for(;i!=i_end;i++,r++){
      *r = *i - lambda;
    }

  }

  
  template<typename Array>
  void mul(const Array& op1,
	   const Array& op2,
	   Array* const res){

    typedef typename Array::const_iterator const_iterator;
    typedef typename Array::iterator       iterator;

    res->resize(op1.width(),op1.height());
    
    const_iterator i     = op1.begin();
    const_iterator i_end = op1.end();
    const_iterator j     = op2.begin();
    iterator       r     = res->begin();
    
    for(;i!=i_end;i++,j++,r++){
      *r = (*i) * (*j);
    }
  }



  
  template<typename Array,typename Scalar>
  void mul(const Array& op,
	   const Scalar lambda,
	   Array* const res){

    typedef typename Array::const_iterator const_iterator;
    typedef typename Array::iterator       iterator;
    
    res->resize(op.width(),op.height());

    const_iterator i     = op.begin();
    const_iterator i_end = op.end();
    iterator       r     = res->begin();
    
    for(;i!=i_end;i++,r++){
      *r = *i * lambda;
    }

  }


  
  template<typename Array>
  void div(const Array& op1,
	   const Array& op2,
	   Array* const res,
	   const typename Array::value_type epsilon){

    typedef typename Array::const_iterator const_iterator;
    typedef typename Array::iterator       iterator;

    res->resize(op1.width(),op1.height());
    
    const_iterator i     = op1.begin();
    const_iterator i_end = op1.end();
    const_iterator j     = op2.begin();
    iterator       r     = res->begin();
    
    for(;i!=i_end;i++,j++,r++){
      *r = (*j > epsilon) ? (*i / *j) : (*i / epsilon);
    }
  }


  
  template<typename Array,typename Scalar>
  void div(const Array& op,
	   const Scalar lambda,
	   Array* const res){

    typedef typename Array::const_iterator const_iterator;
    typedef typename Array::iterator       iterator;
    
    res->resize(op.width(),op.height());

    const_iterator i     = op.begin();
    const_iterator i_end = op.end();
    iterator       r     = res->begin();
    
    for(;i!=i_end;i++,r++){
      *r = *i / lambda;
    }

  }

  

  template<typename Array>
  void min(const Array& op1,
	   const Array& op2,
	   Array* const res){

    typedef typename Array::const_iterator const_iterator;
    typedef typename Array::iterator       iterator;

    res->resize(op1.width(),op1.height());
    
    const_iterator i     = op1.begin();
    const_iterator i_end = op1.end();
    const_iterator j     = op2.begin();
    iterator       r     = res->begin();
    
    for(;i!=i_end;i++,j++,r++){
      *r = std::min(*i,*j);
    }
  }

  
  
  template<typename Array>
  void max(const Array& op1,
	   const Array& op2,
	   Array* const res){

    typedef typename Array::const_iterator const_iterator;
    typedef typename Array::iterator       iterator;

    res->resize(op1.width(),op1.height());
    
    const_iterator i     = op1.begin();
    const_iterator i_end = op1.end();
    const_iterator j     = op2.begin();
    iterator       r     = res->begin();
    
    for(;i!=i_end;i++,j++,r++){
      *r = std::max(*i,*j);
    }
  }


  template<typename Array>
  void log(const Array& op,
	   Array* const res){

    typedef typename Array::const_iterator const_iterator;
    typedef typename Array::iterator       iterator;

    res->resize(op.width(),op.height());
    
    const_iterator i     = op.begin();
    const_iterator i_end = op.end();
    iterator       r     = res->begin();
    
    for(;i!=i_end;i++,r++){
      *r = std::log(*i);
    }
  }

  
  template<typename Array,typename Scalar>
  void clamp(const Array& op,
	     const Scalar m,
	     const Scalar M,
	     Array* const res){

    typedef typename Array::const_iterator const_iterator;
    typedef typename Array::iterator       iterator;
    
    res->resize(op.width(),op.height());

    const_iterator i     = op.begin();
    const_iterator i_end = op.end();
    iterator       r     = res->begin();
    
    for(;i!=i_end;i++,r++){
      *r = Math_tools::clamp(m,M,*i);
    }
  }

  
  template<typename Array,typename Scalar>
  void pow(const Array& op,
	   const Scalar lambda,
	   Array* const res){

    typedef typename Array::const_iterator const_iterator;
    typedef typename Array::iterator       iterator;
    
    res->resize(op.width(),op.height());

    const_iterator i     = op.begin();
    const_iterator i_end = op.end();
    iterator       r     = res->begin();
    
    for(;i!=i_end;i++,r++){
      *r = std::pow(*i,lambda);
    }

  }
 
} // END OF namespace


#endif