def find(self, es_deteccion):
# Obtengo pcd's y depth
object_cloud = self._descriptors['object_cloud']
target_cloud = self._descriptors['pcd']
icp_result = self.simple_follow(
object_cloud,
target_cloud,
)
fue_exitoso = icp_result.score < self.umbral_score
descriptors = {}
if fue_exitoso:
# 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(
icp_result.cloud
)
descriptors.update({
'topleft': topleft,
'bottomright': bottomright,
'detected_cloud': icp_result.cloud,
'detected_transformation': icp_result.transformation,
})
return fue_exitoso, descriptors
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 transformation_detect(self):
nframe = self._descriptors['nframe']
model = self._descriptors['static_obj_model']
transf_file = self.transf_file.format(nframe=nframe)
fue_exitoso = False
tam_region = 0
topleft = (0, 0)
bottomright = (0, 0)
detected_descriptors = {}
if os.path.isfile(transf_file):
fue_exitoso = True
with open(transf_file, 'r') as file_:
transf_matrix = VectorMat()
for line in file_:
if line.strip():
values = line.strip().split()
floats = [float(v) for v in values]
float_vec = FloatVector()
float_vec.extend(floats)
transf_matrix.append(float_vec)
# Deteccion por spin_images/fpfh/cshot
detected_model = transform_cloud(model, transf_matrix)
topleft, bottomright = from_cloud_to_flat_limits(detected_model)
tam_region = max(bottomright[0] - topleft[0],
bottomright[1] - topleft[1])
detected_descriptors.update({
'detected_cloud': detected_model,
})
# scene_cloud = self._descriptors['pcd']
# show_clouds(
# b'Modelo detectado (transformacion estatica) vs escena',
# scene_cloud,
# detected_model,
# )
detected_descriptors.update({
'size': tam_region,
'location': topleft, # topleft=(fila, columna)
'topleft': topleft,
'bottomright': bottomright,
})
return fue_exitoso, detected_descriptors
def transformation_detect(self):
nframe = self._descriptors['nframe']
model = self._descriptors['static_obj_model']
transf_file = self.transf_file.format(nframe=nframe)
fue_exitoso = False
tam_region = 0
topleft = (0, 0)
bottomright = (0, 0)
detected_descriptors = {}
if os.path.isfile(transf_file):
fue_exitoso = True
with open(transf_file, 'r') as file_:
transf_matrix = VectorMat()
for line in file_:
if line.strip():
values = line.strip().split()
floats = [float(v) for v in values]
float_vec = FloatVector()
float_vec.extend(floats)
transf_matrix.append(float_vec)
# Deteccion por spin_images/fpfh/cshot
detected_model = transform_cloud(model, transf_matrix)
topleft, bottomright = from_cloud_to_flat_limits(detected_model)
tam_region = max(bottomright[0] - topleft[0],
bottomright[1] - topleft[1])
detected_descriptors.update({
'detected_cloud': detected_model,
})
# scene_cloud = self._descriptors['pcd']
# show_clouds(
# b'Modelo detectado (transformacion estatica) vs escena',
# scene_cloud,
# detected_model,
# )
detected_descriptors.update({
'size': tam_region,
'location': topleft, # topleft=(fila, columna)
'topleft': topleft,
'bottomright': bottomright,
})
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
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 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 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