FindObjectROS.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2011-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include "FindObjectROS.h"
29 
30 #include <std_msgs/Float32MultiArray.h>
31 #include "find_object_2d/ObjectsStamped.h"
32 
33 #include <cmath>
34 
35 using namespace find_object;
36 
37 FindObjectROS::FindObjectROS(QObject * parent) :
38  FindObject(true, parent),
39  objFramePrefix_("object")
40 {
41  ros::NodeHandle pnh("~"); // public
42  pnh.param("object_prefix", objFramePrefix_, objFramePrefix_);
43  ROS_INFO("object_prefix = %s", objFramePrefix_.c_str());
44 
45  ros::NodeHandle nh; // public
46 
47  pub_ = nh.advertise<std_msgs::Float32MultiArray>("objects", 1);
48  pubStamped_ = nh.advertise<find_object_2d::ObjectsStamped>("objectsStamped", 1);
49 
50  this->connect(this, SIGNAL(objectsFound(find_object::DetectionInfo)), this, SLOT(publish(find_object::DetectionInfo)));
51 }
52 
54 {
55  // send tf before the message
56  if(info.objDetected_.size() && !depth_.empty() && depthConstant_ != 0.0f)
57  {
58  std::vector<tf::StampedTransform> transforms;
59  QMultiMap<int, QSize>::const_iterator iterSizes=info.objDetectedSizes_.constBegin();
60  for(QMultiMap<int, QTransform>::const_iterator iter=info.objDetected_.constBegin();
61  iter!=info.objDetected_.constEnd();
62  ++iter, ++iterSizes)
63  {
64  // get data
65  int id = iter.key();
66  float objectWidth = iterSizes->width();
67  float objectHeight = iterSizes->height();
68 
69  // Find center of the object
70  QPointF center = iter->map(QPointF(objectWidth/2, objectHeight/2));
71  QPointF xAxis = iter->map(QPointF(3*objectWidth/4, objectHeight/2));
72  QPointF yAxis = iter->map(QPointF(objectWidth/2, 3*objectHeight/4));
73 
74  cv::Vec3f center3D = this->getDepth(depth_,
75  center.x()+0.5f, center.y()+0.5f,
76  float(depth_.cols/2)-0.5f, float(depth_.rows/2)-0.5f,
77  1.0f/depthConstant_, 1.0f/depthConstant_);
78 
79  cv::Vec3f axisEndX = this->getDepth(depth_,
80  xAxis.x()+0.5f, xAxis.y()+0.5f,
81  float(depth_.cols/2)-0.5f, float(depth_.rows/2)-0.5f,
82  1.0f/depthConstant_, 1.0f/depthConstant_);
83 
84  cv::Vec3f axisEndY = this->getDepth(depth_,
85  yAxis.x()+0.5f, yAxis.y()+0.5f,
86  float(depth_.cols/2)-0.5f, float(depth_.rows/2)-0.5f,
87  1.0f/depthConstant_, 1.0f/depthConstant_);
88 
89  if(std::isfinite(center3D.val[0]) && std::isfinite(center3D.val[1]) && std::isfinite(center3D.val[2]) &&
90  std::isfinite(axisEndX.val[0]) && std::isfinite(axisEndX.val[1]) && std::isfinite(axisEndX.val[2]) &&
91  std::isfinite(axisEndY.val[0]) && std::isfinite(axisEndY.val[1]) && std::isfinite(axisEndY.val[2]))
92  {
93  tf::StampedTransform transform;
94  transform.setIdentity();
95  transform.child_frame_id_ = QString("%1_%2").arg(objFramePrefix_.c_str()).arg(id).toStdString();
96  transform.frame_id_ = frameId_;
97  transform.stamp_ = stamp_;
98  transform.setOrigin(tf::Vector3(center3D.val[0], center3D.val[1], center3D.val[2]));
99 
100  //set rotation (y inverted)
101  tf::Vector3 xAxis(axisEndX.val[0] - center3D.val[0], axisEndX.val[1] - center3D.val[1], axisEndX.val[2] - center3D.val[2]);
102  xAxis.normalize();
103  tf::Vector3 yAxis(axisEndY.val[0] - center3D.val[0], axisEndY.val[1] - center3D.val[1], axisEndY.val[2] - center3D.val[2]);
104  yAxis.normalize();
105  tf::Vector3 zAxis = xAxis*yAxis;
106  tf::Matrix3x3 rotationMatrix(
107  xAxis.x(), yAxis.x() ,zAxis.x(),
108  xAxis.y(), yAxis.y(), zAxis.y(),
109  xAxis.z(), yAxis.z(), zAxis.z());
110  tf::Quaternion q;
111  rotationMatrix.getRotation(q);
112  // set x axis going front of the object, with z up and z left
113  q *= tf::createQuaternionFromRPY(CV_PI/2.0, CV_PI/2.0, 0);
114  transform.setRotation(q.normalized());
115 
116  transforms.push_back(transform);
117  }
118  else
119  {
120  ROS_WARN("Object %d detected, center 2D at (%f,%f), but invalid depth, cannot set frame \"%s\"! "
121  "(maybe object is too near of the camera or bad depth image)\n",
122  id,
123  center.x(), center.y(),
124  QString("%1_%2").arg(objFramePrefix_.c_str()).arg(id).toStdString().c_str());
125  }
126  }
127  if(transforms.size())
128  {
129  tfBroadcaster_.sendTransform(transforms);
130  }
131  }
132 
134  {
135  std_msgs::Float32MultiArray msg;
136  find_object_2d::ObjectsStamped msgStamped;
137  msg.data = std::vector<float>(info.objDetected_.size()*12);
138  msgStamped.objects.data = std::vector<float>(info.objDetected_.size()*12);
139  int i=0;
140  QMultiMap<int, QSize>::const_iterator iterSizes=info.objDetectedSizes_.constBegin();
141  for(QMultiMap<int, QTransform>::const_iterator iter=info.objDetected_.constBegin();
142  iter!=info.objDetected_.constEnd();
143  ++iter, ++iterSizes)
144  {
145  msg.data[i] = msgStamped.objects.data[i] = iter.key(); ++i;
146  msg.data[i] = msgStamped.objects.data[i] = iterSizes->width(); ++i;
147  msg.data[i] = msgStamped.objects.data[i] = iterSizes->height(); ++i;
148  msg.data[i] = msgStamped.objects.data[i] = iter->m11(); ++i;
149  msg.data[i] = msgStamped.objects.data[i] = iter->m12(); ++i;
150  msg.data[i] = msgStamped.objects.data[i] = iter->m13(); ++i;
151  msg.data[i] = msgStamped.objects.data[i] = iter->m21(); ++i;
152  msg.data[i] = msgStamped.objects.data[i] = iter->m22(); ++i;
153  msg.data[i] = msgStamped.objects.data[i] = iter->m23(); ++i;
154  msg.data[i] = msgStamped.objects.data[i] = iter->m31(); ++i;// dx
155  msg.data[i] = msgStamped.objects.data[i] = iter->m32(); ++i;// dy
156  msg.data[i] = msgStamped.objects.data[i] = iter->m33(); ++i;
157  }
158  if(pub_.getNumSubscribers())
159  {
160  pub_.publish(msg);
161  }
163  {
164  // use same header as the input image (for synchronization and frame reference)
165  msgStamped.header.frame_id = frameId_;
166  msgStamped.header.stamp = stamp_;
167  pubStamped_.publish(msgStamped);
168  }
169  }
170 }
171 
172 void FindObjectROS::setDepthData(const std::string & frameId,
173  const ros::Time & stamp,
174  const cv::Mat & depth,
175  float depthConstant)
176 {
177  frameId_ = frameId;
178  stamp_ = stamp;
179  depth_ = depth;
180  depthConstant_ = depthConstant;
181 }
182 
183 cv::Vec3f FindObjectROS::getDepth(const cv::Mat & depthImage,
184  int x, int y,
185  float cx, float cy,
186  float fx, float fy)
187 {
188  if(!(x >=0 && x<depthImage.cols && y >=0 && y<depthImage.rows))
189  {
190  ROS_ERROR("Point must be inside the image (x=%d, y=%d), image size=(%d,%d)",
191  x, y,
192  depthImage.cols, depthImage.rows);
193  return cv::Vec3f(
194  std::numeric_limits<float>::quiet_NaN (),
195  std::numeric_limits<float>::quiet_NaN (),
196  std::numeric_limits<float>::quiet_NaN ());
197  }
198 
199 
200  cv::Vec3f pt;
201 
202  // Use correct principal point from calibration
203  float center_x = cx; //cameraInfo.K.at(2)
204  float center_y = cy; //cameraInfo.K.at(5)
205 
206  bool isInMM = depthImage.type() == CV_16UC1; // is in mm?
207 
208  // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
209  float unit_scaling = isInMM?0.001f:1.0f;
210  float constant_x = unit_scaling / fx; //cameraInfo.K.at(0)
211  float constant_y = unit_scaling / fy; //cameraInfo.K.at(4)
212  float bad_point = std::numeric_limits<float>::quiet_NaN ();
213 
214  float depth;
215  bool isValid;
216  if(isInMM)
217  {
218  depth = (float)depthImage.at<uint16_t>(y,x);
219  isValid = depth != 0.0f;
220  }
221  else
222  {
223  depth = depthImage.at<float>(y,x);
224  isValid = std::isfinite(depth);
225  }
226 
227  // Check for invalid measurements
228  if (!isValid)
229  {
230  pt.val[0] = pt.val[1] = pt.val[2] = bad_point;
231  }
232  else
233  {
234  // Fill in XYZ
235  pt.val[0] = (float(x) - center_x) * depth * constant_x;
236  pt.val[1] = (float(y) - center_y) * depth * constant_y;
237  pt.val[2] = depth*unit_scaling;
238  }
239  return pt;
240 }
float depthConstant_
Definition: FindObjectROS.h:72
static tf::Quaternion createQuaternionFromRPY(double roll, double pitch, double yaw)
TFSIMD_FORCE_INLINE void setRotation(const Quaternion &q)
tf::TransformBroadcaster tfBroadcaster_
Definition: FindObjectROS.h:75
void publish(const find_object::DetectionInfo &info)
void publish(const boost::shared_ptr< M > &message) const
ros::Publisher pubStamped_
Definition: FindObjectROS.h:67
QMultiMap< int, QSize > objDetectedSizes_
Definition: DetectionInfo.h:72
ros::Publisher pub_
Definition: FindObjectROS.h:66
#define ROS_WARN(...)
std::string frameId_
Definition: FindObjectROS.h:69
void objectsFound(const find_object::DetectionInfo &)
void setIdentity()
TFSIMD_FORCE_INLINE const tfScalar & x() const
std::string child_frame_id_
TFSIMD_FORCE_INLINE const tfScalar & z() const
FindObjectROS(QObject *parent=0)
ros::Time stamp_
Definition: FindObjectROS.h:70
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
QMultiMap< int, QTransform > objDetected_
Definition: DetectionInfo.h:71
TFSIMD_FORCE_INLINE const tfScalar & y() const
void sendTransform(const StampedTransform &transform)
std::string objFramePrefix_
Definition: FindObjectROS.h:74
TFSIMD_FORCE_INLINE Vector3 & normalize()
cv::Vec3f getDepth(const cv::Mat &depthImage, int x, int y, float cx, float cy, float fx, float fy)
uint32_t getNumSubscribers() const
cv::Mat depth_
Definition: FindObjectROS.h:71
void setDepthData(const std::string &frameId, const ros::Time &stamp, const cv::Mat &depth, float depthConstant)
#define ROS_ERROR(...)
TFSIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
std::string frame_id_


find_object_2d
Author(s): Mathieu Labbe
autogenerated on Mon Jun 10 2019 13:21:31