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

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 posición 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/PointObject.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::PointObject>("point", 1);
    
    find_object_2d::PointObject p_object;
   	find_object_2d::Point_id objeto;
            
    p_object.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)))*Z_distance)/585;
        
        float dY = (((480/2) - (((qtTopLeft.x() + qtTopRight.x())/2) + (((qtBottomLeft.y() - qtTopLeft.y())+(qtBottomRight.y() - qtTopRight.y()))/4)))*Z_distance)/585;
        
        float beta = atan2(dY,dZ);
        
        punto.x = dX;
        punto.y = h-((Z_distance/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_object.objeto[i/12] = objeto;

    }
    
    position_pub_.publish(p_object);

}


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

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

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

    ros::spin();

    return 0;
}