text: /*********************************************************************
   * Software License Agreement (BSD License)
   *
   *  Copyright (c) 2008-2010, Willow Garage, Inc.
   *  All rights reserved.
   *
   *  Redistribution and use in source and binary forms, with or without
   *  modification, are permitted provided that the following conditions
   *  are met:
   *
   *   * Redistributions of source code must retain the above copyright
   *     notice, this list of conditions and the following disclaimer.
   *   * Redistributions in binary form must reproduce the above
   *     copyright notice, this list of conditions and the following
   *     disclaimer in the documentation and/or other materials provided
   *     with the distribution.
   *   * Neither the name of the Willow Garage nor the names of its
   *     contributors may be used to endorse or promote products derived
   *     from this software without specific prior written permission.
   *
   *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
   *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
   *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
   *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
   *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
   *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
   *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
   *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
   *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
   *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
   *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
   *  POSSIBILITY OF SUCH DAMAGE.
   *********************************************************************/
  
  //
  // The original code was written by
  //          Marius Muja
  // and later modified and prepared
  //  for integration into OpenCV by 
  //        Antonella Cascitelli,
  //        Marco Di Stefano and
  //          Stefano Fabri
  //        from Univ. of Rome 
  //
  
  #include "precomp.hpp"
  #include "opencv2/highgui/highgui.hpp"
  #include <iostream>
  #include <queue>
  
  namespace cv
  {
  
  using std::queue;
      
  typedef std::pair<int,int> coordinate_t;
  typedef float orientation_t;
  typedef std::vector<coordinate_t> template_coords_t;
  typedef std::vector<orientation_t> template_orientations_t;
  typedef std::pair<Point, float> location_scale_t;
  
  class ChamferMatcher
  {
      
  private:
      class Matching;
      int max_matches_;
      float min_match_distance_;
      
      ///////////////////////// Image iterators ////////////////////////////
      
      class ImageIterator
      {
      public:
          virtual bool hasNext() const = 0;
          virtual location_scale_t next() = 0;
      };
      
      class ImageRange
      {
      public:
          virtual ImageIterator* iterator() const = 0;
      };
      
      // Sliding window
      
      class SlidingWindowImageRange : public ImageRange
      {
          int width_;
          int height_;
          int x_step_;
          int y_step_;
          int scales_;
          float min_scale_;
          float max_scale_;
          
      public:
          SlidingWindowImageRange(int width, int height, int x_step = 3, int y_step = 3, int scales = 5, float min_scale = 0.6, float max_scale = 1.6) :
          width_(width), height_(height), x_step_(x_step),y_step_(y_step), scales_(scales), min_scale_(min_scale), max_scale_(max_scale)
          {
          }
          
          
          ImageIterator* iterator() const;
      };
      
      class LocationImageRange : public ImageRange
      {
          const std::vector<Point>& locations_;
          
          int scales_;
          float min_scale_;
          float max_scale_;
  
  		LocationImageRange(const LocationImageRange&);
  		LocationImageRange& operator=(const LocationImageRange&);
          
      public:
          LocationImageRange(const std::vector<Point>& locations, int scales = 5, float min_scale = 0.6, float max_scale = 1.6) :
          locations_(locations), scales_(scales), min_scale_(min_scale), max_scale_(max_scale)
          {
          }
          
          ImageIterator* iterator() const
          {
              return new LocationImageIterator(locations_, scales_, min_scale_, max_scale_);
          }
      };
      
      
      class LocationScaleImageRange : public ImageRange
      {
          const std::vector<Point>& locations_;
          const std::vector<float>& scales_;
          
  		LocationScaleImageRange(const LocationScaleImageRange&);
  		LocationScaleImageRange& operator=(const LocationScaleImageRange&);
      public:
          LocationScaleImageRange(const std::vector<Point>& locations, const std::vector<float>& scales) :
          locations_(locations), scales_(scales)
          {
              assert(locations.size()==scales.size());
          }
          
          ImageIterator* iterator() const
          {
              return new LocationScaleImageIterator(locations_, scales_);
          }
      };
      
      
      
      
  public:
      /**
       * Class that represents a template for chamfer matching.
       */
      class Template
      {
          friend class ChamferMatcher::Matching;
          friend class ChamferMatcher;
          
          
      public:
          std::vector<Template*> scaled_templates;
          std::vector<int> addr;
          int addr_width;
          float scale;
          template_coords_t coords;
          
          template_orientations_t orientations;
          Size size;
          Point center;
          
      public:
          Template() : addr_width(-1)
          {
          }
          
          Template(Mat& edge_image, float scale_ = 1);
          
          ~Template()
          {
              for (size_t i=0;i<scaled_templates.size();++i) {
                  delete scaled_templates[i];
              }
              scaled_templates.clear();
              coords.clear();
              orientations.clear();
          }
          void show() const;
          
          
          
      private:
          /**
           * Resizes a template
           *
           * @param scale Scale to be resized to
           */
          Template* rescale(float scale);
          
          std::vector<int>& getTemplateAddresses(int width);
      };
      
      
      
      /**
       * Used to represent a matching result.
       */
      
      class Match
      {
      public:
          float cost;
          Point offset;
          const Template* tpl;
      };
      
      typedef std::vector<Match> Matches;
      
  private:
      /**
       * Implements the chamfer matching algorithm on images taking into account both distance from
       * the template pixels to the nearest pixels and orientation alignment between template and image
       * contours.
       */
      class Matching
      {
          float truncate_;
          bool use_orientation_;
          
          std::vector<Template*> templates;
      public:
          Matching(bool use_orientation = true, float truncate = 10) : truncate_(truncate), use_orientation_(use_orientation)
          {
          }
          
          ~Matching()
          {
              for (size_t i = 0; i<templates.size(); i++) {
                  delete templates[i];
              }
          }
          
          /**
           * Add a template to the detector from an edge image.
           * @param templ An edge image
           */
          void addTemplateFromImage(Mat& templ, float scale = 1.0);
          
          /**
           * Run matching using an edge image.
           * @param edge_img Edge image
           * @return a match object
           */
          ChamferMatcher::Matches* matchEdgeImage(Mat& edge_img, const ImageRange& range, float orientation_weight = 0.5, int max_matches = 20, float min_match_distance = 10.0);
          
          void addTemplate(Template& template_);
          
      private:
          
          float orientation_diff(float o1, float o2)
          {
              return fabs(o1-o2);
          }
          
          /**
           * Computes the chamfer matching cost for one position in the target image.
           * @param offset Offset where to compute cost
           * @param dist_img Distance transform image.
           * @param orientation_img Orientation image.
           * @param tpl Template
           * @param templ_orientations Orientations of the target points.
           * @return matching result
           */
          ChamferMatcher::Match* localChamferDistance(Point offset, Mat& dist_img, Mat& orientation_img, Template* tpl,  float orientation_weight);
          
      private:
          /**
           * Matches all templates.
           * @param dist_img Distance transform image.
           * @param orientation_img Orientation image.
           */
          ChamferMatcher::Matches* matchTemplates(Mat& dist_img, Mat& orientation_img, const ImageRange& range, float orientation_weight);
          
          void computeDistanceTransform(Mat& edges_img, Mat& dist_img, Mat& annotate_img, float truncate_dt, float a, float b);
          void computeEdgeOrientations(Mat& edge_img, Mat& orientation_img);
          void fillNonContourOrientations(Mat& annotated_img, Mat& orientation_img);
          
          
      public:
          /**
           * Finds a contour in an edge image. The original image is altered by removing the found contour.
           * @param templ_img Edge image
           * @param coords Coordinates forming the contour.
           * @return True while a contour is still found in the image.
           */
          static bool findContour(Mat& templ_img, template_coords_t& coords);
          
          /**
           * Computes contour points orientations using the approach from:
           *
           * Matas, Shao and Kittler - Estimation of Curvature and Tangent Direction by
           * Median Filtered Differencing
           *
           * @param coords Contour points
           * @param orientations Contour points orientations
           */
          static void findContourOrientations(const template_coords_t& coords, template_orientations_t& orientations);
          
          
          /**
           * Computes the angle of a line segment.
           *
           * @param a One end of the line segment
           * @param b The other end.
           * @param dx
           * @param dy
           * @return Angle in radians.
           */
          static float getAngle(coordinate_t a, coordinate_t b, int& dx, int& dy);
          
          /**
           * Finds a point in the image from which to start contour following.
           * @param templ_img
           * @param p
           * @return
           */
          
          static bool findFirstContourPoint(Mat& templ_img, coordinate_t& p);
          /**
           * Method that extracts a single continuous contour from an image given a starting point.
           * When it extracts the contour it tries to maintain the same direction (at a T-join for example).
           *
           * @param templ_
           * @param coords
           * @param direction
           */
          static void followContour(Mat& templ_img, template_coords_t& coords, int direction);
          
          
      };
      
      
      
      
      class LocationImageIterator : public ImageIterator
      {
          const std::vector<Point>& locations_;
          
          size_t iter_;
          
          int scales_;
          float min_scale_;
          float max_scale_;
          
          float scale_;
          float scale_step_;
          int scale_cnt_;
          
          bool has_next_;
  
  		LocationImageIterator(const LocationImageIterator&);
  		LocationImageIterator& operator=(const LocationImageIterator&);
          
      public:
          LocationImageIterator(const std::vector<Point>& locations, int scales, float min_scale, float max_scale);
          
          bool hasNext() const {
              return has_next_;
          }
          
          location_scale_t next();
      };
      
      class LocationScaleImageIterator : public ImageIterator
      {
          const std::vector<Point>& locations_;
          const std::vector<float>& scales_;
          
          size_t iter_;
          
          bool has_next_;
  
  		LocationScaleImageIterator(const LocationScaleImageIterator&);
  		LocationScaleImageIterator& operator=(const LocationScaleImageIterator&);
          
      public:
          LocationScaleImageIterator(const std::vector<Point>& locations, const std::vector<float>& scales) :
          locations_(locations), scales_(scales)
          {
              assert(locations.size()==scales.size());
              reset();
          }
          
          void reset()
          {
              iter_ = 0;
              has_next_ = (locations_.size()==0 ? false : true);
          }
          
          bool hasNext() const {
              return has_next_;
          }
          
          location_scale_t next();
      };
      
      class SlidingWindowImageIterator : public ImageIterator
      {
          int x_;
          int y_;
          float scale_;
          float scale_step_;
          int scale_cnt_;
          
          bool has_next_;
          
          int width_;
          int height_;
          int x_step_;
          int y_step_;
          int scales_;
          float min_scale_;
          float max_scale_;
          
          
      public:
          
          SlidingWindowImageIterator(int width, int height, int x_step, int y_step, int scales, float min_scale, float max_scale);
          
          bool hasNext() const {
              return has_next_;
          }
          
          location_scale_t next();
      };
      
      
      
      
      int count;
      Matches matches;
      int pad_x;
      int pad_y;
      int scales;
      float minScale;
      float maxScale;
      float orientation_weight;
      float truncate;
      Matching * chamfer_;
      
  public:
      ChamferMatcher(int _max_matches = 20, float _min_match_distance = 1.0, int _pad_x = 3,
                     int _pad_y = 3, int _scales = 5, float _minScale = 0.6, float _maxScale = 1.6,
                     float _orientation_weight = 0.5, float _truncate = 20)
      {
          max_matches_ = _max_matches;
          min_match_distance_ = _min_match_distance;
          pad_x = _pad_x;
          pad_y = _pad_y;
          scales = _scales;
          minScale = _minScale;
          maxScale = _maxScale;
          orientation_weight = _orientation_weight;
          truncate = _truncate;
          count = 0;
          
          matches.resize(max_matches_);
          chamfer_ = new Matching(true);
      }
      
      void showMatch(Mat& img, int index = 0);
      void showMatch(Mat& img, Match match_);
      
      const Matches& matching(Template&, Mat&);
      
  private:
      void addMatch(float cost, Point offset, const Template* tpl);
      
      
  };
  
      
  ///////////////////// implementation ///////////////////////////
  
  ChamferMatcher::SlidingWindowImageIterator::SlidingWindowImageIterator( int width, 
  																		int height, 
  																		int x_step = 3, 
  																		int y_step = 3, 
  																		int scales = 5, 
  																		float min_scale = 0.6, 
  																		float max_scale = 1.6) :
  																			
  																			width_(width), 
  																			height_(height), 
  																			x_step_(x_step),
  																			y_step_(y_step), 
  																			scales_(scales), 
  																			min_scale_(min_scale), 
  																			max_scale_(max_scale)
  {
  	x_ = 0;
  	y_ = 0;
  	scale_cnt_ = 0;
  	scale_ = min_scale_;
  	has_next_ = true;
  	scale_step_ = (max_scale_-min_scale_)/scales_;
  }
  
  location_scale_t ChamferMatcher::SlidingWindowImageIterator::next()
  {
  	location_scale_t next_val = std::make_pair(Point(x_,y_),scale_);
  
  	x_ += x_step_;
  
  	if (x_ >= width_) {
  		x_ = 0;
  		y_ += y_step_;
  
  		if (y_ >= height_) {
  			y_ = 0;
  			scale_ += scale_step_;
  			scale_cnt_++;
  
  			if (scale_cnt_ == scales_) {
  				has_next_ = false;
  				scale_cnt_ = 0;
  				scale_ = min_scale_;
  			}
  		}
  	}
  
  	return next_val;
  }
  
  
  
  ChamferMatcher::ImageIterator* ChamferMatcher::SlidingWindowImageRange::iterator() const
  {
  	return new SlidingWindowImageIterator(width_, height_, x_step_, y_step_, scales_, min_scale_, max_scale_);
  }
  
  
  
  ChamferMatcher::LocationImageIterator::LocationImageIterator(const std::vector<Point>& locations, 
  																int scales = 5, 
  																float min_scale = 0.6, 
  																float max_scale = 1.6) :
  																	locations_(locations), 
  																	scales_(scales), 
  																	min_scale_(min_scale), 
  																	max_scale_(max_scale)
  { 
  	iter_ = 0;
  	scale_cnt_ = 0;
  	scale_ = min_scale_;
  	has_next_ = (locations_.size()==0 ? false : true);
  	scale_step_ = (max_scale_-min_scale_)/scales_;
  }
  
  location_scale_t ChamferMatcher::LocationImageIterator:: next()
  {
  	location_scale_t next_val = std::make_pair(locations_[iter_],scale_);
  
  	iter_ ++;
  	if (iter_==locations_.size()) {
  		iter_ = 0;
  		scale_ += scale_step_;
  		scale_cnt_++;
  
  		if (scale_cnt_ == scales_) {
  			has_next_ = false;
  			scale_cnt_ = 0;
  			scale_ = min_scale_;
  		}
  	}
  
  	return next_val;
  }
  
  
  location_scale_t ChamferMatcher::LocationScaleImageIterator::next()
  {
  	location_scale_t next_val = std::make_pair(locations_[iter_],scales_[iter_]);
  
  	iter_ ++;
  	if (iter_==locations_.size()) {
  		iter_ = 0;
  
  		has_next_ = false;
  	}
  
  	return next_val;
  }
  
  
  
  bool ChamferMatcher::Matching::findFirstContourPoint(Mat& templ_img, coordinate_t& p)
  {
  	for (int y=0;y<templ_img.rows;++y) {
  		for (int x=0;x<templ_img.cols;++x) {
  			if (templ_img.at<uchar>(y,x)!=0) {
  				p.first = x;
  				p.second = y;
  				return true;
  			}
  		}
  	}
  	return false;
  }
  
  
  
  void ChamferMatcher::Matching::followContour(Mat& templ_img, template_coords_t& coords, int direction = -1)
  {
  	const int dir[][2] = { {-1,-1}, {-1,0}, {-1,1}, {0,1}, {1,1}, {1,0}, {1,-1}, {0,-1} };
  	coordinate_t next;
  	coordinate_t next_temp;
  	unsigned char ptr;
  
  	assert (direction==-1 || !coords.empty());
  
  	coordinate_t crt = coords.back();
  
  	// mark the current pixel as visited
  	templ_img.at<uchar>(crt.second,crt.first) = 0;
  	if (direction==-1) {
  		for (int j = 0; j<7; ++j) {
  			next.first = crt.first + dir[j][1];
  			next.second = crt.second + dir[j][0];
  			if (next.first >= 0 && next.first < templ_img.cols &&
  				next.second >= 0 && next.second < templ_img.rows){
  				ptr = templ_img.at<uchar>(next.second, next.first);
  				if (ptr!=0) {
  					coords.push_back(next);
  					followContour(templ_img, coords,j);
  					// try to continue contour in the other direction
  					reverse(coords.begin(), coords.end());
  					followContour(templ_img, coords, (j+4)%8);
  					break;
  				}
  			}
  		}
  	}
  	else {
  		int k = direction;
  		int k_cost = 3;
  		next.first = crt.first + dir[k][1];
  		next.second = crt.second + dir[k][0];
  		if (next.first >= 0 && next.first < templ_img.cols &&
  				next.second >= 0 && next.second < templ_img.rows){
  			ptr = templ_img.at<uchar>(next.second, next.first);
  			if (ptr!=0) {
  				k_cost = std::abs(dir[k][1]) + std::abs(dir[k][0]);
  			}
  			int p = k;
  			int n = k;
  
  			for (int j = 0 ;j<3; ++j) {
  				p = (p + 7) % 8;
  				n = (n + 1) % 8;
  				next.first = crt.first + dir[p][1];
  				next.second = crt.second + dir[p][0];
  				if (next.first >= 0 && next.first < templ_img.cols &&
  					next.second >= 0 && next.second < templ_img.rows){
  					ptr = templ_img.at<uchar>(next.second, next.first);
  					if (ptr!=0) {
  						int p_cost = std::abs(dir[p][1]) + std::abs(dir[p][0]);
  						if (p_cost<k_cost) {
  							k_cost = p_cost;
  							k = p;
  						}
  					}
  					next.first = crt.first + dir[n][1];
  					next.second = crt.second + dir[n][0];
  					if (next.first >= 0 && next.first < templ_img.cols &&
  					next.second >= 0 && next.second < templ_img.rows){
  						ptr = templ_img.at<uchar>(next.second, next.first);
  						if (ptr!=0) {
  							int n_cost = std::abs(dir[n][1]) + std::abs(dir[n][0]);
  							if (n_cost<k_cost) {
  								k_cost = n_cost;
  								k = n;
  							}
  						}
  					}
  				}
  			}
  
  			if (k_cost!=3) {
  				next.first = crt.first + dir[k][1];
  				next.second = crt.second + dir[k][0];
  				if (next.first >= 0 && next.first < templ_img.cols &&
  					next.second >= 0 && next.second < templ_img.rows) {
  					coords.push_back(next);
  					followContour(templ_img, coords, k);
  				}
  			}
  		}
  	}
  }
  
  
  bool ChamferMatcher::Matching::findContour(Mat& templ_img, template_coords_t& coords)
  {
  	coordinate_t start_point;
  
  	bool found = findFirstContourPoint(templ_img,start_point);
  	if (found) {
  		coords.push_back(start_point);
  		followContour(templ_img, coords);
  		return true;
  	}
  
  	return false;
  }
  
  
  float ChamferMatcher::Matching::getAngle(coordinate_t a, coordinate_t b, int& dx, int& dy)
  {
  	dx = b.first-a.first;
  	dy = -(b.second-a.second);  // in image coordinated Y axis points downward
          float angle = atan2((float)dy,(float)dx);
  	
  	if (angle<0) {
                  angle+=(float)CV_PI;
  	}
  	
  	return angle;
  }
  
  
  
  void ChamferMatcher::Matching::findContourOrientations(const template_coords_t& coords, template_orientations_t& orientations)
  {
  	const int M = 5;
  	int coords_size = coords.size();
  
      std::vector<float> angles(2*M);
          orientations.insert(orientations.begin(), coords_size, float(-3*CV_PI)); // mark as invalid in the beginning
  
  	if (coords_size<2*M+1) {  // if contour not long enough to estimate orientations, abort
  		return;
  	}
  
  	for (int i=M;i<coords_size-M;++i) {
  		coordinate_t crt = coords[i];
  		coordinate_t other;
  		int k = 0;
  		int dx, dy;
  		// compute previous M angles
  		for (int j=M;j>0;--j) {
  			other = coords[i-j];
  			angles[k++] = getAngle(other,crt, dx, dy);
  		}
  		// compute next M angles
  		for (int j=1;j<=M;++j) {
  			other = coords[i+j];
  			angles[k++] = getAngle(crt, other, dx, dy);
  		}
  
  		// get the middle two angles
  		nth_element(angles.begin(), angles.begin()+M-1,  angles.end());
  		nth_element(angles.begin()+M-1, angles.begin()+M,  angles.end());
  		//		sort(angles.begin(), angles.end());
  
  		// average them to compute tangent
  		orientations[i] = (angles[M-1]+angles[M])/2;
  	}
  }
  
  //////////////////////// Template /////////////////////////////////////
  
  ChamferMatcher::Template::Template(Mat& edge_image, float scale_) : addr_width(-1), scale(scale_)
  {
  	template_coords_t local_coords;
  	template_orientations_t local_orientations;
  
  	while (ChamferMatcher::Matching::findContour(edge_image, local_coords)) {
  		ChamferMatcher::Matching::findContourOrientations(local_coords, local_orientations);
  
  		coords.insert(coords.end(), local_coords.begin(), local_coords.end());
  		orientations.insert(orientations.end(), local_orientations.begin(), local_orientations.end());
  		local_coords.clear();
  		local_orientations.clear();
  	}
  
  
  	size = edge_image.size();
  	Point min, max;
  	min.x = size.width;
  	min.y = size.height;
  	max.x = 0;
  	max.y = 0;
  
  	center = Point(0,0);
  	for (size_t i=0;i<coords.size();++i) {
  		center.x += coords[i].first;
  		center.y += coords[i].second;
  
  		if (min.x>coords[i].first) min.x = coords[i].first;
  		if (min.y>coords[i].second) min.y = coords[i].second;
  		if (max.x<coords[i].first) max.x = coords[i].first;
  		if (max.y<coords[i].second) max.y = coords[i].second;
  	}
  
  	size.width = max.x - min.x;
  	size.height = max.y - min.y;
  
  	center.x /= (coords.size()==0?1:coords.size());
  	center.y /= (coords.size()==0?1:coords.size());
  
  	for (size_t i=0;i<coords.size();++i) {
  		coords[i].first -= center.x;
  		coords[i].second -= center.y;
  	}
  }
  
  
  vector<int>& ChamferMatcher::Template::getTemplateAddresses(int width)
  {
  	if (addr_width!=width) {
  		addr.resize(coords.size());
  		addr_width = width;
  
  		for (size_t i=0; i<coords.size();++i) {
  			addr[i] = coords[i].second*width+coords[i].first;
  		}
  	}
  	return addr;
  }
  
  
  /**
   * Resizes a template
   *
   * @param scale Scale to be resized to
   */
  ChamferMatcher::Template* ChamferMatcher::Template::rescale(float new_scale)
  {
  
  	if (fabs(scale-new_scale)<1e-6) return this;
  
  	for (size_t i=0;i<scaled_templates.size();++i) {
  		if (fabs(scaled_templates[i]->scale-new_scale)<1e-6) {
  			return scaled_templates[i];
  		}
  	}
  
  	float scale_factor = new_scale/scale;
  
  	Template* tpl = new Template();
  	tpl->scale = new_scale;
  
  	tpl->center.x = int(center.x*scale_factor+0.5);
  	tpl->center.y = int(center.y*scale_factor+0.5);
  
  	tpl->size.width = int(size.width*scale_factor+0.5);
  	tpl->size.height = int(size.height*scale_factor+0.5);
  
  	tpl->coords.resize(coords.size());
  	tpl->orientations.resize(orientations.size());
  	for (size_t i=0;i<coords.size();++i) {
  		tpl->coords[i].first = int(coords[i].first*scale_factor+0.5);
  		tpl->coords[i].second = int(coords[i].second*scale_factor+0.5);
  		tpl->orientations[i] = orientations[i];
  	}
  	scaled_templates.push_back(tpl);
  
  	return tpl;
  
  }
  
  
  
  void ChamferMatcher::Template::show() const
  {
  	int pad = 50;
  	//Attention size is not correct
  	Mat templ_color (Size(size.width+(pad*2), size.height+(pad*2)), CV_8UC3);
  	templ_color.setTo(0);
  
  	for (size_t i=0;i<coords.size();++i) {
  
  		int x = center.x+coords[i].first+pad;
  		int y = center.y+coords[i].second+pad;
  		templ_color.at<Vec3b>(y,x)[1]=255;
  		//CV_PIXEL(unsigned char, templ_color,x,y)[1] = 255;
  
  		if (i%3==0) {
                          if (orientations[i] < -CV_PI) {
  				continue;
  			}
  			Point p1;
  			p1.x = x;
  			p1.y = y;
  			Point p2;
  			p2.x = x + pad*(int)(sin(orientations[i])*100)/100;
  			p2.y = y + pad*(int)(cos(orientations[i])*100)/100;
  
  			line(templ_color, p1,p2, CV_RGB(255,0,0));
  		}
  	}
  
  
  	circle(templ_color,Point(center.x + pad, center.y + pad),1,CV_RGB(0,255,0));
  
  	namedWindow("templ",1);
  	imshow("templ",templ_color);
  
  	cvWaitKey(0);
  
  	templ_color.release();
  }
  
  
  //////////////////////// Matching /////////////////////////////////////
  
  
  void ChamferMatcher::Matching::addTemplateFromImage(Mat& templ, float scale)
  {
  	Template* cmt = new Template(templ, scale);
  	if(templates.size() > 0)
  		templates.clear();
  	templates.push_back(cmt);
  	cmt->show();
  }
  
  void ChamferMatcher::Matching::addTemplate(Template& template_){
  	if(templates.size() > 0)
  		templates.clear();
  	templates.push_back(&template_);
  }
  /**
   * Alternative version of computeDistanceTransform, will probably be used to compute distance
   * transform annotated with edge orientation.
   */
  void ChamferMatcher::Matching::computeDistanceTransform(Mat& edges_img, Mat& dist_img, Mat& annotate_img, float truncate_dt, float a = 1.0, float b = 1.5)
  {
  	int d[][2] = { {-1,-1}, { 0,-1}, { 1,-1},
  			{-1,0},          { 1,0},
  			{-1,1}, { 0,1}, { 1,1} };
  
  
  	Size s = edges_img.size();
  	int w = s.width;
  	int h = s.height;
  	// set distance to the edge pixels to 0 and put them in the queue
      std::queue<std::pair<int,int> > q;
  
  
  
  	for (int y=0;y<h;++y) {
  		for (int x=0;x<w;++x) {
  
  			unsigned char edge_val = edges_img.at<uchar>(y,x);
  			if ( (edge_val!=0) ) {
  				q.push(std::make_pair(x,y));
  				dist_img.at<float>(y,x)= 0;
  
  				if (&annotate_img!=NULL) {
  					annotate_img.at<Vec2i>(y,x)[0]=x;
  					annotate_img.at<Vec2i>(y,x)[1]=y;
  				}
  			}
  			else {
  				dist_img.at<float>(y,x)=-1;
  			}
  		}
  	}
  
  	// breadth first computation of distance transform
      std::pair<int,int> crt;
  	while (!q.empty()) {
  		crt = q.front();
  		q.pop();
  
  		int x = crt.first;
  		int y = crt.second;
  
  		float dist_orig = dist_img.at<float>(y,x);
  		float dist;
  
  		for (size_t i=0;i<sizeof(d)/sizeof(d[0]);++i) {
  			int nx = x + d[i][0];
  			int ny = y + d[i][1];
  
  			if (nx<0 || ny<0 || nx>=w || ny>=h) continue;
  
  			if (std::abs(d[i][0]+d[i][1])==1) {
  				dist = (dist_orig)+a;
  			}
  			else {
  				dist = (dist_orig)+b;
  			}
  
  			float dt = dist_img.at<float>(ny,nx);
  
  			if (dt==-1 || dt>dist) {
  				dist_img.at<float>(ny,nx) = dist;
  				q.push(std::make_pair(nx,ny));
  
  				if (&annotate_img!=NULL) {
  					annotate_img.at<Vec2i>(ny,nx)[0]=annotate_img.at<Vec2i>(y,x)[0];
  					annotate_img.at<Vec2i>(ny,nx)[1]=annotate_img.at<Vec2i>(y,x)[1];
  				}
  			}
  		}
  	}
  	// truncate dt
  
  	if (truncate_dt>0) {
  		Mat dist_img_thr = dist_img.clone();
  		threshold(dist_img, dist_img_thr, truncate_dt,0.0 ,THRESH_TRUNC);
  		dist_img_thr.copyTo(dist_img);
  	}
  }
  
  
  void ChamferMatcher::Matching::computeEdgeOrientations(Mat& edge_img, Mat& orientation_img)
  {
  	Mat contour_img(edge_img.size(), CV_8UC1);
  
          orientation_img.setTo(3*(-CV_PI));
  	template_coords_t coords;
  	template_orientations_t orientations;
  
  	while (ChamferMatcher::Matching::findContour(edge_img, coords)) {
  
  		ChamferMatcher::Matching::findContourOrientations(coords, orientations);
  
  		// set orientation pixel in orientation image
  		for (size_t i = 0; i<coords.size();++i) {
  			int x = coords[i].first;
  			int y = coords[i].second;
                          //			if (orientations[i]>-CV_PI)
  			//	{
  			//CV_PIXEL(unsigned char, contour_img, x, y)[0] = 255;
  			contour_img.at<uchar>(y,x)=255;
  			//	}
  			//CV_PIXEL(float, orientation_img, x, y)[0] = orientations[i];
  			orientation_img.at<float>(y,x)=orientations[i];
  		}
  		
  		
  		coords.clear();
  		orientations.clear();
  	}
  
  	//imwrite("contours.pgm", contour_img);
  }
  
  
  void ChamferMatcher::Matching::fillNonContourOrientations(Mat& annotated_img, Mat& orientation_img)
  {
  	int cols = annotated_img.cols;
  	int rows = annotated_img.rows;
  
  	assert(orientation_img.cols==cols && orientation_img.rows==rows);
  
  	for (int y=0;y<rows;++y) {
  		for (int x=0;x<cols;++x) {
  			int xorig = annotated_img.at<Vec2i>(y,x)[0];
  			int yorig = annotated_img.at<Vec2i>(y,x)[1];
  
  			if (x!=xorig || y!=yorig) {
  				//orientation_img.at<float>(yorig,xorig)=orientation_img.at<float>(y,x);
  				orientation_img.at<float>(y,x)=orientation_img.at<float>(yorig,xorig);
  			}
  		}
  	}
  }
  
  
  ChamferMatcher::Match* ChamferMatcher::Matching::localChamferDistance(Point offset, Mat& dist_img, Mat& orientation_img,
  		ChamferMatcher::Template* tpl, float alpha)
  {
  	int x = offset.x;
  	int y = offset.y;
  
  	float beta = 1-alpha;
  
      std::vector<int>& addr = tpl->getTemplateAddresses(dist_img.cols);
  
  	float* ptr = dist_img.ptr<float>(y)+x;
  	
  	
  	float sum_distance = 0;
  	for (size_t i=0; i<addr.size();++i) {
  		if(addr[i] < (dist_img.cols*dist_img.rows) - (offset.y*dist_img.cols + offset.x)){
  			sum_distance += *(ptr+addr[i]);
  		}
  	}
  
  	float cost = (sum_distance/truncate_)/addr.size();
  	
  
  	if (&orientation_img!=NULL) {
  		float* optr = orientation_img.ptr<float>(y)+x;
  		float sum_orientation = 0;
  		int cnt_orientation = 0;
  
  		for (size_t i=0;i<addr.size();++i) {
  		
  			if(addr[i] < (orientation_img.cols*orientation_img.rows) - (offset.y*orientation_img.cols + offset.x)){
                                  if (tpl->orientations[i]>=-CV_PI && (*(optr+addr[i]))>=-CV_PI) {
  					sum_orientation += orientation_diff(tpl->orientations[i], (*(optr+addr[i])));
  					cnt_orientation++;
  				}
  			}
  		}
  
  		if (cnt_orientation>0) {
                          cost = (float)(beta*cost+alpha*(sum_orientation/(2*CV_PI))/cnt_orientation);
  		}
  
  	}
  
  	if(cost > 0){
  		ChamferMatcher::Match* istance(new ChamferMatcher::Match());
  		istance->cost = cost;
  		istance->offset = offset;
  		istance->tpl = tpl;
  
  		return istance;
  	}
  
  	return NULL; 
  }
  
  
  ChamferMatcher::Matches* ChamferMatcher::Matching::matchTemplates(Mat& dist_img, Mat& orientation_img, const ImageRange& range, float orientation_weight)			
  {
  		
  	ChamferMatcher::Matches* matches(new Matches());
  	// try each template
  	for(size_t i = 0; i < templates.size(); i++) {
  		ImageIterator* it = range.iterator();
  		while (it->hasNext()) {
  			location_scale_t crt = it->next();
  
  			Point loc = crt.first;
  			float scale = crt.second;
  			Template* tpl = templates[i]->rescale(scale);
  	
  
  			if (loc.x-tpl->center.x<0 || loc.x+tpl->size.width/2>=dist_img.cols) continue;
  			if (loc.y-tpl->center.y<0 || loc.y+tpl->size.height/2>=dist_img.rows) continue;
  
  			ChamferMatcher::Match* is = localChamferDistance(loc, dist_img, orientation_img, tpl, orientation_weight);
  			if(is)
  				matches->push_back(*is);
  		}
  
  		delete it;
  	}
  	return matches;
  }
  
  
  
  /**
   * Run matching using an edge image.
   * @param edge_img Edge image
   * @return a match object
   */
  ChamferMatcher::Matches* ChamferMatcher::Matching::matchEdgeImage(Mat& edge_img, const ImageRange& range, float orientation_weight, int /*max_matches*/, float /*min_match_distance*/)
  {
  	CV_Assert(edge_img.channels()==1);
  
  	Mat dist_img;
  	Mat annotated_img;
  	Mat orientation_img;
  
  	annotated_img.create(edge_img.size(), CV_32SC2);
  	dist_img.create(edge_img.size(),CV_32FC1);
  	dist_img.setTo(0);
  	// Computing distance transform
  	computeDistanceTransform(edge_img,dist_img, annotated_img, truncate_);
  
  
  	//orientation_img = NULL;
  	if (use_orientation_) {
  		orientation_img.create(edge_img.size(), CV_32FC1);
  		orientation_img.setTo(0);
  		Mat edge_clone = edge_img.clone();
  		computeEdgeOrientations(edge_clone, orientation_img );
  		edge_clone.release();
  		fillNonContourOrientations(annotated_img, orientation_img);
  	}
  
  
  	// Template matching
  	ChamferMatcher::Matches* matches = matchTemplates(	dist_img, 
  														orientation_img,
  														range, 
  														orientation_weight);
  
  
  	if (use_orientation_) {
  		orientation_img.release();
  	}
  	dist_img.release();
  	annotated_img.release();
  
  	return matches;
  }
  
  
  void ChamferMatcher::addMatch(float cost, Point offset, const Template* tpl)
  {
  	bool new_match = true;
  	for (int i=0; i<count; ++i) {
  		if (std::abs(matches[i].offset.x-offset.x)+std::abs(matches[i].offset.y-offset.y)<min_match_distance_) {
  			// too close, not a new match
  			new_match = false;
  			// if better cost, replace existing match
  			if (cost<matches[i].cost) {
  				matches[i].cost = cost;
  				matches[i].offset = offset;
  				matches[i].tpl = tpl;
  			}
  			// re-bubble to keep ordered
  			int k = i;
  			while (k>0) {
  				if (matches[k-1].cost>matches[k].cost) {
                      std::swap(matches[k-1],matches[k]);
  				}
  				k--;
  			}
  
  			break;
  		}
  	}
  
  	if (new_match) {
  		// if we don't have enough matches yet, add it to the array
  		if (count<max_matches_) {
  			matches[count].cost = cost;
  			matches[count].offset = offset;
  			matches[count].tpl = tpl;
  			count++;
  		}
  		// otherwise find the right position to insert it
  		else {
  			// if higher cost than the worst current match, just ignore it
  			if (matches[count-1].cost<cost) {
  				return;
  			}
  
  			int j = 0;
  			// skip all matches better than current one
  			while (matches[j].cost<cost) j++;
  
  			// shift matches one position
  			int k = count-2;
  			while (k>=j) {
  				matches[k+1] = matches[k];
  				k--;
  			}
  
  			matches[j].cost = cost;
  			matches[j].offset = offset;
  			matches[j].tpl = tpl;
  		}
  	}
  }
  
  void ChamferMatcher::showMatch(Mat& img, int index)
  {
  	if (index>=count) {
          std::cout << "Index too big.\n" << std::endl;
  	}
  
  	assert(img.channels()==3);
  
  	Match match = matches[index];
  
  	const template_coords_t& templ_coords = match.tpl->coords;
  	int x, y;
  	for (size_t i=0;i<templ_coords.size();++i) {
  		x = match.offset.x + templ_coords[i].first;
  		y = match.offset.y + templ_coords[i].second;
  
  		if ( x > img.cols-1 || x < 0 || y > img.rows-1 || y < 0) continue;
  		img.at<Vec3b>(y,x)[0]=0;
  		img.at<Vec3b>(y,x)[2]=0;
  		img.at<Vec3b>(y,x)[1]=255;
  	}
  }
  
  void ChamferMatcher::showMatch(Mat& img, Match match)
  {
  	assert(img.channels()==3);
  
  	const template_coords_t& templ_coords = match.tpl->coords;
  	for (size_t i=0;i<templ_coords.size();++i) {
  		int x = match.offset.x + templ_coords[i].first;
  		int y = match.offset.y + templ_coords[i].second;
  		if ( x > img.cols-1 || x < 0 || y > img.rows-1 || y < 0) continue;
  		img.at<Vec3b>(y,x)[0]=0;
  		img.at<Vec3b>(y,x)[2]=0;
  		img.at<Vec3b>(y,x)[1]=255;
  	}
  	match.tpl->show();
  }
  
  const ChamferMatcher::Matches& ChamferMatcher::matching(Template& tpl, Mat& image_){
  	chamfer_->addTemplate(tpl);
  
  	matches.clear();
  	matches.resize(max_matches_);
  	count = 0;
  
  
  	Matches* matches_ = chamfer_->matchEdgeImage(	image_, 
  													ChamferMatcher::
  														SlidingWindowImageRange(image_.cols, 
  																				image_.rows,
  																				pad_x,
  																				pad_y,
  																				scales,
  																				minScale,
  																				maxScale), 
  													orientation_weight, 
  													max_matches_, 
  													min_match_distance_);
  
  
  	
  	for(int i = 0; i < (int)matches_->size(); i++){
  		addMatch(matches_->at(i).cost, matches_->at(i).offset, matches_->at(i).tpl);
  	}
  
  	matches_->clear();
  	delete matches_;
  	matches_ = NULL;
  
  	matches.resize(count);
  
  
  	return matches;
  
  }
  
      
  int chamerMatching( Mat& img, Mat& templ,
                      std::vector<std::vector<Point> >& results, std::vector<float>& costs,
                      double templScale, int maxMatches, double minMatchDistance, int padX,
                      int padY, int scales, double minScale, double maxScale,
                      double orientationWeight, double truncate )
  {
      CV_Assert(img.type() == CV_8UC1 && templ.type() == CV_8UC1);
      
      ChamferMatcher matcher_(maxMatches, (float)minMatchDistance, padX, padY, scales,
                              (float)minScale, (float)maxScale,
                              (float)orientationWeight, (float)truncate);
  
      ChamferMatcher::Template template_(templ, (float)templScale);
      ChamferMatcher::Matches match_instances = matcher_.matching(template_, img);
      
      size_t i, nmatches = match_instances.size();
      
      results.resize(nmatches);
      costs.resize(nmatches);
      
      int bestIdx = -1;
      double minCost = DBL_MAX;
      
      for( i = 0; i < nmatches; i++ )
      {
          const ChamferMatcher::Match& match = match_instances[i];
          double cval = match.cost;
          if( cval < minCost)
          {
              minCost = cval;
              bestIdx = (int)i;
          }
          costs[i] = (float)cval;
          
          const template_coords_t& templ_coords = match.tpl->coords;
          std::vector<Point>& templPoints = results[i];
          size_t j, npoints = templ_coords.size();
          templPoints.resize(npoints);
          
          for (j = 0; j < npoints; j++ )
          {
              int x = match.offset.x + templ_coords[j].first;
              int y = match.offset.y + templ_coords[j].second;
              templPoints[j] = Point(x,y);
          }
      }
      
      return bestIdx;
  }
  
  }
  