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 // Intel License Agreement
11 // For Open Source Computer Vision Library
12 //
13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
14 // Third party copyrights are property of their respective owners.
15 //
16 // Redistribution and use in source and binary forms, with or without modification,
17 // are permitted provided that the following conditions are met:
18 //
19 // * Redistribution's of source code must retain the above copyright notice,
20 // this list of conditions and the following disclaimer.
21 //
22 // * Redistribution's in binary form must reproduce the above copyright notice,
23 // this list of conditions and the following disclaimer in the documentation
24 // and/or other materials provided with the distribution.
25 //
26 // * The name of Intel Corporation may not be used to endorse or promote products
27 // derived from this software without specific prior written permission.
28 //
29 // This software is provided by the copyright holders and contributors "as is" and
30 // any express or implied warranties, including, but not limited to, the implied
31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
32 // In no event shall the Intel Corporation or contributors be liable for any direct,
33 // indirect, incidental, special, exemplary, or consequential damages
34 // (including, but not limited to, procurement of substitute goods or services;
35 // loss of use, data, or profits; or business interruption) however caused
36 // and on any theory of liability, whether in contract, strict liability,
37 // or tort (including negligence or otherwise) arising in any way out of
38 // the use of this software, even if advised of the possibility of such damage.
39 //
40 //M*/
41
42 /*
43 Partially based on Yossi Rubner code:
44 =========================================================================
45 emd.c
46
47 Last update: 3/14/98
48
49 An implementation of the Earth Movers Distance.
50 Based of the solution for the Transportation problem as described in
51 "Introduction to Mathematical Programming" by F. S. Hillier and
52 G. J. Lieberman, McGraw-Hill, 1990.
53
54 Copyright (C) 1998 Yossi Rubner
55 Computer Science Department, Stanford University
56 E-Mail: rubner@cs.stanford.edu URL: http://vision.stanford.edu/~rubner
57 ==========================================================================
58 */
59 #ifdef CHROMIUM_OPENCV
60 #include "emd_wrapper.h"
61
62 #include <algorithm>
63 #include <vector>
64 #include <cassert>
65 #include <cstring>
66
67 #include "base/numerics/checked_math.h"
68 #else
69 #include "precomp.hpp"
70 #endif
71
72 #define MAX_ITERATIONS 500
73 #define CV_EMD_INF ((float)1e20)
74 #define CV_EMD_EPS ((float)1e-5)
75
76 #ifdef CHROMIUM_OPENCV
77 namespace cv {
78 template <typename T>
79 using AutoBuffer = std::vector<T>;
80 }
81 typedef void CvArr;
82 typedef float (* CvDistanceFunction)( const float* a, const float* b, void* user_param );
83
84 #define cvSqrt(value) ((float)sqrt(value))
85 #define CV_Error(code, msg) do { (void)(msg); abort(); } while (0)
86 #define CV_Assert(expr) do { if (!(expr)) abort(); } while (0)
87 #endif
88
89 /* CvNode1D is used for lists, representing 1D sparse array */
90 typedef struct CvNode1D
91 {
92 float val;
93 struct CvNode1D *next;
94 }
95 CvNode1D;
96
97 /* CvNode2D is used for lists, representing 2D sparse matrix */
98 typedef struct CvNode2D
99 {
100 float val;
101 struct CvNode2D *next[2]; /* next row & next column */
102 int i, j;
103 }
104 CvNode2D;
105
106
107 typedef struct CvEMDState
108 {
109 int ssize, dsize;
110
111 float **cost;
112 CvNode2D *_x;
113 CvNode2D *end_x;
114 CvNode2D *enter_x;
115 char **is_x;
116
117 CvNode2D **rows_x;
118 CvNode2D **cols_x;
119
120 CvNode1D *u;
121 CvNode1D *v;
122
123 int* idx1;
124 int* idx2;
125
126 /* find_loop buffers */
127 CvNode2D **loop;
128 char *is_used;
129
130 /* russel buffers */
131 float *s;
132 float *d;
133 float **delta;
134
135 float weight, max_cost;
136 char *buffer;
137 }
138 CvEMDState;
139
140 /* static function declaration */
141 static int icvInitEMD( const float *signature1, int size1,
142 const float *signature2, int size2,
143 int dims, CvDistanceFunction dist_func, void *user_param,
144 const float* cost, int cost_step,
145 CvEMDState * state, float *lower_bound,
146 cv::AutoBuffer<char>& _buffer );
147
148 static int icvFindBasicVariables( float **cost, char **is_x,
149 CvNode1D * u, CvNode1D * v, int ssize, int dsize );
150
151 static float icvIsOptimal( float **cost, char **is_x,
152 CvNode1D * u, CvNode1D * v,
153 int ssize, int dsize, CvNode2D * enter_x );
154
155 static void icvRussel( CvEMDState * state );
156
157
158 static bool icvNewSolution( CvEMDState * state );
159 static int icvFindLoop( CvEMDState * state );
160
161 static void icvAddBasicVariable( CvEMDState * state,
162 int min_i, int min_j,
163 CvNode1D * prev_u_min_i,
164 CvNode1D * prev_v_min_j,
165 CvNode1D * u_head );
166
167 static float icvDistL2( const float *x, const float *y, void *user_param );
168 static float icvDistL1( const float *x, const float *y, void *user_param );
169 static float icvDistC( const float *x, const float *y, void *user_param );
170
171 #ifdef CHROMIUM_OPENCV
GetDataFromPointDistribution(const opencv::PointDistribution & distribution)172 std::vector<float> GetDataFromPointDistribution(const opencv::PointDistribution& distribution) {
173 std::vector<float> data;
174 data.reserve((distribution.dimensions + 1) * distribution.positions.size());
175 for (size_t i = 0; i < distribution.positions.size(); i++) {
176 data.push_back(distribution.weights[i]);
177 data.insert(data.end(), distribution.positions[i].begin(),
178 distribution.positions[i].end());
179 }
180
181 return data;
182 }
183 #endif
184
185 /* The main function */
186 #ifndef CHROMIUM_OPENCV
187 CV_IMPL
188 #endif
cvCalcEMD2(const CvArr * signature_arr1,const CvArr * signature_arr2,int dist_type,CvDistanceFunction dist_func,const CvArr * cost_matrix,CvArr * flow_matrix,float * lower_bound,void * user_param)189 float cvCalcEMD2( const CvArr* signature_arr1,
190 const CvArr* signature_arr2,
191 int dist_type,
192 CvDistanceFunction dist_func,
193 const CvArr* cost_matrix,
194 CvArr* flow_matrix,
195 float *lower_bound,
196 void *user_param )
197 {
198 cv::AutoBuffer<char> local_buf;
199 CvEMDState state;
200 float emd = 0;
201
202 memset( &state, 0, sizeof(state));
203
204 double total_cost = 0;
205 int result = 0;
206 float eps, min_delta;
207 CvNode2D *xp = 0;
208 #ifdef CHROMIUM_OPENCV
209 opencv::PointDistribution* signature1 =
210 (opencv::PointDistribution*)signature_arr1;
211 opencv::PointDistribution* signature2 =
212 (opencv::PointDistribution*)signature_arr2;
213
214 int dims = signature1->dimensions;
215 int size1 = signature1->positions.size();
216 int size2 = signature2->positions.size();
217 dist_func = icvDistL1;
218
219 std::vector<float> signature1_data =
220 GetDataFromPointDistribution(*signature1);
221 std::vector<float> signature2_data =
222 GetDataFromPointDistribution(*signature2);
223 user_param = (void*)(size_t)dims;
224 #else
225 CvMat sign_stub1, *signature1 = (CvMat*)signature_arr1;
226 CvMat sign_stub2, *signature2 = (CvMat*)signature_arr2;
227 CvMat cost_stub, *cost = &cost_stub;
228 CvMat flow_stub, *flow = (CvMat*)flow_matrix;
229 int dims, size1, size2;
230
231 signature1 = cvGetMat( signature1, &sign_stub1 );
232 signature2 = cvGetMat( signature2, &sign_stub2 );
233
234 if( signature1->cols != signature2->cols )
235 CV_Error( CV_StsUnmatchedSizes, "The arrays must have equal number of columns (which is number of dimensions but 1)" );
236
237 dims = signature1->cols - 1;
238 size1 = signature1->rows;
239 size2 = signature2->rows;
240
241 if( !CV_ARE_TYPES_EQ( signature1, signature2 ))
242 CV_Error( CV_StsUnmatchedFormats, "The array must have equal types" );
243
244 if( CV_MAT_TYPE( signature1->type ) != CV_32FC1 )
245 CV_Error( CV_StsUnsupportedFormat, "The signatures must be 32fC1" );
246
247 if( flow )
248 {
249 flow = cvGetMat( flow, &flow_stub );
250
251 if( flow->rows != size1 || flow->cols != size2 )
252 CV_Error( CV_StsUnmatchedSizes,
253 "The flow matrix size does not match to the signatures' sizes" );
254
255 if( CV_MAT_TYPE( flow->type ) != CV_32FC1 )
256 CV_Error( CV_StsUnsupportedFormat, "The flow matrix must be 32fC1" );
257 }
258
259 cost->data.fl = 0;
260 cost->step = 0;
261
262 if( dist_type < 0 )
263 {
264 if( cost_matrix )
265 {
266 if( dist_func )
267 CV_Error( CV_StsBadArg,
268 "Only one of cost matrix or distance function should be non-NULL in case of user-defined distance" );
269
270 if( lower_bound )
271 CV_Error( CV_StsBadArg,
272 "The lower boundary can not be calculated if the cost matrix is used" );
273
274 cost = cvGetMat( cost_matrix, &cost_stub );
275 if( cost->rows != size1 || cost->cols != size2 )
276 CV_Error( CV_StsUnmatchedSizes,
277 "The cost matrix size does not match to the signatures' sizes" );
278
279 if( CV_MAT_TYPE( cost->type ) != CV_32FC1 )
280 CV_Error( CV_StsUnsupportedFormat, "The cost matrix must be 32fC1" );
281 }
282 else if( !dist_func )
283 CV_Error( CV_StsNullPtr, "In case of user-defined distance Distance function is undefined" );
284 }
285 else
286 {
287 if( dims == 0 )
288 CV_Error( CV_StsBadSize,
289 "Number of dimensions can be 0 only if a user-defined metric is used" );
290 user_param = (void *) (size_t)dims;
291 switch (dist_type)
292 {
293 case CV_DIST_L1:
294 dist_func = icvDistL1;
295 break;
296 case CV_DIST_L2:
297 dist_func = icvDistL2;
298 break;
299 case CV_DIST_C:
300 dist_func = icvDistC;
301 break;
302 default:
303 CV_Error( CV_StsBadFlag, "Bad or unsupported metric type" );
304 }
305 }
306 #endif
307
308 #ifdef CHROMIUM_OPENCV
309 result = icvInitEMD(signature1_data.data(), size1, signature2_data.data(),
310 size2, dims, icvDistL2, user_param, nullptr, 0, &state,
311 lower_bound, local_buf);
312 #else
313 result = icvInitEMD( signature1->data.fl, size1,
314 signature2->data.fl, size2,
315 dims, dist_func, user_param,
316 cost->data.fl, cost->step,
317 &state, lower_bound, local_buf );
318 #endif
319
320 if( result > 0 && lower_bound )
321 {
322 emd = *lower_bound;
323 return emd;
324 }
325
326 eps = CV_EMD_EPS * state.max_cost;
327
328 /* if ssize = 1 or dsize = 1 then we are done, else ... */
329 if( state.ssize > 1 && state.dsize > 1 )
330 {
331 int itr;
332
333 for( itr = 1; itr < MAX_ITERATIONS; itr++ )
334 {
335 /* find basic variables */
336 result = icvFindBasicVariables( state.cost, state.is_x,
337 state.u, state.v, state.ssize, state.dsize );
338 if( result < 0 )
339 break;
340
341 /* check for optimality */
342 min_delta = icvIsOptimal( state.cost, state.is_x,
343 state.u, state.v,
344 state.ssize, state.dsize, state.enter_x );
345
346 if( min_delta == CV_EMD_INF )
347 CV_Error( CV_StsNoConv, "" );
348
349 /* if no negative deltamin, we found the optimal solution */
350 if( min_delta >= -eps )
351 break;
352
353 /* improve solution */
354 if(!icvNewSolution( &state ))
355 CV_Error( CV_StsNoConv, "" );
356 }
357 }
358
359 /* compute the total flow */
360 for( xp = state._x; xp < state.end_x; xp++ )
361 {
362 float val = xp->val;
363 int i = xp->i;
364 int j = xp->j;
365
366 if( xp == state.enter_x )
367 continue;
368
369 int ci = state.idx1[i];
370 int cj = state.idx2[j];
371
372 if( ci >= 0 && cj >= 0 )
373 {
374 total_cost += (double)val * state.cost[i][j];
375 #ifndef CHROMIUM_OPENCV
376 if( flow )
377 ((float*)(flow->data.ptr + flow->step*ci))[cj] = val;
378 #endif
379 }
380 }
381
382 emd = (float) (total_cost / state.weight);
383 return emd;
384 }
385
386
387 /************************************************************************************\
388 * initialize structure, allocate buffers and generate initial golution *
389 \************************************************************************************/
icvInitEMD(const float * signature1,int size1,const float * signature2,int size2,int dims,CvDistanceFunction dist_func,void * user_param,const float * cost,int cost_step,CvEMDState * state,float * lower_bound,cv::AutoBuffer<char> & _buffer)390 static int icvInitEMD( const float* signature1, int size1,
391 const float* signature2, int size2,
392 int dims, CvDistanceFunction dist_func, void* user_param,
393 const float* cost, int cost_step,
394 CvEMDState* state, float* lower_bound,
395 cv::AutoBuffer<char>& _buffer )
396 {
397 float s_sum = 0, d_sum = 0, diff;
398 int i, j;
399 int ssize = 0, dsize = 0;
400 int equal_sums = 1;
401 int buffer_size;
402 float max_cost = 0;
403 char *buffer, *buffer_end;
404
405 memset( state, 0, sizeof( *state ));
406 assert( cost_step % sizeof(float) == 0 );
407 cost_step /= sizeof(float);
408
409 /* calculate buffer size */
410 buffer_size = (size1+1) * (size2+1) * (sizeof( float ) + /* cost */
411 sizeof( char ) + /* is_x */
412 sizeof( float )) + /* delta matrix */
413 (size1 + size2 + 2) * (sizeof( CvNode2D ) + /* _x */
414 sizeof( CvNode2D * ) + /* cols_x & rows_x */
415 sizeof( CvNode1D ) + /* u & v */
416 sizeof( float ) + /* s & d */
417 sizeof( int ) + sizeof(CvNode2D*)) + /* idx1 & idx2 */
418 (size1+1) * (sizeof( float * ) + sizeof( char * ) + /* rows pointers for */
419 sizeof( float * )) + 256; /* cost, is_x and delta */
420
421 if( buffer_size < (int) (dims * 2 * sizeof( float )))
422 {
423 buffer_size = dims * 2 * sizeof( float );
424 }
425
426 /* allocate buffers */
427 #ifdef CHROMIUM_OPENCV
428 _buffer.resize(buffer_size);
429 #else
430 _buffer.allocate(buffer_size);
431 #endif
432
433 state->buffer = buffer = _buffer.data();
434 buffer_end = buffer + buffer_size;
435
436 state->idx1 = (int*) buffer;
437 buffer += (size1 + 1) * sizeof( int );
438
439 state->idx2 = (int*) buffer;
440 buffer += (size2 + 1) * sizeof( int );
441
442 state->s = (float *) buffer;
443 buffer += (size1 + 1) * sizeof( float );
444
445 state->d = (float *) buffer;
446 buffer += (size2 + 1) * sizeof( float );
447
448 /* sum up the supply and demand */
449 for( i = 0; i < size1; i++ )
450 {
451 float weight = signature1[i * (dims + 1)];
452
453 if( weight > 0 )
454 {
455 s_sum += weight;
456 state->s[ssize] = weight;
457 state->idx1[ssize++] = i;
458
459 }
460 else if( weight < 0 )
461 CV_Error(CV_StsBadArg, "signature1 must not contain negative weights");
462 }
463
464 for( i = 0; i < size2; i++ )
465 {
466 float weight = signature2[i * (dims + 1)];
467
468 if( weight > 0 )
469 {
470 d_sum += weight;
471 state->d[dsize] = weight;
472 state->idx2[dsize++] = i;
473 }
474 else if( weight < 0 )
475 CV_Error(CV_StsBadArg, "signature2 must not contain negative weights");
476 }
477
478 if( ssize == 0 )
479 CV_Error(CV_StsBadArg, "signature1 must contain at least one non-zero value");
480 if( dsize == 0 )
481 CV_Error(CV_StsBadArg, "signature2 must contain at least one non-zero value");
482
483 /* if supply different than the demand, add a zero-cost dummy cluster */
484 diff = s_sum - d_sum;
485 if( fabs( diff ) >= CV_EMD_EPS * s_sum )
486 {
487 equal_sums = 0;
488 if( diff < 0 )
489 {
490 state->s[ssize] = -diff;
491 state->idx1[ssize++] = -1;
492 }
493 else
494 {
495 state->d[dsize] = diff;
496 state->idx2[dsize++] = -1;
497 }
498 }
499
500 state->ssize = ssize;
501 state->dsize = dsize;
502 state->weight = s_sum > d_sum ? s_sum : d_sum;
503
504 if( lower_bound && equal_sums ) /* check lower bound */
505 {
506 int sz1 = size1 * (dims + 1), sz2 = size2 * (dims + 1);
507 float lb = 0;
508
509 float* xs = (float *) buffer;
510 float* xd = xs + dims;
511
512 memset( xs, 0, dims*sizeof(xs[0]));
513 memset( xd, 0, dims*sizeof(xd[0]));
514
515 for( j = 0; j < sz1; j += dims + 1 )
516 {
517 float weight = signature1[j];
518 for( i = 0; i < dims; i++ )
519 xs[i] += signature1[j + i + 1] * weight;
520 }
521
522 for( j = 0; j < sz2; j += dims + 1 )
523 {
524 float weight = signature2[j];
525 for( i = 0; i < dims; i++ )
526 xd[i] += signature2[j + i + 1] * weight;
527 }
528
529 lb = dist_func( xs, xd, user_param ) / state->weight;
530 i = *lower_bound <= lb;
531 *lower_bound = lb;
532 if( i )
533 return 1;
534 }
535
536 /* assign pointers */
537 state->is_used = (char *) buffer;
538 /* init delta matrix */
539 state->delta = (float **) buffer;
540 buffer += ssize * sizeof( float * );
541
542 for( i = 0; i < ssize; i++ )
543 {
544 state->delta[i] = (float *) buffer;
545 buffer += dsize * sizeof( float );
546 }
547
548 state->loop = (CvNode2D **) buffer;
549 buffer += (ssize + dsize + 1) * sizeof(CvNode2D*);
550
551 state->_x = state->end_x = (CvNode2D *) buffer;
552 buffer += (ssize + dsize) * sizeof( CvNode2D );
553
554 /* init cost matrix */
555 state->cost = (float **) buffer;
556 buffer += ssize * sizeof( float * );
557
558 /* compute the distance matrix */
559 for( i = 0; i < ssize; i++ )
560 {
561 int ci = state->idx1[i];
562
563 state->cost[i] = (float *) buffer;
564 buffer += dsize * sizeof( float );
565
566 if( ci >= 0 )
567 {
568 for( j = 0; j < dsize; j++ )
569 {
570 int cj = state->idx2[j];
571 if( cj < 0 )
572 state->cost[i][j] = 0;
573 else
574 {
575 float val;
576 if( dist_func )
577 {
578 val = dist_func( signature1 + ci * (dims + 1) + 1,
579 signature2 + cj * (dims + 1) + 1,
580 user_param );
581 }
582 else
583 {
584 assert( cost );
585 val = cost[cost_step*ci + cj];
586 }
587 state->cost[i][j] = val;
588 if( max_cost < val )
589 max_cost = val;
590 }
591 }
592 }
593 else
594 {
595 for( j = 0; j < dsize; j++ )
596 state->cost[i][j] = 0;
597 }
598 }
599
600 state->max_cost = max_cost;
601
602 memset( buffer, 0, buffer_end - buffer );
603
604 state->rows_x = (CvNode2D **) buffer;
605 buffer += ssize * sizeof( CvNode2D * );
606
607 state->cols_x = (CvNode2D **) buffer;
608 buffer += dsize * sizeof( CvNode2D * );
609
610 state->u = (CvNode1D *) buffer;
611 buffer += ssize * sizeof( CvNode1D );
612
613 state->v = (CvNode1D *) buffer;
614 buffer += dsize * sizeof( CvNode1D );
615
616 /* init is_x matrix */
617 state->is_x = (char **) buffer;
618 buffer += ssize * sizeof( char * );
619
620 for( i = 0; i < ssize; i++ )
621 {
622 state->is_x[i] = buffer;
623 buffer += dsize;
624 }
625
626 assert( buffer <= buffer_end );
627
628 icvRussel( state );
629
630 state->enter_x = (state->end_x)++;
631 return 0;
632 }
633
634
635 /****************************************************************************************\
636 * icvFindBasicVariables *
637 \****************************************************************************************/
icvFindBasicVariables(float ** cost,char ** is_x,CvNode1D * u,CvNode1D * v,int ssize,int dsize)638 static int icvFindBasicVariables( float **cost, char **is_x,
639 CvNode1D * u, CvNode1D * v, int ssize, int dsize )
640 {
641 int i, j;
642 int u_cfound, v_cfound;
643 CvNode1D u0_head, u1_head, *cur_u, *prev_u;
644 CvNode1D v0_head, v1_head, *cur_v, *prev_v;
645 bool found;
646
647 CV_Assert(u != 0 && v != 0);
648
649 /* initialize the rows list (u) and the columns list (v) */
650 u0_head.next = u;
651 for( i = 0; i < ssize; i++ )
652 {
653 u[i].next = u + i + 1;
654 }
655 u[ssize - 1].next = 0;
656 u1_head.next = 0;
657
658 v0_head.next = ssize > 1 ? v + 1 : 0;
659 for( i = 1; i < dsize; i++ )
660 {
661 v[i].next = v + i + 1;
662 }
663 v[dsize - 1].next = 0;
664 v1_head.next = 0;
665
666 /* there are ssize+dsize variables but only ssize+dsize-1 independent equations,
667 so set v[0]=0 */
668 v[0].val = 0;
669 v1_head.next = v;
670 v1_head.next->next = 0;
671
672 /* loop until all variables are found */
673 u_cfound = v_cfound = 0;
674 while( u_cfound < ssize || v_cfound < dsize )
675 {
676 found = false;
677 if( v_cfound < dsize )
678 {
679 /* loop over all marked columns */
680 prev_v = &v1_head;
681 cur_v = v1_head.next;
682 found = found || (cur_v != 0);
683 for( ; cur_v != 0; cur_v = cur_v->next )
684 {
685 float cur_v_val = cur_v->val;
686
687 j = (int)(cur_v - v);
688 /* find the variables in column j */
689 prev_u = &u0_head;
690 for( cur_u = u0_head.next; cur_u != 0; )
691 {
692 i = (int)(cur_u - u);
693 if( is_x[i][j] )
694 {
695 /* compute u[i] */
696 cur_u->val = cost[i][j] - cur_v_val;
697 /* ...and add it to the marked list */
698 prev_u->next = cur_u->next;
699 cur_u->next = u1_head.next;
700 u1_head.next = cur_u;
701 cur_u = prev_u->next;
702 }
703 else
704 {
705 prev_u = cur_u;
706 cur_u = cur_u->next;
707 }
708 }
709 prev_v->next = cur_v->next;
710 v_cfound++;
711 }
712 }
713
714 if( u_cfound < ssize )
715 {
716 /* loop over all marked rows */
717 prev_u = &u1_head;
718 cur_u = u1_head.next;
719 found = found || (cur_u != 0);
720 for( ; cur_u != 0; cur_u = cur_u->next )
721 {
722 float cur_u_val = cur_u->val;
723 float *_cost;
724 char *_is_x;
725
726 i = (int)(cur_u - u);
727 _cost = cost[i];
728 _is_x = is_x[i];
729 /* find the variables in rows i */
730 prev_v = &v0_head;
731 for( cur_v = v0_head.next; cur_v != 0; )
732 {
733 j = (int)(cur_v - v);
734 if( _is_x[j] )
735 {
736 /* compute v[j] */
737 cur_v->val = _cost[j] - cur_u_val;
738 /* ...and add it to the marked list */
739 prev_v->next = cur_v->next;
740 cur_v->next = v1_head.next;
741 v1_head.next = cur_v;
742 cur_v = prev_v->next;
743 }
744 else
745 {
746 prev_v = cur_v;
747 cur_v = cur_v->next;
748 }
749 }
750 prev_u->next = cur_u->next;
751 u_cfound++;
752 }
753 }
754
755 if( !found )
756 return -1;
757 }
758
759 return 0;
760 }
761
762
763 /****************************************************************************************\
764 * icvIsOptimal *
765 \****************************************************************************************/
766 static float
icvIsOptimal(float ** cost,char ** is_x,CvNode1D * u,CvNode1D * v,int ssize,int dsize,CvNode2D * enter_x)767 icvIsOptimal( float **cost, char **is_x,
768 CvNode1D * u, CvNode1D * v, int ssize, int dsize, CvNode2D * enter_x )
769 {
770 float delta, min_delta = CV_EMD_INF;
771 int i, j, min_i = 0, min_j = 0;
772
773 /* find the minimal cij-ui-vj over all i,j */
774 for( i = 0; i < ssize; i++ )
775 {
776 float u_val = u[i].val;
777 float *_cost = cost[i];
778 char *_is_x = is_x[i];
779
780 for( j = 0; j < dsize; j++ )
781 {
782 if( !_is_x[j] )
783 {
784 delta = _cost[j] - u_val - v[j].val;
785 if( min_delta > delta )
786 {
787 min_delta = delta;
788 min_i = i;
789 min_j = j;
790 }
791 }
792 }
793 }
794
795 enter_x->i = min_i;
796 enter_x->j = min_j;
797
798 return min_delta;
799 }
800
801 /****************************************************************************************\
802 * icvNewSolution *
803 \****************************************************************************************/
804 static bool
icvNewSolution(CvEMDState * state)805 icvNewSolution( CvEMDState * state )
806 {
807 int i, j;
808 float min_val = CV_EMD_INF;
809 int steps;
810 CvNode2D head = {0, {0}, 0, 0}, *cur_x, *next_x, *leave_x = 0;
811 CvNode2D *enter_x = state->enter_x;
812 CvNode2D **loop = state->loop;
813
814 /* enter the new basic variable */
815 i = enter_x->i;
816 j = enter_x->j;
817 state->is_x[i][j] = 1;
818 enter_x->next[0] = state->rows_x[i];
819 enter_x->next[1] = state->cols_x[j];
820 enter_x->val = 0;
821 state->rows_x[i] = enter_x;
822 state->cols_x[j] = enter_x;
823
824 /* find a chain reaction */
825 steps = icvFindLoop( state );
826
827 if( steps == 0 )
828 return false;
829
830 /* find the largest value in the loop */
831 for( i = 1; i < steps; i += 2 )
832 {
833 float temp = loop[i]->val;
834
835 if( min_val > temp )
836 {
837 leave_x = loop[i];
838 min_val = temp;
839 }
840 }
841
842 /* update the loop */
843 for( i = 0; i < steps; i += 2 )
844 {
845 float temp0 = loop[i]->val + min_val;
846 float temp1 = loop[i + 1]->val - min_val;
847
848 loop[i]->val = temp0;
849 loop[i + 1]->val = temp1;
850 }
851
852 /* remove the leaving basic variable */
853 CV_Assert(leave_x != NULL);
854 i = leave_x->i;
855 j = leave_x->j;
856 state->is_x[i][j] = 0;
857
858 head.next[0] = state->rows_x[i];
859 cur_x = &head;
860 while( (next_x = cur_x->next[0]) != leave_x )
861 {
862 cur_x = next_x;
863 CV_Assert( cur_x );
864 }
865 cur_x->next[0] = next_x->next[0];
866 state->rows_x[i] = head.next[0];
867
868 head.next[1] = state->cols_x[j];
869 cur_x = &head;
870 while( (next_x = cur_x->next[1]) != leave_x )
871 {
872 cur_x = next_x;
873 CV_Assert( cur_x );
874 }
875 cur_x->next[1] = next_x->next[1];
876 state->cols_x[j] = head.next[1];
877
878 /* set enter_x to be the new empty slot */
879 state->enter_x = leave_x;
880
881 return true;
882 }
883
884
885
886 /****************************************************************************************\
887 * icvFindLoop *
888 \****************************************************************************************/
889 static int
icvFindLoop(CvEMDState * state)890 icvFindLoop( CvEMDState * state )
891 {
892 int i, steps = 1;
893 CvNode2D *new_x;
894 CvNode2D **loop = state->loop;
895 CvNode2D *enter_x = state->enter_x, *_x = state->_x;
896 char *is_used = state->is_used;
897
898 memset( is_used, 0, state->ssize + state->dsize );
899
900 new_x = loop[0] = enter_x;
901 is_used[enter_x - _x] = 1;
902 steps = 1;
903
904 do
905 {
906 if( (steps & 1) == 1 )
907 {
908 /* find an unused x in the row */
909 new_x = state->rows_x[new_x->i];
910 while( new_x != 0 && is_used[new_x - _x] )
911 new_x = new_x->next[0];
912 }
913 else
914 {
915 /* find an unused x in the column, or the entering x */
916 new_x = state->cols_x[new_x->j];
917 while( new_x != 0 && is_used[new_x - _x] && new_x != enter_x )
918 new_x = new_x->next[1];
919 if( new_x == enter_x )
920 break;
921 }
922
923 if( new_x != 0 ) /* found the next x */
924 {
925 /* add x to the loop */
926 loop[steps++] = new_x;
927 is_used[new_x - _x] = 1;
928 }
929 else /* didn't find the next x */
930 {
931 /* backtrack */
932 do
933 {
934 i = steps & 1;
935 new_x = loop[steps - 1];
936 do
937 {
938 new_x = new_x->next[i];
939 }
940 while( new_x != 0 && is_used[new_x - _x] );
941
942 if( new_x == 0 )
943 {
944 is_used[loop[--steps] - _x] = 0;
945 }
946 }
947 while( new_x == 0 && steps > 0 );
948
949 is_used[loop[steps - 1] - _x] = 0;
950 loop[steps - 1] = new_x;
951 is_used[new_x - _x] = 1;
952 }
953 }
954 while( steps > 0 );
955
956 return steps;
957 }
958
959
960
961 /****************************************************************************************\
962 * icvRussel *
963 \****************************************************************************************/
964 static void
icvRussel(CvEMDState * state)965 icvRussel( CvEMDState * state )
966 {
967 int i, j, min_i = -1, min_j = -1;
968 float min_delta, diff;
969 CvNode1D u_head, *cur_u, *prev_u;
970 CvNode1D v_head, *cur_v, *prev_v;
971 CvNode1D *prev_u_min_i = 0, *prev_v_min_j = 0, *remember;
972 CvNode1D *u = state->u, *v = state->v;
973 int ssize = state->ssize, dsize = state->dsize;
974 float eps = CV_EMD_EPS * state->max_cost;
975 float **cost = state->cost;
976 float **delta = state->delta;
977
978 /* initialize the rows list (ur), and the columns list (vr) */
979 u_head.next = u;
980 for( i = 0; i < ssize; i++ )
981 {
982 u[i].next = u + i + 1;
983 }
984 u[ssize - 1].next = 0;
985
986 v_head.next = v;
987 for( i = 0; i < dsize; i++ )
988 {
989 v[i].val = -CV_EMD_INF;
990 v[i].next = v + i + 1;
991 }
992 v[dsize - 1].next = 0;
993
994 /* find the maximum row and column values (ur[i] and vr[j]) */
995 for( i = 0; i < ssize; i++ )
996 {
997 float u_val = -CV_EMD_INF;
998 float *cost_row = cost[i];
999
1000 for( j = 0; j < dsize; j++ )
1001 {
1002 float temp = cost_row[j];
1003
1004 if( u_val < temp )
1005 u_val = temp;
1006 if( v[j].val < temp )
1007 v[j].val = temp;
1008 }
1009 u[i].val = u_val;
1010 }
1011
1012 /* compute the delta matrix */
1013 for( i = 0; i < ssize; i++ )
1014 {
1015 float u_val = u[i].val;
1016 float *delta_row = delta[i];
1017 float *cost_row = cost[i];
1018
1019 for( j = 0; j < dsize; j++ )
1020 {
1021 delta_row[j] = cost_row[j] - u_val - v[j].val;
1022 }
1023 }
1024
1025 /* find the basic variables */
1026 do
1027 {
1028 /* find the smallest delta[i][j] */
1029 min_i = -1;
1030 min_delta = CV_EMD_INF;
1031 prev_u = &u_head;
1032 for( cur_u = u_head.next; cur_u != 0; cur_u = cur_u->next )
1033 {
1034 i = (int)(cur_u - u);
1035 float *delta_row = delta[i];
1036
1037 prev_v = &v_head;
1038 for( cur_v = v_head.next; cur_v != 0; cur_v = cur_v->next )
1039 {
1040 j = (int)(cur_v - v);
1041 if( min_delta > delta_row[j] )
1042 {
1043 min_delta = delta_row[j];
1044 min_i = i;
1045 min_j = j;
1046 prev_u_min_i = prev_u;
1047 prev_v_min_j = prev_v;
1048 }
1049 prev_v = cur_v;
1050 }
1051 prev_u = cur_u;
1052 }
1053
1054 if( min_i < 0 )
1055 break;
1056
1057 /* add x[min_i][min_j] to the basis, and adjust supplies and cost */
1058 remember = prev_u_min_i->next;
1059 icvAddBasicVariable( state, min_i, min_j, prev_u_min_i, prev_v_min_j, &u_head );
1060
1061 /* update the necessary delta[][] */
1062 if( remember == prev_u_min_i->next ) /* line min_i was deleted */
1063 {
1064 for( cur_v = v_head.next; cur_v != 0; cur_v = cur_v->next )
1065 {
1066 j = (int)(cur_v - v);
1067 if( cur_v->val == cost[min_i][j] ) /* column j needs updating */
1068 {
1069 float max_val = -CV_EMD_INF;
1070
1071 /* find the new maximum value in the column */
1072 for( cur_u = u_head.next; cur_u != 0; cur_u = cur_u->next )
1073 {
1074 float temp = cost[cur_u - u][j];
1075
1076 if( max_val < temp )
1077 max_val = temp;
1078 }
1079
1080 /* if needed, adjust the relevant delta[*][j] */
1081 diff = max_val - cur_v->val;
1082 cur_v->val = max_val;
1083 if( fabs( diff ) < eps )
1084 {
1085 for( cur_u = u_head.next; cur_u != 0; cur_u = cur_u->next )
1086 delta[cur_u - u][j] += diff;
1087 }
1088 }
1089 }
1090 }
1091 else /* column min_j was deleted */
1092 {
1093 for( cur_u = u_head.next; cur_u != 0; cur_u = cur_u->next )
1094 {
1095 i = (int)(cur_u - u);
1096 if( cur_u->val == cost[i][min_j] ) /* row i needs updating */
1097 {
1098 float max_val = -CV_EMD_INF;
1099
1100 /* find the new maximum value in the row */
1101 for( cur_v = v_head.next; cur_v != 0; cur_v = cur_v->next )
1102 {
1103 float temp = cost[i][cur_v - v];
1104
1105 if( max_val < temp )
1106 max_val = temp;
1107 }
1108
1109 /* if needed, adjust the relevant delta[i][*] */
1110 diff = max_val - cur_u->val;
1111 cur_u->val = max_val;
1112
1113 if( fabs( diff ) < eps )
1114 {
1115 for( cur_v = v_head.next; cur_v != 0; cur_v = cur_v->next )
1116 delta[i][cur_v - v] += diff;
1117 }
1118 }
1119 }
1120 }
1121 }
1122 while( u_head.next != 0 || v_head.next != 0 );
1123 }
1124
1125
1126
1127 /****************************************************************************************\
1128 * icvAddBasicVariable *
1129 \****************************************************************************************/
1130 static void
icvAddBasicVariable(CvEMDState * state,int min_i,int min_j,CvNode1D * prev_u_min_i,CvNode1D * prev_v_min_j,CvNode1D * u_head)1131 icvAddBasicVariable( CvEMDState * state,
1132 int min_i, int min_j,
1133 CvNode1D * prev_u_min_i, CvNode1D * prev_v_min_j, CvNode1D * u_head )
1134 {
1135 float temp;
1136 CvNode2D *end_x = state->end_x;
1137
1138 if( state->s[min_i] < state->d[min_j] + state->weight * CV_EMD_EPS )
1139 { /* supply exhausted */
1140 temp = state->s[min_i];
1141 state->s[min_i] = 0;
1142 state->d[min_j] -= temp;
1143 }
1144 else /* demand exhausted */
1145 {
1146 temp = state->d[min_j];
1147 state->d[min_j] = 0;
1148 state->s[min_i] -= temp;
1149 }
1150
1151 /* x(min_i,min_j) is a basic variable */
1152 state->is_x[min_i][min_j] = 1;
1153
1154 end_x->val = temp;
1155 end_x->i = min_i;
1156 end_x->j = min_j;
1157 end_x->next[0] = state->rows_x[min_i];
1158 end_x->next[1] = state->cols_x[min_j];
1159 state->rows_x[min_i] = end_x;
1160 state->cols_x[min_j] = end_x;
1161 state->end_x = end_x + 1;
1162
1163 /* delete supply row only if the empty, and if not last row */
1164 if( state->s[min_i] == 0 && u_head->next->next != 0 )
1165 prev_u_min_i->next = prev_u_min_i->next->next; /* remove row from list */
1166 else
1167 prev_v_min_j->next = prev_v_min_j->next->next; /* remove column from list */
1168 }
1169
1170
1171 /****************************************************************************************\
1172 * standard metrics *
1173 \****************************************************************************************/
1174 static float
icvDistL1(const float * x,const float * y,void * user_param)1175 icvDistL1( const float *x, const float *y, void *user_param )
1176 {
1177 int i, dims = (int)(size_t)user_param;
1178 double s = 0;
1179
1180 for( i = 0; i < dims; i++ )
1181 {
1182 double t = x[i] - y[i];
1183
1184 s += fabs( t );
1185 }
1186 return (float)s;
1187 }
1188
1189 static float
icvDistL2(const float * x,const float * y,void * user_param)1190 icvDistL2( const float *x, const float *y, void *user_param )
1191 {
1192 int i, dims = (int)(size_t)user_param;
1193 double s = 0;
1194
1195 for( i = 0; i < dims; i++ )
1196 {
1197 double t = x[i] - y[i];
1198
1199 s += t * t;
1200 }
1201 return cvSqrt( (float)s );
1202 }
1203
1204 static float
icvDistC(const float * x,const float * y,void * user_param)1205 icvDistC( const float *x, const float *y, void *user_param )
1206 {
1207 int i, dims = (int)(size_t)user_param;
1208 double s = 0;
1209
1210 for( i = 0; i < dims; i++ )
1211 {
1212 double t = fabs( x[i] - y[i] );
1213
1214 if( s < t )
1215 s = t;
1216 }
1217 return (float)s;
1218 }
1219
1220 #ifdef CHROMIUM_OPENCV
1221 namespace opencv {
1222
1223 namespace {
1224
ValidatePointDistribution(const PointDistribution & distribution)1225 bool ValidatePointDistribution(const PointDistribution& distribution) {
1226 if (distribution.positions.size() != distribution.weights.size()) {
1227 return false;
1228 }
1229
1230 if (distribution.weights.empty()) {
1231 return false;
1232 }
1233
1234 for (const float f : distribution.weights) {
1235 if (f < 0) {
1236 return false;
1237 }
1238 }
1239
1240 if (std::all_of(distribution.weights.begin(), distribution.weights.end(),
1241 [](float f) { return f == 0; })) {
1242 return false;
1243 }
1244
1245 for (const std::vector<float>& point : distribution.positions) {
1246 if (point.size() != distribution.dimensions) {
1247 return false;
1248 }
1249 }
1250
1251 return true;
1252 }
1253
1254 // Makes sure the buffers allocated during the EMD computation don't overflow
1255 // in size.
ValidateSize(const PointDistribution & distribution1,const PointDistribution & distribution2)1256 bool ValidateSize(const PointDistribution& distribution1,
1257 const PointDistribution& distribution2) {
1258 base::CheckedNumeric<size_t> size1(distribution1.positions.size());
1259 size1 *= distribution1.dimensions + 1;
1260
1261 base::CheckedNumeric<size_t> size2(distribution2.positions.size());
1262 size2 *= distribution2.dimensions + 1;
1263
1264 // This computation should always match the buffer allocation in icvInitEMD
1265 base::CheckedNumeric<size_t> total_size =
1266 (size1 + 1) * (size2 + 1) *
1267 (sizeof(float) + sizeof(char) + sizeof(float)) +
1268 (size1 + size2 + 2) *
1269 (sizeof(CvNode2D) + sizeof(CvNode2D*) + sizeof(CvNode1D) +
1270 sizeof(float) + sizeof(int) + sizeof(CvNode2D*)) +
1271 (size1 + 1) * (sizeof(float*) + sizeof(char*) + sizeof(float*)) + 256;
1272
1273 return total_size.IsValid();
1274 }
1275
1276 } // namespace
1277
EMD(const PointDistribution & distribution1,const PointDistribution & distribution2)1278 base::Optional<double> EMD(const PointDistribution& distribution1,
1279 const PointDistribution& distribution2) {
1280 if (!ValidatePointDistribution(distribution1) ||
1281 !ValidatePointDistribution(distribution2)) {
1282 return base::nullopt;
1283 }
1284
1285 if (distribution1.dimensions != distribution2.dimensions) {
1286 return base::nullopt;
1287 }
1288
1289 if (distribution1.dimensions == 0) {
1290 return base::nullopt;
1291 }
1292
1293 if (!ValidateSize(distribution1,distribution2)) {
1294 return base::nullopt;
1295 }
1296
1297 return cvCalcEMD2(&distribution1, &distribution2, 0, 0, nullptr, nullptr,
1298 nullptr, nullptr);
1299 }
1300
1301 } // namespace opencv
1302 #else
EMD(InputArray _signature1,InputArray _signature2,int distType,InputArray _cost,float * lowerBound,OutputArray _flow)1303 float cv::EMD( InputArray _signature1, InputArray _signature2,
1304 int distType, InputArray _cost,
1305 float* lowerBound, OutputArray _flow )
1306 {
1307 CV_INSTRUMENT_REGION();
1308
1309 Mat signature1 = _signature1.getMat(), signature2 = _signature2.getMat();
1310 Mat cost = _cost.getMat(), flow;
1311
1312 CvMat _csignature1 = cvMat(signature1);
1313 CvMat _csignature2 = cvMat(signature2);
1314 CvMat _ccost = cvMat(cost), _cflow;
1315 if( _flow.needed() )
1316 {
1317 _flow.create(signature1.rows, signature2.rows, CV_32F);
1318 flow = _flow.getMat();
1319 flow = Scalar::all(0);
1320 _cflow = cvMat(flow);
1321 }
1322
1323 return cvCalcEMD2( &_csignature1, &_csignature2, distType, 0, cost.empty() ? 0 : &_ccost,
1324 _flow.needed() ? &_cflow : 0, lowerBound, 0 );
1325 }
1326
wrapperEMD(InputArray _signature1,InputArray _signature2,int distType,InputArray _cost,Ptr<float> lowerBound,OutputArray _flow)1327 float cv::wrapperEMD(InputArray _signature1, InputArray _signature2,
1328 int distType, InputArray _cost,
1329 Ptr<float> lowerBound, OutputArray _flow)
1330 {
1331 return EMD(_signature1, _signature2, distType, _cost, lowerBound.get(), _flow);
1332 }
1333 #endif
1334
1335 /* End of file. */
1336