def find(self, es_deteccion):
# Obtengo pcd's y depth
object_cloud = self._descriptors['object_cloud']
target_cloud = self._descriptors['pcd']
obj_model = self._descriptors['obj_model']
model_points = points(obj_model)
self.adapt_area.set_default_distances(obj_model)
if not self.adapt_leaf.was_started():
self.adapt_leaf.set_first_values(model_points)
accepted_points = model_points * self.perc_obj_model_points
icp_result = self.simple_follow(
object_cloud,
target_cloud,
)
points_from_scene = 0
if icp_result.has_converged:
obj_from_scene_points = self.get_object_points_from_scene(
icp_result.cloud,
target_cloud,
)
points_from_scene = points(obj_from_scene_points)
fue_exitoso = icp_result.score < self.umbral_score
fue_exitoso = (
fue_exitoso and
points_from_scene >= accepted_points
)
descriptors = {}
# print "Convergio ICP buscador:", icp_result.has_converged
# print "Score ICP buscador:", icp_result.score, "(<", self.umbral_score, ")"
if fue_exitoso:
self.adapt_leaf.set_found_points(points_from_scene)
# filas = len(depth_img)
# columnas = len(depth_img[0])
# Busco los limites en el dominio de las filas y columnas del RGB
topleft, bottomright = from_cloud_to_flat_limits(
obj_from_scene_points
)
descriptors.update({
'topleft': topleft,
'bottomright': bottomright,
'detected_cloud': obj_from_scene_points,
'detected_transformation': icp_result.transformation,
})
else:
self.adapt_leaf.reset()
return fue_exitoso, descriptors
def train(self):
obj_model = self.img_provider.obj_pcd()
pts = points(obj_model)
self._obj_descriptors.update({
'obj_model': obj_model,
'obj_model_points': pts,
'static_obj_model': obj_model,
'static_obj_model_points': pts,
})
def detect(self):
fue_exitoso, detected_descriptors = (super(
DepthStaticDetectorWithPCDFiltering, self).detect())
# Si lo que se detecto en RGB no posee la cantidad de puntos minimos
# necesarios en depth, se considera no detectado aunque la BD diga lo
# contrario
ubicacion = detected_descriptors['location']
tam_region = detected_descriptors['size']
depth_img = self._descriptors['depth_img']
filas = len(depth_img)
columnas = len(depth_img[0])
ubicacion_punto_diagonal = (min(ubicacion[0] + tam_region, filas - 1),
min(ubicacion[1] + tam_region,
columnas - 1))
rows_cols_limits = from_flat_to_cloud_limits(
ubicacion,
ubicacion_punto_diagonal,
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]
cloud = self._descriptors['pcd']
cloud = filter_cloud(cloud, str("y"), float(r_top_limit),
float(r_bottom_limit))
cloud = filter_cloud(cloud, str("x"), float(c_left_limit),
float(c_right_limit))
detected_descriptors.update({
'object_cloud': cloud,
'min_x_cloud': c_left_limit,
'max_x_cloud': c_right_limit,
'min_y_cloud': r_top_limit,
'max_y_cloud': r_bottom_limit,
})
accepted_points = (self._descriptors['obj_model_points'] *
self.perc_obj_model_pts)
if points(cloud) < accepted_points:
detected_descriptors = {
'size': 0,
'location': (0, 0), # location=(fila, columna)
'topleft': (0, 0),
'bottomright': (0, 0),
}
fue_exitoso = False
return fue_exitoso, detected_descriptors
def train(self):
obj_model = self.img_provider.obj_pcd()
pts = points(obj_model)
self._obj_descriptors.update(
{
'obj_model': obj_model,
'obj_model_points': pts,
'static_obj_model': obj_model,
'static_obj_model_points': pts,
}
)
def segmentando_escena():
img_provider = FrameNamesAndImageProvider(
'videos/rgbd/scenes/', 'desk', '1',
'videos/rgbd/objs/', 'coffee_mug', '5',
)
obj_pcd = img_provider.obj_pcd()
min_max = get_min_max(obj_pcd)
obj_width = (min_max.max_x - min_max.min_x) * 2
obj_height = (min_max.max_y - min_max.min_y) * 2
pcd = img_provider.pcd()
min_max = get_min_max(pcd)
scene_min_col = min_max.min_x
scene_max_col = min_max.max_x
scene_min_row = min_max.min_y
scene_max_row = min_max.max_y
path = b'pruebas_guardadas/pcd_segmentado/'
save_pcd(pcd, path + b'scene.pcd')
# #######################################################
# Algunos calculos a mano para corroborar que ande bien
# #######################################################
scene_width = scene_max_col - scene_min_col
frames_ancho = (scene_width / obj_width) * 2 - 1
print("Frames a lo ancho:", round(frames_ancho))
scene_height = scene_max_row - scene_min_row
frames_alto = (scene_height / obj_height) * 2 - 1
print("Frames a lo alto:", round(frames_alto))
print("Frames totales supuestos:", frames_ancho * frames_alto)
########################################################
counter = 0
for limits in (BusquedaPorFramesSolapados()
.iterate_frame_boxes(scene_min_col,
scene_max_col,
scene_min_row,
scene_max_row,
obj_width,
obj_height)):
cloud = filter_cloud(pcd, b'x', limits['min_x'], limits['max_x'])
cloud = filter_cloud(cloud, b'y', limits['min_y'], limits['max_y'])
if points(cloud) > 0:
save_pcd(cloud, path + b'filtered_scene_{i}_box.pcd'.format(i=counter))
counter += 1
def train(self):
# Depth
obj_model = self.img_provider.obj_pcd()
pts = points(obj_model)
self._obj_descriptors.update({
'obj_model': obj_model,
'obj_model_points': pts,
})
# RGB
obj_templates, obj_masks = self.img_provider.obj_rgb_templates_and_masks(
)
self._obj_descriptors.update({
'object_templates': obj_templates,
'object_masks': obj_masks,
})
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 train(self):
# Depth
obj_model = self.img_provider.obj_pcd()
pts = points(obj_model)
self._obj_descriptors.update(
{
'obj_model': obj_model,
'obj_model_points': pts,
}
)
# RGB
obj_templates, obj_masks = self.img_provider.obj_rgb_templates_and_masks()
self._obj_descriptors.update(
{
'object_templates': obj_templates,
'object_masks': obj_masks,
}
)
def from_cloud_to_flat_limits(cloud):
top = 1e10
left = 1e10
bottom = 0
right = 0
for i in range(points(cloud)):
point_xyz = get_point(cloud, i)
if point_xyz.z > 0:
point_flat = from_cloud_to_flat(point_xyz.y, point_xyz.x,
point_xyz.z)
top = min(point_flat[0], top)
bottom = max(point_flat[0], bottom)
left = min(point_flat[1], left)
right = max(point_flat[1], right)
#### NUEVO Y MAS CORTO METODO
# minmax = get_min_max(cloud)
# topleft_1 = from_cloud_to_flat(minmax.min_y, minmax.min_x, minmax.min_z)
# topleft_2 = from_cloud_to_flat(minmax.min_y, minmax.min_x, minmax.max_z)
# topright_1 = from_cloud_to_flat(minmax.min_y, minmax.max_x, minmax.min_z)
# topright_2 = from_cloud_to_flat(minmax.min_y, minmax.max_x, minmax.max_z)
#
# bottomleft_1 = from_cloud_to_flat(minmax.max_y, minmax.min_x, minmax.min_z)
# bottomleft_2 = from_cloud_to_flat(minmax.max_y, minmax.min_x, minmax.max_z)
# bottomright_1 = from_cloud_to_flat(minmax.max_y, minmax.max_x, minmax.min_z)
# bottomright_2 = from_cloud_to_flat(minmax.max_y, minmax.max_x, minmax.max_z)
#
# top = min(topleft_1[0], topleft_2[0], topright_1[0], topright_2[0])
# bottom = max(bottomleft_1[0], bottomleft_2[0],
# bottomright_1[0], bottomright_2[0])
#
# left = min(topleft_1[1], topleft_2[1], bottomleft_1[1], bottomleft_2[1])
# right = max(topright_1[1], topright_2[1],
# bottomright_1[1], bottomright_2[1])
####
return (top, left), (bottom, right)
def from_cloud_to_flat_limits(cloud):
top = 1e10
left = 1e10
bottom = 0
right = 0
for i in range(points(cloud)):
point_xyz = get_point(cloud, i)
if point_xyz.z > 0:
point_flat = from_cloud_to_flat(point_xyz.y, point_xyz.x, point_xyz.z)
top = min(point_flat[0], top)
bottom = max(point_flat[0], bottom)
left = min(point_flat[1], left)
right = max(point_flat[1], right)
#### NUEVO Y MAS CORTO METODO
# minmax = get_min_max(cloud)
# topleft_1 = from_cloud_to_flat(minmax.min_y, minmax.min_x, minmax.min_z)
# topleft_2 = from_cloud_to_flat(minmax.min_y, minmax.min_x, minmax.max_z)
# topright_1 = from_cloud_to_flat(minmax.min_y, minmax.max_x, minmax.min_z)
# topright_2 = from_cloud_to_flat(minmax.min_y, minmax.max_x, minmax.max_z)
#
# bottomleft_1 = from_cloud_to_flat(minmax.max_y, minmax.min_x, minmax.min_z)
# bottomleft_2 = from_cloud_to_flat(minmax.max_y, minmax.min_x, minmax.max_z)
# bottomright_1 = from_cloud_to_flat(minmax.max_y, minmax.max_x, minmax.min_z)
# bottomright_2 = from_cloud_to_flat(minmax.max_y, minmax.max_x, minmax.max_z)
#
# top = min(topleft_1[0], topleft_2[0], topright_1[0], topright_2[0])
# bottom = max(bottomleft_1[0], bottomleft_2[0],
# bottomright_1[0], bottomright_2[0])
#
# left = min(topleft_1[1], topleft_2[1], bottomleft_1[1], bottomleft_2[1])
# right = max(topright_1[1], topright_2[1],
# bottomright_1[1], bottomright_2[1])
####
return (top, left), (bottom, right)
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 detect(self):
fue_exitoso, detected_descriptors = (
super(DepthStaticDetectorWithPCDFiltering, self).detect()
)
# Si lo que se detecto en RGB no posee la cantidad de puntos minimos
# necesarios en depth, se considera no detectado aunque la BD diga lo
# contrario
ubicacion = detected_descriptors['location']
tam_region = detected_descriptors['size']
depth_img = self._descriptors['depth_img']
filas = len(depth_img)
columnas = len(depth_img[0])
ubicacion_punto_diagonal = (
min(ubicacion[0] + tam_region, filas - 1),
min(ubicacion[1] + tam_region, columnas - 1)
)
rows_cols_limits = from_flat_to_cloud_limits(
ubicacion,
ubicacion_punto_diagonal,
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]
cloud = self._descriptors['pcd']
cloud = filter_cloud(
cloud,
str("y"),
float(r_top_limit),
float(r_bottom_limit)
)
cloud = filter_cloud(
cloud,
str("x"),
float(c_left_limit),
float(c_right_limit)
)
detected_descriptors.update(
{
'object_cloud': cloud,
'min_x_cloud': c_left_limit,
'max_x_cloud': c_right_limit,
'min_y_cloud': r_top_limit,
'max_y_cloud': r_bottom_limit,
}
)
accepted_points = (
self._descriptors['obj_model_points'] * self.perc_obj_model_pts
)
if points(cloud) < accepted_points:
detected_descriptors = {
'size': 0,
'location': (0, 0), # location=(fila, columna)
'topleft': (0, 0),
'bottomright': (0, 0),
}
fue_exitoso = False
return fue_exitoso, 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 find(self, es_deteccion):
# Obtengo pcd's y depth
object_cloud = self._descriptors['obj_model']
target_cloud = self._descriptors['pcd']
obj_model = self._descriptors['obj_model']
model_points = self._descriptors['obj_model_points']
self.adapt_area.set_default_distances(obj_model)
if not self.adapt_leaf.was_started():
self.adapt_leaf.set_first_values(model_points)
accepted_points = model_points * self.perc_obj_model_points
icp_result = self.simple_follow(
object_cloud,
target_cloud,
)
points_from_scene = 0
if icp_result.has_converged:
obj_from_scene_points = self.get_object_points_from_scene(
icp_result.cloud,
target_cloud,
)
points_from_scene = points(obj_from_scene_points)
fue_exitoso = icp_result.score < self.umbral_score
fue_exitoso = (
fue_exitoso and
points_from_scene >= accepted_points
)
descriptors = {}
############################
# show_clouds(
# b'Modelo luego de icp vs zona de busqueda en la escena. Fue exitoso: {t}'.format(t=b'Y' if fue_exitoso else b'N'),
# icp_result.cloud,
# target_cloud
# )
############################
if fue_exitoso:
self.adapt_leaf.set_found_points(points_from_scene)
# filas = len(depth_img)
# columnas = len(depth_img[0])
# Busco los limites en el dominio de las filas y columnas del RGB
topleft, bottomright = from_cloud_to_flat_limits(
obj_from_scene_points
)
############################
# show_clouds(
# b'Nube de puntos tomada de la escena vs zona de busqueda en la escena',
# target_cloud,
# obj_from_scene_points
# )
############################
descriptors.update({
'topleft': topleft,
'bottomright': bottomright,
'detected_cloud': obj_from_scene_points,
'detected_transformation': icp_result.transformation,
})
else:
self.adapt_leaf.reset()
return fue_exitoso, 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 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 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