def homography(self,last_puzzle,puzzle,warp,maxCorners=5000,qualityLevel=.01,minDistance=3):
#Detect
f_feats = goodFeaturesToTrack(frame,maxCorners,qualityLevel,minDistance)#,mask=self.orb_mask)
#Extract
f_kps = [KeyPoint(x=f[0][0],y=f[0][1],_size=20) for f in f_feats]
f_kps,f_des = self.orb.compute(frame,f_kps)
#Detect
w_feats = goodFeaturesToTrack(warp,maxCorners,qualityLevel,minDistance)#,mask=self.orb_mask)
#Extract
w_kps = [KeyPoint(x=f[0][0],y=f[0][1],_size=20) for f in w_feats]
w_kps,w_des = self.orb.compute(warp,w_kps)
#Match
ret = []
# #THE QUERY IMAGE IS THE ACTUAL IMAGE &
# #THE TRAIN IMAGE IS THE LAST IMAGE
# #U STOUPID KUNT
matches = self.bfm.knnMatch(w_des,f_des,k=2)
for m,n in matches:
# if m.distance <= 1 * n.distance:
ret.append((w_kps[m.queryIdx].pt,f_kps[m.trainIdx].pt))
#filter
Rt = None
H, mask, warp_pts, orig_pts = None,None,None,None
if len(ret) > 0:
warp_pts = float32([r[0] for r in ret]).reshape(-1,1,2)
orig_pts = float32([r[1] for r in ret]).reshape(-1,1,2)
H,mask = findHomography(warp_pts,orig_pts,RANSAC,5.0)
return H,mask,warp_pts,orig_pts
def kp_and_des_for_blank_image(im, descriptor_extractor):
h, w = im.shape[:2]
x, y = (int(w / 2), int(h / 2))
size = 1
kp = KeyPoint(x, y, size, -1, 0, 0, -1)
des = np.zeros((descriptor_extractor.descriptorSize()), dtype=np.uint8)
return [kp], [des]
def load_ground_truth(self, ground_truth_file_path):
# type: (str) -> ([], [])
"""
TODO docs
"""
# generate file path
file_path = self.package_path + ground_truth_file_path
features = ([], [])
if path.isfile(file_path):
# load keypoints of pickle file
with open(file_path, 'rb') as f:
features = pickle.load(f)
rospy.loginfo('Loaded ground truth file at: %(path)s' %
{'path': file_path})
keypoint_values = features['keypoint_values']
descriptors = features['descriptors']
meta = features['meta']
# TODO broken
# self.check_meta_information(meta)
# convert keypoint values to cv2 Keypoints
keypoints = [
KeyPoint(kp[0], kp[1], kp[2], kp[3], kp[4], kp[5], kp[6])
for kp in keypoint_values
]
return (keypoints, descriptors)
else:
rospy.logerr('NO ground truth file found at: %(path)s' %
{'path': file_path})
def load_feature_map(self, feature_map_file_path):
# type: (str) -> ([], [])
"""
Loads the map describing the surrounding field background
"""
# generate file path
file_path = self.package_path + feature_map_file_path
features = ([], [])
if path.isfile(file_path):
# load keypoints of pickle file
with open(file_path, 'rb') as f:
features = pickle.load(f)
rospy.loginfo('Loaded map file at: %(path)s' % {'path': file_path})
keypoint_values = features['keypoint_values']
descriptors = features['descriptors']
meta = features['meta']
# TODO broken
# self.check_meta_information(meta)
# convert keypoint values to cv2 Keypoints
keypoints = [
KeyPoint(kp[0], kp[1], kp[2], kp[3], kp[4], kp[5], kp[6])
for kp in keypoint_values
]
return ((keypoints, descriptors), meta)
else:
rospy.logerr('NO map file found at: %(path)s' %
{'path': file_path})
def getListKPFromSearializedObj(listOfKP):
'''
Возвращение из сериализованных данных списка KeyPoint-ов
'''
return [
KeyPoint(listOfKP[i]['x'], listOfKP[i]['y'], listOfKP[i]['size'],
listOfKP[i]['angle'], listOfKP[i]['response'],
listOfKP[i]['octave'], listOfKP[i]['class_id'])
for i in range(1, len(listOfKP))
]
def computeKeypointsWithOrientations(keypoint, octave_index, gaussian_image, radius_factor=3, num_bins=36,
peak_ratio=0.8, scale_factor=1.5):
"""Compute orientations for each keypoint
"""
logger.debug('Computing keypoint orientations...')
keypoints_with_orientations = []
image_shape = gaussian_image.shape
scale = scale_factor * keypoint.size / float32(
2 ** (octave_index + 1)) # compare with keypoint.size computation in localizeExtremumViaQuadraticFit()
radius = int(round(radius_factor * scale))
weight_factor = -0.5 / (scale ** 2)
raw_histogram = zeros(num_bins)
smooth_histogram = zeros(num_bins)
for i in range(-radius, radius + 1):
region_y = int(round(keypoint.pt[1] / float32(2 ** octave_index))) + i
if region_y > 0 and region_y < image_shape[0] - 1:
for j in range(-radius, radius + 1):
region_x = int(round(keypoint.pt[0] / float32(2 ** octave_index))) + j
if region_x > 0 and region_x < image_shape[1] - 1:
dx = gaussian_image[region_y, region_x + 1] - gaussian_image[region_y, region_x - 1]
dy = gaussian_image[region_y - 1, region_x] - gaussian_image[region_y + 1, region_x]
gradient_magnitude = sqrt(dx * dx + dy * dy)
gradient_orientation = rad2deg(arctan2(dy, dx))
weight = exp(weight_factor * (
i ** 2 + j ** 2)) # constant in front of exponential can be dropped because we will find peaks later
histogram_index = int(round(gradient_orientation * num_bins / 360.))
raw_histogram[histogram_index % num_bins] += weight * gradient_magnitude
for n in range(num_bins):
smooth_histogram[n] = (6 * raw_histogram[n] + 4 * (raw_histogram[n - 1] + raw_histogram[(n + 1) % num_bins]) +
raw_histogram[n - 2] + raw_histogram[(n + 2) % num_bins]) / 16.
orientation_max = max(smooth_histogram)
orientation_peaks = \
where(logical_and(smooth_histogram > roll(smooth_histogram, 1), smooth_histogram > roll(smooth_histogram, -1)))[0]
for peak_index in orientation_peaks:
peak_value = smooth_histogram[peak_index]
if peak_value >= peak_ratio * orientation_max:
# Quadratic peak interpolation
# The interpolation update is given by equation (6.30) in https://ccrma.stanford.edu/~jos/sasp/Quadratic_Interpolation_Spectral_Peaks.html
left_value = smooth_histogram[(peak_index - 1) % num_bins]
right_value = smooth_histogram[(peak_index + 1) % num_bins]
interpolated_peak_index = (peak_index + 0.5 * (left_value - right_value) / (
left_value - 2 * peak_value + right_value)) % num_bins
orientation = 360. - interpolated_peak_index * 360. / num_bins
if abs(orientation - 360.) < float_tolerance:
orientation = 0
new_keypoint = KeyPoint(*keypoint.pt, keypoint.size, orientation, keypoint.response, keypoint.octave)
keypoints_with_orientations.append(new_keypoint)
return keypoints_with_orientations
def localizeExtremumViaQuadraticFit(i, j, image_index, octave_index, num_intervals, dog_images_in_octave, sigma,
contrast_threshold, image_border_width, eigenvalue_ratio=10,
num_attempts_until_convergence=5):
"""Iteratively refine pixel positions of scale-space extrema via quadratic fit around each extremum's neighbors
"""
logger.debug('Localizing scale-space extrema...')
extremum_is_outside_image = False
image_shape = dog_images_in_octave[0].shape
for attempt_index in range(num_attempts_until_convergence):
# need to convert from uint8 to float32 to compute derivatives and need to rescale pixel values to [0, 1] to apply Lowe's thresholds
first_image, second_image, third_image = dog_images_in_octave[image_index - 1:image_index + 2]
pixel_cube = stack([first_image[i - 1:i + 2, j - 1:j + 2],
second_image[i - 1:i + 2, j - 1:j + 2],
third_image[i - 1:i + 2, j - 1:j + 2]]).astype('float32') / 255.
gradient = computeGradientAtCenterPixel(pixel_cube)
hessian = computeHessianAtCenterPixel(pixel_cube)
extremum_update = -lstsq(hessian, gradient, rcond=None)[0]
if abs(extremum_update[0]) < 0.5 and abs(extremum_update[1]) < 0.5 and abs(extremum_update[2]) < 0.5:
break
j += int(round(extremum_update[0]))
i += int(round(extremum_update[1]))
image_index += int(round(extremum_update[2]))
# make sure the new pixel_cube will lie entirely within the image
if i < image_border_width or i >= image_shape[0] - image_border_width or j < image_border_width or j >= \
image_shape[1] - image_border_width or image_index < 1 or image_index > num_intervals:
extremum_is_outside_image = True
break
if extremum_is_outside_image:
logger.debug('Updated extremum moved outside of image before reaching convergence. Skipping...')
return None
if attempt_index >= num_attempts_until_convergence - 1:
logger.debug('Exceeded maximum number of attempts without reaching convergence for this extremum. Skipping...')
return None
functionValueAtUpdatedExtremum = pixel_cube[1, 1, 1] + 0.5 * dot(gradient, extremum_update)
if abs(functionValueAtUpdatedExtremum) * num_intervals >= contrast_threshold:
xy_hessian = hessian[:2, :2]
xy_hessian_trace = trace(xy_hessian)
xy_hessian_det = det(xy_hessian)
if xy_hessian_det > 0 and eigenvalue_ratio * (xy_hessian_trace ** 2) < (
(eigenvalue_ratio + 1) ** 2) * xy_hessian_det:
# Contrast check passed -- construct and return OpenCV KeyPoint object
keypoint = KeyPoint()
keypoint.pt = (
(j + extremum_update[0]) * (2 ** octave_index), (i + extremum_update[1]) * (2 ** octave_index))
keypoint.octave = octave_index + image_index * (2 ** 8) + int(round((extremum_update[2] + 0.5) * 255)) * (
2 ** 16)
keypoint.size = sigma * (2 ** ((image_index + extremum_update[2]) / float32(num_intervals))) * (
2 ** (octave_index + 1)) # octave_index + 1 because the input image was doubled
keypoint.response = abs(functionValueAtUpdatedExtremum)
return keypoint, image_index
return None
def localizar_Extremo_via_AjusteCuadratico(i, j, indice_imagen, indice_octava, num_intervalos, imagenes_dog_por_octava, sigma, contrast_threshold, ancho_borde_imagen, eigenvalue_ratio=10, num_attempts_until_convergence=5):
"""Refina iterativamente las posiciones de los píxeles de los extremos del espacio de escala mediante un ajuste cuadrático alrededor de los vecinos de cada extremo
Por eso es costoso esta wada
"""
logger.debug('Localizando los extremos del espacio-escala...')
extremo_esta_fuera_Imagen = False
image_shape = imagenes_dog_por_octava[0].shape
for attempt_index in range(num_attempts_until_convergence):
'''
Los extremos relacionados con el espacio de escala deben convertirse de uint8 a float32
para calcular derivadas y deben cambiar la escala de los valores de píxeles a [0, 1] para aplicar los umbrales de Lowe's.
'''
primera_imagen, segunda_imagen, tercera_imagen = imagenes_dog_por_octava[indice_imagen-1:indice_imagen+2]
pixel_cube = stack([primera_imagen[i-1:i+2, j-1:j+2],
segunda_imagen[i-1:i+2, j-1:j+2],
tercera_imagen[i-1:i+2, j-1:j+2]]).astype('float32') / 255.
gradient = calcular_Gradiente_Pixel_Central(pixel_cube)
hessiana = calcular_Hessiana_Pixel_central(pixel_cube)
extremo_actualizado = -lstsq(hessiana, gradient, rcond=None)[0]
if abs(extremo_actualizado[0]) < 0.5 and abs(extremo_actualizado[1]) < 0.5 and abs(extremo_actualizado[2]) < 0.5:
break
j += int(round(extremo_actualizado[0]))
i += int(round(extremo_actualizado[1]))
indice_imagen += int(round(extremo_actualizado[2]))
# Nos aseguramos de que el nuevo cubo de píxeles se encuentre completamente dentro de la imagen
if i < ancho_borde_imagen or i >= image_shape[0] - ancho_borde_imagen or j < ancho_borde_imagen or j >= image_shape[1] - ancho_borde_imagen or indice_imagen < 1 or indice_imagen > num_intervalos:
extremo_esta_fuera_Imagen = True
break
if extremo_esta_fuera_Imagen:
logger.debug('El extremo actualizado se movió fuera de la imagen antes de alcanzar la convergencia -> Salto...')
return None
if attempt_index >= num_attempts_until_convergence - 1:
logger.debug('Se superó el número máximo de intentos sin alcanzar la convergencia para este extremo -> Salto...')
return None
ValorFuncion_Extrenmo_actualizado = pixel_cube[1, 1, 1] + 0.5 * dot(gradient, extremo_actualizado)
if abs(ValorFuncion_Extrenmo_actualizado) * num_intervalos >= contrast_threshold:
xy_hessian = hessiana[:2, :2]
xy_hessian_traza = trace(xy_hessian)
xy_hessian_determinante = det(xy_hessian)
if xy_hessian_determinante > 0 and eigenvalue_ratio * (xy_hessian_traza ** 2) < ((eigenvalue_ratio + 1) ** 2) * xy_hessian_determinante:
# Comprobación de contraste superada: construir y devolver el objeto Punto Clave
keypoint = KeyPoint()
keypoint.pt = ((j + extremo_actualizado[0]) * (2 ** indice_octava), (i + extremo_actualizado[1]) * (2 ** indice_octava))
keypoint.octave = indice_octava + indice_imagen * (2 ** 8) + int(round((extremo_actualizado[2] + 0.5) * 255)) * (2 ** 16)
#indice_octava + 1 porque la imagen de entrada se duplicó Pesha PS PELOTUDO
keypoint.size = sigma * (2 ** ((indice_imagen + extremo_actualizado[2]) / float32(num_intervalos))) * (2 ** (indice_octava + 1))
keypoint.response = abs(ValorFuncion_Extrenmo_actualizado)
return keypoint, indice_imagen
return None
def extract_features(self, frame: np.ndarray) -> ProcessedFrameData:
"""
This method extracts ORB features
"""
res = goodFeaturesToTrack(image=frame,
maxCorners=self.DEFAULT_nfeatures,
qualityLevel=0.01,
minDistance=10)
list_of_keypoints: List[KeyPoint] = [
KeyPoint(point[0], point[1], -1) for point in list(res.squeeze())
]
return ProcessedFrameData.build(frame=frame,
list_of_keypoints=list_of_keypoints,
descriptors=res)
def calculateOrientation(self, kp, img_index, octv_index):
img = self.g_pyr[octv_index][img_index]
img_shape = img.shape
scale = self.params["scale_factor"] * kp.size / (2**(octv_index + 1))
rad = int(round(self.params["radius_factor"] * scale))
weight_factor = -(scale * scale) / 2
num_bins = self.params["num_bins"]
raw_histogram = zeros(num_bins)
smooth_histogram = zeros(num_bins)
for i in range(-rad, rad + 1):
reg_y = int(round(kp.pt[1] / (2**octv_index))) + i
if reg_y > 0 and reg_y < img_shape[0] - 1:
for j in range(-rad, rad + 1):
reg_x = int(round(kp.pt[0] / (2**octv_index))) + j
if reg_x > 0 and reg_x < img_shape[1] - 1:
dx = img[reg_y, reg_x + 1] - img[reg_y, reg_x - 1]
dy = img[reg_y + 1, reg_x] - img[reg_y - 1, reg_x]
grad_mag = sqrt(dx**2 + dy**2)
grad_orient = 360 / (2 * pi) * arctan2(dy, dx)
weight = exp(weight_factor * (i * i + j * j))
hist_index = int(round(grad_orient * num_bins / 360))
raw_histogram[hist_index %
num_bins] += weight * grad_mag
for n in range(num_bins):
smooth_histogram[n] = (
6 * raw_histogram[n] + 4 *
(raw_histogram[n - 1] + raw_histogram[(n + 1) % num_bins]) +
raw_histogram[n - 2] + raw_histogram[(n + 2) % num_bins]) / 16.
orientation_max = max(smooth_histogram)
orientation_peaks = where(
logical_and(smooth_histogram > roll(smooth_histogram, 1),
smooth_histogram > roll(smooth_histogram, -1)))[0]
for peak_indx in orientation_peaks:
peak_val = smooth_histogram[peak_indx]
if peak_val >= self.params["peak_ratio"] * orientation_max:
left_value = smooth_histogram[(peak_indx - 1) % num_bins]
right_value = smooth_histogram[(peak_indx + 1) % num_bins]
interpolated_peak_indx = (
peak_indx + 0.5 * (left_value - right_value) /
(left_value - 2 * peak_val + right_value)) % num_bins
orientation = 360. - interpolated_peak_indx * 360. / num_bins
if abs(orientation - 360.) < self.params["float_tol"]:
orientation = 0
new_kp = KeyPoint(*kp.pt, kp.size, orientation,
kp.response, kp.octave)
self.kpos.append(new_kp)
def computeKeypointsWithOrientations(keypoint, indice_octava, imagen_gaussiana, radius_factor=3, num_bins=36, proporcion_pico=0.8, escala_factor=1.5):
"""Calcula las orientaciones de cada miserable Punto Clave, entendieron sabandijas
"""
logger.debug('Calculando las orientaciones de los Puntos Clave...')
keypoints_con_orientaciones = []
image_shape = imagen_gaussiana.shape
escala = escala_factor * keypoint.size / float32(2 ** (indice_octava + 1)) # comparar con el cálculo keypoint.size en localizar_Extremo_via_AjusteCuadratico ()
radio= int(round(radius_factor * escala))
factor_peso = -0.5 / (escala ** 2)
histog_crudo = zeros(num_bins) # histograma sin procesar
histog_suave= zeros(num_bins)
for i in range(-radio, radio+ 1):
region_y = int(round(keypoint.pt[1] / float32(2 ** indice_octava))) + i
if region_y > 0 and region_y < image_shape[0] - 1:
for j in range(-radio, radio+ 1):
region_x = int(round(keypoint.pt[0] / float32(2 ** indice_octava))) + j
if region_x > 0 and region_x < image_shape[1] - 1:
dx = imagen_gaussiana[region_y, region_x + 1] - imagen_gaussiana[region_y, region_x - 1]
dy = imagen_gaussiana[region_y - 1, region_x] - imagen_gaussiana[region_y + 1, region_x]
magnitud_gradiente = sqrt(dx * dx + dy * dy)
orientacion_gradiente = rad2deg(arctan2(dy, dx))
# la constante frente al exponencial puede ser eliminada porque encontraremos despues encontraremos picos
peso = exp(factor_peso * (i ** 2 + j ** 2))
histogram_index = int(round(orientacion_gradiente * num_bins / 360.))
histog_crudo[histogram_index % num_bins] += peso * magnitud_gradiente
for n in range(num_bins):
histog_suave[n] = (6 * histog_crudo[n] + 4 * (histog_crudo[n - 1] + histog_crudo[(n + 1) % num_bins]) + histog_crudo[n - 2] + histog_crudo[(n + 2) % num_bins]) / 16.
orientacion_max = max(histog_suave)
picos_orientacion = where(logical_and(histog_suave> roll(histog_suave, 1), histog_suave> roll(histog_suave, -1)))[0]
for indice_pico in picos_orientacion:
valor_pico = histog_suave[indice_pico]
if valor_pico >= proporcion_pico * orientacion_max:
# Interpolación de picos cuadráticos
# La actualización de la interpolación viene dada por la ecuación (6.30) en:
# https://ccrma.stanford.edu/~jos/sasp/Quadratic_Interpolation_Spectral_Peaks.html
valor_Izquierdo = histog_suave[(indice_pico - 1) % num_bins]
valor_Derecho = histog_suave[(indice_pico + 1) % num_bins]
interpolated_indice_pico = (indice_pico + 0.5 * (valor_Izquierdo - valor_Derecho) / (valor_Izquierdo - 2 * valor_pico + valor_Derecho)) % num_bins
orientacion = 360. - interpolated_indice_pico * 360. / num_bins
if abs(orientacion - 360.) < float_tolerance:
orientacion = 0
new_keypoint = KeyPoint(*keypoint.pt, keypoint.size, orientacion, keypoint.response, keypoint.octave)
keypoints_con_orientaciones.append(new_keypoint)
return keypoints_con_orientaciones
def extract(self,frame,maxCorners=5000,qualityLevel=.01,minDistance=3):
#Detect
feats = goodFeaturesToTrack(frame,maxCorners,qualityLevel,minDistance)#,mask=self.orb_mask)
#Extract
kps = [KeyPoint(x=f[0][0],y=f[0][1],_size=20) for f in feats]
kps,des = self.orb.compute(frame,kps)
#Match
ret = []
if self.last is not None:
#THE QUERY IMAGE IS THE ACTUAL IMAGE &
#THE TRAIN IMAGE IS THE LAST IMAGE
#U STOUPID KUNT
matches = self.bfm.knnMatch(des,self.last['des'],k=2)
for m,n in matches:
if m.distance < 0.7 * n.distance:
ret.append((kps[m.queryIdx].pt,self.last['kps'][m.trainIdx].pt))
#filter
Rt = None
if len(ret) > 0:
ret = array(ret)
#Normalize
ret[:,0,:] = self.normalize(ret[:,0,:])
ret[:,1,:] = self.normalize(ret[:,1,:])
try:
# print(f"{len(ret)=}, {ret[:,0].shape=}, {ret[:,1].shape=}")
model, inliers = ransac((ret[:, 0],ret[:, 1]),
#FundamentalMatrixTransform,
EssentialMatrixTransform,
min_samples=8,
residual_threshold=0.005,
max_trials=200)
ret = ret[inliers]
Rt = self.extractRt(model.params)
except:
pass
self.last = {'kps': kps,'des':des}
return ret,Rt
def run(self):
# Estrazione degli Harris Corner.
corners = cornerHarris(self.image, blockSize=2, ksize=3, k=0.04)
# Calcolo orientamento keypoints tramite gradienti e ad angolo tra questi ultimi.
grad_x = Sobel(self.image, CV_32F, 1, 0, ksize=3, scale=4.5)
grad_y = Sobel(self.image, CV_32F, 0, 1, ksize=3, scale=4.5)
theta = phase(grad_x, grad_y, angleInDegrees=True)
# Filtraggio e creazione degli oggetti Keypoint di OpenCV.
strength = int(self.config.get('mops')['corner_ratio'] * corners.max())
for x in range(0, self.image.shape[0]):
for y in range(0, self.image.shape[1]):
if int(corners[x][y]) > strength:
self.keypoints.append(
KeyPoint(y,
x,
_size=3,
_angle=theta[x][y],
_response=corners[x][y],
_octave=self.level))
def localizeExtremumViaQuadraticFit(i, j, image_index, octave_index, num_intervals, dog_images_in_octave, sigma, contrast_threshold, image_border_width, eigenvalue_ratio=10, num_attempts_until_convergence=5):
"""
在scale-space中，迭代进行每个像素点的位置更新
"""
logger.info('Localizing scale-space extrema...')
extremum_is_outside_image = False
image_shape = dog_images_in_octave[0].shape
for attempt_index in range(num_attempts_until_convergence):
first_image, second_image, third_image = dog_images_in_octave[image_index-1:image_index+2]
pixel_cube = stack([first_image[i-1:i+2, j-1:j+2],
second_image[i-1:i+2, j-1:j+2],
third_image[i-1:i+2, j-1:j+2]]).astype('float32') / 255.
gradient = computeGradientAtCenterPixel(pixel_cube)
hessian = computeHessianAtCenterPixel(pixel_cube)
extremum_update = -lstsq(hessian, gradient, rcond=None)[0]
if abs(extremum_update[0]) < 0.5 and abs(extremum_update[1]) < 0.5 and abs(extremum_update[2]) < 0.5:
break
j += int(round(extremum_update[0]))
i += int(round(extremum_update[1]))
image_index += int(round(extremum_update[2]))
if i < image_border_width or i >= image_shape[0] - image_border_width or j < image_border_width or j >= image_shape[1] - image_border_width or image_index < 1 or image_index > num_intervals:
extremum_is_outside_image = True
break
functionValueAtUpdatedExtremum = pixel_cube[1, 1, 1] + 0.5 * dot(gradient, extremum_update)
if abs(functionValueAtUpdatedExtremum) * num_intervals >= contrast_threshold:
xy_hessian = hessian[:2, :2]
xy_hessian_trace = trace(xy_hessian)
xy_hessian_det = det(xy_hessian)
if xy_hessian_det > 0 and eigenvalue_ratio * (xy_hessian_trace ** 2) < ((eigenvalue_ratio + 1) ** 2) * xy_hessian_det:
# 设计Keypoints
keypoint = KeyPoint()
keypoint.pt = ((j + extremum_update[0]) * (2 ** octave_index), (i + extremum_update[1]) * (2 ** octave_index))
keypoint.octave = octave_index + image_index * (2 ** 8) + int(round((extremum_update[2] + 0.5) * 255)) * (2 ** 16)
keypoint.size = sigma * (2 ** ((image_index + extremum_update[2]) / float32(num_intervals))) * (2 ** (octave_index + 1)) # octave_index + 1 because the input image was doubled
keypoint.response = abs(functionValueAtUpdatedExtremum)
return keypoint, image_index
return None
def localize_extremum_via_quadratic_fit(i,
j,
image_index,
octave_index,
num_intervals,
dog_images_in_octave,
sigma,
contrast_threshold,
image_border_width,
eigenvalue_ratio=10,
num_attempts_until_convergence=5):
"""
利用泰勒展开对像素进行亚像素（子像素）精确定位。
"""
extremum_is_outside_image = False
image_shape = dog_images_in_octave[0].shape
for attempt_index in range(num_attempts_until_convergence):
# need to convert from uint8 to float32 to compute derivatives and need
# to rescale pixel values to [0, 1] to apply Lowe's thresholds
first_image, second_image, third_image = dog_images_in_octave[
image_index - 1:image_index + 2]
pixel_cube = stack([
first_image[i - 1:i + 2, j - 1:j + 2],
second_image[i - 1:i + 2, j - 1:j + 2], third_image[i - 1:i + 2,
j - 1:j + 2]
]).astype('float32') / 255.
gradient = compute_gradient_at_center_pixel(pixel_cube)
hessian = compute_hessian_at_center_pixel(pixel_cube)
extremum_update = -lstsq(hessian, gradient, rcond=None)[0]
if abs(extremum_update[0]) < 0.5 and abs(
extremum_update[1]) < 0.5 and abs(extremum_update[2]) < 0.5:
break
j += int(round(extremum_update[0]))
i += int(round(extremum_update[1]))
image_index += int(round(extremum_update[2]))
# make sure the new pixel_cube will lie entirely within the image
if i < image_border_width or i >= image_shape[0] - image_border_width or j < image_border_width or j >= \
image_shape[1] - image_border_width or image_index < 1 or image_index > num_intervals:
extremum_is_outside_image = True
break
if extremum_is_outside_image:
return None
if attempt_index >= num_attempts_until_convergence - 1:
return None
functionValueAtUpdatedExtremum = pixel_cube[1, 1, 1] + 0.5 * dot(
gradient, extremum_update)
if abs(functionValueAtUpdatedExtremum) * \
num_intervals >= contrast_threshold:
xy_hessian = hessian[:2, :2]
xy_hessian_trace = trace(xy_hessian)
xy_hessian_det = det(xy_hessian)
if xy_hessian_det > 0 and eigenvalue_ratio * (xy_hessian_trace**2) < (
(eigenvalue_ratio + 1)**2) * xy_hessian_det:
# Contrast check passed -- construct and return OpenCV KeyPoint
# object
keypoint = KeyPoint()
keypoint.pt = ((j + extremum_update[0]) * (2**octave_index),
(i + extremum_update[1]) * (2**octave_index))
keypoint.octave = octave_index + image_index * \
(2 ** 8) + int(round((extremum_update[2] + 0.5) * 255)) * (2 ** 16)
keypoint.size = sigma * (2**(
(image_index + extremum_update[2]) / float32(num_intervals)
)) * (2**(octave_index + 1)
) # octave_index + 1 because the input image was doubled
keypoint.response = abs(functionValueAtUpdatedExtremum)
return keypoint, image_index
return None
def ComputeKeypointsWithOrientations(Keypoint,
OctaveIndex,
GaussianImg,
LenOfHistogram=36,
PeakRatio=0.8):
"""
计算关键点方向
@param Keypoint: 关键点
@param OctaveIndex: 关键点所在组数，用于求Sigma_oct
@param GaussianImg: 关键点所在高斯金字塔的图片
@param LenOfHistogram: 梯度直方图的长度这里是36，一个柱10度
@param PeakRatio: 大于主方向的PeakRatio倍，也要存储，Lowe推荐0.8
@return: 带方向的极值点KeyPoint
"""
KeyPointWithOrientations = []
ImgShape = np.shape(GaussianImg)
Sigma_oct = 1.5 * Keypoint.size / np.float32(2**(OctaveIndex + 1))
Radius = int(np.round(3 * Sigma_oct))
WeightFactor = -0.5 / (Sigma_oct**2)
Histogram = np.zeros(LenOfHistogram)
SmoothHistogram = np.zeros(LenOfHistogram)
for i in range(-Radius, Radius + 1):
Region_y = int(np.round(
Keypoint.pt[1] / np.float32(2**OctaveIndex))) + i
if Region_y > 0 and Region_y < ImgShape[0] - 1:
for j in range(-Radius, Radius + 1):
Region_x = int(
np.round(Keypoint.pt[0] / np.float32(2**OctaveIndex))) + j
if Region_x > 0 and Region_x < ImgShape[1] - 1:
if np.sqrt(i**2 + j**2) < Radius: # 圆形区域内
dx = GaussianImg[Region_y, Region_x +
1] - GaussianImg[Region_y,
Region_x - 1]
dy = GaussianImg[Region_y - 1,
Region_x] - GaussianImg[Region_y + 1,
Region_x]
Magnitude = np.sqrt(dx * dx + dy * dy)
Orientation = np.rad2deg(np.arctan2(dy, dx)) # 1-360
Weight = np.exp(WeightFactor * (i**2 + j**2))
HistogramIndex = int(
np.round(Orientation * LenOfHistogram /
360.)) % LenOfHistogram
Histogram[HistogramIndex] = Histogram[
HistogramIndex] + Weight * Magnitude
# 在直方图统计时，每相邻三个像素点采用高斯加权，根据Lowe的建议，模板采用[0.25,0.5,0.25],并且连续加权两次.
for i in range(2):
for n in range(LenOfHistogram):
if n == 0:
SmoothHistogram[n] = 0.5 * Histogram[n] + 0.25 * (
Histogram[LenOfHistogram - 1] + Histogram[n + 1])
if n == LenOfHistogram - 1:
SmoothHistogram[n] = 0.5 * Histogram[n] + 0.25 * (
Histogram[n - 1] + Histogram[0])
else:
SmoothHistogram[n] = 0.5 * Histogram[n] + 0.25 * (
Histogram[n - 1] + Histogram[n + 1])
Histogram = SmoothHistogram
OrientationMax = max(SmoothHistogram)
SmoothHistogramPeaks = np.where(
np.logical_and(
SmoothHistogram > np.roll(SmoothHistogram, 1),
SmoothHistogram > np.roll(SmoothHistogram, -1)))[0] # 找平滑后的直方图的极值点
for PeakIndex in SmoothHistogramPeaks:
PeakValue = SmoothHistogram[PeakIndex]
if PeakValue >= PeakRatio * OrientationMax: # 统计主方向和大于0.8倍主方向的方向
LeftValue = SmoothHistogram[(PeakIndex - 1) % LenOfHistogram]
RightValue = SmoothHistogram[(PeakIndex + 1) % LenOfHistogram]
InterpolatedPeakIndex = 0.5 * (LeftValue - RightValue) / (
LeftValue - 2 * PeakValue + RightValue)
Orientation = (PeakIndex +
InterpolatedPeakIndex) * (360 / LenOfHistogram)
Orientation = 360 - Orientation
NewkeyPoint = KeyPoint(*tuple(0.5 * np.array(Keypoint.pt)),
0.5 * Keypoint.size, Orientation,
Keypoint.response, Keypoint.octave)
# 这里的KeyPoint的极值点就全部检测完成了，为回到初始图片的大小所以乘以0.5
KeyPointWithOrientations.append(NewkeyPoint)
return KeyPointWithOrientations
def values2key_points(self, pt_x, pt_y, size, angle, response, octave,
class_id):
return KeyPoint(pt_x, pt_y, size, angle, response, octave, class_id)
def values2key_points(self, pt_x, pt_y, size, angle, response, octave,
class_id):
"""
A better constructor for the CV2 Keypoint class
"""
return KeyPoint(pt_x, pt_y, size, angle, response, octave, class_id)
def DetecteKeyPoint(i,
j,
ImgIndex,
OctaveIndex,
NumOfIntervals,
DoGImgInOctave,
Sigma0,
ContrastThreshold,
ImgBorderWidth,
EigenvalueRatio=10,
NumOfAttempts=5):
"""
探测极值点是否是关键点
@param i: 极值点坐标i
@param j: 极值点坐标j
@param ImgIndex: 图片的层数s
@param OctaveIndex: 图片所在组数O
@param NumOfIntervals: SIFT里的大S
@param DoGImgInOctave: 差分高斯金字塔的一组图片，探测时可能移动层数
@param Sigma0: SIFT里的Sigma0
@param ContrastThreshold: SIFT里用于消除噪音点
@param ImgBorderWidth: 抛弃的图片边缘的长度，便于计算，网上说一般为5
@param EigenvalueRatio: 消除边缘响应的阈值里的r
@param NumOfAttempts: 关键点的探测次数
@return: 如果探测到关键点就返回KeyPoint，探测不到就返回None
"""
ImgShape = (len(DoGImgInOctave[0]), len(DoGImgInOctave[0][0]))
for AttemptIndex in range(NumOfAttempts):
FirstImg, SecondImg, ThirdImg = DoGImgInOctave[ImgIndex - 1:ImgIndex +
2]
DoGArray = np.array([
FirstImg[i - 1:i + 2, j - 1:j + 2],
SecondImg[i - 1:i + 2, j - 1:j + 2], ThirdImg[i - 1:i + 2,
j - 1:j + 2]
]).astype(np.float) / 255 # Lowe要求归一化，为了跟阈值比较，消除噪音点
Gradient = ComputeGradientAtCenterPixel(DoGArray)
Hessian = ComputeHessianAtCenterPixel(DoGArray)
CoordinatesUpdateValueOfKeyPoint = -1 * np.dot(
ComputeInv3(Hessian),
np.array([Gradient]).reshape((3, 1)))
CoordinatesUpdateValueOfKeyPoint.reshape(-1) # 偏移量
if abs(CoordinatesUpdateValueOfKeyPoint[0]) < 0.5 and abs(
CoordinatesUpdateValueOfKeyPoint[1]) < 0.5 and abs(
CoordinatesUpdateValueOfKeyPoint[2]) < 0.5:
break
j += int(np.round(CoordinatesUpdateValueOfKeyPoint[0]))
i += int(np.round(CoordinatesUpdateValueOfKeyPoint[1]))
ImgIndex += int(np.round(CoordinatesUpdateValueOfKeyPoint[2]))
if i < ImgBorderWidth or i >= ImgShape[
0] - ImgBorderWidth or j < ImgBorderWidth or j >= ImgShape[
1] - ImgBorderWidth or ImgIndex < 1 or ImgIndex > NumOfIntervals:
return None
if AttemptIndex >= NumOfAttempts - 1:
return None
NewKeyPointValue = DoGArray[1, 1, 1] + 0.5 * np.dot(
Gradient, CoordinatesUpdateValueOfKeyPoint)
if abs(NewKeyPointValue) * NumOfIntervals >= ContrastThreshold:
xy_Hessian = Hessian[:2, :2]
Tr = xy_Hessian[0, 0] + xy_Hessian[1, 1]
Det = xy_Hessian[0, 0] * xy_Hessian[1, 1] - (xy_Hessian[0, 1]**2)
if Det > 0 and EigenvalueRatio * (Tr**2) < (
(EigenvalueRatio + 1)**2) * Det:
Keypoint = KeyPoint()
Keypoint.pt = ((j + CoordinatesUpdateValueOfKeyPoint[0]) *
(2**OctaveIndex),
(i + CoordinatesUpdateValueOfKeyPoint[1]) *
(2**OctaveIndex))
# Keypoint.pt 要以初始图片大小为准，这里的初始图片是扩大二倍后的图片
Keypoint.octave = OctaveIndex + ImgIndex * (2**8) + np.int(
np.round(
(CoordinatesUpdateValueOfKeyPoint[2]) * 255)) * (2**16)
# Keypoint.octave 0-7位保存Octave，8-15位保存ImgIndex， 16-23位保存sigma，防止在后面用Sigma返过来求s时，s直接取整会出错
Keypoint.size = Sigma0 * (2**(
(ImgIndex + CoordinatesUpdateValueOfKeyPoint[2]) /
np.float32(NumOfIntervals))) * (2**(OctaveIndex + 1))
# Keypoint.size 这里的初始图片是扩大二倍后的图片，各个Sigma要多乘一个2, 所以OctaveIndex + 1
Keypoint.response = abs(NewKeyPointValue)
# 为什么不直接从初始图片开始算？SIFT关键点主要在-1组，具体在报告中介绍
return Keypoint, int(ImgIndex +
CoordinatesUpdateValueOfKeyPoint[2])
return None
def localize(self, x, y, top, center, bottom, index_img, index_octv, octv):
outside_image = False
img_shape = center.shape
num_intervals = len(octv) + 1
for attempt in range(self.params["convergence_attempts"] + 1):
cube = stack([
bottom[x - 1:x + 2, y - 1:y + 2],
center[x - 1:x + 2, y - 1:y + 2], top[x - 1:x + 2, y - 1:y + 2]
]).astype('float32') / 255
update = -lstsq(LocateKeypoints.getHessian(cube),
LocateKeypoints.getGrad(cube),
rcond=None)[0]
if all(update < 0.5):
break
x += int(round(update[0]))
y += int(round(update[1]))
index_octv += int(round(update[2]))
image_border_width = self.params["image_border_width"]
if (x < image_border_width
or x >= img_shape[0] - image_border_width
or y < image_border_width
or y >= img_shape[1] - image_border_width or index_img < 1
or index_img >= num_intervals - 1):
outside_image = True
break
top, center, bottom = octv[index_img +
1], octv[index_img], octv[index_img - 1]
if outside_image:
logger.debug(
f'Updated extremum {x},{y} moved outside of image before reaching convergence. Skipping...'
)
return None
if attempt == self.params["convergence_attempts"]:
logger.debug(f'The extremum {x},{y} did not converge. Skipping...')
return None
functionValueAtUpdatedExtremum = cube[1, 1, 1] + 0.5 * dot(
LocateKeypoints.getGrad(cube), update)
eigenvalue_ratio = self.params["eigenvalue_ratio"]
if abs(functionValueAtUpdatedExtremum
) * num_intervals >= self.params["contrast_threshold"]:
xy_hessian = LocateKeypoints.getHessian(cube)[:2, :2]
xy_hessian_trace = trace(xy_hessian)
xy_hessian_det = det(xy_hessian)
if xy_hessian_det > 0 and eigenvalue_ratio * (
xy_hessian_trace**2) < (
(eigenvalue_ratio + 1)**2) * xy_hessian_det:
# Contrast check passed -- construct and return OpenCV KeyPoint object
keypoint = KeyPoint()
keypoint.pt = ((y + update[0]) * (2**index_octv),
(x + update[1]) * (2**index_octv))
keypoint.octave = index_octv + index_img * (2**8) + int(
round((update[2] + 0.5) * 255)) * (2**16)
keypoint.size = self.params["sigma"] * (2**(
(index_img + update[2]) / float32(num_intervals))) * (
2**(index_octv + 1)
) # octave_index + 1 because the input image was doubled
keypoint.response = abs(functionValueAtUpdatedExtremum)
return keypoint, index_img, index_octv
return None