1 /*M///////////////////////////////////////////////////////////////////////////////////////
2 //
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
4 //
5 // By downloading, copying, installing or using the software you agree to this license.
6 // If you do not agree to this license, do not download, install,
7 // copy or use the software.
8 //
9 //
10 // License Agreement
11 // For Open Source Computer Vision Library
12 //
13 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
14 // Copyright (C) 2009, Willow Garage Inc., all rights reserved.
15 // Third party copyrights are property of their respective owners.
16 //
17 // Redistribution and use in source and binary forms, with or without modification,
18 // are permitted provided that the following conditions are met:
19 //
20 // * Redistribution's of source code must retain the above copyright notice,
21 // this list of conditions and the following disclaimer.
22 //
23 // * Redistribution's in binary form must reproduce the above copyright notice,
24 // this list of conditions and the following disclaimer in the documentation
25 // and/or other materials provided with the distribution.
26 //
27 // * The name of the copyright holders may not be used to endorse or promote products
28 // derived from this software without specific prior written permission.
29 //
30 // This software is provided by the copyright holders and contributors "as is" and
31 // any express or implied warranties, including, but not limited to, the implied
32 // warranties of merchantability and fitness for a particular purpose are disclaimed.
33 // In no event shall the Intel Corporation or contributors be liable for any direct,
34 // indirect, incidental, special, exemplary, or consequential damages
35 // (including, but not limited to, procurement of substitute goods or services;
36 // loss of use, data, or profits; or business interruption) however caused
37 // and on any theory of liability, whether in contract, strict liability,
38 // or tort (including negligence or otherwise) arising in any way out of
39 // the use of this software, even if advised of the possibility of such damage.
40 //
41 //M*/
42
43 /*
44 * Implementation of the paper Shape Matching and Object Recognition Using Shape Contexts
45 * Belongie et al., 2002 by Juan Manuel Perez for GSoC 2013.
46 */
47
48 #include "precomp.hpp"
49 #include "opencv2/core.hpp"
50 #include "scd_def.hpp"
51 #include <limits>
52
53 namespace cv
54 {
55 class ShapeContextDistanceExtractorImpl : public ShapeContextDistanceExtractor
56 {
57 public:
58 /* Constructors */
ShapeContextDistanceExtractorImpl(int _nAngularBins,int _nRadialBins,float _innerRadius,float _outerRadius,int _iterations,const Ptr<HistogramCostExtractor> & _comparer,const Ptr<ShapeTransformer> & _transformer)59 ShapeContextDistanceExtractorImpl(int _nAngularBins, int _nRadialBins, float _innerRadius, float _outerRadius, int _iterations,
60 const Ptr<HistogramCostExtractor> &_comparer, const Ptr<ShapeTransformer> &_transformer)
61 {
62 nAngularBins=_nAngularBins;
63 nRadialBins=_nRadialBins;
64 innerRadius=_innerRadius;
65 outerRadius=_outerRadius;
66 rotationInvariant=false;
67 comparer=_comparer;
68 iterations=_iterations;
69 transformer=_transformer;
70 bendingEnergyWeight=0.3f;
71 imageAppearanceWeight=0.0f;
72 shapeContextWeight=1.0f;
73 sigma=10.0f;
74 name_ = "ShapeDistanceExtractor.SCD";
75 costFlag = 0;
76 }
77
78 /* Destructor */
~ShapeContextDistanceExtractorImpl()79 ~ShapeContextDistanceExtractorImpl() CV_OVERRIDE
80 {
81 }
82
83 //! the main operator
84 virtual float computeDistance(InputArray contour1, InputArray contour2) CV_OVERRIDE;
85
86 //! Setters/Getters
setAngularBins(int _nAngularBins)87 virtual void setAngularBins(int _nAngularBins) CV_OVERRIDE { CV_Assert(_nAngularBins>0); nAngularBins=_nAngularBins; }
getAngularBins() const88 virtual int getAngularBins() const CV_OVERRIDE { return nAngularBins; }
89
setRadialBins(int _nRadialBins)90 virtual void setRadialBins(int _nRadialBins) CV_OVERRIDE { CV_Assert(_nRadialBins>0); nRadialBins=_nRadialBins; }
getRadialBins() const91 virtual int getRadialBins() const CV_OVERRIDE { return nRadialBins; }
92
setInnerRadius(float _innerRadius)93 virtual void setInnerRadius(float _innerRadius) CV_OVERRIDE { CV_Assert(_innerRadius>0); innerRadius=_innerRadius; }
getInnerRadius() const94 virtual float getInnerRadius() const CV_OVERRIDE { return innerRadius; }
95
setOuterRadius(float _outerRadius)96 virtual void setOuterRadius(float _outerRadius) CV_OVERRIDE { CV_Assert(_outerRadius>0); outerRadius=_outerRadius; }
getOuterRadius() const97 virtual float getOuterRadius() const CV_OVERRIDE { return outerRadius; }
98
setRotationInvariant(bool _rotationInvariant)99 virtual void setRotationInvariant(bool _rotationInvariant) CV_OVERRIDE { rotationInvariant=_rotationInvariant; }
getRotationInvariant() const100 virtual bool getRotationInvariant() const CV_OVERRIDE { return rotationInvariant; }
101
setCostExtractor(Ptr<HistogramCostExtractor> _comparer)102 virtual void setCostExtractor(Ptr<HistogramCostExtractor> _comparer) CV_OVERRIDE { comparer = _comparer; }
getCostExtractor() const103 virtual Ptr<HistogramCostExtractor> getCostExtractor() const CV_OVERRIDE { return comparer; }
104
setShapeContextWeight(float _shapeContextWeight)105 virtual void setShapeContextWeight(float _shapeContextWeight) CV_OVERRIDE { shapeContextWeight=_shapeContextWeight; }
getShapeContextWeight() const106 virtual float getShapeContextWeight() const CV_OVERRIDE { return shapeContextWeight; }
107
setImageAppearanceWeight(float _imageAppearanceWeight)108 virtual void setImageAppearanceWeight(float _imageAppearanceWeight) CV_OVERRIDE { imageAppearanceWeight=_imageAppearanceWeight; }
getImageAppearanceWeight() const109 virtual float getImageAppearanceWeight() const CV_OVERRIDE { return imageAppearanceWeight; }
110
setBendingEnergyWeight(float _bendingEnergyWeight)111 virtual void setBendingEnergyWeight(float _bendingEnergyWeight) CV_OVERRIDE { bendingEnergyWeight=_bendingEnergyWeight; }
getBendingEnergyWeight() const112 virtual float getBendingEnergyWeight() const CV_OVERRIDE { return bendingEnergyWeight; }
113
setStdDev(float _sigma)114 virtual void setStdDev(float _sigma) CV_OVERRIDE { sigma=_sigma; }
getStdDev() const115 virtual float getStdDev() const CV_OVERRIDE { return sigma; }
116
setImages(InputArray _image1,InputArray _image2)117 virtual void setImages(InputArray _image1, InputArray _image2) CV_OVERRIDE
118 {
119 Mat image1_=_image1.getMat(), image2_=_image2.getMat();
120 CV_Assert((image1_.depth()==0) && (image2_.depth()==0));
121 image1=image1_;
122 image2=image2_;
123 }
124
getImages(OutputArray _image1,OutputArray _image2) const125 virtual void getImages(OutputArray _image1, OutputArray _image2) const CV_OVERRIDE
126 {
127 CV_Assert((!image1.empty()) && (!image2.empty()));
128 image1.copyTo(_image1);
129 image2.copyTo(_image2);
130 }
131
setIterations(int _iterations)132 virtual void setIterations(int _iterations) CV_OVERRIDE {CV_Assert(_iterations>0); iterations=_iterations;}
getIterations() const133 virtual int getIterations() const CV_OVERRIDE {return iterations;}
134
setTransformAlgorithm(Ptr<ShapeTransformer> _transformer)135 virtual void setTransformAlgorithm(Ptr<ShapeTransformer> _transformer) CV_OVERRIDE {transformer=_transformer;}
getTransformAlgorithm() const136 virtual Ptr<ShapeTransformer> getTransformAlgorithm() const CV_OVERRIDE {return transformer;}
137
138 //! write/read
write(FileStorage & fs) const139 virtual void write(FileStorage& fs) const CV_OVERRIDE
140 {
141 writeFormat(fs);
142 fs << "name" << name_
143 << "nRads" << nRadialBins
144 << "nAngs" << nAngularBins
145 << "iters" << iterations
146 << "img_1" << image1
147 << "img_2" << image2
148 << "beWei" << bendingEnergyWeight
149 << "scWei" << shapeContextWeight
150 << "iaWei" << imageAppearanceWeight
151 << "costF" << costFlag
152 << "rotIn" << rotationInvariant
153 << "sigma" << sigma;
154 }
155
read(const FileNode & fn)156 virtual void read(const FileNode& fn) CV_OVERRIDE
157 {
158 CV_Assert( (String)fn["name"] == name_ );
159 nRadialBins = (int)fn["nRads"];
160 nAngularBins = (int)fn["nAngs"];
161 iterations = (int)fn["iters"];
162 bendingEnergyWeight = (float)fn["beWei"];
163 shapeContextWeight = (float)fn["scWei"];
164 imageAppearanceWeight = (float)fn["iaWei"];
165 costFlag = (int)fn["costF"];
166 sigma = (float)fn["sigma"];
167 }
168
169 protected:
170 int nAngularBins;
171 int nRadialBins;
172 float innerRadius;
173 float outerRadius;
174 bool rotationInvariant;
175 int costFlag;
176 int iterations;
177 Ptr<ShapeTransformer> transformer;
178 Ptr<HistogramCostExtractor> comparer;
179 Mat image1;
180 Mat image2;
181 float bendingEnergyWeight;
182 float imageAppearanceWeight;
183 float shapeContextWeight;
184 float sigma;
185 String name_;
186 };
187
computeDistance(InputArray contour1,InputArray contour2)188 float ShapeContextDistanceExtractorImpl::computeDistance(InputArray contour1, InputArray contour2)
189 {
190 CV_INSTRUMENT_REGION();
191
192 // Checking //
193 Mat sset1=contour1.getMat(), sset2=contour2.getMat(), set1, set2;
194 if (set1.type() != CV_32F)
195 sset1.convertTo(set1, CV_32F);
196 else
197 sset1.copyTo(set1);
198
199 if (set2.type() != CV_32F)
200 sset2.convertTo(set2, CV_32F);
201 else
202 sset2.copyTo(set2);
203
204 CV_Assert((set1.channels()==2) && (set1.cols>0));
205 CV_Assert((set2.channels()==2) && (set2.cols>0));
206
207 // Force vectors column-based
208 if (set1.dims > 1)
209 set1 = set1.reshape(2, 1);
210 if (set2.dims > 1)
211 set2 = set2.reshape(2, 1);
212
213 if (imageAppearanceWeight!=0)
214 {
215 CV_Assert((!image1.empty()) && (!image2.empty()));
216 }
217
218 // Initializing Extractor, Descriptor structures and Matcher //
219 SCD set1SCE(nAngularBins, nRadialBins, innerRadius, outerRadius, rotationInvariant);
220 Mat set1SCD;
221 SCD set2SCE(nAngularBins, nRadialBins, innerRadius, outerRadius, rotationInvariant);
222 Mat set2SCD;
223 SCDMatcher matcher;
224 std::vector<DMatch> matches;
225
226 // Distance components (The output is a linear combination of these 3) //
227 float sDistance=0, bEnergy=0, iAppearance=0;
228 float beta;
229
230 // Initializing some variables //
231 std::vector<int> inliers1, inliers2;
232
233 Ptr<ThinPlateSplineShapeTransformer> transDown = transformer.dynamicCast<ThinPlateSplineShapeTransformer>();
234
235 Mat warpedImage;
236 int ii, jj, pt;
237
238 for (ii=0; ii<iterations; ii++)
239 {
240 // Extract SCD descriptor in the set1 //
241 set1SCE.extractSCD(set1, set1SCD, inliers1);
242
243 // Extract SCD descriptor of the set2 (TARGET) //
244 set2SCE.extractSCD(set2, set2SCD, inliers2, set1SCE.getMeanDistance());
245
246 // regularization parameter with annealing rate annRate //
247 beta=set1SCE.getMeanDistance();
248 beta *= beta;
249
250 // match //
251 matcher.matchDescriptors(set1SCD, set2SCD, matches, comparer, inliers1, inliers2);
252
253 // apply TPS transform //
254 if ( !transDown.empty() )
255 transDown->setRegularizationParameter(beta);
256 transformer->estimateTransformation(set1, set2, matches);
257 bEnergy += transformer->applyTransformation(set1, set1);
258
259 // Image appearance //
260 if (imageAppearanceWeight!=0)
261 {
262 // Have to accumulate the transformation along all the iterations
263 if (ii==0)
264 {
265 if ( !transDown.empty() )
266 {
267 image2.copyTo(warpedImage);
268 }
269 else
270 {
271 image1.copyTo(warpedImage);
272 }
273 }
274 transformer->warpImage(warpedImage, warpedImage);
275 }
276 }
277
278 Mat gaussWindow, diffIm;
279 if (imageAppearanceWeight!=0)
280 {
281 // compute appearance cost
282 if ( !transDown.empty() )
283 {
284 resize(warpedImage, warpedImage, image1.size(), 0, 0, INTER_LINEAR_EXACT);
285 Mat temp=(warpedImage-image1);
286 multiply(temp, temp, diffIm);
287 }
288 else
289 {
290 resize(warpedImage, warpedImage, image2.size(), 0, 0, INTER_LINEAR_EXACT);
291 Mat temp=(warpedImage-image2);
292 multiply(temp, temp, diffIm);
293 }
294 gaussWindow = Mat::zeros(warpedImage.rows, warpedImage.cols, CV_32F);
295 for (pt=0; pt<sset1.cols; pt++)
296 {
297 Point2f p = sset1.at<Point2f>(0,pt);
298 for (ii=0; ii<diffIm.rows; ii++)
299 {
300 for (jj=0; jj<diffIm.cols; jj++)
301 {
302 float val = float(std::exp( -float( (p.x-jj)*(p.x-jj) + (p.y-ii)*(p.y-ii) )/(2*sigma*sigma) ) / (sigma*sigma*2*CV_PI));
303 gaussWindow.at<float>(ii,jj) += val;
304 }
305 }
306 }
307
308 Mat appIm(diffIm.rows, diffIm.cols, CV_32F);
309 for (ii=0; ii<diffIm.rows; ii++)
310 {
311 for (jj=0; jj<diffIm.cols; jj++)
312 {
313 float elema=float( diffIm.at<uchar>(ii,jj) )/255;
314 float elemb=gaussWindow.at<float>(ii,jj);
315 appIm.at<float>(ii,jj) = elema*elemb;
316 }
317 }
318 iAppearance = float(cv::sum(appIm)[0]/sset1.cols);
319 }
320 sDistance = matcher.getMatchingCost();
321
322 return (sDistance*shapeContextWeight+bEnergy*bendingEnergyWeight+iAppearance*imageAppearanceWeight);
323 }
324
createShapeContextDistanceExtractor(int nAngularBins,int nRadialBins,float innerRadius,float outerRadius,int iterations,const Ptr<HistogramCostExtractor> & comparer,const Ptr<ShapeTransformer> & transformer)325 Ptr <ShapeContextDistanceExtractor> createShapeContextDistanceExtractor(int nAngularBins, int nRadialBins, float innerRadius, float outerRadius, int iterations,
326 const Ptr<HistogramCostExtractor> &comparer, const Ptr<ShapeTransformer> &transformer)
327 {
328 return Ptr <ShapeContextDistanceExtractor> ( new ShapeContextDistanceExtractorImpl(nAngularBins, nRadialBins, innerRadius,
329 outerRadius, iterations, comparer, transformer) );
330 }
331
332 //! SCD
extractSCD(cv::Mat & contour,cv::Mat & descriptors,const std::vector<int> & queryInliers,const float _meanDistance)333 void SCD::extractSCD(cv::Mat &contour, cv::Mat &descriptors, const std::vector<int> &queryInliers, const float _meanDistance)
334 {
335 cv::Mat contourMat = contour;
336 cv::Mat disMatrix = cv::Mat::zeros(contourMat.cols, contourMat.cols, CV_32F);
337 cv::Mat angleMatrix = cv::Mat::zeros(contourMat.cols, contourMat.cols, CV_32F);
338
339 std::vector<double> logspaces, angspaces;
340 logarithmicSpaces(logspaces);
341 angularSpaces(angspaces);
342 buildNormalizedDistanceMatrix(contourMat, disMatrix, queryInliers, _meanDistance);
343 buildAngleMatrix(contourMat, angleMatrix);
344
345 // Now, build the descriptor matrix (each row is a point) //
346 descriptors = cv::Mat::zeros(contourMat.cols, descriptorSize(), CV_32F);
347
348 for (int ptidx=0; ptidx<contourMat.cols; ptidx++)
349 {
350 for (int cmp=0; cmp<contourMat.cols; cmp++)
351 {
352 if (ptidx==cmp) continue;
353 if ((int)queryInliers.size()>0)
354 {
355 if (queryInliers[ptidx]==0 || queryInliers[cmp]==0) continue; //avoid outliers
356 }
357
358 int angidx=-1, radidx=-1;
359 for (int i=0; i<nRadialBins; i++)
360 {
361 if (disMatrix.at<float>(ptidx, cmp)<logspaces[i])
362 {
363 radidx=i;
364 break;
365 }
366 }
367 for (int i=0; i<nAngularBins; i++)
368 {
369 if (angleMatrix.at<float>(ptidx, cmp)<angspaces[i])
370 {
371 angidx=i;
372 break;
373 }
374 }
375 if (angidx!=-1 && radidx!=-1)
376 {
377 int idx = angidx+radidx*nAngularBins;
378 descriptors.at<float>(ptidx, idx)++;
379 }
380 }
381 }
382 }
383
logarithmicSpaces(std::vector<double> & vecSpaces) const384 void SCD::logarithmicSpaces(std::vector<double> &vecSpaces) const
385 {
386 double logmin=log10(innerRadius);
387 double logmax=log10(outerRadius);
388 double delta=(logmax-logmin)/(nRadialBins-1);
389 double accdelta=0;
390
391 for (int i=0; i<nRadialBins; i++)
392 {
393 double val = std::pow(10,logmin+accdelta);
394 vecSpaces.push_back(val);
395 accdelta += delta;
396 }
397 }
398
angularSpaces(std::vector<double> & vecSpaces) const399 void SCD::angularSpaces(std::vector<double> &vecSpaces) const
400 {
401 double delta=2*CV_PI/nAngularBins;
402 double val=0;
403
404 for (int i=0; i<nAngularBins; i++)
405 {
406 val += delta;
407 vecSpaces.push_back(val);
408 }
409 }
410
buildNormalizedDistanceMatrix(cv::Mat & contour,cv::Mat & disMatrix,const std::vector<int> & queryInliers,const float _meanDistance)411 void SCD::buildNormalizedDistanceMatrix(cv::Mat &contour, cv::Mat &disMatrix, const std::vector<int> &queryInliers, const float _meanDistance)
412 {
413 cv::Mat contourMat = contour;
414 cv::Mat mask(disMatrix.rows, disMatrix.cols, CV_8U);
415
416 for (int i=0; i<contourMat.cols; i++)
417 {
418 for (int j=0; j<contourMat.cols; j++)
419 {
420 disMatrix.at<float>(i,j) = (float)norm( cv::Mat(contourMat.at<cv::Point2f>(0,i)-contourMat.at<cv::Point2f>(0,j)), cv::NORM_L2 );
421 if (_meanDistance<0)
422 {
423 if (queryInliers.size()>0)
424 {
425 mask.at<char>(i,j)=char(queryInliers[j] && queryInliers[i]);
426 }
427 else
428 {
429 mask.at<char>(i,j)=1;
430 }
431 }
432 }
433 }
434
435 if (_meanDistance<0)
436 {
437 meanDistance=(float)mean(disMatrix, mask)[0];
438 }
439 else
440 {
441 meanDistance=_meanDistance;
442 }
443 disMatrix/=meanDistance+FLT_EPSILON;
444 }
445
buildAngleMatrix(cv::Mat & contour,cv::Mat & angleMatrix) const446 void SCD::buildAngleMatrix(cv::Mat &contour, cv::Mat &angleMatrix) const
447 {
448 cv::Mat contourMat = contour;
449
450 // if descriptor is rotationInvariant compute massCenter //
451 cv::Point2f massCenter(0,0);
452 if (rotationInvariant)
453 {
454 for (int i=0; i<contourMat.cols; i++)
455 {
456 massCenter+=contourMat.at<cv::Point2f>(0,i);
457 }
458 massCenter.x=massCenter.x/(float)contourMat.cols;
459 massCenter.y=massCenter.y/(float)contourMat.cols;
460 }
461
462
463 for (int i=0; i<contourMat.cols; i++)
464 {
465 for (int j=0; j<contourMat.cols; j++)
466 {
467 if (i==j)
468 {
469 angleMatrix.at<float>(i,j)=0.0;
470 }
471 else
472 {
473 cv::Point2f dif = contourMat.at<cv::Point2f>(0,i) - contourMat.at<cv::Point2f>(0,j);
474 angleMatrix.at<float>(i,j) = std::atan2(dif.y, dif.x);
475
476 if (rotationInvariant)
477 {
478 cv::Point2f refPt = contourMat.at<cv::Point2f>(0,i) - massCenter;
479 float refAngle = atan2(refPt.y, refPt.x);
480 angleMatrix.at<float>(i,j) -= refAngle;
481 }
482 angleMatrix.at<float>(i,j) = float(fmod(double(angleMatrix.at<float>(i,j)+(double)FLT_EPSILON),2*CV_PI)+CV_PI);
483 }
484 }
485 }
486 }
487
488 //! SCDMatcher
matchDescriptors(cv::Mat & descriptors1,cv::Mat & descriptors2,std::vector<cv::DMatch> & matches,cv::Ptr<cv::HistogramCostExtractor> & comparer,std::vector<int> & inliers1,std::vector<int> & inliers2)489 void SCDMatcher::matchDescriptors(cv::Mat &descriptors1, cv::Mat &descriptors2, std::vector<cv::DMatch> &matches,
490 cv::Ptr<cv::HistogramCostExtractor> &comparer, std::vector<int> &inliers1, std::vector<int> &inliers2)
491 {
492 matches.clear();
493
494 // Build the cost Matrix between descriptors //
495 cv::Mat costMat;
496 buildCostMatrix(descriptors1, descriptors2, costMat, comparer);
497
498 // Solve the matching problem using the hungarian method //
499 hungarian(costMat, matches, inliers1, inliers2, descriptors1.rows, descriptors2.rows);
500 }
501
buildCostMatrix(const cv::Mat & descriptors1,const cv::Mat & descriptors2,cv::Mat & costMatrix,cv::Ptr<cv::HistogramCostExtractor> & comparer) const502 void SCDMatcher::buildCostMatrix(const cv::Mat &descriptors1, const cv::Mat &descriptors2,
503 cv::Mat &costMatrix, cv::Ptr<cv::HistogramCostExtractor> &comparer) const
504 {
505 CV_INSTRUMENT_REGION();
506
507 comparer->buildCostMatrix(descriptors1, descriptors2, costMatrix);
508 }
509
hungarian(cv::Mat & costMatrix,std::vector<cv::DMatch> & outMatches,std::vector<int> & inliers1,std::vector<int> & inliers2,int sizeScd1,int sizeScd2)510 void SCDMatcher::hungarian(cv::Mat &costMatrix, std::vector<cv::DMatch> &outMatches, std::vector<int> &inliers1,
511 std::vector<int> &inliers2, int sizeScd1, int sizeScd2)
512 {
513 std::vector<int> free(costMatrix.rows, 0), collist(costMatrix.rows, 0);
514 std::vector<int> matches(costMatrix.rows, 0), colsol(costMatrix.rows), rowsol(costMatrix.rows);
515 std::vector<float> d(costMatrix.rows), pred(costMatrix.rows), v(costMatrix.rows);
516
517 const float LOWV = 1e-10f;
518 bool unassignedfound;
519 int i=0, imin=0, numfree=0, prvnumfree=0, f=0, i0=0, k=0, freerow=0;
520 int j=0, j1=0, j2=0, endofpath=0, last=0, low=0, up=0;
521 float min=0, h=0, umin=0, usubmin=0, v2=0;
522
523 // COLUMN REDUCTION //
524 for (j = costMatrix.rows-1; j >= 0; j--)
525 {
526 // find minimum cost over rows.
527 min = costMatrix.at<float>(0,j);
528 imin = 0;
529 for (i = 1; i < costMatrix.rows; i++)
530 if (costMatrix.at<float>(i,j) < min)
531 {
532 min = costMatrix.at<float>(i,j);
533 imin = i;
534 }
535 v[j] = min;
536
537 if (++matches[imin] == 1)
538 {
539 rowsol[imin] = j;
540 colsol[j] = imin;
541 }
542 else
543 {
544 colsol[j]=-1;
545 }
546 }
547
548 // REDUCTION TRANSFER //
549 for (i=0; i<costMatrix.rows; i++)
550 {
551 if (matches[i] == 0)
552 {
553 free[numfree++] = i;
554 }
555 else
556 {
557 if (matches[i] == 1)
558 {
559 j1=rowsol[i];
560 min=std::numeric_limits<float>::max();
561 for (j=0; j<costMatrix.rows; j++)
562 {
563 if (j!=j1)
564 {
565 if (costMatrix.at<float>(i,j)-v[j] < min)
566 {
567 min=costMatrix.at<float>(i,j)-v[j];
568 }
569 }
570 }
571 v[j1] = v[j1]-min;
572 }
573 }
574 }
575 // AUGMENTING ROW REDUCTION //
576 int loopcnt = 0;
577 do
578 {
579 loopcnt++;
580 k=0;
581 prvnumfree=numfree;
582 numfree=0;
583 while (k < prvnumfree)
584 {
585 i=free[k];
586 k++;
587 umin = costMatrix.at<float>(i,0)-v[0];
588 j1=0;
589 usubmin = std::numeric_limits<float>::max();
590 for (j=1; j<costMatrix.rows; j++)
591 {
592 h = costMatrix.at<float>(i,j)-v[j];
593 if (h < usubmin)
594 {
595 if (h >= umin)
596 {
597 usubmin = h;
598 j2 = j;
599 }
600 else
601 {
602 usubmin = umin;
603 umin = h;
604 j2 = j1;
605 j1 = j;
606 }
607 }
608 }
609 i0 = colsol[j1];
610
611 if (fabs(umin-usubmin) > LOWV) //if( umin < usubmin )
612 {
613 v[j1] = v[j1] - (usubmin - umin);
614 }
615 else // minimum and subminimum equal.
616 {
617 if (i0 >= 0) // minimum column j1 is assigned.
618 {
619 j1 = j2;
620 i0 = colsol[j2];
621 }
622 }
623 // (re-)assign i to j1, possibly de-assigning an i0.
624 rowsol[i]=j1;
625 colsol[j1]=i;
626
627 if (i0 >= 0)
628 {
629 //if( umin < usubmin )
630 if (fabs(umin-usubmin) > LOWV)
631 {
632 free[--k] = i0;
633 }
634 else
635 {
636 free[numfree++] = i0;
637 }
638 }
639 }
640 }while (loopcnt<2); // repeat once.
641
642 // AUGMENT SOLUTION for each free row //
643 for (f = 0; f<numfree; f++)
644 {
645 freerow = free[f]; // start row of augmenting path.
646 // Dijkstra shortest path algorithm.
647 // runs until unassigned column added to shortest path tree.
648 for (j = 0; j < costMatrix.rows; j++)
649 {
650 d[j] = costMatrix.at<float>(freerow,j) - v[j];
651 pred[j] = float(freerow);
652 collist[j] = j; // init column list.
653 }
654
655 low=0; // columns in 0..low-1 are ready, now none.
656 up=0; // columns in low..up-1 are to be scanned for current minimum, now none.
657 unassignedfound = false;
658 do
659 {
660 if (up == low)
661 {
662 last=low-1;
663 min = d[collist[up++]];
664 for (k = up; k < costMatrix.rows; k++)
665 {
666 j = collist[k];
667 h = d[j];
668 if (h <= min)
669 {
670 if (h < min) // new minimum.
671 {
672 up = low; // restart list at index low.
673 min = h;
674 }
675 collist[k] = collist[up];
676 collist[up++] = j;
677 }
678 }
679 for (k=low; k<up; k++)
680 {
681 if (colsol[collist[k]] < 0)
682 {
683 endofpath = collist[k];
684 unassignedfound = true;
685 break;
686 }
687 }
688 }
689
690 if (!unassignedfound)
691 {
692 // update 'distances' between freerow and all unscanned columns, via next scanned column.
693 j1 = collist[low];
694 low++;
695 i = colsol[j1];
696 h = costMatrix.at<float>(i,j1)-v[j1]-min;
697
698 for (k = up; k < costMatrix.rows; k++)
699 {
700 j = collist[k];
701 v2 = costMatrix.at<float>(i,j) - v[j] - h;
702 if (v2 < d[j])
703 {
704 pred[j] = float(i);
705 if (v2 == min)
706 {
707 if (colsol[j] < 0)
708 {
709 // if unassigned, shortest augmenting path is complete.
710 endofpath = j;
711 unassignedfound = true;
712 break;
713 }
714 else
715 {
716 collist[k] = collist[up];
717 collist[up++] = j;
718 }
719 }
720 d[j] = v2;
721 }
722 }
723 }
724 }while (!unassignedfound);
725
726 // update column prices.
727 for (k = 0; k <= last; k++)
728 {
729 j1 = collist[k];
730 v[j1] = v[j1] + d[j1] - min;
731 }
732
733 // reset row and column assignments along the alternating path.
734 do
735 {
736 i = int(pred[endofpath]);
737 colsol[endofpath] = i;
738 j1 = endofpath;
739 endofpath = rowsol[i];
740 rowsol[i] = j1;
741 }while (i != freerow);
742 }
743
744 // calculate symmetric shape context cost
745 cv::Mat trueCostMatrix(costMatrix, cv::Rect(0,0,sizeScd1, sizeScd2));
746 CV_Assert(!trueCostMatrix.empty());
747 float leftcost = 0;
748 for (int nrow=0; nrow<trueCostMatrix.rows; nrow++)
749 {
750 double minval;
751 minMaxIdx(trueCostMatrix.row(nrow), &minval);
752 leftcost+=float(minval);
753 }
754 leftcost /= trueCostMatrix.rows;
755
756 float rightcost = 0;
757 for (int ncol=0; ncol<trueCostMatrix.cols; ncol++)
758 {
759 double minval;
760 minMaxIdx(trueCostMatrix.col(ncol), &minval);
761 rightcost+=float(minval);
762 }
763 rightcost /= trueCostMatrix.cols;
764
765 minMatchCost = std::max(leftcost,rightcost);
766
767 // Save in a DMatch vector
768 for (i=0;i<costMatrix.cols;i++)
769 {
770 cv::DMatch singleMatch(colsol[i],i,costMatrix.at<float>(colsol[i],i));//queryIdx,trainIdx,distance
771 outMatches.push_back(singleMatch);
772 }
773
774 // Update inliers
775 inliers1.reserve(sizeScd1);
776 for (size_t kc = 0; kc<inliers1.size(); kc++)
777 {
778 if (rowsol[kc]<sizeScd2) // if a real match
779 inliers1[kc]=1;
780 else
781 inliers1[kc]=0;
782 }
783 inliers2.reserve(sizeScd2);
784 for (size_t kc = 0; kc<inliers2.size(); kc++)
785 {
786 if (colsol[kc]<sizeScd1) // if a real match
787 inliers2[kc]=1;
788 else
789 inliers2[kc]=0;
790 }
791 }
792
793 }
794