def do_phasecong_prep(radius,theta,im,nscale,minWaveLength,mult,sigmaOnf):
"""docstring for do_phasecong_prep"""
radius = ifftshift(radius) # Quadrant shift radius and theta so that filters
theta = ifftshift(theta) # are constructed with 0 frequency at the corners.
radius[0,0] = 1 # Get rid of the 0 radius value at the 0
# frequency point (now at top-left corner)
# so that taking the log of the radius will
# not cause trouble.
sintheta = sin(theta)
costheta = cos(theta)
# Filters are constructed in terms of two components.
# 1) The radial component, which controls the frequency band that the filter
# responds to
# 2) The angular component, which controls the orientation that the filter
# responds to.
# The two components are multiplied together to construct the overall filter.
# Construct the radial filter components...
# First construct a low-pass filter that is as large as possible, yet falls
# away to zero at the boundaries. All log Gabor filters are multiplied by
# this to ensure no extra frequencies at the 'corners' of the FFT are
# incorporated as this seems to upset the normalisation process when
# calculating phase congrunecy.
lp = lowpassfilter(im.shape,.45,15) # Radius .45, 'sharpness' 15
logGabor = [] #zeros((1,nscale))
for s in range(nscale):
wavelength = minWaveLength*mult**(s)
fo = 1.0/wavelength # Centre frequency of filter.
logGabor.append( exp((-(log(radius/fo))**2) / (2 * log(sigmaOnf)**2)) )
logGabor[s] = logGabor[s]*lp # Apply low-pass filter
logGabor[s][0,0] = 0 # Set the value at the 0 frequency point of the filter
# back to zero (undo the radius fudge).
return (sintheta,costheta,logGabor)
print('IMU ready!')
#Kalman filter parameters for a 1-dim case x 3
#dimension with error distribution = gaussian
#
A = numpy.eye(3)
H = numpy.eye(3)
B = numpy.eye(3) * 0
Q = numpy.eye(3) * 0.001
#play with Q to tune the smoothness
R = numpy.eye(3) * 0.01
xhat = numpy.matrix([[0], [0], [0]])
P = numpy.eye(3)
kf = KalmanFilterLinear(A, B, H, xhat, P, Q, R)
lpf = lowpassfilter(0.025)
fax = 0
fay = 0
faz = 0
fgx = 0
fgy = 0
fgz = 0
faxk = 0
fayk = 0
fazk = 0
faxf = 0
fayf = 0
fazf = 0
rg = 0
pg = 0
import cv2
import sys
from serial import Serial
import threading
import time
from struct import pack, unpack
from lowpassfilter import lowpassfilter
currentx = 0
lpf = lowpassfilter(0.5)
#connect function
def connect():
try:
conn = Serial('/dev/cu.usbmodem14401',
baudrate=9600,
dsrdtr=0,
rtscts=0,
timeout=1) #cu.usbmodemFA131
except IOError:
print("Error opening serial port.")
sys.exit(2)
return conn
#get square with largest area
def largestArea(faces):
largestx = 500
largesty = 0
def initLowpass(self):
""" initializes the low pass metrics
"""
self.lpf=lowpassfilter(0.5)
self.metrics['lowpass'] = IMUMetric('lowpass',0, Setting.get('imu_archive_low',False))
def initLowpass(self):
self.lpf=lowpassfilter(0.5)
self.metrics['lowpass'] = IMUMetric('lowpass',0, Setting.get('imu_archive_low',False))
#Kalman filter parameters for a 1-dim case x 3 #dimension with error distribution = gaussian # A = numpy.eye(3) H = numpy.eye(3) B = numpy.eye(3)*0 Q = numpy.eye(3)*0.001 #play with Q to tune the smoothness R = numpy.eye(3)*0.01 xhat = numpy.matrix([[0],[0],[0]]) P= numpy.eye(3) kf = KalmanFilterLinear(A,B,H,xhat,P,Q,R) lpf=lowpassfilter(0.025) fax=0 fay=0 faz=0 fgx=0 fgy=0 fgz=0 faxk=0 fayk=0 fazk=0 faxf=0 fayf=0 fazf=0 rg=0 pg=0
