modules/contrib/src/chamfermatching.cpp

   1 /*********************************************************************
   2  * Software License Agreement (BSD License)
   3  *
   4  *  Copyright (c) 2008-2010, Willow Garage, Inc.
   5  *  All rights reserved.
   6  *
   7  *  Redistribution and use in source and binary forms, with or without
   8  *  modification, are permitted provided that the following conditions
   9  *  are met:
  10  *
  11  *   * Redistributions of source code must retain the above copyright
  12  *     notice, this list of conditions and the following disclaimer.
  13  *   * Redistributions in binary form must reproduce the above
  14  *     copyright notice, this list of conditions and the following
  15  *     disclaimer in the documentation and/or other materials provided
  16  *     with the distribution.
  17  *   * Neither the name of the Willow Garage nor the names of its
  18  *     contributors may be used to endorse or promote products derived
  19  *     from this software without specific prior written permission.
  20  *
  21  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  22  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  23  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  24  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  25  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  26  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
  27  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  28  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  29  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  30  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
  31  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  32  *  POSSIBILITY OF SUCH DAMAGE.
  33  *********************************************************************/
  34
  35 //
  36 // The original code was written by
  37 //          Marius Muja
  38 // and later modified and prepared
  39 //  for integration into OpenCV by
  40 //        Antonella Cascitelli,
  41 //        Marco Di Stefano and
  42 //          Stefano Fabri
  43 //        from Univ. of Rome
  44 //
  45
  46 #include "precomp.hpp"
  47 #include "opencv2/opencv_modules.hpp"
  48 #ifdef HAVE_OPENCV_HIGHGUI
  49 #  include "opencv2/highgui.hpp"
  50 #endif
  51 #include <iostream>
  52 #include <queue>
  53
  54 namespace cv
  55 {
  56
  57 typedef std::pair<int,int> coordinate_t;
  58 typedef float orientation_t;
  59 typedef std::vector<coordinate_t> template_coords_t;
  60 typedef std::vector<orientation_t> template_orientations_t;
  61 typedef std::pair<Point, float> location_scale_t;
  62
  63 class ChamferMatcher
  64 {
  65
  66 private:
  67     class Matching;
  68     int max_matches_;
  69     float min_match_distance_;
  70
  71     ///////////////////////// Image iterators ////////////////////////////
  72
  73     class ImageIterator
  74     {
  75     public:
  76         virtual ~ImageIterator() {}
  77         virtual bool hasNext() const = 0;
  78         virtual location_scale_t next() = 0;
  79     };
  80
  81     class ImageRange
  82     {
  83     public:
  84         virtual ImageIterator* iterator() const = 0;
  85         virtual ~ImageRange() {}
  86     };
  87
  88     // Sliding window
  89
  90     class SlidingWindowImageRange : public ImageRange
  91     {
  92         int width_;
  93         int height_;
  94         int x_step_;
  95         int y_step_;
  96         int scales_;
  97         float min_scale_;
  98         float max_scale_;
  99
 100     public:
 101         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) :
 102         width_(width), height_(height), x_step_(x_step),y_step_(y_step), scales_(_scales), min_scale_(min_scale), max_scale_(max_scale)
 103         {
 104         }
 105
 106
 107         ImageIterator* iterator() const;
 108     };
 109
 110     class LocationImageRange : public ImageRange
 111     {
 112         const std::vector<Point>& locations_;
 113
 114         int scales_;
 115         float min_scale_;
 116         float max_scale_;
 117
 118         LocationImageRange(const LocationImageRange&);
 119         LocationImageRange& operator=(const LocationImageRange&);
 120
 121     public:
 122         LocationImageRange(const std::vector<Point>& locations, int _scales = 5, float min_scale = 0.6, float max_scale = 1.6) :
 123         locations_(locations), scales_(_scales), min_scale_(min_scale), max_scale_(max_scale)
 124         {
 125         }
 126
 127         ImageIterator* iterator() const
 128         {
 129             return new LocationImageIterator(locations_, scales_, min_scale_, max_scale_);
 130         }
 131     };
 132
 133
 134     class LocationScaleImageRange : public ImageRange
 135     {
 136         const std::vector<Point>& locations_;
 137         const std::vector<float>& scales_;
 138
 139         LocationScaleImageRange(const LocationScaleImageRange&);
 140         LocationScaleImageRange& operator=(const LocationScaleImageRange&);
 141     public:
 142         LocationScaleImageRange(const std::vector<Point>& locations, const std::vector<float>& _scales) :
 143         locations_(locations), scales_(_scales)
 144         {
 145             CV_Assert(locations.size()==_scales.size());
 146         }
 147
 148         ImageIterator* iterator() const
 149         {
 150             return new LocationScaleImageIterator(locations_, scales_);
 151         }
 152     };
 153
 154
 155
 156
 157 public:
 158     /**
 159      * Class that represents a template for chamfer matching.
 160      */
 161     class Template
 162     {
 163         friend class ChamferMatcher::Matching;
 164         friend class ChamferMatcher;
 165
 166
 167     public:
 168         std::vector<Template*> scaled_templates;
 169         std::vector<int> addr;
 170         int addr_width;
 171         float scale;
 172         template_coords_t coords;
 173
 174         template_orientations_t orientations;
 175         Size size;
 176         Point center;
 177
 178     public:
 179         Template() : addr_width(-1)
 180         {
 181         }
 182
 183         Template(Mat& edge_image, float scale_ = 1);
 184
 185         ~Template()
 186         {
 187             for (size_t i=0;i<scaled_templates.size();++i) {
 188                 delete scaled_templates[i];
 189             }
 190             scaled_templates.clear();
 191             coords.clear();
 192             orientations.clear();
 193         }
 194         void show() const;
 195
 196
 197
 198     private:
 199         /**
 200          * Resizes a template
 201          *
 202          * @param scale Scale to be resized to
 203          */
 204         Template* rescale(float scale);
 205
 206         std::vector<int>& getTemplateAddresses(int width);
 207     };
 208
 209
 210
 211     /**
 212      * Used to represent a matching result.
 213      */
 214
 215     class Match
 216     {
 217     public:
 218         float cost;
 219         Point offset;
 220         const Template* tpl;
 221     };
 222
 223     typedef std::vector<Match> Matches;
 224
 225 private:
 226     /**
 227      * Implements the chamfer matching algorithm on images taking into account both distance from
 228      * the template pixels to the nearest pixels and orientation alignment between template and image
 229      * contours.
 230      */
 231     class Matching
 232     {
 233         float truncate_;
 234         bool use_orientation_;
 235
 236         std::vector<Template*> templates;
 237     public:
 238         Matching(bool use_orientation = true, float _truncate = 10) : truncate_(_truncate), use_orientation_(use_orientation)
 239         {
 240         }
 241
 242         ~Matching()
 243         {
 244             for (size_t i = 0; i<templates.size(); i++) {
 245                 delete templates[i];
 246             }
 247         }
 248
 249         /**
 250          * Add a template to the detector from an edge image.
 251          * @param templ An edge image
 252          */
 253         void addTemplateFromImage(Mat& templ, float scale = 1.0);
 254
 255         /**
 256          * Run matching using an edge image.
 257          * @param edge_img Edge image
 258          * @return a match object
 259          */
 260         ChamferMatcher::Matches* matchEdgeImage(Mat& edge_img, const ImageRange& range, float orientation_weight = 0.5, int max_matches = 20, float min_match_distance = 10.0);
 261
 262         void addTemplate(Template& template_);
 263
 264     private:
 265
 266         float orientation_diff(float o1, float o2)
 267         {
 268             return fabs(o1-o2);
 269         }
 270
 271         /**
 272          * Computes the chamfer matching cost for one position in the target image.
 273          * @param offset Offset where to compute cost
 274          * @param dist_img Distance transform image.
 275          * @param orientation_img Orientation image.
 276          * @param tpl Template
 277          * @param templ_orientations Orientations of the target points.
 278          * @return matching result
 279          */
 280         ChamferMatcher::Match* localChamferDistance(Point offset, Mat& dist_img, Mat& orientation_img, Template* tpl,  float orientation_weight);
 281
 282     private:
 283         /**
 284          * Matches all templates.
 285          * @param dist_img Distance transform image.
 286          * @param orientation_img Orientation image.
 287          */
 288         ChamferMatcher::Matches* matchTemplates(Mat& dist_img, Mat& orientation_img, const ImageRange& range, float orientation_weight);
 289
 290         void computeDistanceTransform(Mat& edges_img, Mat& dist_img, Mat& annotate_img, float truncate_dt, float a, float b);
 291         void computeEdgeOrientations(Mat& edge_img, Mat& orientation_img);
 292         void fillNonContourOrientations(Mat& annotated_img, Mat& orientation_img);
 293
 294
 295     public:
 296         /**
 297          * Finds a contour in an edge image. The original image is altered by removing the found contour.
 298          * @param templ_img Edge image
 299          * @param coords Coordinates forming the contour.
 300          * @return True while a contour is still found in the image.
 301          */
 302         static bool findContour(Mat& templ_img, template_coords_t& coords);
 303
 304         /**
 305          * Computes contour points orientations using the approach from:
 306          *
 307          * Matas, Shao and Kittler - Estimation of Curvature and Tangent Direction by
 308          * Median Filtered Differencing
 309          *
 310          * @param coords Contour points
 311          * @param orientations Contour points orientations
 312          */
 313         static void findContourOrientations(const template_coords_t& coords, template_orientations_t& orientations);
 314
 315
 316         /**
 317          * Computes the angle of a line segment.
 318          *
 319          * @param a One end of the line segment
 320          * @param b The other end.
 321          * @param dx
 322          * @param dy
 323          * @return Angle in radians.
 324          */
 325         static float getAngle(coordinate_t a, coordinate_t b, int& dx, int& dy);
 326
 327         /**
 328          * Finds a point in the image from which to start contour following.
 329          * @param templ_img
 330          * @param p
 331          * @return
 332          */
 333
 334         static bool findFirstContourPoint(Mat& templ_img, coordinate_t& p);
 335         /**
 336          * Method that extracts a single continuous contour from an image given a starting point.
 337          * When it extracts the contour it tries to maintain the same direction (at a T-join for example).
 338          *
 339          * @param templ_
 340          * @param coords
 341          * @param direction
 342          */
 343         static void followContour(Mat& templ_img, template_coords_t& coords, int direction);
 344
 345
 346     };
 347
 348
 349
 350
 351     class LocationImageIterator : public ImageIterator
 352     {
 353         const std::vector<Point>& locations_;
 354
 355         size_t iter_;
 356
 357         int scales_;
 358         float min_scale_;
 359         float max_scale_;
 360
 361         float scale_;
 362         float scale_step_;
 363         int scale_cnt_;
 364
 365         bool has_next_;
 366
 367         LocationImageIterator(const LocationImageIterator&);
 368         LocationImageIterator& operator=(const LocationImageIterator&);
 369
 370     public:
 371         LocationImageIterator(const std::vector<Point>& locations, int _scales, float min_scale, float max_scale);
 372
 373         bool hasNext() const {
 374             return has_next_;
 375         }
 376
 377         location_scale_t next();
 378     };
 379
 380     class LocationScaleImageIterator : public ImageIterator
 381     {
 382         const std::vector<Point>& locations_;
 383         const std::vector<float>& scales_;
 384
 385         size_t iter_;
 386
 387         bool has_next_;
 388
 389         LocationScaleImageIterator(const LocationScaleImageIterator&);
 390         LocationScaleImageIterator& operator=(const LocationScaleImageIterator&);
 391
 392     public:
 393         LocationScaleImageIterator(const std::vector<Point>& locations, const std::vector<float>& _scales) :
 394         locations_(locations), scales_(_scales)
 395         {
 396             CV_Assert(locations.size()==_scales.size());
 397             reset();
 398         }
 399
 400         void reset()
 401         {
 402             iter_ = 0;
 403             has_next_ = (locations_.size()==0 ? false : true);
 404         }
 405
 406         bool hasNext() const {
 407             return has_next_;
 408         }
 409
 410         location_scale_t next();
 411     };
 412
 413     class SlidingWindowImageIterator : public ImageIterator
 414     {
 415         int x_;
 416         int y_;
 417         float scale_;
 418         float scale_step_;
 419         int scale_cnt_;
 420
 421         bool has_next_;
 422
 423         int width_;
 424         int height_;
 425         int x_step_;
 426         int y_step_;
 427         int scales_;
 428         float min_scale_;
 429         float max_scale_;
 430
 431
 432     public:
 433
 434         SlidingWindowImageIterator(int width, int height, int x_step, int y_step, int scales, float min_scale, float max_scale);
 435
 436         bool hasNext() const {
 437             return has_next_;
 438         }
 439
 440         location_scale_t next();
 441     };
 442
 443
 444
 445
 446     int count;
 447     Matches matches;
 448     int pad_x;
 449     int pad_y;
 450     int scales;
 451     float minScale;
 452     float maxScale;
 453     float orientation_weight;
 454     float truncate;
 455     Matching * chamfer_;
 456
 457 public:
 458     ChamferMatcher(int _max_matches = 20, float _min_match_distance = 1.0, int _pad_x = 3,
 459                    int _pad_y = 3, int _scales = 5, float _minScale = 0.6, float _maxScale = 1.6,
 460                    float _orientation_weight = 0.5, float _truncate = 20)
 461     {
 462         max_matches_ = _max_matches;
 463         min_match_distance_ = _min_match_distance;
 464         pad_x = _pad_x;
 465         pad_y = _pad_y;
 466         scales = _scales;
 467         minScale = _minScale;
 468         maxScale = _maxScale;
 469         orientation_weight = _orientation_weight;
 470         truncate = _truncate;
 471         count = 0;
 472
 473         matches.resize(max_matches_);
 474         chamfer_ = new Matching(true);
 475     }
 476
 477     ~ChamferMatcher()
 478     {
 479         delete chamfer_;
 480     }
 481
 482     void showMatch(Mat& img, int index = 0);
 483     void showMatch(Mat& img, Match match_);
 484
 485     const Matches& matching(Template&, Mat&);
 486
 487 private:
 488     ChamferMatcher(const ChamferMatcher&);
 489     ChamferMatcher& operator=(const ChamferMatcher&);
 490     void addMatch(float cost, Point offset, const Template* tpl);
 491
 492
 493 };
 494
 495
 496 ///////////////////// implementation ///////////////////////////
 497
 498 ChamferMatcher::SlidingWindowImageIterator::SlidingWindowImageIterator( int width,
 499                                                                         int height,
 500                                                                         int x_step = 3,
 501                                                                         int y_step = 3,
 502                                                                         int _scales = 5,
 503                                                                         float min_scale = 0.6,
 504                                                                         float max_scale = 1.6) :
 505
 506                                                                             width_(width),
 507                                                                             height_(height),
 508                                                                             x_step_(x_step),
 509                                                                             y_step_(y_step),
 510                                                                             scales_(_scales),
 511                                                                             min_scale_(min_scale),
 512                                                                             max_scale_(max_scale)
 513 {
 514     x_ = 0;
 515     y_ = 0;
 516     scale_cnt_ = 0;
 517     scale_ = min_scale_;
 518     has_next_ = true;
 519     scale_step_ = (max_scale_-min_scale_)/scales_;
 520 }
 521
 522 location_scale_t ChamferMatcher::SlidingWindowImageIterator::next()
 523 {
 524     location_scale_t next_val = std::make_pair(Point(x_,y_),scale_);
 525
 526     x_ += x_step_;
 527
 528     if (x_ >= width_) {
 529         x_ = 0;
 530         y_ += y_step_;
 531
 532         if (y_ >= height_) {
 533             y_ = 0;
 534             scale_ += scale_step_;
 535             scale_cnt_++;
 536
 537             if (scale_cnt_ == scales_) {
 538                 has_next_ = false;
 539                 scale_cnt_ = 0;
 540                 scale_ = min_scale_;
 541             }
 542         }
 543     }
 544
 545     return next_val;
 546 }
 547
 548
 549
 550 ChamferMatcher::ImageIterator* ChamferMatcher::SlidingWindowImageRange::iterator() const
 551 {
 552     return new SlidingWindowImageIterator(width_, height_, x_step_, y_step_, scales_, min_scale_, max_scale_);
 553 }
 554
 555
 556
 557 ChamferMatcher::LocationImageIterator::LocationImageIterator(const std::vector<Point>& locations,
 558                                                                 int _scales = 5,
 559                                                                 float min_scale = 0.6,
 560                                                                 float max_scale = 1.6) :
 561                                                                     locations_(locations),
 562                                                                     scales_(_scales),
 563                                                                     min_scale_(min_scale),
 564                                                                     max_scale_(max_scale)
 565 {
 566     iter_ = 0;
 567     scale_cnt_ = 0;
 568     scale_ = min_scale_;
 569     has_next_ = (locations_.size()==0 ? false : true);
 570     scale_step_ = (max_scale_-min_scale_)/scales_;
 571 }
 572
 573 location_scale_t ChamferMatcher::LocationImageIterator:: next()
 574 {
 575     location_scale_t next_val = std::make_pair(locations_[iter_],scale_);
 576
 577     iter_ ++;
 578     if (iter_==locations_.size()) {
 579         iter_ = 0;
 580         scale_ += scale_step_;
 581         scale_cnt_++;
 582
 583         if (scale_cnt_ == scales_) {
 584             has_next_ = false;
 585             scale_cnt_ = 0;
 586             scale_ = min_scale_;
 587         }
 588     }
 589
 590     return next_val;
 591 }
 592
 593
 594 location_scale_t ChamferMatcher::LocationScaleImageIterator::next()
 595 {
 596     location_scale_t next_val = std::make_pair(locations_[iter_],scales_[iter_]);
 597
 598     iter_ ++;
 599     if (iter_==locations_.size()) {
 600         iter_ = 0;
 601
 602         has_next_ = false;
 603     }
 604
 605     return next_val;
 606 }
 607
 608
 609
 610 bool ChamferMatcher::Matching::findFirstContourPoint(Mat& templ_img, coordinate_t& p)
 611 {
 612     for (int y=0;y<templ_img.rows;++y) {
 613         for (int x=0;x<templ_img.cols;++x) {
 614             if (templ_img.at<uchar>(y,x)!=0) {
 615                 p.first = x;
 616                 p.second = y;
 617                 return true;
 618             }
 619         }
 620     }
 621     return false;
 622 }
 623
 624
 625
 626 void ChamferMatcher::Matching::followContour(Mat& templ_img, template_coords_t& coords, int direction = -1)
 627 {
 628     const int dir[][2] = { {-1,-1}, {-1,0}, {-1,1}, {0,1}, {1,1}, {1,0}, {1,-1}, {0,-1} };
 629     coordinate_t next;
 630     unsigned char ptr;
 631
 632     CV_Assert (direction==-1 || !coords.empty());
 633
 634     coordinate_t crt = coords.back();
 635
 636     // mark the current pixel as visited
 637     templ_img.at<uchar>(crt.second,crt.first) = 0;
 638     if (direction==-1) {
 639         for (int j = 0; j<7; ++j) {
 640             next.first = crt.first + dir[j][1];
 641             next.second = crt.second + dir[j][0];
 642             if (next.first >= 0 && next.first < templ_img.cols &&
 643                 next.second >= 0 && next.second < templ_img.rows){
 644                 ptr = templ_img.at<uchar>(next.second, next.first);
 645                 if (ptr!=0) {
 646                     coords.push_back(next);
 647                     followContour(templ_img, coords,j);
 648                     // try to continue contour in the other direction
 649                     reverse(coords.begin(), coords.end());
 650                     followContour(templ_img, coords, (j+4)%8);
 651                     break;
 652                 }
 653             }
 654         }
 655     }
 656     else {
 657         int k = direction;
 658         int k_cost = 3;
 659         next.first = crt.first + dir[k][1];
 660         next.second = crt.second + dir[k][0];
 661         if (next.first >= 0 && next.first < templ_img.cols &&
 662                 next.second >= 0 && next.second < templ_img.rows){
 663             ptr = templ_img.at<uchar>(next.second, next.first);
 664             if (ptr!=0) {
 665                 k_cost = std::abs(dir[k][1]) + std::abs(dir[k][0]);
 666             }
 667             int p = k;
 668             int n = k;
 669
 670             for (int j = 0 ;j<3; ++j) {
 671                 p = (p + 7) % 8;
 672                 n = (n + 1) % 8;
 673                 next.first = crt.first + dir[p][1];
 674                 next.second = crt.second + dir[p][0];
 675                 if (next.first >= 0 && next.first < templ_img.cols &&
 676                     next.second >= 0 && next.second < templ_img.rows){
 677                     ptr = templ_img.at<uchar>(next.second, next.first);
 678                     if (ptr!=0) {
 679                         int p_cost = std::abs(dir[p][1]) + std::abs(dir[p][0]);
 680                         if (p_cost<k_cost) {
 681                             k_cost = p_cost;
 682                             k = p;
 683                         }
 684                     }
 685                     next.first = crt.first + dir[n][1];
 686                     next.second = crt.second + dir[n][0];
 687                     if (next.first >= 0 && next.first < templ_img.cols &&
 688                     next.second >= 0 && next.second < templ_img.rows){
 689                         ptr = templ_img.at<uchar>(next.second, next.first);
 690                         if (ptr!=0) {
 691                             int n_cost = std::abs(dir[n][1]) + std::abs(dir[n][0]);
 692                             if (n_cost<k_cost) {
 693                                 k_cost = n_cost;
 694                                 k = n;
 695                             }
 696                         }
 697                     }
 698                 }
 699             }
 700
 701             if (k_cost!=3) {
 702                 next.first = crt.first + dir[k][1];
 703                 next.second = crt.second + dir[k][0];
 704                 if (next.first >= 0 && next.first < templ_img.cols &&
 705                     next.second >= 0 && next.second < templ_img.rows) {
 706                     coords.push_back(next);
 707                     followContour(templ_img, coords, k);
 708                 }
 709             }
 710         }
 711     }
 712 }
 713
 714
 715 bool ChamferMatcher::Matching::findContour(Mat& templ_img, template_coords_t& coords)
 716 {
 717     coordinate_t start_point;
 718
 719     bool found = findFirstContourPoint(templ_img,start_point);
 720     if (found) {
 721         coords.push_back(start_point);
 722         followContour(templ_img, coords);
 723         return true;
 724     }
 725
 726     return false;
 727 }
 728
 729
 730 float ChamferMatcher::Matching::getAngle(coordinate_t a, coordinate_t b, int& dx, int& dy)
 731 {
 732     dx = b.first-a.first;
 733     dy = -(b.second-a.second);  // in image coordinated Y axis points downward
 734         float angle = atan2((float)dy,(float)dx);
 735
 736     if (angle<0) {
 737                 angle+=(float)CV_PI;
 738     }
 739
 740     return angle;
 741 }
 742
 743
 744
 745 void ChamferMatcher::Matching::findContourOrientations(const template_coords_t& coords, template_orientations_t& orientations)
 746 {
 747     const int M = 5;
 748     int coords_size = (int)coords.size();
 749
 750     std::vector<float> angles(2*M);
 751         orientations.insert(orientations.begin(), coords_size, float(-3*CV_PI)); // mark as invalid in the beginning
 752
 753     if (coords_size<2*M+1) {  // if contour not long enough to estimate orientations, abort
 754         return;
 755     }
 756
 757     for (int i=M;i<coords_size-M;++i) {
 758         coordinate_t crt = coords[i];
 759         coordinate_t other;
 760         int k = 0;
 761         int dx, dy;
 762         // compute previous M angles
 763         for (int j=M;j>0;--j) {
 764             other = coords[i-j];
 765             angles[k++] = getAngle(other,crt, dx, dy);
 766         }
 767         // compute next M angles
 768         for (int j=1;j<=M;++j) {
 769             other = coords[i+j];
 770             angles[k++] = getAngle(crt, other, dx, dy);
 771         }
 772
 773         // get the middle two angles
 774         std::nth_element(angles.begin(), angles.begin()+M-1,  angles.end());
 775         std::nth_element(angles.begin()+M-1, angles.begin()+M,  angles.end());
 776         //        sort(angles.begin(), angles.end());
 777
 778         // average them to compute tangent
 779         orientations[i] = (angles[M-1]+angles[M])/2;
 780     }
 781 }
 782
 783 //////////////////////// Template /////////////////////////////////////
 784
 785 ChamferMatcher::Template::Template(Mat& edge_image, float scale_) : addr_width(-1), scale(scale_)
 786 {
 787     template_coords_t local_coords;
 788     template_orientations_t local_orientations;
 789
 790     while (ChamferMatcher::Matching::findContour(edge_image, local_coords)) {
 791         ChamferMatcher::Matching::findContourOrientations(local_coords, local_orientations);
 792
 793         coords.insert(coords.end(), local_coords.begin(), local_coords.end());
 794         orientations.insert(orientations.end(), local_orientations.begin(), local_orientations.end());
 795         local_coords.clear();
 796         local_orientations.clear();
 797     }
 798
 799
 800     size = edge_image.size();
 801     Point min, max;
 802     min.x = size.width;
 803     min.y = size.height;
 804     max.x = 0;
 805     max.y = 0;
 806
 807     center = Point(0,0);
 808     for (size_t i=0;i<coords.size();++i) {
 809         center.x += coords[i].first;
 810         center.y += coords[i].second;
 811
 812         if (min.x>coords[i].first) min.x = coords[i].first;
 813         if (min.y>coords[i].second) min.y = coords[i].second;
 814         if (max.x<coords[i].first) max.x = coords[i].first;
 815         if (max.y<coords[i].second) max.y = coords[i].second;
 816     }
 817
 818     size.width = max.x - min.x;
 819     size.height = max.y - min.y;
 820     int coords_size = (int)coords.size();
 821
 822     center.x /= MAX(coords_size, 1);
 823     center.y /= MAX(coords_size, 1);
 824
 825     for (int i=0;i<coords_size;++i) {
 826         coords[i].first -= center.x;
 827         coords[i].second -= center.y;
 828     }
 829 }
 830
 831
 832 std::vector<int>& ChamferMatcher::Template::getTemplateAddresses(int width)
 833 {
 834     if (addr_width!=width) {
 835         addr.resize(coords.size());
 836         addr_width = width;
 837
 838         for (size_t i=0; i<coords.size();++i) {
 839             addr[i] = coords[i].second*width+coords[i].first;
 840         }
 841     }
 842     return addr;
 843 }
 844
 845
 846 /**
 847  * Resizes a template
 848  *
 849  * @param scale Scale to be resized to
 850  */
 851 ChamferMatcher::Template* ChamferMatcher::Template::rescale(float new_scale)
 852 {
 853
 854     if (fabs(scale-new_scale)<1e-6) return this;
 855
 856     for (size_t i=0;i<scaled_templates.size();++i) {
 857         if (fabs(scaled_templates[i]->scale-new_scale)<1e-6) {
 858             return scaled_templates[i];
 859         }
 860     }
 861
 862     float scale_factor = new_scale/scale;
 863
 864     Template* tpl = new Template();
 865     tpl->scale = new_scale;
 866
 867     tpl->center.x = int(center.x*scale_factor+0.5);
 868     tpl->center.y = int(center.y*scale_factor+0.5);
 869
 870     tpl->size.width = int(size.width*scale_factor+0.5);
 871     tpl->size.height = int(size.height*scale_factor+0.5);
 872
 873     tpl->coords.resize(coords.size());
 874     tpl->orientations.resize(orientations.size());
 875     for (size_t i=0;i<coords.size();++i) {
 876         tpl->coords[i].first = int(coords[i].first*scale_factor+0.5);
 877         tpl->coords[i].second = int(coords[i].second*scale_factor+0.5);
 878         tpl->orientations[i] = orientations[i];
 879     }
 880     scaled_templates.push_back(tpl);
 881
 882     return tpl;
 883
 884 }
 885
 886
 887
 888 void ChamferMatcher::Template::show() const
 889 {
 890     int pad = 50;
 891     //Attention size is not correct
 892     Mat templ_color (Size(size.width+(pad*2), size.height+(pad*2)), CV_8UC3);
 893     templ_color.setTo(0);
 894
 895     for (size_t i=0;i<coords.size();++i) {
 896
 897         int x = center.x+coords[i].first+pad;
 898         int y = center.y+coords[i].second+pad;
 899         templ_color.at<Vec3b>(y,x)[1]=255;
 900         //CV_PIXEL(unsigned char, templ_color,x,y)[1] = 255;
 901
 902         if (i%3==0) {
 903                         if (orientations[i] < -CV_PI) {
 904                 continue;
 905             }
 906             Point p1;
 907             p1.x = x;
 908             p1.y = y;
 909             Point p2;
 910             p2.x = x + pad*(int)(sin(orientations[i])*100)/100;
 911             p2.y = y + pad*(int)(cos(orientations[i])*100)/100;
 912
 913             line(templ_color, p1,p2, Scalar(255,0,0));
 914         }
 915     }
 916
 917     circle(templ_color,Point(center.x + pad, center.y + pad),1,Scalar(0,255,0));
 918
 919 #ifdef HAVE_OPENCV_HIGHGUI
 920     namedWindow("templ",1);
 921     imshow("templ",templ_color);
 922     waitKey();
 923 #else
 924     CV_Error(Error::StsNotImplemented, "OpenCV has been compiled without GUI support");
 925 #endif
 926
 927     templ_color.release();
 928 }
 929
 930
 931 //////////////////////// Matching /////////////////////////////////////
 932
 933
 934 void ChamferMatcher::Matching::addTemplateFromImage(Mat& templ, float scale)
 935 {
 936     Template* cmt = new Template(templ, scale);
 937     templates.clear();
 938     templates.push_back(cmt);
 939     cmt->show();
 940 }
 941
 942 void ChamferMatcher::Matching::addTemplate(Template& template_){
 943     templates.clear();
 944     templates.push_back(&template_);
 945 }
 946 /**
 947  * Alternative version of computeDistanceTransform, will probably be used to compute distance
 948  * transform annotated with edge orientation.
 949  */
 950 void ChamferMatcher::Matching::computeDistanceTransform(Mat& edges_img, Mat& dist_img, Mat& annotate_img, float truncate_dt, float a = 1.0, float b = 1.5)
 951 {
 952     int d[][2] = { {-1,-1}, { 0,-1}, { 1,-1},
 953             {-1,0},          { 1,0},
 954             {-1,1}, { 0,1}, { 1,1} };
 955
 956
 957     Size s = edges_img.size();
 958     int w = s.width;
 959     int h = s.height;
 960     // set distance to the edge pixels to 0 and put them in the queue
 961     std::queue<std::pair<int,int> > q;
 962
 963     for (int y=0;y<h;++y) {
 964         for (int x=0;x<w;++x) {
 965             // initialize
 966             if (&annotate_img!=NULL) {
 967                 annotate_img.at<Vec2i>(y,x)[0]=x;
 968                 annotate_img.at<Vec2i>(y,x)[1]=y;
 969             }
 970
 971             uchar edge_val = edges_img.at<uchar>(y,x);
 972             if( (edge_val!=0) ) {
 973                 q.push(std::make_pair(x,y));
 974                 dist_img.at<float>(y,x)= 0;
 975             }
 976             else {
 977                 dist_img.at<float>(y,x)=-1;
 978             }
 979         }
 980     }
 981
 982     // breadth first computation of distance transform
 983     std::pair<int,int> crt;
 984     while (!q.empty()) {
 985         crt = q.front();
 986         q.pop();
 987
 988         int x = crt.first;
 989         int y = crt.second;
 990
 991         float dist_orig = dist_img.at<float>(y,x);
 992         float dist;
 993
 994         for (size_t i=0;i<sizeof(d)/sizeof(d[0]);++i) {
 995             int nx = x + d[i][0];
 996             int ny = y + d[i][1];
 997
 998             if (nx<0 || ny<0 || nx>=w || ny>=h) continue;
 999
1000             if (std::abs(d[i][0]+d[i][1])==1) {
1001                 dist = (dist_orig)+a;
1002             }
1003             else {
1004                 dist = (dist_orig)+b;
1005             }
1006
1007             float dt = dist_img.at<float>(ny,nx);
1008
1009             if (dt==-1 || dt>dist) {
1010                 dist_img.at<float>(ny,nx) = dist;
1011                 q.push(std::make_pair(nx,ny));
1012
1013                 if (&annotate_img!=NULL) {
1014                     annotate_img.at<Vec2i>(ny,nx)[0]=annotate_img.at<Vec2i>(y,x)[0];
1015                     annotate_img.at<Vec2i>(ny,nx)[1]=annotate_img.at<Vec2i>(y,x)[1];
1016                 }
1017             }
1018         }
1019     }
1020     // truncate dt
1021
1022     if (truncate_dt>0) {
1023         Mat dist_img_thr = dist_img.clone();
1024         threshold(dist_img, dist_img_thr, truncate_dt,0.0 ,THRESH_TRUNC);
1025         dist_img_thr.copyTo(dist_img);
1026     }
1027 }
1028
1029
1030 void ChamferMatcher::Matching::computeEdgeOrientations(Mat& edge_img, Mat& orientation_img)
1031 {
1032     Mat contour_img(edge_img.size(), CV_8UC1);
1033
1034         orientation_img.setTo(3*(-CV_PI));
1035     template_coords_t coords;
1036     template_orientations_t orientations;
1037
1038     while (ChamferMatcher::Matching::findContour(edge_img, coords)) {
1039
1040         ChamferMatcher::Matching::findContourOrientations(coords, orientations);
1041
1042         // set orientation pixel in orientation image
1043         for (size_t i = 0; i<coords.size();++i) {
1044             int x = coords[i].first;
1045             int y = coords[i].second;
1046                         //            if (orientations[i]>-CV_PI)
1047             //    {
1048             //CV_PIXEL(unsigned char, contour_img, x, y)[0] = 255;
1049             contour_img.at<uchar>(y,x)=255;
1050             //    }
1051             //CV_PIXEL(float, orientation_img, x, y)[0] = orientations[i];
1052             orientation_img.at<float>(y,x)=orientations[i];
1053         }
1054
1055
1056         coords.clear();
1057         orientations.clear();
1058     }
1059
1060     //imwrite("contours.pgm", contour_img);
1061 }
1062
1063
1064 void ChamferMatcher::Matching::fillNonContourOrientations(Mat& annotated_img, Mat& orientation_img)
1065 {
1066     int cols = annotated_img.cols;
1067     int rows = annotated_img.rows;
1068
1069     CV_Assert(orientation_img.cols==cols && orientation_img.rows==rows);
1070
1071     for (int y=0;y<rows;++y) {
1072         for (int x=0;x<cols;++x) {
1073             int xorig = annotated_img.at<Vec2i>(y,x)[0];
1074             int yorig = annotated_img.at<Vec2i>(y,x)[1];
1075
1076             if (x!=xorig || y!=yorig) {
1077                 //orientation_img.at<float>(yorig,xorig)=orientation_img.at<float>(y,x);
1078                 orientation_img.at<float>(y,x)=orientation_img.at<float>(yorig,xorig);
1079             }
1080         }
1081     }
1082 }
1083
1084
1085 ChamferMatcher::Match* ChamferMatcher::Matching::localChamferDistance(Point offset, Mat& dist_img, Mat& orientation_img,
1086         ChamferMatcher::Template* tpl, float alpha)
1087 {
1088     int x = offset.x;
1089     int y = offset.y;
1090
1091     float beta = 1-alpha;
1092
1093     std::vector<int>& addr = tpl->getTemplateAddresses(dist_img.cols);
1094
1095     float* ptr = dist_img.ptr<float>(y)+x;
1096
1097
1098     float sum_distance = 0;
1099     for (size_t i=0; i<addr.size();++i) {
1100         if(addr[i] < (dist_img.cols*dist_img.rows) - (offset.y*dist_img.cols + offset.x)){
1101             sum_distance += *(ptr+addr[i]);
1102         }
1103     }
1104
1105     float cost = (sum_distance/truncate_)/addr.size();
1106
1107
1108     if (&orientation_img!=NULL) {
1109         float* optr = orientation_img.ptr<float>(y)+x;
1110         float sum_orientation = 0;
1111         int cnt_orientation = 0;
1112
1113         for (size_t i=0;i<addr.size();++i) {
1114
1115             if(addr[i] < (orientation_img.cols*orientation_img.rows) - (offset.y*orientation_img.cols + offset.x)){
1116                                 if (tpl->orientations[i]>=-CV_PI && (*(optr+addr[i]))>=-CV_PI) {
1117                     sum_orientation += orientation_diff(tpl->orientations[i], (*(optr+addr[i])));
1118                     cnt_orientation++;
1119                 }
1120             }
1121         }
1122
1123         if (cnt_orientation>0) {
1124                         cost = (float)(beta*cost+alpha*(sum_orientation/(2*CV_PI))/cnt_orientation);
1125         }
1126
1127     }
1128
1129     if(cost > 0){
1130         ChamferMatcher::Match* istance = new ChamferMatcher::Match();
1131         istance->cost = cost;
1132         istance->offset = offset;
1133         istance->tpl = tpl;
1134
1135         return istance;
1136     }
1137
1138     return NULL;
1139 }
1140
1141
1142 ChamferMatcher::Matches* ChamferMatcher::Matching::matchTemplates(Mat& dist_img, Mat& orientation_img, const ImageRange& range, float _orientation_weight)
1143 {
1144
1145     ChamferMatcher::Matches* pmatches(new Matches());
1146     // try each template
1147     for(size_t i = 0; i < templates.size(); i++) {
1148         ImageIterator* it = range.iterator();
1149         while (it->hasNext()) {
1150             location_scale_t crt = it->next();
1151
1152             Point loc = crt.first;
1153             float scale = crt.second;
1154             Template* tpl = templates[i]->rescale(scale);
1155
1156
1157             if (loc.x-tpl->center.x<0 || loc.x+tpl->size.width/2>=dist_img.cols) continue;
1158             if (loc.y-tpl->center.y<0 || loc.y+tpl->size.height/2>=dist_img.rows) continue;
1159
1160             ChamferMatcher::Match* is = localChamferDistance(loc, dist_img, orientation_img, tpl, _orientation_weight);
1161             if(is)
1162             {
1163                 pmatches->push_back(*is);
1164                 delete is;
1165             }
1166         }
1167
1168         delete it;
1169     }
1170     return pmatches;
1171 }
1172
1173
1174
1175 /**
1176  * Run matching using an edge image.
1177  * @param edge_img Edge image
1178  * @return a match object
1179  */
1180 ChamferMatcher::Matches* ChamferMatcher::Matching::matchEdgeImage(Mat& edge_img, const ImageRange& range,
1181                     float _orientation_weight, int /*max_matches*/, float /*min_match_distance*/)
1182 {
1183     CV_Assert(edge_img.channels()==1);
1184
1185     Mat dist_img;
1186     Mat annotated_img;
1187     Mat orientation_img;
1188
1189     annotated_img.create(edge_img.size(), CV_32SC2);
1190     dist_img.create(edge_img.size(),CV_32FC1);
1191     dist_img.setTo(0);
1192     // Computing distance transform
1193     computeDistanceTransform(edge_img,dist_img, annotated_img, truncate_);
1194
1195
1196     //orientation_img = NULL;
1197     if (use_orientation_) {
1198         orientation_img.create(edge_img.size(), CV_32FC1);
1199         orientation_img.setTo(0);
1200         Mat edge_clone = edge_img.clone();
1201         computeEdgeOrientations(edge_clone, orientation_img );
1202         edge_clone.release();
1203         fillNonContourOrientations(annotated_img, orientation_img);
1204     }
1205
1206
1207     // Template matching
1208     ChamferMatcher::Matches* pmatches = matchTemplates(    dist_img,
1209                                                         orientation_img,
1210                                                         range,
1211                                                         _orientation_weight);
1212
1213
1214     if (use_orientation_) {
1215         orientation_img.release();
1216     }
1217     dist_img.release();
1218     annotated_img.release();
1219
1220     return pmatches;
1221 }
1222
1223
1224 void ChamferMatcher::addMatch(float cost, Point offset, const Template* tpl)
1225 {
1226     bool new_match = true;
1227     for (int i=0; i<count; ++i) {
1228         if (std::abs(matches[i].offset.x-offset.x)+std::abs(matches[i].offset.y-offset.y)<min_match_distance_) {
1229             // too close, not a new match
1230             new_match = false;
1231             // if better cost, replace existing match
1232             if (cost<matches[i].cost) {
1233                 matches[i].cost = cost;
1234                 matches[i].offset = offset;
1235                 matches[i].tpl = tpl;
1236             }
1237             // re-bubble to keep ordered
1238             int k = i;
1239             while (k>0) {
1240                 if (matches[k-1].cost>matches[k].cost) {
1241                     std::swap(matches[k-1],matches[k]);
1242                 }
1243                 k--;
1244             }
1245
1246             break;
1247         }
1248     }
1249
1250     if (new_match) {
1251         // if we don't have enough matches yet, add it to the array
1252         if (count<max_matches_) {
1253             matches[count].cost = cost;
1254             matches[count].offset = offset;
1255             matches[count].tpl = tpl;
1256             count++;
1257         }
1258         // otherwise find the right position to insert it
1259         else {
1260             // if higher cost than the worst current match, just ignore it
1261             if (matches[count-1].cost<cost) {
1262                 return;
1263             }
1264
1265             int j = 0;
1266             // skip all matches better than current one
1267             while (matches[j].cost<cost) j++;
1268
1269             // shift matches one position
1270             int k = count-2;
1271             while (k>=j) {
1272                 matches[k+1] = matches[k];
1273                 k--;
1274             }
1275
1276             matches[j].cost = cost;
1277             matches[j].offset = offset;
1278             matches[j].tpl = tpl;
1279         }
1280     }
1281 }
1282
1283 void ChamferMatcher::showMatch(Mat& img, int index)
1284 {
1285     if (index>=count) {
1286         std::cout << "Index too big.\n" << std::endl;
1287     }
1288
1289     CV_Assert(img.channels()==3);
1290
1291     Match match = matches[index];
1292
1293     const template_coords_t& templ_coords = match.tpl->coords;
1294     int x, y;
1295     for (size_t i=0;i<templ_coords.size();++i) {
1296         x = match.offset.x + templ_coords[i].first;
1297         y = match.offset.y + templ_coords[i].second;
1298
1299         if ( x > img.cols-1 || x < 0 || y > img.rows-1 || y < 0) continue;
1300         img.at<Vec3b>(y,x)[0]=0;
1301         img.at<Vec3b>(y,x)[2]=0;
1302         img.at<Vec3b>(y,x)[1]=255;
1303     }
1304 }
1305
1306 void ChamferMatcher::showMatch(Mat& img, Match match)
1307 {
1308     CV_Assert(img.channels()==3);
1309
1310     const template_coords_t& templ_coords = match.tpl->coords;
1311     for (size_t i=0;i<templ_coords.size();++i) {
1312         int x = match.offset.x + templ_coords[i].first;
1313         int y = match.offset.y + templ_coords[i].second;
1314         if ( x > img.cols-1 || x < 0 || y > img.rows-1 || y < 0) continue;
1315         img.at<Vec3b>(y,x)[0]=0;
1316         img.at<Vec3b>(y,x)[2]=0;
1317         img.at<Vec3b>(y,x)[1]=255;
1318     }
1319     match.tpl->show();
1320 }
1321
1322 const ChamferMatcher::Matches& ChamferMatcher::matching(Template& tpl, Mat& image_){
1323     chamfer_->addTemplate(tpl);
1324
1325     matches.clear();
1326     matches.resize(max_matches_);
1327     count = 0;
1328
1329
1330     Matches* matches_ = chamfer_->matchEdgeImage(    image_,
1331                                                     ChamferMatcher::
1332                                                         SlidingWindowImageRange(image_.cols,
1333                                                                                 image_.rows,
1334                                                                                 pad_x,
1335                                                                                 pad_y,
1336                                                                                 scales,
1337                                                                                 minScale,
1338                                                                                 maxScale),
1339                                                     orientation_weight,
1340                                                     max_matches_,
1341                                                     min_match_distance_);
1342
1343
1344
1345     for(int i = 0; i < (int)matches_->size(); i++){
1346         addMatch(matches_->at(i).cost, matches_->at(i).offset, matches_->at(i).tpl);
1347     }
1348
1349     matches_->clear();
1350     delete matches_;
1351     matches_ = NULL;
1352
1353     matches.resize(count);
1354
1355
1356     return matches;
1357
1358 }
1359
1360
1361 int chamerMatching( Mat& img, Mat& templ,
1362                     std::vector<std::vector<Point> >& results, std::vector<float>& costs,
1363                     double templScale, int maxMatches, double minMatchDistance, int padX,
1364                     int padY, int scales, double minScale, double maxScale,
1365                     double orientationWeight, double truncate )
1366 {
1367     CV_Assert(img.type() == CV_8UC1 && templ.type() == CV_8UC1);
1368
1369     ChamferMatcher matcher_(maxMatches, (float)minMatchDistance, padX, padY, scales,
1370                             (float)minScale, (float)maxScale,
1371                             (float)orientationWeight, (float)truncate);
1372
1373     ChamferMatcher::Template template_(templ, (float)templScale);
1374     ChamferMatcher::Matches match_instances = matcher_.matching(template_, img);
1375
1376     size_t i, nmatches = match_instances.size();
1377
1378     results.resize(nmatches);
1379     costs.resize(nmatches);
1380
1381     int bestIdx = -1;
1382     double minCost = DBL_MAX;
1383
1384     for( i = 0; i < nmatches; i++ )
1385     {
1386         const ChamferMatcher::Match& match = match_instances[i];
1387         double cval = match.cost;
1388         if( cval < minCost)
1389         {
1390             minCost = cval;
1391             bestIdx = (int)i;
1392         }
1393         costs[i] = (float)cval;
1394
1395         const template_coords_t& templ_coords = match.tpl->coords;
1396         std::vector<Point>& templPoints = results[i];
1397         size_t j, npoints = templ_coords.size();
1398         templPoints.resize(npoints);
1399
1400         for (j = 0; j < npoints; j++ )
1401         {
1402             int x = match.offset.x + templ_coords[j].first;
1403             int y = match.offset.y + templ_coords[j].second;
1404             templPoints[j] = Point(x,y);
1405         }
1406     }
1407
1408     return bestIdx;
1409 }
1410
1411 }