//*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                           License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of the copyright holders may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/

#include "precomp.hpp"
#include <limits>

using namespace cv;
using namespace std;

static inline Point2f applyHomography( const Mat_<double>& H, const Point2f& pt )
{
    double z = H(2,0)*pt.x + H(2,1)*pt.y + H(2,2);
    if( z )
    {
        double w = 1./z;
        return Point2f( (float)((H(0,0)*pt.x + H(0,1)*pt.y + H(0,2))*w), (float)((H(1,0)*pt.x + H(1,1)*pt.y + H(1,2))*w) );
    }
    return Point2f( numeric_limits<float>::max(), numeric_limits<float>::max() );
}

static inline void linearizeHomographyAt( const Mat_<double>& H, const Point2f& pt, Mat_<double>& A )
{
    A.create(2,2);
    double p1 = H(0,0)*pt.x + H(0,1)*pt.y + H(0,2),
           p2 = H(1,0)*pt.x + H(1,1)*pt.y + H(1,2),
           p3 = H(2,0)*pt.x + H(2,1)*pt.y + H(2,2),
           p3_2 = p3*p3;
    if( p3 )
    {
        A(0,0) = H(0,0)/p3 - p1*H(2,0)/p3_2; // fxdx
        A(0,1) = H(0,1)/p3 - p1*H(2,1)/p3_2; // fxdy

        A(1,0) = H(1,0)/p3 - p2*H(2,0)/p3_2; // fydx
        A(1,1) = H(1,1)/p3 - p2*H(2,1)/p3_2; // fydx
    }
    else
        A.setTo(Scalar::all(numeric_limits<double>::max()));
}

class EllipticKeyPoint
{
public:
    EllipticKeyPoint();
    EllipticKeyPoint( const Point2f& _center, const Scalar& _ellipse );

    static void convert( const vector<KeyPoint>& src, vector<EllipticKeyPoint>& dst );
    static void convert( const vector<EllipticKeyPoint>& src, vector<KeyPoint>& dst );

    static Mat_<double> getSecondMomentsMatrix( const Scalar& _ellipse );
    Mat_<double> getSecondMomentsMatrix() const;

    void calcProjection( const Mat_<double>& H, EllipticKeyPoint& projection ) const;
    static void calcProjection( const vector<EllipticKeyPoint>& src, const Mat_<double>& H, vector<EllipticKeyPoint>& dst );

    Point2f center;
    Scalar ellipse; // 3 elements a, b, c: ax^2+2bxy+cy^2=1
    Size_<float> axes; // half lenght of elipse axes
    Size_<float> boundingBox; // half sizes of bounding box which sides are parallel to the coordinate axes
};

EllipticKeyPoint::EllipticKeyPoint()
{
    *this = EllipticKeyPoint(Point2f(0,0), Scalar(1, 0, 1) );
}

EllipticKeyPoint::EllipticKeyPoint( const Point2f& _center, const Scalar& _ellipse )
{
    center = _center;
    ellipse = _ellipse;

    Mat_<double> M = getSecondMomentsMatrix(_ellipse), eval;
    eigen( M, eval );
    assert( eval.rows == 2 && eval.cols == 1 );
    axes.width = 1.f / (float)sqrt(eval(0,0));
    axes.height = 1.f / (float)sqrt(eval(1,0));

    double ac_b2 = ellipse[0]*ellipse[2] - ellipse[1]*ellipse[1];
    boundingBox.width = (float)sqrt(ellipse[2]/ac_b2);
    boundingBox.height = (float)sqrt(ellipse[0]/ac_b2);
}

Mat_<double> EllipticKeyPoint::getSecondMomentsMatrix( const Scalar& _ellipse )
{
    Mat_<double> M(2, 2);
    M(0,0) = _ellipse[0];
    M(1,0) = M(0,1) = _ellipse[1];
    M(1,1) = _ellipse[2];
    return M;
}

Mat_<double> EllipticKeyPoint::getSecondMomentsMatrix() const
{
    return getSecondMomentsMatrix(ellipse);
}

void EllipticKeyPoint::calcProjection( const Mat_<double>& H, EllipticKeyPoint& projection ) const
{
    Point2f dstCenter = applyHomography(H, center);

    Mat_<double> invM; invert(getSecondMomentsMatrix(), invM);
    Mat_<double> Aff; linearizeHomographyAt(H, center, Aff);
    Mat_<double> dstM; invert(Aff*invM*Aff.t(), dstM);

    projection = EllipticKeyPoint( dstCenter, Scalar(dstM(0,0), dstM(0,1), dstM(1,1)) );
}

void EllipticKeyPoint::convert( const vector<KeyPoint>& src, vector<EllipticKeyPoint>& dst )
{
    if( !src.empty() )
    {
        dst.resize(src.size());
        for( size_t i = 0; i < src.size(); i++ )
        {
            float rad = src[i].size/2;
            assert( rad );
            float fac = 1.f/(rad*rad);
            dst[i] = EllipticKeyPoint( src[i].pt, Scalar(fac, 0, fac) );
        }
    }
}

void EllipticKeyPoint::convert( const vector<EllipticKeyPoint>& src, vector<KeyPoint>& dst )
{
    if( !src.empty() )
    {
        dst.resize(src.size());
        for( size_t i = 0; i < src.size(); i++ )
        {
            Size_<float> axes = src[i].axes;
            float rad = sqrt(axes.height*axes.width);
            dst[i] = KeyPoint(src[i].center, 2*rad );
        }
    }
}

void EllipticKeyPoint::calcProjection( const vector<EllipticKeyPoint>& src, const Mat_<double>& H, vector<EllipticKeyPoint>& dst )
{
    if( !src.empty() )
    {
        assert( !H.empty() && H.cols == 3 && H.rows == 3);
        dst.resize(src.size());
        vector<EllipticKeyPoint>::const_iterator srcIt = src.begin();
        vector<EllipticKeyPoint>::iterator       dstIt = dst.begin();
        for( ; srcIt != src.end(); ++srcIt, ++dstIt )
            srcIt->calcProjection(H, *dstIt);
    }
}

static void filterEllipticKeyPointsByImageSize( vector<EllipticKeyPoint>& keypoints, const Size& imgSize )
{
    if( !keypoints.empty() )
    {
        vector<EllipticKeyPoint> filtered;
        filtered.reserve(keypoints.size());
        vector<EllipticKeyPoint>::const_iterator it = keypoints.begin();
        for( int i = 0; it != keypoints.end(); ++it, i++ )
        {
            if( it->center.x + it->boundingBox.width < imgSize.width &&
                it->center.x - it->boundingBox.width > 0 &&
                it->center.y + it->boundingBox.height < imgSize.height &&
                it->center.y - it->boundingBox.height > 0 )
                filtered.push_back(*it);
        }
        keypoints.assign(filtered.begin(), filtered.end());
    }
}

static void overlap( const vector<EllipticKeyPoint>& keypoints1, const vector<EllipticKeyPoint>& keypoints2t, bool commonPart,
                     SparseMat_<float>& overlaps )
{
    overlaps.clear();
    if( keypoints1.empty() || keypoints2t.empty() )
        return;

    int size[] = { keypoints1.size(), keypoints2t.size() };
    overlaps.create( 2, size );

    for( size_t i1 = 0; i1 < keypoints1.size(); i1++ )
    {
        EllipticKeyPoint kp1 = keypoints1[i1];
        float maxDist = sqrt(kp1.axes.width*kp1.axes.height),
              fac = 30.f/maxDist;
        if( !commonPart )
            fac=3;

        maxDist = maxDist*4;
        fac = 1.f/(fac*fac);

        EllipticKeyPoint keypoint1a = EllipticKeyPoint( kp1.center, Scalar(fac*kp1.ellipse[0], fac*kp1.ellipse[1], fac*kp1.ellipse[2]) );

        for( size_t i2 = 0; i2 < keypoints2t.size(); i2++ )
        {
            EllipticKeyPoint kp2 = keypoints2t[i2];
            Point2f diff = kp2.center - kp1.center;

            if( norm(diff) < maxDist )
            {
                EllipticKeyPoint keypoint2a = EllipticKeyPoint( kp2.center, Scalar(fac*kp2.ellipse[0], fac*kp2.ellipse[1], fac*kp2.ellipse[2]) );
                //find the largest eigenvalue
                float maxx =  ceil(( keypoint1a.boundingBox.width > (diff.x+keypoint2a.boundingBox.width)) ?
                                     keypoint1a.boundingBox.width : (diff.x+keypoint2a.boundingBox.width));
                float minx = floor((-keypoint1a.boundingBox.width < (diff.x-keypoint2a.boundingBox.width)) ?
                                    -keypoint1a.boundingBox.width : (diff.x-keypoint2a.boundingBox.width));

                float maxy =  ceil(( keypoint1a.boundingBox.height > (diff.y+keypoint2a.boundingBox.height)) ?
                                     keypoint1a.boundingBox.height : (diff.y+keypoint2a.boundingBox.height));
                float miny = floor((-keypoint1a.boundingBox.height < (diff.y-keypoint2a.boundingBox.height)) ?
                                    -keypoint1a.boundingBox.height : (diff.y-keypoint2a.boundingBox.height));
                float mina = (maxx-minx) < (maxy-miny) ? (maxx-minx) : (maxy-miny) ;
                float dr = mina/50.f;
                float bua = 0.f, bna = 0.f;
                //compute the area
                for( float rx1 = minx; rx1 <= maxx; rx1+=dr )
                {
                    float rx2 = rx1-diff.x;
                    for( float ry1=miny; ry1<=maxy; ry1+=dr )
                    {
                        float ry2=ry1-diff.y;
                        //compute the distance from the ellipse center
                        float e1 = (float)(keypoint1a.ellipse[0]*rx1*rx1+2*keypoint1a.ellipse[1]*rx1*ry1+keypoint1a.ellipse[2]*ry1*ry1);
                        float e2 = (float)(keypoint2a.ellipse[0]*rx2*rx2+2*keypoint2a.ellipse[1]*rx2*ry2+keypoint2a.ellipse[2]*ry2*ry2);
                        //compute the area
                        if( e1<1 && e2<1 ) bna++;
                        if( e1<1 || e2<1 ) bua++;
                    }
                }
                if( bna > 0)
                    overlaps.ref(i1,i2) = bna/bua;
            }
        }
    }
}

static void calculateRepeatability( const Mat& img1, const Mat& img2, const Mat& H1to2,
                                    const vector<KeyPoint>& _keypoints1, const vector<KeyPoint>& _keypoints2,
                                    float& repeatability, int& correspondencesCount,
                                    SparseMat_<uchar>* thresholdedOverlapMask=0 )
{
    vector<EllipticKeyPoint> keypoints1, keypoints2, keypoints1t, keypoints2t;
    EllipticKeyPoint::convert( _keypoints1, keypoints1 );
    EllipticKeyPoint::convert( _keypoints2, keypoints2 );

    // calculate projections of key points
    EllipticKeyPoint::calcProjection( keypoints1, H1to2, keypoints1t );
    Mat H2to1; invert(H1to2, H2to1);
    EllipticKeyPoint::calcProjection( keypoints2, H2to1, keypoints2t );

    bool ifEvaluateDetectors = !thresholdedOverlapMask; // == commonPart
    float overlapThreshold;
    if( ifEvaluateDetectors )
    {
        overlapThreshold = 1.f - 0.4f;

        // remove key points from outside of the common image part
        Size sz1 = img1.size(), sz2 = img2.size();
        filterEllipticKeyPointsByImageSize( keypoints1, sz1 );
        filterEllipticKeyPointsByImageSize( keypoints1t, sz2 );
        filterEllipticKeyPointsByImageSize( keypoints2, sz2 );
        filterEllipticKeyPointsByImageSize( keypoints2t, sz1 );
    }
    else
    {
        overlapThreshold = 1.f - 0.5f;
    }
    int minCount = min( keypoints1.size(), keypoints2t.size() );

    // calculate overlap errors
    SparseMat_<float> overlaps;
    overlap( keypoints1, keypoints2t, ifEvaluateDetectors, overlaps );

    correspondencesCount = -1;
    repeatability = -1.f;
    const int* size = overlaps.size();
    if( !size || overlaps.nzcount() == 0 )
        return;

    if( ifEvaluateDetectors )
    {
        // threshold the overlaps
        for( int y = 0; y < size[0]; y++ )
        {
            for( int x = 0; x < size[1]; x++ )
            {
                if ( overlaps(y,x) < overlapThreshold )
                    overlaps.erase(y,x);
            }
        }

        // regions one-to-one matching
        correspondencesCount = 0;
        while( overlaps.nzcount() > 0 )
        {
            double maxOverlap = 0;
            int maxIdx[2];
            minMaxLoc( overlaps, 0, &maxOverlap, 0, maxIdx );
            for( size_t i1 = 0; i1 < keypoints1.size(); i1++ )
                overlaps.erase(i1, maxIdx[1]);
            for( size_t i2 = 0; i2 < keypoints2t.size(); i2++ )
                overlaps.erase(maxIdx[0], i2);
            correspondencesCount++;
        }
        repeatability = minCount ? (float)correspondencesCount/minCount : -1;
    }
    else
    {
        thresholdedOverlapMask->create( 2, size );
        for( int y = 0; y < size[0]; y++ )
        {
            for( int x = 0; x < size[1]; x++ )
            {
                float val = overlaps(y,x);
                if ( val >= overlapThreshold )
                    thresholdedOverlapMask->ref(y,x) = 1;
            }
        }
    }
}

void cv::evaluateFeatureDetector( const Mat& img1, const Mat& img2, const Mat& H1to2,
                              vector<KeyPoint>* _keypoints1, vector<KeyPoint>* _keypoints2,
                              float& repeatability, int& correspCount,
                              const Ptr<FeatureDetector>& _fdetector )
{
    Ptr<FeatureDetector> fdetector(_fdetector);
    vector<KeyPoint> *keypoints1, *keypoints2, buf1, buf2;
    keypoints1 = _keypoints1 != 0 ? _keypoints1 : &buf1;
    keypoints2 = _keypoints2 != 0 ? _keypoints2 : &buf2;

    if( (keypoints1->empty() || keypoints2->empty()) && fdetector.empty() )
        CV_Error( CV_StsBadArg, "fdetector must be no empty when keypoints1 or keypoints2 is empty" );

    if( keypoints1->empty() )
        fdetector->detect( img1, *keypoints1 );
    if( keypoints2->empty() )
        fdetector->detect( img2, *keypoints2 );

    calculateRepeatability( img1, img2, H1to2, *keypoints1, *keypoints2, repeatability, correspCount );
}

struct DMatchForEvaluation : public DMatch
{
    uchar isCorrect;
    DMatchForEvaluation( const DMatch &dm ) : DMatch( dm ) {}
};

static inline float recall( int correctMatchCount, int correspondenceCount )
{
    return correspondenceCount ? (float)correctMatchCount / (float)correspondenceCount : -1;
}

static inline float precision( int correctMatchCount, int falseMatchCount )
{
    return correctMatchCount + falseMatchCount ? (float)correctMatchCount / (float)(correctMatchCount + falseMatchCount) : -1;
}

void cv::computeRecallPrecisionCurve( const vector<vector<DMatch> >& matches1to2,
                                      const vector<vector<uchar> >& correctMatches1to2Mask,
                                      vector<Point2f>& recallPrecisionCurve )
{
    CV_Assert( matches1to2.size() == correctMatches1to2Mask.size() );

    vector<DMatchForEvaluation> allMatches;
    int correspondenceCount = 0;
    for( size_t i = 0; i < matches1to2.size(); i++ )
    {
        for( size_t j = 0; j < matches1to2[i].size(); j++ )
        {
            DMatchForEvaluation match = matches1to2[i][j];
            match.isCorrect = correctMatches1to2Mask[i][j] ;
            allMatches.push_back( match );
            correspondenceCount += match.isCorrect != 0 ? 1 : 0;
        }
    }

    std::sort( allMatches.begin(), allMatches.end() );

    int correctMatchCount = 0, falseMatchCount = 0;
    recallPrecisionCurve.resize( allMatches.size() );
    for( size_t i = 0; i < allMatches.size(); i++ )
    {
        if( allMatches[i].isCorrect )
            correctMatchCount++;
        else
            falseMatchCount++;

        float r = recall( correctMatchCount, correspondenceCount );
        float p =  precision( correctMatchCount, falseMatchCount );
        recallPrecisionCurve[i] = Point2f(1-p, r);
    }
}

float cv::getRecall( const vector<Point2f>& recallPrecisionCurve, float l_precision )
{
    float recall = -1;

    if( l_precision >= 0 && l_precision <= 1 )
    {
        int bestIdx = -1;
        float minDiff = FLT_MAX;
        for( size_t i = 0; i < recallPrecisionCurve.size(); i++ )
        {
            float curDiff = std::fabs(l_precision - recallPrecisionCurve[i].x);
            if( curDiff <= minDiff )
            {
                bestIdx = i;
                minDiff = curDiff;
            }
        }

        recall = recallPrecisionCurve[bestIdx].y;
    }

    return recall;
}

void cv::evaluateGenericDescriptorMatcher( const Mat& img1, const Mat& img2, const Mat& H1to2,
                                           vector<KeyPoint>& keypoints1, vector<KeyPoint>& keypoints2,
                                           vector<vector<DMatch> >* _matches1to2, vector<vector<uchar> >* _correctMatches1to2Mask,
                                           vector<Point2f>& recallPrecisionCurve,
                                           const Ptr<GenericDescriptorMatch>& _dmatch )
{
    Ptr<GenericDescriptorMatch> dmatch = _dmatch;
    dmatch->clear();

    vector<vector<DMatch> > *matches1to2, buf1;
    vector<vector<uchar> > *correctMatches1to2Mask, buf2;
    matches1to2 = _matches1to2 != 0 ? _matches1to2 : &buf1;
    correctMatches1to2Mask = _correctMatches1to2Mask != 0 ? _correctMatches1to2Mask : &buf2;

    if( keypoints1.empty() )
        CV_Error( CV_StsBadArg, "keypoints1 must be no empty" );

    if( matches1to2->empty() && dmatch.empty() )
        CV_Error( CV_StsBadArg, "dmatch must be no empty when matches1to2 is empty" );

    bool computeKeypoints2ByPrj = keypoints2.empty();
    if( computeKeypoints2ByPrj )
    {
        assert(0);
        // TODO: add computing keypoints2 from keypoints1 using H1to2
    }

    if( matches1to2->empty() || computeKeypoints2ByPrj )
    {
        dmatch->clear();
        dmatch->add( img2, keypoints2 );
        // TODO: use more sophisticated strategy to choose threshold
        dmatch->match( img1, keypoints1, *matches1to2, std::numeric_limits<float>::max() );
    }
    float repeatability;
    int correspCount;
    SparseMat_<uchar> thresholdedOverlapMask; // thresholded allOverlapErrors
    calculateRepeatability( img1, img2, H1to2,
                            keypoints1, keypoints2,
                            repeatability, correspCount,
                            &thresholdedOverlapMask );

    correctMatches1to2Mask->resize(matches1to2->size());
    int ddd = 0;
    for( size_t i = 0; i < matches1to2->size(); i++ )
    {
        (*correctMatches1to2Mask)[i].resize((*matches1to2)[i].size());
        for( size_t j = 0;j < (*matches1to2)[i].size(); j++ )
        {
            int indexQuery = (*matches1to2)[i][j].indexQuery;
            int indexTrain = (*matches1to2)[i][j].indexTrain;
            (*correctMatches1to2Mask)[i][j] = thresholdedOverlapMask( indexQuery, indexTrain );
            ddd += thresholdedOverlapMask( indexQuery, indexTrain ) != 0 ? 1 : 0;
        }
    }

    computeRecallPrecisionCurve( *matches1to2, *correctMatches1to2Mask, recallPrecisionCurve );
}