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