remove non-ideal pairs when using crosscheck in batchdistance

modules/core/src/batch_distance.cpp

@@ -297,19 +297,21 @@ void cv::batchDistance( InputArray _src1, InputArray _src2,
         nidx = Scalar::all(-1);
     }
 
+
     if( crosscheck )
     {
         CV_Assert( K == 1 && update == 0 && mask.empty() );
         CV_Assert(!nidx.empty());
-        Mat tdist, tidx;
+        Mat tdist, tidx, sdist, sidx;
         batchDistance(src2, src1, tdist, dtype, tidx, normType, K, mask, 0, false);
+        batchDistance(src1, src2, sdist, dtype, sidx, normType, K, mask, 0, false);
 
         // if an idx-th element from src1 appeared to be the nearest to i-th element of src2,
         // we update the minimum mutual distance between idx-th element of src1 and the whole src2 set.
         // As a result, if nidx[idx] = i*, it means that idx-th element of src1 is the nearest
         // to i*-th element of src2 and i*-th element of src2 is the closest to idx-th element of src1.
         // If nidx[idx] = -1, it means that there is no such ideal couple for it in src2.
-        // This O(N) procedure is called cross-check and it helps to eliminate some false matches.
+        // This O(2N) procedure is called cross-check and it helps to eliminate some false matches.
         if( dtype == CV_32S )
         {
             for( int i = 0; i < tdist.rows; i++ )
@@ -336,6 +338,13 @@ void cv::batchDistance( InputArray _src1, InputArray _src2,
                 }
             }
         }
+        for( int i = 0; i < sdist.rows; i++ )
+        {
+            if( tidx.at<int>(sidx.at<int>(i)) != i )
+            {
+                nidx.at<int>(i) = -1;
+            }
+        }
         return;
     }