def _ecefToLocal(self, ecef):
"""
Converts ECEF np.Array to a 2d np.Array in local coordinate space.
"""
ecef -= self.localZECEF
return np.array((np.dot(self.localUnitXECEF,
ecef), np.dot(self.localUnitYECEF, ecef)),
np.double)
def __init__(self, referenceGPS):
self.localZECEF = self._gpsToECEF(referenceGPS)
localUnitZECEF = self.localZECEF / np.linalg.norm(self.localZECEF)
unitToPole = calcs.unit(northPoleECEF - self.localZECEF)
self.localUnitYECEF = unitToPole - localUnitZECEF * np.dot(
unitToPole, localUnitZECEF)
self.localUnitYECEF = calcs.unit(self.localUnitYECEF)
self.localUnitXECEF = np.cross(self.localUnitYECEF, localUnitZECEF)
self.localUnitZECEF = calcs.unit(self.localZECEF)
def MeasureSimilarity(img1, img2):
if img1.size != img2.size:
Announce('Wrong measurement')
_img1_reshaped = img1.reshape(img1.size).astype(float)
_img2_reshaped = img2.reshape(img2.size).astype(float)
sub = np.subtract(_img1_reshaped, _img2_reshaped)
ones = np.ones(sub.size)
result = np.dot(sub, ones)
return result
def AddMyFeatures_Soroosh(filename, cap, frame, configObject):
my_results = []
if len(so_features) == 0:
ReadFeatures(configObject['inputfeatures'])
_frame_blur = ApplyBlurImage(frame)
_frame_blur_gray = cv2.cvtColor(cv2.cvtColor(_frame_blur, cv2.COLOR_BGR2GRAY), cv2.COLOR_GRAY2BGR)
_standwindow = ReadStandWindow(frame)
_midwindow = ReadMidWindow(frame)
_highwindow = ReadHighWindow(frame)
_standwindow_blur = ReadStandWindow(_frame_blur)
_midwindow_blur = ReadMidWindow(_frame_blur)
_highwindow_blur = ReadHighWindow(_frame_blur)
_standwindow_blur_gray = ReadStandWindow(_frame_blur_gray)
_midwindow_blur_gray = ReadMidWindow(_frame_blur_gray)
_highwindow_blur_gray = ReadHighWindow(_frame_blur_gray)
_keys = so_features.keys()
for _key in sorted(_keys):
if 'feature_stand' in _key:
if 'color' in _key:
my_results.append(MeasureSimilarity(cv2.cvtColor(cv2.cvtColor(so_features[_key], cv2.COLOR_BGR2GRAY), cv2.COLOR_GRAY2BGR),_standwindow_blur_gray))
else:
my_results.append(MeasureSimilarity(so_features[_key],_standwindow_blur_gray))
if 'feature_mid' in _key:
if 'color' in _key:
my_results.append(MeasureSimilarity(cv2.cvtColor(cv2.cvtColor(so_features[_key], cv2.COLOR_BGR2GRAY), cv2.COLOR_GRAY2BGR),_midwindow_blur_gray))
else:
my_results.append(MeasureSimilarity(so_features[_key],_midwindow_blur_gray))
if 'feature_high' in _key:
if 'color' in _key:
my_results.append(MeasureSimilarity(cv2.cvtColor(cv2.cvtColor(so_features[_key], cv2.COLOR_BGR2GRAY), cv2.COLOR_GRAY2BGR),_highwindow_blur_gray))
else:
my_results.append(MeasureSimilarity(so_features[_key],_highwindow_blur_gray))
#Announce('Filter: {0} : Applied. {1}'.format(_key, my_results))
reshaped_1 = _standwindow.reshape(_standwindow.size).astype(float)
#RGB of stand
ss_1_1 = reshaped_1[0:_standwindow.size:3]
ss_1_2 = reshaped_1[1:_standwindow.size:3]
ss_1_3 = reshaped_1[2:_standwindow.size:3]
ones1_1 = np.ones(ss_1_1.size)
ones1_2 = np.ones(ss_1_2.size)
ones1_3 = np.ones(ss_1_3.size)
sum1_1 = np.dot(ss_1_1, ones1_1)
sum1_2 = np.dot(ss_1_2, ones1_2)
sum1_3 = np.dot(ss_1_3, ones1_3)
my_results.append(sum1_1)
my_results.append(sum1_2)
my_results.append(sum1_3)
#RGB of mid
ss_2_1 = reshaped_1[0:_midwindow.size:3]
ss_2_2 = reshaped_1[1:_midwindow.size:3]
ss_2_3 = reshaped_1[2:_midwindow.size:3]
ones2_1 = np.ones(ss_2_1.size)
ones2_2 = np.ones(ss_2_2.size)
ones2_3 = np.ones(ss_2_3.size)
sum2_1 = np.dot(ss_2_1, ones2_1)
sum2_2 = np.dot(ss_2_2, ones2_2)
sum2_3 = np.dot(ss_2_3, ones2_3)
my_results.append(sum2_1)
my_results.append(sum2_2)
my_results.append(sum2_3)
#RGB of high
ss_3_1 = reshaped_1[0:_midwindow.size:3]
ss_3_2 = reshaped_1[1:_midwindow.size:3]
ss_3_3 = reshaped_1[2:_midwindow.size:3]
ones3_1 = np.ones(ss_3_1.size)
ones3_2 = np.ones(ss_3_2.size)
ones3_3 = np.ones(ss_3_3.size)
sum3_1 = np.dot(ss_3_1, ones3_1)
sum3_2 = np.dot(ss_3_2, ones3_2)
sum3_3 = np.dot(ss_3_3, ones3_3)
my_results.append(sum3_1)
my_results.append(sum3_2)
my_results.append(sum3_3)
return my_results
def turnTime(direction, desiredDirection, speed, acceleration):
# TODO: min should not be necessary (float64)
turnAngleCos = min(1.0, np.dot(direction, desiredDirection))
turnAngle = math.acos(turnAngleCos)
return turnAngle * speed / acceleration
