"""
Test of feature detectors using opencv used to generate the times shown in table 1
"""
#%%-----------------------------------------------------------------------------
# imports and load data
#-------------------------------------------------------------------------------
import numpy as np
import time
import cv2

imgs = np.load('random positions.npy')

#%%-----------------------------------------------------------------------------
# #Harris
#-------------------------------------------------------------------------------
print 'Doing: Harris corners'
maxCorners = 500
qualityLevel = 0.01
minDistance = 1
blockSize = 3 #KK used 3 the default
useHarris = True
Harris_k = 0.04
harris = cv2.GFTTDetector_create(maxCorners, qualityLevel, minDistance, blockSize, useHarris, Harris_k)

#times for harris method
Ns = np.arange( 10, 510, 10)
t_harris = []
for N in Ns:
    # setup detector
    harris.setMaxFeatures(N)
    
    #average time
    tic = time.time()
    for n in xrange(0,100):
        kps = harris.detect(imgs[n])
    
    toc =time.time()
    t = (toc-tic)/(n+1) * 1000 
    t_harris.append( t)
    print 'N features : ',N, ' = ', t, 'ms per frame'
    
print 'min, max, mean ',np.min(t_harris), np.max(t_harris), np.mean(t_harris)
print '-------------------------------------------------------------------'

#%%-----------------------------------------------------------------------------
# GFTT / Shi-Tomasi
#-------------------------------------------------------------------------------
print 'Doing: GFTT'
maxCorners = 500
qualityLevel = 0.01
minDistance = 1
blockSize = 3 #KK used 3 the default
useHarris = False
gftt = cv2.GFTTDetector_create(maxCorners, qualityLevel, minDistance, blockSize, useHarris)

#times for GFTT method
Ns = np.arange( 10, 510, 10)
t_gftt = []
for N in Ns:
    # setup detector
    gftt.setMaxFeatures(N)
    
    #average time
    tic = time.time()
    for n in xrange(0,100):
        kps = gftt.detect(imgs[n])
    
    toc =time.time()
    t = (toc-tic)/(n+1) * 1000 
    t_gftt.append( t)
    print 'N features : ',N, ' = ', t, 'ms per frame'


print 'min, max, mean ',np.min(t_gftt), np.max(t_gftt), np.mean(t_gftt)
print '-------------------------------------------------------------------'

#%%-----------------------------------------------------------------------------
# Fast detector
#-------------------------------------------------------------------------------
print 'Doing: FAST'

threshold = 255
nonmaxSuppression = True
fast = cv2.FastFeatureDetector_create( threshold, nonmaxSuppression, cv2.FastFeatureDetector_TYPE_9_16)

#times for FAST method
Ns = np.arange( 10, 2010, 10)
t_fast = []
for N in Ns:
    #find approx threshold for first image
    kps = fast.detect(imgs[0])
    while len(kps) < N:
        threshold = threshold - 1
        fast.setThreshold(threshold)
        kps = fast.detect(imgs[0])
    print 'using threshold: ',threshold, 'Number of features: ', len(kps)
    
    #average time
    tic = time.time()
    for n in xrange(0,100):
        kps = fast.detect(imgs[n])
    toc =time.time()
    t = (toc-tic)/(n+1) * 1000 
    t_fast.append( t)
    print 'N features : ',N, ' = ', t, 'ms per frame'
    
print 'min, max, mean ',np.min(t_fast), np.max(t_fast), np.mean(t_fast)
print '-------------------------------------------------------------------'

#%%-----------------------------------------------------------------------------
# AGAST detector
#-------------------------------------------------------------------------------
print 'Doing: AGAST'

threshold = 255
nonmaxSuppression = True
agast = cv2.AgastFeatureDetector_create( threshold, nonmaxSuppression, cv2.FastFeatureDetector_TYPE_9_16)

#times for AGAST method
Ns = np.arange( 10, 510, 10)
t_agast = []
for N in Ns:
    #find approx threshold for first image
    kps = agast.detect(imgs[0])
    while len(kps) < N:
        threshold = threshold - 1
        agast.setThreshold(threshold)
        kps = agast.detect(imgs[0])
    print 'using threshold: ',threshold, 'Number of features: ', len(kps)
    
    #average time
    tic = time.time()
    for n in xrange(0,100):
        kps = agast.detect(imgs[n])
    toc =time.time()
    t = (toc-tic)/(n+1) * 1000 
    t_agast.append( t)
    print 'N features : ',N, ' = ', t, 'ms per frame'

print 'min, max, mean ',np.min(t_agast), np.max(t_agast), np.mean(t_agast)
print '-------------------------------------------------------------------'

#%%-----------------------------------------------------------------------------
# USURF - Difference of Gaussians detector
#-------------------------------------------------------------------------------
print 'Doing: USURF'

threshold =  2000 #KK used varibale threshold 300 - 50000
nOctaves = 1        #KK =1 no image rescaling 4 by default
nOctaveLayers = 1 #KK used three default is 1 the minimum?
extended = False
upright = True
usurf = cv2.xfeatures2d.SURF_create(threshold, nOctaves, nOctaveLayers ,extended, upright)

#times for U-SURF method
Ns = np.arange( 10, 510, 10)
t_usurf = []


for N in Ns:
    #find approx threshold for first image
    kps = usurf.detect(imgs[0])
    while len(kps) < N:
        threshold = threshold - 1
        usurf.setHessianThreshold(threshold)
        kps = usurf.detect(imgs[0])
    print 'using threshold: ',threshold, 'Number of features: ', len(kps)
    
    #average time
    tic = time.time()
    for n in xrange(0,100):
        kps = usurf.detect(imgs[n])
    toc =time.time()
    t = (toc-tic)/(n+1) * 1000 
    t_usurf.append( t)
    print 'N features : ',N, ' = ', t, 'ms per frame'

print 'min, max, mean ',np.min(t_usurf), np.max(t_usurf), np.mean(t_usurf)
print '-------------------------------------------------------------------'

#%%-----------------------------------------------------------------------------
# ORB - orientated FAST and BRIEF descriptor
#-------------------------------------------------------------------------------
print 'Doing: ORB'

nFeatures= 500
scaleFactor = 1.2   #pyramid scale factor
nLevels = 1       #pyramid levels 1 as no scale changes
edgeThreshold = 0 #size of border to not detect in
firstLevel = 0  #zero from opencv docs
#for descriptors
WTA_K = 2       #he number of points that produce each element of the oriented BRIEF descriptor. The default value 2 means the BRIEF where we take a random point pair and compare their brightnesses, so we get 0/1 response (index of brightest)
scoreType = cv2.ORB_HARRIS_SCORE #or ORB_FAST_SCORE
patchSize = 31  #size of patch used to find BRIEF dscriptor
fastThreshold = 50

orb = cv2.ORB_create(nFeatures, scaleFactor, nLevels, edgeThreshold, firstLevel, WTA_K, scoreType, patchSize, fastThreshold)
#work around for orb bug - this needs to be called first
cv2.ocl.setUseOpenCL(False)

#times for ORB / oFAST  method
Ns = np.arange( 10, 510, 10)
t_ofast = []
threshold = 255
orb.setFastThreshold(threshold)
for N in Ns:
    #find approx threshold for first image
    kps = []
    kps = orb.detect(imgs[0])
    while len(kps) < N:
        threshold = threshold - 1
        orb.setFastThreshold(threshold)
        kps = orb.detect(imgs[0])
        #print 'threshold: ',threshold, 'found: ', len(kps)

    print 'using threshold: ',threshold, 'found: ', len(kps)
    #average time
    tic = time.time()
    for n in xrange(0,100):
        kps = orb.detect(imgs[n])
    toc =time.time()
    t = (toc-tic)/(n+1) * 1000 
    t_ofast.append( t)
    print 'N features : ',N, ' = ', t, 'ms per frame'

print 'min, max, mean ',np.min(t_ofast), np.max(t_ofast), np.mean(t_ofast)
print '-------------------------------------------------------------------'

#%%-----------------------------------------------------------------------------
# BRISK (orientated fast/agast like detector)
#-------------------------------------------------------------------------------
print 'Doing: BRISK'

octaves = 0         #detection octaves. Use 0 to do single scale.
patternScale = 1.8  #apply this scale to the pattern used for sampling the neighbourhood of a keypoint.
                    #in the paper the pattern is approximately 1 = +-18px, 1.8=+-32
threshold = 91     #FAST threshold
brisk = cv2.BRISK_create(threshold, octaves, patternScale)

#times for brisk / oAGAST  method
Ns = np.arange( 10, 510, 10)
t_brisk = []
for N in Ns:
    #find approx threshold for first image
    kps = brisk.detect(imgs[0])
    while len(kps) < N:
        threshold = threshold - 1
        brisk = cv2.BRISK_create(threshold, octaves, patternScale)
        kps = brisk.detect(imgs[0])
    print 'using threshold: ',threshold, 'found: ', len(kps)
    
    #average time
    tic = time.time()
    for n in xrange(0,100):
        kps = brisk.detect(imgs[n])
    toc =time.time()
    t = (toc-tic)/(n+1) * 1000 
    t_brisk.append( t)
    print 'N features : ',N, ' = ', t, 'ms per frame'
    
print 'min, max, mean ',np.min(t_brisk), np.max(t_brisk), np.mean(t_brisk)
print '-------------------------------------------------------------------'

#%%-----------------------------------------------------------------------------
# SURF - orientated Difference of Gaussians detector
#-------------------------------------------------------------------------------
print 'Doing: SURF'

threshold =  2000 #KK used varibale threshold 300 - 50000
nOctaves = 1        #KK =1 no image rescaling 4 by default
nOctaveLayers = 1 #KK used three default is 1 the minimum?
extended = False
upright = False
surf = cv2.xfeatures2d.SURF_create(threshold, nOctaves, nOctaveLayers ,extended, upright)

#times for SURF method
Ns = np.arange( 10, 510, 10)
t_surf = []

for N in Ns:
    #find approx threshold for first image
    kps = surf.detect(imgs[0])
    while len(kps) < N:
        threshold = threshold - 1
        surf.setHessianThreshold(threshold)
        kps = surf.detect(imgs[0])
    print 'using threshold: ',threshold, 'Number of features: ', len(kps)
    
    #average time
    tic = time.time()
    for n in xrange(0,100):
        kps = surf.detect(imgs[n])
    toc =time.time()
    t = (toc-tic)/(n+1) * 1000 
    t_surf.append( t)
    print 'N features : ',N, ' = ', t, 'ms per frame'
    
print 'min, max, mean ',np.min(t_surf), np.max(t_surf), np.mean(t_surf)
print '-------------------------------------------------------------------'