find_object_2d: FindObjectROS.cpp Source File

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 "FindObjectROS.h"
00029 
00030 #include <std_msgs/Float32MultiArray.h>
00031 #include "find_object_2d/ObjectsStamped.h"
00032 
00033 #include <cmath>
00034 
00035 using namespace find_object;
00036 
00037 FindObjectROS::FindObjectROS(const std::string & objFramePrefix, QObject * parent) :
00038         FindObject(parent),
00039         objFramePrefix_("object")
00040 {
00041         ros::NodeHandle pnh("~"); // public
00042         pnh.param("object_prefix", objFramePrefix_, objFramePrefix_);
00043 
00044         ros::NodeHandle nh; // public
00045 
00046         pub_ = nh.advertise<std_msgs::Float32MultiArray>("objects", 1);
00047         pubStamped_ = nh.advertise<find_object_2d::ObjectsStamped>("objectsStamped", 1);
00048 
00049         this->connect(this, SIGNAL(objectsFound(find_object::DetectionInfo)), this, SLOT(publish(find_object::DetectionInfo)));
00050 }
00051 
00052 void FindObjectROS::publish(const find_object::DetectionInfo & info)
00053 {
00054         // send tf before the message
00055         if(info.objDetected_.size() && !depth_.empty() && depthConstant_ != 0.0f)
00056         {
00057                 std::vector<tf::StampedTransform> transforms;
00058                 QMultiMap<int, QSize>::const_iterator iterSizes=info.objDetectedSizes_.constBegin();
00059                 for(QMultiMap<int, QTransform>::const_iterator iter=info.objDetected_.constBegin();
00060                         iter!=info.objDetected_.constEnd();
00061                         ++iter, ++iterSizes)
00062                 {
00063                         // get data
00064                         int id = iter.key();
00065                         float objectWidth = iterSizes->width();
00066                         float objectHeight = iterSizes->height();
00067 
00068                         // Find center of the object
00069                         QPointF center = iter->map(QPointF(objectWidth/2, objectHeight/2));
00070                         QPointF xAxis = iter->map(QPointF(3*objectWidth/4, objectHeight/2));
00071                         QPointF yAxis = iter->map(QPointF(objectWidth/2, 3*objectHeight/4));
00072 
00073                         cv::Vec3f center3D = this->getDepth(depth_,
00074                                         center.x()+0.5f, center.y()+0.5f,
00075                                         float(depth_.cols/2)-0.5f, float(depth_.rows/2)-0.5f,
00076                                         1.0f/depthConstant_, 1.0f/depthConstant_);
00077 
00078                         cv::Vec3f axisEndX = this->getDepth(depth_,
00079                                         xAxis.x()+0.5f, xAxis.y()+0.5f,
00080                                         float(depth_.cols/2)-0.5f, float(depth_.rows/2)-0.5f,
00081                                         1.0f/depthConstant_, 1.0f/depthConstant_);
00082 
00083                         cv::Vec3f axisEndY = this->getDepth(depth_,
00084                                         yAxis.x()+0.5f, yAxis.y()+0.5f,
00085                                         float(depth_.cols/2)-0.5f, float(depth_.rows/2)-0.5f,
00086                                         1.0f/depthConstant_, 1.0f/depthConstant_);
00087 
00088                         if(std::isfinite(center3D.val[0]) && std::isfinite(center3D.val[1]) && std::isfinite(center3D.val[2]) &&
00089                                 std::isfinite(axisEndX.val[0]) && std::isfinite(axisEndX.val[1]) && std::isfinite(axisEndX.val[2]) &&
00090                                 std::isfinite(axisEndY.val[0]) && std::isfinite(axisEndY.val[1]) && std::isfinite(axisEndY.val[2]))
00091                         {
00092                                 tf::StampedTransform transform;
00093                                 transform.setIdentity();
00094                                 transform.child_frame_id_ = QString("%1_%2").arg(objFramePrefix_.c_str()).arg(id).toStdString();
00095                                 transform.frame_id_ = frameId_;
00096                                 transform.stamp_ = stamp_;
00097                                 transform.setOrigin(tf::Vector3(center3D.val[0], center3D.val[1], center3D.val[2]));
00098 
00099                                 //set rotation (y inverted)
00100                                 tf::Vector3 xAxis(axisEndX.val[0] - center3D.val[0], axisEndX.val[1] - center3D.val[1], axisEndX.val[2] - center3D.val[2]);
00101                                 xAxis.normalize();
00102                                 tf::Vector3 yAxis(axisEndY.val[0] - center3D.val[0], axisEndY.val[1] - center3D.val[1], axisEndY.val[2] - center3D.val[2]);
00103                                 yAxis.normalize();
00104                                 tf::Vector3 zAxis = xAxis*yAxis;
00105                                 tf::Matrix3x3 rotationMatrix(
00106                                                         xAxis.x(), yAxis.x() ,zAxis.x(),
00107                                                         xAxis.y(), yAxis.y(), zAxis.y(),
00108                                                         xAxis.z(), yAxis.z(), zAxis.z());
00109                                 tf::Quaternion q;
00110                                 rotationMatrix.getRotation(q);
00111                                 // set x axis going front of the object, with z up and z left
00112                                 q *= tf::createQuaternionFromRPY(CV_PI/2.0, CV_PI/2.0, 0);
00113                                 transform.setRotation(q.normalized());
00114 
00115                                 transforms.push_back(transform);
00116                         }
00117                         else
00118                         {
00119                                 ROS_WARN("Object %d detected, center 2D at (%f,%f), but invalid depth, cannot set frame \"%s\"! "
00120                                                  "(maybe object is too near of the camera or bad depth image)\n",
00121                                                 id,
00122                                                 center.x(), center.y(),
00123                                                 QString("%1_%2").arg(objFramePrefix_.c_str()).arg(id).toStdString().c_str());
00124                         }
00125                 }
00126                 if(transforms.size())
00127                 {
00128                         tfBroadcaster_.sendTransform(transforms);
00129                 }
00130         }
00131 
00132         if(pub_.getNumSubscribers() || pubStamped_.getNumSubscribers())
00133         {
00134                 std_msgs::Float32MultiArray msg;
00135                 find_object_2d::ObjectsStamped msgStamped;
00136                 msg.data = std::vector<float>(info.objDetected_.size()*12);
00137                 msgStamped.objects.data = std::vector<float>(info.objDetected_.size()*12);
00138                 int i=0;
00139                 QMultiMap<int, QSize>::const_iterator iterSizes=info.objDetectedSizes_.constBegin();
00140                 for(QMultiMap<int, QTransform>::const_iterator iter=info.objDetected_.constBegin();
00141                         iter!=info.objDetected_.constEnd();
00142                         ++iter, ++iterSizes)
00143                 {
00144                         msg.data[i] = msgStamped.objects.data[i] = iter.key(); ++i;
00145                         msg.data[i] = msgStamped.objects.data[i] = iterSizes->width(); ++i;
00146                         msg.data[i] = msgStamped.objects.data[i] = iterSizes->height(); ++i;
00147                         msg.data[i] = msgStamped.objects.data[i] = iter->m11(); ++i;
00148                         msg.data[i] = msgStamped.objects.data[i] = iter->m12(); ++i;
00149                         msg.data[i] = msgStamped.objects.data[i] = iter->m13(); ++i;
00150                         msg.data[i] = msgStamped.objects.data[i] = iter->m21(); ++i;
00151                         msg.data[i] = msgStamped.objects.data[i] = iter->m22(); ++i;
00152                         msg.data[i] = msgStamped.objects.data[i] = iter->m23(); ++i;
00153                         msg.data[i] = msgStamped.objects.data[i] = iter->m31(); ++i;// dx
00154                         msg.data[i] = msgStamped.objects.data[i] = iter->m32(); ++i;// dy
00155                         msg.data[i] = msgStamped.objects.data[i] = iter->m33(); ++i;
00156                 }
00157                 if(pub_.getNumSubscribers())
00158                 {
00159                         pub_.publish(msg);
00160                 }
00161                 if(pubStamped_.getNumSubscribers())
00162                 {
00163                         // use same header as the input image (for synchronization and frame reference)
00164                         msgStamped.header.frame_id = frameId_;
00165                         msgStamped.header.stamp = stamp_;
00166                         pubStamped_.publish(msgStamped);
00167                 }
00168         }
00169 }
00170 
00171 void FindObjectROS::setDepthData(const std::string & frameId,
00172                 const ros::Time & stamp,
00173                 const cv::Mat & depth,
00174                 float depthConstant)
00175 {
00176         frameId_ = frameId;
00177         stamp_ = stamp;
00178         depth_ = depth;
00179         depthConstant_ = depthConstant;
00180 }
00181 
00182 cv::Vec3f FindObjectROS::getDepth(const cv::Mat & depthImage,
00183                                    int x, int y,
00184                                    float cx, float cy,
00185                                    float fx, float fy)
00186 {
00187         if(!(x >=0 && x<depthImage.cols && y >=0 && y<depthImage.rows))
00188         {
00189                 ROS_ERROR("Point must be inside the image (x=%d, y=%d), image size=(%d,%d)",
00190                                 x, y,
00191                                 depthImage.cols, depthImage.rows);
00192                 return cv::Vec3f(
00193                                 std::numeric_limits<float>::quiet_NaN (),
00194                                 std::numeric_limits<float>::quiet_NaN (),
00195                                 std::numeric_limits<float>::quiet_NaN ());
00196         }
00197 
00198 
00199         cv::Vec3f pt;
00200 
00201         // Use correct principal point from calibration
00202         float center_x = cx; //cameraInfo.K.at(2)
00203         float center_y = cy; //cameraInfo.K.at(5)
00204 
00205         bool isInMM = depthImage.type() == CV_16UC1; // is in mm?
00206 
00207         // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
00208         float unit_scaling = isInMM?0.001f:1.0f;
00209         float constant_x = unit_scaling / fx; //cameraInfo.K.at(0)
00210         float constant_y = unit_scaling / fy; //cameraInfo.K.at(4)
00211         float bad_point = std::numeric_limits<float>::quiet_NaN ();
00212 
00213         float depth;
00214         bool isValid;
00215         if(isInMM)
00216         {
00217                 depth = (float)depthImage.at<uint16_t>(y,x);
00218                 isValid = depth != 0.0f;
00219         }
00220         else
00221         {
00222                 depth = depthImage.at<float>(y,x);
00223                 isValid = std::isfinite(depth);
00224         }
00225 
00226         // Check for invalid measurements
00227         if (!isValid)
00228         {
00229                 pt.val[0] = pt.val[1] = pt.val[2] = bad_point;
00230         }
00231         else
00232         {
00233                 // Fill in XYZ
00234                 pt.val[0] = (float(x) - center_x) * depth * constant_x;
00235                 pt.val[1] = (float(y) - center_y) * depth * constant_y;
00236                 pt.val[2] = depth*unit_scaling;
00237         }
00238         return pt;
00239 }