def simple_follow(self, object_cloud, target_cloud):
"""
Tomando como centro el centro del cuadrado que contiene al objeto
en el frame anterior, busco el mismo objeto en una zona N veces mayor
a la original.
"""
target_cloud = self._filter_target_cloud(target_cloud)
if points(target_cloud) > 0:
# Calculate ICP
icp_result = icp(object_cloud, target_cloud, self._icp_defaults)
else:
icp_result = ICPResult()
icp_result.has_converged = False
icp_result.score = 100
return icp_result
def simple_follow(self, object_cloud, target_cloud):
"""
Tomando como centro el centro del cuadrado que contiene al objeto
en el frame anterior, busco el mismo objeto en una zona 4 veces mayor
a la original.
"""
# TODO: se puede hacer una busqueda mejor, en espiral o algo asi
# tomando como valor de comparacion el score que devuelve ICP
r_top_limit = self._descriptors['min_y_cloud']
r_bottom_limit = self._descriptors['max_y_cloud']
c_left_limit = self._descriptors['min_x_cloud']
c_right_limit = self._descriptors['max_x_cloud']
# Define row and column limits for the zone to search the object
# In this case, we look on a box N times the size of the original
n = 2
factor = 0.5 * (n - 1)
height = r_bottom_limit - r_top_limit
width = c_right_limit - c_left_limit
r_top_limit -= height * factor
r_bottom_limit += height * factor
c_left_limit -= width * factor
c_right_limit += width * factor
# Filter points corresponding to the zone where the object being
# followed is supposed to be
target_cloud = filter_cloud(
target_cloud,
str("y"),
float(r_top_limit),
float(r_bottom_limit)
)
target_cloud = filter_cloud(
target_cloud,
str("x"),
float(c_left_limit),
float(c_right_limit)
)
# Calculate ICP
icp_result = icp(object_cloud, target_cloud, self._icp_defaults)
return icp_result
def alignment_prerejective(self, descriptors):
# obtengo tamaño del objeto detetado y me quedo con uno X veces mas grande
model_cloud = descriptors['detected_cloud']
if not self.adapt_leaf.was_started():
model_points = self._descriptors['static_obj_model_points']
self.adapt_leaf.set_first_values(model_points)
obj_limits = get_min_max(model_cloud)
length_func = lambda mul, l: l * mul / 2.0
max_length = max(
obj_limits.max_x - obj_limits.min_x,
obj_limits.max_y - obj_limits.min_y,
obj_limits.max_z - obj_limits.min_z,
)
x_center = obj_limits.max_x - (obj_limits.max_x - obj_limits.min_x)
y_center = obj_limits.max_y - (obj_limits.max_y - obj_limits.min_y)
z_center = obj_limits.max_z - (obj_limits.max_z - obj_limits.min_z)
half_side_length = length_func(self.obj_mult, max_length)
# obtengo limites de la escena
scene_cloud = self._descriptors['pcd']
# Filtro la escena y me quedo con la bounding-box de la deteccion por
# transformaciones
cloud = filter_cloud(scene_cloud, b'x', x_center - half_side_length,
x_center + half_side_length)
cloud = filter_cloud(cloud, b'y', y_center - half_side_length,
y_center + half_side_length)
cloud = filter_cloud(cloud, b'z', z_center - half_side_length,
z_center + half_side_length)
# show_clouds(
# b'Escena filtrando el bounding-box',
# cloud,
# model_cloud,
# )
detected_descriptors = {
'topleft': (0, 0), # (fila, columna)
'bottomright': 0,
}
fue_exitoso = False
accepted_points = (self._descriptors['static_obj_model_points'] *
self.perc_obj_model_points)
if points(cloud) > accepted_points:
ap_result = self._best_alignment_prerejective(model_cloud, cloud)
# print "Convergio AP:", ap_result.has_converged
# print "Score AP:", ap_result.score, "(<", self.umbral_score, ")"
# show_clouds(
# b'Escena filtrada vs alignment_prerejective',
# cloud,
# ap_result.cloud,
# )
if ap_result.has_converged and ap_result.score < self.umbral_score:
# Calculate ICP
icp_result = icp(ap_result.cloud, cloud, self._icp_defaults)
# print "Convergio ICP:", icp_result.has_converged
# print "Score ICP:", icp_result.score, "(<", self.umbral_score, ")"
# show_clouds(
# b'Escena filtrada vs icp',
# cloud,
# icp_result.cloud,
# )
if (icp_result.has_converged
and icp_result.score < self.umbral_score):
# Filtro los puntos de la escena que se corresponden con el
# objeto que estoy buscando
obj_scene_cloud = filter_object_from_scene_cloud(
icp_result.cloud, # object
scene_cloud, # complete scene
self.adapt_leaf.leaf_ratio(), # radius
False, # show values
)
obj_scene_points = points(obj_scene_cloud)
fue_exitoso = obj_scene_points > accepted_points
if fue_exitoso:
self.adapt_leaf.set_found_points(obj_scene_points)
else:
self.adapt_leaf.reset()
minmax = get_min_max(obj_scene_cloud)
topleft, bottomright = from_cloud_to_flat_limits(
obj_scene_cloud)
tam_region = max(bottomright[0] - topleft[0],
bottomright[1] - topleft[1])
detected_descriptors.update({
'min_x_cloud': minmax.min_x,
'max_x_cloud': minmax.max_x,
'min_y_cloud': minmax.min_y,
'max_y_cloud': minmax.max_y,
'min_z_cloud': minmax.min_z,
'max_z_cloud': minmax.max_z,
'object_cloud': obj_scene_cloud,
'detected_cloud':
icp_result.cloud, # lo guardo solo para la estadistica
'size': tam_region,
'location': topleft,
'topleft': topleft, # (fila, columna)
'bottomright': bottomright,
})
# show_clouds(
# b'Modelo detectado y filtrado vs escena',
# scene_cloud,
# obj_scene_cloud,
# )
# show_clouds(
# b'Modelo detectado por TRANSF, AP e ICP vs escena',
# scene_cloud,
# icp_result.cloud,
# )
return fue_exitoso, detected_descriptors
def detect(self):
# from analisis import Rectangle
# Deteccion RGB
rgb_fue_exitoso, rgb_desc = super(RGBDDetector, self).detect()
depth_desc = {
'topleft': (0, 0), # (fila, columna)
'bottomright': 0,
}
depth_fue_exitoso = False
if rgb_fue_exitoso:
# Tomo los datos del objeto a encontrar
model_cloud = self._descriptors['obj_model']
model_cloud_points = self._descriptors['obj_model_points']
# Parametro de aceptacion del resultado nro 1
accepted_points = model_cloud_points * self.perc_obj_model_points
# Esto es para adaptar el rango de busqueda de puntos de la escena
if not self.adapt_leaf.was_started():
self.adapt_leaf.set_first_values(model_cloud_points)
# Nube de puntos de la escena
scene_cloud = self._descriptors['pcd']
# Tamaño de la escena RGB
rgb_height = self._descriptors['scene_rgb'].shape[0]
rgb_width = self._descriptors['scene_rgb'].shape[1]
topleft = rgb_desc['topleft']
bottomright = rgb_desc['bottomright']
# MuestraBusquedaEnVivo('Deteccion RGB').run(
# self._descriptors['scene_rgb'],
# topleft,
# bottomright,
# )
# Obtengo un bounding rectangulo cuya longitud en x e y es el doble
# del encontrado por RGB pero con mismo centro de masa
width = abs(bottomright[1] - topleft[1])
height = abs(bottomright[0] - topleft[0])
extra_width = max(int(width * self.depth_area_extra), 1)
extra_height = max(int(height * self.depth_area_extra), 1)
new_topleft = (max(topleft[0] - extra_height,
0), max(topleft[1] - extra_width, 0))
new_bottomright = (min(bottomright[0] + extra_height,
rgb_height - 1),
min(bottomright[1] + extra_width,
rgb_width - 1))
# Rectangulo RGB y rectangulo nuevo
# found_rgb = Rectangle(topleft, bottomright)
# new_rgb = Rectangle(new_topleft, new_bottomright)
# print "Rectangulo RGB detectado:", found_rgb.area()
# print "Rectangulo RGB para Depth:", new_rgb.area()
#
# MuestraBusquedaEnVivo('Deteccion RGB').run(
# self._descriptors['scene_rgb'],
# new_topleft,
# new_bottomright,
# )
# Paso los valores del nuevo bounding box de RGB a DEPTH
depth_img = self._descriptors['depth_img']
top_bottom, left_right = from_flat_to_cloud_limits(
new_topleft, new_bottomright, depth_img)
# print "Depth topleft: (", top_bottom[0], ",", left_right[0], ")"
# print "Depth bottomright: (", top_bottom[1], ",", left_right[1], ")"
# Filtro la escena y obtengo solo los puntos señalados por la
# deteccion RGB
first_cloud = filter_cloud(
scene_cloud,
b'x', # X son las columnas
left_right[0],
left_right[1],
)
cloud = filter_cloud(
first_cloud,
b'y', # Y son las filas
top_bottom[0],
top_bottom[1],
)
best_aligned_scene = None
best_alignment_score = self.umbral_score # lesser is better
# Corro la deteccion en depth varias veces, solo si la cantidad de
# puntos es adecuada. Para mejorar un poco la condicion inicial de
# cada corrida, el modelo de objeto que uso es el que va quedando
# despues de cada alineacion, haya sido fallida o no
if points(cloud) > model_cloud_points:
for i in range(3):
# Calculate alignment
ap_result = align(model_cloud, cloud, self._ap_defaults)
if ap_result.has_converged:
model_cloud = ap_result.cloud
if ap_result.score < best_alignment_score:
best_alignment_score = ap_result.score
best_aligned_scene = ap_result.cloud
# Su hubo una buena alineacion
if best_aligned_scene is not None:
# print " HUBO UNA ALINEACION CORRECTA!!!!"
# Calculate ICP
icp_result = icp(best_aligned_scene, cloud, self._icp_defaults)
if (icp_result.has_converged
and icp_result.score < self.umbral_score):
# print " HUBO UN ICP CORRECTO!!!!"
# Filtro los puntos de la escena que se corresponden con el
# objeto que estoy buscando
# show_clouds(
# b'Modelo alineado por AP e ICP vs escena parcial segun RGB',
# icp_result.cloud,
# cloud,
# )
# show_clouds(
# b'Modelo alineado por AP e ICP vs escena completa',
# icp_result.cloud,
# scene_cloud,
# )
obj_scene_cloud = filter_object_from_scene_cloud(
icp_result.cloud, # object
cloud, # partial scene
self.adapt_leaf.leaf_ratio(), # radius
False, # show values
)
obj_scene_points = points(obj_scene_cloud)
depth_fue_exitoso = obj_scene_points > accepted_points
if depth_fue_exitoso:
self.adapt_leaf.set_found_points(obj_scene_points)
# print " HUBO UNA DETECCION!!!!"
else:
self.adapt_leaf.reset()
topleft, bottomright = from_cloud_to_flat_limits(
obj_scene_cloud)
depth_desc.update({
'object_cloud': obj_scene_cloud,
'obj_model':
icp_result.cloud, # original model transformed
'detected_cloud':
icp_result.cloud, # lo guardo solo para la estadistica
'topleft': topleft, # (fila, columna)
'bottomright': bottomright,
})
# show_clouds(
# b'Modelo detectado vs escena',
# icp_result.cloud,
# scene_cloud
# )
rgb_desc.update(depth_desc)
return depth_fue_exitoso, rgb_desc
def detect(self):
model_cloud = self._descriptors['obj_model']
model_cloud_points = self._descriptors['obj_model_points']
accepted_points = model_cloud_points * self.perc_obj_model_points
if not self.adapt_leaf.was_started():
self.adapt_leaf.set_first_values(model_cloud_points)
scene_cloud = self._descriptors['pcd']
# obtengo tamaño del modelo del objeto a detectar y tomo una region
# X veces mas grande
obj_limits = get_min_max(model_cloud)
# obtengo limites de la escena
scene_limits = get_min_max(scene_cloud)
detected_descriptors = {
'topleft': (0, 0), # (fila, columna)
'bottomright': 0,
}
fue_exitoso = False
best_aligned_scene = None
best_alignment_score = self.umbral_score # lesser is better
best_limits = {}
# Busco la mejor alineacion del objeto segmentando la escena
for limits in (BusquedaPorFramesSolapados().iterate_frame_boxes(
obj_limits, scene_limits, obj_mult=self.obj_mult)):
cloud = filter_cloud(scene_cloud, b'x', limits['min_x'],
limits['max_x'])
cloud = filter_cloud(cloud, b'y', limits['min_y'], limits['max_y'])
if points(cloud) > model_cloud_points:
# Calculate alignment
ap_result = align(model_cloud, cloud, self._ap_defaults)
if (ap_result.has_converged
and ap_result.score < best_alignment_score):
best_alignment_score = ap_result.score
best_aligned_scene = ap_result.cloud
best_limits.update(limits)
# Su hubo una buena alineacion
if best_aligned_scene is not None:
cloud = filter_cloud(scene_cloud, b'x', best_limits['min_x'],
best_limits['max_x'])
cloud = filter_cloud(cloud, b'y', best_limits['min_y'],
best_limits['max_y'])
# Calculate ICP
icp_result = icp(best_aligned_scene, cloud, self._icp_defaults)
if icp_result.has_converged and icp_result.score < self.umbral_score:
# Filtro los puntos de la escena que se corresponden con el
# objeto que estoy buscando
obj_scene_cloud = filter_object_from_scene_cloud(
icp_result.cloud, # object
scene_cloud, # complete scene
self.adapt_leaf.leaf_ratio(), # radius
False, # show values
)
obj_scene_points = points(obj_scene_cloud)
fue_exitoso = obj_scene_points > accepted_points
if fue_exitoso:
self.adapt_leaf.set_found_points(obj_scene_points)
else:
self.adapt_leaf.reset()
minmax = get_min_max(obj_scene_cloud)
topleft, bottomright = from_cloud_to_flat_limits(
obj_scene_cloud)
detected_descriptors.update({
'min_x_cloud': minmax.min_x,
'max_x_cloud': minmax.max_x,
'min_y_cloud': minmax.min_y,
'max_y_cloud': minmax.max_y,
'min_z_cloud': minmax.min_z,
'max_z_cloud': minmax.max_z,
'object_cloud': obj_scene_cloud,
'obj_model':
icp_result.cloud, # original model transformed
'detected_cloud':
icp_result.cloud, # lo guardo solo para la estadistica
'topleft': topleft, # (fila, columna)
'bottomright': bottomright,
})
# show_clouds(
# b'Modelo detectado vs escena',
# icp_result.cloud,
# scene_cloud
# )
return fue_exitoso, detected_descriptors
def calculate_descriptors(self, detected_descriptors):
"""
Obtengo la nube de puntos correspondiente a la ubicacion y region
pasadas por parametro.
"""
detected_descriptors = (super(
StaticDetectorWithModelAlignment,
self).calculate_descriptors(detected_descriptors))
model_cloud = self._descriptors['obj_model']
# scene_cloud = self._descriptors['pcd']
# Esta es la nube de puntos proveniente de filtrar el cuadrado marcado
# por la base de datos en la imagen RGB
detected_cloud = detected_descriptors['object_cloud']
accepted_points = (self._descriptors['obj_model_points'] *
self.perc_obj_model_pts)
# Defino los valores minimos que debería tener el resultado. Si se
# pueden mejorar en el "for" de más adelante, genial!
minmax = get_min_max(detected_cloud)
detected_descriptors.update({
'min_x_cloud': minmax.min_x,
'max_x_cloud': minmax.max_x,
'min_y_cloud': minmax.min_y,
'max_y_cloud': minmax.max_y,
'min_z_cloud': minmax.min_z,
'max_z_cloud': minmax.max_z,
# original model transformed
'obj_model': model_cloud,
# lo guardo solo para que se guarde la nube de puntos
# cuando hago las corridas
'detected_cloud': detected_cloud,
})
# Corro varias veces alignment e icp tratando de hacer bien la
# alineacion
transformed_model_cloud = model_cloud
# show_clouds(
# b'Objeto detectado cuadrado vs escena',
# scene_cloud,
# detected_cloud,
# )
# show_clouds(
# b'Modelo vs objeto detectado cuadrado',
# model_cloud,
# detected_cloud,
# )
best_result_cloud = None
best_threshold = self.icp_threshold
for i in range(3):
# Calculate alignment
ap_result = align(transformed_model_cloud, detected_cloud,
self._ap_defaults)
# Calculate ICP
if ap_result.has_converged:
transformed_model_cloud = ap_result.cloud
icp_res = icp(transformed_model_cloud, detected_cloud,
self._icp_defaults)
if icp_res.has_converged:
transformed_model_cloud = icp_res.cloud
if icp_res.has_converged and icp_res.score < best_threshold:
best_threshold = icp_res.score
best_result_cloud = icp_res.cloud
# show_clouds(
# b'Modelo detectado vs escena',
# scene_cloud,
# transformed_model_cloud,
# )
if best_result_cloud is not None:
# Filtro los puntos de la escena que se corresponden con el
# objeto que estoy buscando
obj_scene_cloud = filter_object_from_scene_cloud(
best_result_cloud, # object
detected_cloud, # partial scene
self.leaf_size, # radius
False, # show values
)
# show_clouds(
# b'Modelo detectado y filtrado vs escena',
# scene_cloud,
# obj_scene_cloud,
# )
obj_scene_points = points(obj_scene_cloud)
extraccion_exitosa = obj_scene_points > accepted_points
if extraccion_exitosa:
minmax = get_min_max(obj_scene_cloud)
detected_descriptors.update({
'min_x_cloud':
minmax.min_x,
'max_x_cloud':
minmax.max_x,
'min_y_cloud':
minmax.min_y,
'max_y_cloud':
minmax.max_y,
'min_z_cloud':
minmax.min_z,
'max_z_cloud':
minmax.max_z,
'object_cloud':
obj_scene_cloud,
# original model transformed
'obj_model':
best_result_cloud,
# lo guardo solo para que se guarde la nube de puntos
# cuando hago las corridas
'detected_cloud':
best_result_cloud,
})
# show_clouds(
# b'Extraccion EXITOSA. Viendo objeto transformado',
# scene_cloud,
# transformed_model_cloud,
# )
return detected_descriptors
def simple_follow(self, object_cloud, target_cloud):
"""
Tomando como centro el centro del cuadrado que contiene al objeto
en el frame anterior, busco el mismo objeto en una zona N veces mayor
a la original.
"""
topleft = self._descriptors['topleft']
bottomright = self._descriptors['bottomright']
img = self._descriptors['scene_rgb']
height = bottomright[0] - topleft[0]
width = bottomright[1] - topleft[1]
height_move = int(height / 2) + 1
width_move = int(width / 2) + 1
depth_img = self._descriptors['depth_img']
filas = len(depth_img)
columnas = len(depth_img[0])
search_topleft = (
max(topleft[0] - height_move, 0),
max(topleft[1] - width_move, 0)
)
search_bottomright = (
min(bottomright[0] + height_move, filas - 1),
min(bottomright[1] + width_move, columnas - 1)
)
rows_cols_limits = from_flat_to_cloud_limits(
search_topleft,
search_bottomright,
depth_img,
)
r_top_limit = rows_cols_limits[0][0]
r_bottom_limit = rows_cols_limits[0][1]
c_left_limit = rows_cols_limits[1][0]
c_right_limit = rows_cols_limits[1][1]
target_cloud = filter_cloud(
target_cloud,
str("y"),
float(r_top_limit),
float(r_bottom_limit)
)
target_cloud = filter_cloud(
target_cloud,
str("x"),
float(c_left_limit),
float(c_right_limit)
)
#################################################
# show_clouds(
# b'Modelo vs zona de busqueda en la escena',
# target_cloud,
# object_cloud
# )
#
# # Si se quiere ver como va buscando, descomentar la siguiente linea
# MuestraBusquedaEnVivo('Buscando el objeto').run(
# img,
# search_topleft,
# search_bottomright,
# frenar=True,
# )
#################################################
if points(target_cloud) > 0:
# Calculate ICP
icp_result = icp(object_cloud, target_cloud, self._icp_defaults)
else:
icp_result = ICPResult()
icp_result.has_converged = False
icp_result.score = 100
return icp_result
def alignment_prerejective(self, descriptors):
# obtengo tamaño del objeto detetado y me quedo con uno X veces mas grande
model_cloud = descriptors['detected_cloud']
if not self.adapt_leaf.was_started():
model_points = self._descriptors['static_obj_model_points']
self.adapt_leaf.set_first_values(model_points)
obj_limits = get_min_max(model_cloud)
length_func = lambda mul, l: l * mul / 2.0
max_length = max(
obj_limits.max_x - obj_limits.min_x,
obj_limits.max_y - obj_limits.min_y,
obj_limits.max_z - obj_limits.min_z,
)
x_center = obj_limits.max_x - (obj_limits.max_x - obj_limits.min_x)
y_center = obj_limits.max_y - (obj_limits.max_y - obj_limits.min_y)
z_center = obj_limits.max_z - (obj_limits.max_z - obj_limits.min_z)
half_side_length = length_func(self.obj_mult, max_length)
# obtengo limites de la escena
scene_cloud = self._descriptors['pcd']
# Filtro la escena y me quedo con la bounding-box de la deteccion por
# transformaciones
cloud = filter_cloud(
scene_cloud,
b'x',
x_center - half_side_length,
x_center + half_side_length
)
cloud = filter_cloud(
cloud,
b'y',
y_center - half_side_length,
y_center + half_side_length
)
cloud = filter_cloud(
cloud,
b'z',
z_center - half_side_length,
z_center + half_side_length
)
# show_clouds(
# b'Escena filtrando el bounding-box',
# cloud,
# model_cloud,
# )
detected_descriptors = {
'topleft': (0, 0), # (fila, columna)
'bottomright': 0,
}
fue_exitoso = False
accepted_points = (
self._descriptors['static_obj_model_points'] *
self.perc_obj_model_points
)
if points(cloud) > accepted_points:
ap_result = self._best_alignment_prerejective(model_cloud, cloud)
# print "Convergio AP:", ap_result.has_converged
# print "Score AP:", ap_result.score, "(<", self.umbral_score, ")"
# show_clouds(
# b'Escena filtrada vs alignment_prerejective',
# cloud,
# ap_result.cloud,
# )
if ap_result.has_converged and ap_result.score < self.umbral_score:
# Calculate ICP
icp_result = icp(ap_result.cloud, cloud, self._icp_defaults)
# print "Convergio ICP:", icp_result.has_converged
# print "Score ICP:", icp_result.score, "(<", self.umbral_score, ")"
# show_clouds(
# b'Escena filtrada vs icp',
# cloud,
# icp_result.cloud,
# )
if (icp_result.has_converged and
icp_result.score < self.umbral_score):
# Filtro los puntos de la escena que se corresponden con el
# objeto que estoy buscando
obj_scene_cloud = filter_object_from_scene_cloud(
icp_result.cloud, # object
scene_cloud, # complete scene
self.adapt_leaf.leaf_ratio(), # radius
False, # show values
)
obj_scene_points = points(obj_scene_cloud)
fue_exitoso = obj_scene_points > accepted_points
if fue_exitoso:
self.adapt_leaf.set_found_points(obj_scene_points)
else:
self.adapt_leaf.reset()
minmax = get_min_max(obj_scene_cloud)
topleft, bottomright = from_cloud_to_flat_limits(
obj_scene_cloud
)
tam_region = max(bottomright[0] - topleft[0],
bottomright[1] - topleft[1])
detected_descriptors.update({
'min_x_cloud': minmax.min_x,
'max_x_cloud': minmax.max_x,
'min_y_cloud': minmax.min_y,
'max_y_cloud': minmax.max_y,
'min_z_cloud': minmax.min_z,
'max_z_cloud': minmax.max_z,
'object_cloud': obj_scene_cloud,
'detected_cloud': icp_result.cloud, # lo guardo solo para la estadistica
'size': tam_region,
'location': topleft,
'topleft': topleft, # (fila, columna)
'bottomright': bottomright,
})
# show_clouds(
# b'Modelo detectado y filtrado vs escena',
# scene_cloud,
# obj_scene_cloud,
# )
# show_clouds(
# b'Modelo detectado por TRANSF, AP e ICP vs escena',
# scene_cloud,
# icp_result.cloud,
# )
return fue_exitoso, detected_descriptors
def detect(self):
# from analisis import Rectangle
# Deteccion RGB
rgb_fue_exitoso, rgb_desc = super(RGBDDetector, self).detect()
depth_desc = {
'topleft': (0, 0), # (fila, columna)
'bottomright': 0,
}
depth_fue_exitoso = False
if rgb_fue_exitoso:
# Tomo los datos del objeto a encontrar
model_cloud = self._descriptors['obj_model']
model_cloud_points = self._descriptors['obj_model_points']
# Parametro de aceptacion del resultado nro 1
accepted_points = model_cloud_points * self.perc_obj_model_points
# Esto es para adaptar el rango de busqueda de puntos de la escena
if not self.adapt_leaf.was_started():
self.adapt_leaf.set_first_values(model_cloud_points)
# Nube de puntos de la escena
scene_cloud = self._descriptors['pcd']
# Tamaño de la escena RGB
rgb_height = self._descriptors['scene_rgb'].shape[0]
rgb_width = self._descriptors['scene_rgb'].shape[1]
topleft = rgb_desc['topleft']
bottomright = rgb_desc['bottomright']
# MuestraBusquedaEnVivo('Deteccion RGB').run(
# self._descriptors['scene_rgb'],
# topleft,
# bottomright,
# )
# Obtengo un bounding rectangulo cuya longitud en x e y es el doble
# del encontrado por RGB pero con mismo centro de masa
width = abs(bottomright[1] - topleft[1])
height = abs(bottomright[0] - topleft[0])
extra_width = max(int(width * self.depth_area_extra), 1)
extra_height = max(int(height * self.depth_area_extra), 1)
new_topleft = (
max(topleft[0] - extra_height, 0),
max(topleft[1] - extra_width, 0)
)
new_bottomright = (
min(bottomright[0] + extra_height, rgb_height - 1),
min(bottomright[1] + extra_width, rgb_width - 1)
)
# Rectangulo RGB y rectangulo nuevo
# found_rgb = Rectangle(topleft, bottomright)
# new_rgb = Rectangle(new_topleft, new_bottomright)
# print "Rectangulo RGB detectado:", found_rgb.area()
# print "Rectangulo RGB para Depth:", new_rgb.area()
#
# MuestraBusquedaEnVivo('Deteccion RGB').run(
# self._descriptors['scene_rgb'],
# new_topleft,
# new_bottomright,
# )
# Paso los valores del nuevo bounding box de RGB a DEPTH
depth_img = self._descriptors['depth_img']
top_bottom, left_right = from_flat_to_cloud_limits(
new_topleft,
new_bottomright,
depth_img
)
# print "Depth topleft: (", top_bottom[0], ",", left_right[0], ")"
# print "Depth bottomright: (", top_bottom[1], ",", left_right[1], ")"
# Filtro la escena y obtengo solo los puntos señalados por la
# deteccion RGB
first_cloud = filter_cloud(
scene_cloud,
b'x', # X son las columnas
left_right[0],
left_right[1],
)
cloud = filter_cloud(
first_cloud,
b'y', # Y son las filas
top_bottom[0],
top_bottom[1],
)
best_aligned_scene = None
best_alignment_score = self.umbral_score # lesser is better
# Corro la deteccion en depth varias veces, solo si la cantidad de
# puntos es adecuada. Para mejorar un poco la condicion inicial de
# cada corrida, el modelo de objeto que uso es el que va quedando
# despues de cada alineacion, haya sido fallida o no
if points(cloud) > model_cloud_points:
for i in range(3):
# Calculate alignment
ap_result = align(model_cloud, cloud, self._ap_defaults)
if ap_result.has_converged:
model_cloud = ap_result.cloud
if ap_result.score < best_alignment_score:
best_alignment_score = ap_result.score
best_aligned_scene = ap_result.cloud
# Su hubo una buena alineacion
if best_aligned_scene is not None:
# print " HUBO UNA ALINEACION CORRECTA!!!!"
# Calculate ICP
icp_result = icp(best_aligned_scene, cloud, self._icp_defaults)
if (icp_result.has_converged and
icp_result.score < self.umbral_score):
# print " HUBO UN ICP CORRECTO!!!!"
# Filtro los puntos de la escena que se corresponden con el
# objeto que estoy buscando
# show_clouds(
# b'Modelo alineado por AP e ICP vs escena parcial segun RGB',
# icp_result.cloud,
# cloud,
# )
# show_clouds(
# b'Modelo alineado por AP e ICP vs escena completa',
# icp_result.cloud,
# scene_cloud,
# )
obj_scene_cloud = filter_object_from_scene_cloud(
icp_result.cloud, # object
cloud, # partial scene
self.adapt_leaf.leaf_ratio(), # radius
False, # show values
)
obj_scene_points = points(obj_scene_cloud)
depth_fue_exitoso = obj_scene_points > accepted_points
if depth_fue_exitoso:
self.adapt_leaf.set_found_points(obj_scene_points)
# print " HUBO UNA DETECCION!!!!"
else:
self.adapt_leaf.reset()
topleft, bottomright = from_cloud_to_flat_limits(
obj_scene_cloud
)
depth_desc.update({
'object_cloud': obj_scene_cloud,
'obj_model': icp_result.cloud, # original model transformed
'detected_cloud': icp_result.cloud, # lo guardo solo para la estadistica
'topleft': topleft, # (fila, columna)
'bottomright': bottomright,
})
# show_clouds(
# b'Modelo detectado vs escena',
# icp_result.cloud,
# scene_cloud
# )
rgb_desc.update(depth_desc)
return depth_fue_exitoso, rgb_desc
def detect(self):
model_cloud = self._descriptors['obj_model']
model_cloud_points = self._descriptors['obj_model_points']
accepted_points = model_cloud_points * self.perc_obj_model_points
if not self.adapt_leaf.was_started():
self.adapt_leaf.set_first_values(model_cloud_points)
scene_cloud = self._descriptors['pcd']
# obtengo tamaño del modelo del objeto a detectar y tomo una region
# X veces mas grande
obj_limits = get_min_max(model_cloud)
# obtengo limites de la escena
scene_limits = get_min_max(scene_cloud)
detected_descriptors = {
'topleft': (0, 0), # (fila, columna)
'bottomright': 0,
}
fue_exitoso = False
best_aligned_scene = None
best_alignment_score = self.umbral_score # lesser is better
best_limits = {}
# Busco la mejor alineacion del objeto segmentando la escena
for limits in (BusquedaPorFramesSolapados()
.iterate_frame_boxes(obj_limits, scene_limits,
obj_mult=self.obj_mult)):
cloud = filter_cloud(
scene_cloud,
b'x',
limits['min_x'],
limits['max_x']
)
cloud = filter_cloud(
cloud,
b'y',
limits['min_y'],
limits['max_y']
)
if points(cloud) > model_cloud_points:
# Calculate alignment
ap_result = align(model_cloud, cloud, self._ap_defaults)
if (ap_result.has_converged and
ap_result.score < best_alignment_score):
best_alignment_score = ap_result.score
best_aligned_scene = ap_result.cloud
best_limits.update(limits)
# Su hubo una buena alineacion
if best_aligned_scene is not None:
cloud = filter_cloud(
scene_cloud,
b'x',
best_limits['min_x'],
best_limits['max_x']
)
cloud = filter_cloud(
cloud,
b'y',
best_limits['min_y'],
best_limits['max_y']
)
# Calculate ICP
icp_result = icp(best_aligned_scene, cloud, self._icp_defaults)
if icp_result.has_converged and icp_result.score < self.umbral_score:
# Filtro los puntos de la escena que se corresponden con el
# objeto que estoy buscando
obj_scene_cloud = filter_object_from_scene_cloud(
icp_result.cloud, # object
scene_cloud, # complete scene
self.adapt_leaf.leaf_ratio(), # radius
False, # show values
)
obj_scene_points = points(obj_scene_cloud)
fue_exitoso = obj_scene_points > accepted_points
if fue_exitoso:
self.adapt_leaf.set_found_points(obj_scene_points)
else:
self.adapt_leaf.reset()
minmax = get_min_max(obj_scene_cloud)
topleft, bottomright = from_cloud_to_flat_limits(
obj_scene_cloud
)
detected_descriptors.update({
'min_x_cloud': minmax.min_x,
'max_x_cloud': minmax.max_x,
'min_y_cloud': minmax.min_y,
'max_y_cloud': minmax.max_y,
'min_z_cloud': minmax.min_z,
'max_z_cloud': minmax.max_z,
'object_cloud': obj_scene_cloud,
'obj_model': icp_result.cloud, # original model transformed
'detected_cloud': icp_result.cloud, # lo guardo solo para la estadistica
'topleft': topleft, # (fila, columna)
'bottomright': bottomright,
})
# show_clouds(
# b'Modelo detectado vs escena',
# icp_result.cloud,
# scene_cloud
# )
return fue_exitoso, detected_descriptors
def calculate_descriptors(self, detected_descriptors):
"""
Obtengo la nube de puntos correspondiente a la ubicacion y region
pasadas por parametro.
"""
detected_descriptors = (
super(StaticDetectorWithModelAlignment, self)
.calculate_descriptors(detected_descriptors)
)
model_cloud = self._descriptors['obj_model']
# scene_cloud = self._descriptors['pcd']
# Esta es la nube de puntos proveniente de filtrar el cuadrado marcado
# por la base de datos en la imagen RGB
detected_cloud = detected_descriptors['object_cloud']
accepted_points = (
self._descriptors['obj_model_points'] * self.perc_obj_model_pts
)
# Defino los valores minimos que debería tener el resultado. Si se
# pueden mejorar en el "for" de más adelante, genial!
minmax = get_min_max(detected_cloud)
detected_descriptors.update({
'min_x_cloud': minmax.min_x,
'max_x_cloud': minmax.max_x,
'min_y_cloud': minmax.min_y,
'max_y_cloud': minmax.max_y,
'min_z_cloud': minmax.min_z,
'max_z_cloud': minmax.max_z,
# original model transformed
'obj_model': model_cloud,
# lo guardo solo para que se guarde la nube de puntos
# cuando hago las corridas
'detected_cloud': detected_cloud,
})
# Corro varias veces alignment e icp tratando de hacer bien la
# alineacion
transformed_model_cloud = model_cloud
# show_clouds(
# b'Objeto detectado cuadrado vs escena',
# scene_cloud,
# detected_cloud,
# )
# show_clouds(
# b'Modelo vs objeto detectado cuadrado',
# model_cloud,
# detected_cloud,
# )
best_result_cloud = None
best_threshold = self.icp_threshold
for i in range(3):
# Calculate alignment
ap_result = align(
transformed_model_cloud,
detected_cloud,
self._ap_defaults
)
# Calculate ICP
if ap_result.has_converged:
transformed_model_cloud = ap_result.cloud
icp_res = icp(
transformed_model_cloud,
detected_cloud,
self._icp_defaults
)
if icp_res.has_converged:
transformed_model_cloud = icp_res.cloud
if icp_res.has_converged and icp_res.score < best_threshold:
best_threshold = icp_res.score
best_result_cloud = icp_res.cloud
# show_clouds(
# b'Modelo detectado vs escena',
# scene_cloud,
# transformed_model_cloud,
# )
if best_result_cloud is not None:
# Filtro los puntos de la escena que se corresponden con el
# objeto que estoy buscando
obj_scene_cloud = filter_object_from_scene_cloud(
best_result_cloud, # object
detected_cloud, # partial scene
self.leaf_size, # radius
False, # show values
)
# show_clouds(
# b'Modelo detectado y filtrado vs escena',
# scene_cloud,
# obj_scene_cloud,
# )
obj_scene_points = points(obj_scene_cloud)
extraccion_exitosa = obj_scene_points > accepted_points
if extraccion_exitosa:
minmax = get_min_max(obj_scene_cloud)
detected_descriptors.update({
'min_x_cloud': minmax.min_x,
'max_x_cloud': minmax.max_x,
'min_y_cloud': minmax.min_y,
'max_y_cloud': minmax.max_y,
'min_z_cloud': minmax.min_z,
'max_z_cloud': minmax.max_z,
'object_cloud': obj_scene_cloud,
# original model transformed
'obj_model': best_result_cloud,
# lo guardo solo para que se guarde la nube de puntos
# cuando hago las corridas
'detected_cloud': best_result_cloud,
})
# show_clouds(
# b'Extraccion EXITOSA. Viendo objeto transformado',
# scene_cloud,
# transformed_model_cloud,
# )
return detected_descriptors