Detección y cálculo de posición de objetos (cámara web)

< volver a principal

Detección de objetos

Para esta tarea se ha empleado el package find_object_2d desarrollado por Mathieu Labbé (Université de Sherbrooke ). Se trata de una aplicación con entorno gráfico que permite detectar objetos presentes en la escena, de los cuales ha sido previamente guardada una imagen con sus características.

Captura find_object_2d

En la captura del entorno gráfico podemos observar como aparece enmarcado el objeto encontrado en la escena. Este marco se sitúa en las esquinas del objeto a partir de la imagen guardada con sus características. El entorno dispone de un menú para añadir objetos a partir de objetos presentes en la escena, tomando una imagen y seleccionando el área donde se encuentra el objeto. Estos objetos añadidos desde las escena podemos guardarlos para volver a usarlos.

La aplicación proporciona diversos datos de cada objeto encontrado en la escena, pero vamos a utilizar los siguientes:

• Ancho del objeto muestra (número de píxeles de ancho de la imagen de muestra).
• Alto del objeto muestra (número de píxeles de alto de la imagen de muestra).
• Posición de las esquinas del objeto en la escena.

Cálculo de posición de objetos

A partir de los datos obtenidos del package find_object_2d se realizan una serie de cálculos para determinar la posición del objeto respecto al brazo del MYRAbot. Para esto necesitaremos conocer y establecer una serie de parámetros físicos:

• Posición de la cámara web respecto al brazo (h, d).
• Ángulo de inclinación de la cámara web respecto a la horizontal (α).
• Distancia perpendicular al objetivo de la cámara a la que vamos a situar los objetos para obtener la imagen de muestra (z).

Esquema cálculo posición objetos

Para el cálculo de la distancia a los objetos se ha tomado el área de estos como base de cálculo, ya que su variación entre las imágenes tomadas es proporcional y lineal respecto a la distancia al objetivo de la cámara. Las ecuaciones obtenidas a partir del esquema son las siguientes, los parámetros se han obtenido realizando mediciones para diferentes posiciones conocidas de un objeto:

Programa de publicación de posición de objetos

Lo primero crearemos unos mensajes personalizados dentro del package "find_object_2d" para la publicación de las posiciones de los objetos en la escena. Como en la escena puede haber más de un objeto reconocido debemos guardar los datos en un array, de dimensión el número de objetos encontrados. Comenzaremos creando un mensaje base llamado "Point_id.msg" con el siguiente contenido, el identificador del objeto y el punto donde se encuentra:

int16 id
geometry_msgs/Point punto

Ahora necesitamos crear un mensaje que contenga un array de los mensajes anteriores. Este mensaje lo llamaremos "PointObjects.msg" con el siguiente contenido:

Point_id[] objeto

El programa crea el nodo "objects_detected" que está suscrito al topic "objects", publicado por el programa "find_object_2d_node", y publica el topic "point", con el identificador y posición de los objetos encontrados en la escena. Crearemos en este mismo package un archivo llamado "objects_detected.cpp" en el directorio "src" con el siguiente contenido:

#include <ros/ros.h>
#include <std_msgs/Float32MultiArray.h>
#include <opencv2/opencv.hpp>
#include <QTransform>
#include <geometry_msgs/Point.h>
#include <std_msgs/Int16.h>
#include <find_object_2d/PointObjects.h>
#include <find_object_2d/Point_id.h>

#define dZ0 450
#define alfa 40
#define h 310
#define d 50
#define PI 3.14159265


void objectsDetectedCallback(const std_msgs::Float32MultiArray& msg)
{
    ros::NodeHandle nh;
    ros::Publisher position_pub_=nh.advertise<find_object_2d::PointObjects>("point", 1);
   
    find_object_2d::PointObjects p_objects;
    find_object_2d::Point_id objeto;
           
    p_objects.objeto = std::vector<find_object_2d::Point_id>(msg.data.size()/12);
           
    for(unsigned int i=0; i<msg.data.size(); i+=12)
    {
        // get data
        int id = (int)msg.data[i];
        float objectWidth = msg.data[i+1];
        float objectHeight = msg.data[i+2];

        // Find corners Qt
        QTransform qtHomography(msg.data[i+3], msg.data[i+4], msg.data[i+5],
                                msg.data[i+6], msg.data[i+7], msg.data[i+8],
                                msg.data[i+9], msg.data[i+10], msg.data[i+11]);

        QPointF qtTopLeft = qtHomography.map(QPointF(0,0));
        QPointF qtTopRight = qtHomography.map(QPointF(objectWidth,0));
        QPointF qtBottomLeft = qtHomography.map(QPointF(0,objectHeight));
        QPointF qtBottomRight = qtHomography.map(QPointF(objectWidth,objectHeight));
       
        geometry_msgs::Point punto;
       
        float dArea = (objectHeight*objectWidth) - ((((qtTopRight.x() - qtTopLeft.x()) + (qtBottomRight.x() - qtBottomLeft.x()))/2)*(((qtBottomLeft.y() - qtTopLeft.y()) + (qtBottomRight.y() - qtTopRight.y()))/2));

        float dZ = dZ0 + (dArea/35);
       
        float dX = (((640/2) - (((qtTopLeft.x() + qtBottomLeft.x())/2) + (((qtTopRight.x() - qtTopLeft.x())+(qtBottomRight.x() - qtBottomLeft.x()))/4)))*dZ)/585;
       
        float dY = (((480/2) - (((qtTopLeft.y() + qtTopRight.y())/2) + (((qtBottomLeft.y() - qtTopLeft.y())+(qtBottomRight.y() - qtTopRight.y()))/4)))*dZ)/585;
       
        float beta = atan2(dY,dZ);
       
        punto.x = dX;
        punto.y = h-((dZ/cos(beta))*sin(((alfa*PI)/180)-beta));
        punto.z = ((dZ/cos(beta))*cos(((alfa*PI)/180)-beta))-d;
               
        objeto.punto = punto;
        objeto.id = id;
       
        p_objects.objeto[i/12] = objeto;

    }
   
    position_pub_.publish(p_objects);

}


int main(int argc, char** argv)
{
    ros::init(argc, argv, "objects_detected");

    ros::NodeHandle nh;
    ros::Subscriber subs = nh.subscribe("objects", 1, objectsDetectedCallback);

    ros::Publisher position_pub_=nh.advertise<find_object_2d::PointObjects>("point", 1);  

    ros::spin();

    return 0;
}

Compilando el programa

Para generar el ejecutable de nuestro programa tenemos que compilarlo, para ello tenemos que añadir las siguientes líneas de código al archivo "CMakeLists.txt" del package:

rosbuild_add_executable(objects_detected src/objects_detected.cpp)
target_link_libraries(objects_detected ${LIBRARIES})

Ejecutando el programa

Para poder lanzar nuestra cámara web necesitamos instalar el package camera_umd y ejecutar el siguiente comando en un terminal:

rosrun uvc_camera camera_node

• nota: Puede que en nuestro equipo se encuentren conectadas más de una cámara web,y la que queremos emplear no sea el dispositivo por defecto "/dev/video0". Para poder cambiar el dispositivo que vamos a emplear devemos ejecutar el siguiente comando en un terminal, indicando el número de nuestro dispositivo:

rosparam set uvc_camera/device /dev/videoNÚMERO_DE_NUESTRO_DISPOSITIVO

Para lanzar la cámara web, ejecutar find_object_2d y nuestro programa en un solo paso vamos a crear un launcher que guardaremos en el directorio "launch" dentro del package con el nombre "encontrar_objetos.launch". El contenido del launch será el siguiente:

<launch>
 
  <node name="camera_node" pkg="uvc_camera" type="camera_node">
    <param name="device" value="/dev/videoNÚMERO_DE_NUESTRO_DISPOSITIVO"/>
  </node>
 
  <node name="find_object_2d" pkg="find_object_2d" type="find_object_2d">
    <remap from="image" to="image_raw"/>
  </node>
 
  <node name="objects_detected" pkg="find_object_2d" type="objects_detected" />
 
</launch>

< volver a principal