def probando_filtro_por_ejes():
img_provider = FrameNamesAndImageProvider(
'videos/rgbd/scenes/', 'desk', '1',
'videos/rgbd/objs/', 'coffee_mug', '5',
)
pcd = img_provider.pcd()
filtered_pcd = img_provider.pcd()
min_max = get_min_max(pcd)
left = min_max.max_x - 0.46
right = min_max.max_x - 0.35
diff = right - left
lefter_left = left - (diff / 2)
righter_right = right + (diff / 2)
filtered_pcd = filter_cloud(filtered_pcd, b"x", lefter_left, righter_right)
show_clouds(b"completo vs filtrado amplio en x", pcd, filtered_pcd)
filtered_pcd = filter_cloud(filtered_pcd, b"x", left, right)
show_clouds(b"completo vs filtrado angosto en x", pcd, filtered_pcd)
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 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 _filter_target_cloud(self, target_cloud):
# TODO: ver http://docs.pointclouds.org/1.7.0/crop__box_8h_source.html
# TODO: ver http://www.pcl-users.org/How-to-use-Crop-Box-td3888183.html
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']
d_front_limit = self._descriptors['min_z_cloud']
d_back_limit = self._descriptors['max_z_cloud']
# Define row and column limits for the zone to search the object
x_move = self.adapt_area.estimate_distance('x')
y_move = self.adapt_area.estimate_distance('y')
z_move = self.adapt_area.estimate_distance('z')
if x_move < 0 or y_move < 0 or z_move < 0:
raise Exception(('Ojo que esta mal el area de busqueda. '
'x,y,z={x},{y},{z}'.format(x=x_move, y=y_move,
z=z_move)))
r_top_limit -= y_move
r_bottom_limit += y_move
c_left_limit -= x_move
c_right_limit += x_move
d_front_limit -= z_move
d_back_limit += z_move
# 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)
)
target_cloud = filter_cloud(
target_cloud,
str("z"),
float(d_front_limit),
float(d_back_limit)
)
return target_cloud
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 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 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