print_objects_detected_node.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2011-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 #include <ros/ros.h>
00029 #include <std_msgs/Float32MultiArray.h>
00030 #include <opencv2/opencv.hpp>
00031 #include <QTransform>
00032 
00038 void objectsDetectedCallback(const std_msgs::Float32MultiArray & msg)
00039 {
00040     printf("---\n");
00041     if(msg.data.size())
00042     {
00043                 for(unsigned int i=0; i<msg.data.size(); i+=12)
00044                 {
00045                         // get data
00046                         int id = (int)msg.data[i];
00047                         float objectWidth = msg.data[i+1];
00048                         float objectHeight = msg.data[i+2];
00049 
00050                         // Find corners Qt
00051                         QTransform qtHomography(msg.data[i+3], msg.data[i+4], msg.data[i+5],
00052                                                                         msg.data[i+6], msg.data[i+7], msg.data[i+8],
00053                                                                         msg.data[i+9], msg.data[i+10], msg.data[i+11]);
00054 
00055                         QPointF qtTopLeft = qtHomography.map(QPointF(0,0));
00056                         QPointF qtTopRight = qtHomography.map(QPointF(objectWidth,0));
00057                         QPointF qtBottomLeft = qtHomography.map(QPointF(0,objectHeight));
00058                         QPointF qtBottomRight = qtHomography.map(QPointF(objectWidth,objectHeight));
00059 
00060                         printf("Object %d detected, Qt corners at (%f,%f) (%f,%f) (%f,%f) (%f,%f)\n",
00061                                         id,
00062                                         qtTopLeft.x(), qtTopLeft.y(),
00063                                         qtTopRight.x(), qtTopRight.y(),
00064                                         qtBottomLeft.x(), qtBottomLeft.y(),
00065                                         qtBottomRight.x(), qtBottomRight.y());
00066 
00067                         // Example with OpenCV
00068                         if(0)
00069                         {
00070                                 // Find corners OpenCV
00071                                 cv::Mat cvHomography(3, 3, CV_32F);
00072                                 cvHomography.at<float>(0,0) = msg.data[i+3];
00073                                 cvHomography.at<float>(1,0) = msg.data[i+4];
00074                                 cvHomography.at<float>(2,0) = msg.data[i+5];
00075                                 cvHomography.at<float>(0,1) = msg.data[i+6];
00076                                 cvHomography.at<float>(1,1) = msg.data[i+7];
00077                                 cvHomography.at<float>(2,1) = msg.data[i+8];
00078                                 cvHomography.at<float>(0,2) = msg.data[i+9];
00079                                 cvHomography.at<float>(1,2) = msg.data[i+10];
00080                                 cvHomography.at<float>(2,2) = msg.data[i+11];
00081                                 std::vector<cv::Point2f> inPts, outPts;
00082                                 inPts.push_back(cv::Point2f(0,0));
00083                                 inPts.push_back(cv::Point2f(objectWidth,0));
00084                                 inPts.push_back(cv::Point2f(0,objectHeight));
00085                                 inPts.push_back(cv::Point2f(objectWidth,objectHeight));
00086                                 cv::perspectiveTransform(inPts, outPts, cvHomography);
00087 
00088                                 printf("Object %d detected, CV corners at (%f,%f) (%f,%f) (%f,%f) (%f,%f)\n",
00089                                                 id,
00090                                                 outPts.at(0).x, outPts.at(0).y,
00091                                                 outPts.at(1).x, outPts.at(1).y,
00092                                                 outPts.at(2).x, outPts.at(2).y,
00093                                                 outPts.at(3).x, outPts.at(3).y);
00094                         }
00095                 }
00096     }
00097     else
00098     {
00099         printf("No objects detected.\n");
00100     }
00101 }
00102 
00103 
00104 int main(int argc, char** argv)
00105 {
00106     ros::init(argc, argv, "objects_detected");
00107 
00108     ros::NodeHandle nh;
00109     ros::Subscriber subs;
00110     subs = nh.subscribe("objects", 1, objectsDetectedCallback);
00111 
00112     ros::spin();
00113 
00114     return 0;
00115 }


find_object_2d
Author(s): Mathieu Labbe
autogenerated on Fri Feb 12 2016 01:18:17