ros1/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 #include <std_msgs/Float32MultiArray.h>
30 #include "find_object_2d/ObjectsStamped.h"
31 #include "find_object_2d/DetectionInfo.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  usePnP_(true)
41 {
42  ros::NodeHandle pnh("~"); // public
43  pnh.param("object_prefix", objFramePrefix_, objFramePrefix_);
44  pnh.param("pnp", usePnP_, usePnP_);
45  ROS_INFO("object_prefix = %s", objFramePrefix_.c_str());
46  ROS_INFO("pnp = %s", usePnP_?"true":"false");
47 
48  ros::NodeHandle nh; // public
49 
50  pub_ = nh.advertise<std_msgs::Float32MultiArray>("objects", 1);
51  pubStamped_ = nh.advertise<find_object_2d::ObjectsStamped>("objectsStamped", 1);
52  pubInfo_ = nh.advertise<find_object_2d::DetectionInfo>("info", 1);
53 
54  this->connect(this, SIGNAL(objectsFound(const find_object::DetectionInfo &, const find_object::Header &, const cv::Mat &, float)), this, SLOT(publish(const find_object::DetectionInfo &, const find_object::Header &, const cv::Mat &, float)));
55 }
56 
57 void FindObjectROS::publish(const find_object::DetectionInfo & info, const Header & header, const cv::Mat & depth, float depthConstant)
58 {
59  // send tf before the message
60  if(info.objDetected_.size() && !depth.empty() && depthConstant != 0.0f)
61  {
62  std::vector<tf::StampedTransform> transforms;
63  char multiSubId = 'b';
64  int previousId = -1;
65  QMultiMap<int, QSize>::const_iterator iterSizes=info.objDetectedSizes_.constBegin();
66  for(QMultiMap<int, QTransform>::const_iterator iter=info.objDetected_.constBegin();
67  iter!=info.objDetected_.constEnd();
68  ++iter, ++iterSizes)
69  {
70  // get data
71  int id = iter.key();
72  float objectWidth = iterSizes->width();
73  float objectHeight = iterSizes->height();
74 
75  QString multiSuffix;
76  if(id == previousId)
77  {
78  multiSuffix = QString("_") + multiSubId++;
79  }
80  else
81  {
82  multiSubId = 'b';
83  }
84  previousId = id;
85 
86  // Find center of the object
87  QPointF center = iter->map(QPointF(objectWidth/2, objectHeight/2));
88  cv::Vec3f center3D = this->getDepth(depth,
89  center.x()+0.5f, center.y()+0.5f,
90  float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f,
91  1.0f/depthConstant, 1.0f/depthConstant);
92 
93  cv::Vec3f axisEndX;
94  cv::Vec3f axisEndY;
95  if(!usePnP_)
96  {
97  QPointF xAxis = iter->map(QPointF(3*objectWidth/4, objectHeight/2));
98  axisEndX = this->getDepth(depth,
99  xAxis.x()+0.5f, xAxis.y()+0.5f,
100  float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f,
101  1.0f/depthConstant, 1.0f/depthConstant);
102 
103  QPointF yAxis = iter->map(QPointF(objectWidth/2, 3*objectHeight/4));
104  axisEndY = this->getDepth(depth,
105  yAxis.x()+0.5f, yAxis.y()+0.5f,
106  float(depth.cols/2)-0.5f, float(depth.rows/2)-0.5f,
107  1.0f/depthConstant, 1.0f/depthConstant);
108  }
109 
110  if((std::isfinite(center3D.val[2]) && usePnP_) ||
111  (std::isfinite(center3D.val[0]) && std::isfinite(center3D.val[1]) && std::isfinite(center3D.val[2]) &&
112  std::isfinite(axisEndX.val[0]) && std::isfinite(axisEndX.val[1]) && std::isfinite(axisEndX.val[2]) &&
113  std::isfinite(axisEndY.val[0]) && std::isfinite(axisEndY.val[1]) && std::isfinite(axisEndY.val[2])))
114  {
115  tf::StampedTransform transform;
116  transform.setIdentity();
117  transform.child_frame_id_ = QString("%1_%2%3").arg(objFramePrefix_.c_str()).arg(id).arg(multiSuffix).toStdString();
118  transform.frame_id_ = header.frameId_.toStdString();
119  transform.stamp_.sec = header.sec_;
120  transform.stamp_.nsec = header.nsec_;
121 
122  tf::Quaternion q;
123  if(usePnP_)
124  {
125  std::vector<cv::Point3f> objectPoints(4);
126  std::vector<cv::Point2f> imagePoints(4);
127 
128  objectPoints[0] = cv::Point3f(-0.5, -(objectHeight/objectWidth)/2.0f,0);
129  objectPoints[1] = cv::Point3f(0.5,-(objectHeight/objectWidth)/2.0f,0);
130  objectPoints[2] = cv::Point3f(0.5,(objectHeight/objectWidth)/2.0f,0);
131  objectPoints[3] = cv::Point3f(-0.5,(objectHeight/objectWidth)/2.0f,0);
132 
133  QPointF pt = iter->map(QPointF(0, 0));
134  imagePoints[0] = cv::Point2f(pt.x(), pt.y());
135  pt = iter->map(QPointF(objectWidth, 0));
136  imagePoints[1] = cv::Point2f(pt.x(), pt.y());
137  pt = iter->map(QPointF(objectWidth, objectHeight));
138  imagePoints[2] = cv::Point2f(pt.x(), pt.y());
139  pt = iter->map(QPointF(0, objectHeight));
140  imagePoints[3] = cv::Point2f(pt.x(), pt.y());
141 
142  cv::Mat cameraMatrix = cv::Mat::eye(3,3,CV_64FC1);
143  cameraMatrix.at<double>(0,0) = 1.0f/depthConstant;
144  cameraMatrix.at<double>(1,1) = 1.0f/depthConstant;
145  cameraMatrix.at<double>(0,2) = float(depth.cols/2)-0.5f;
146  cameraMatrix.at<double>(1,2) = float(depth.rows/2)-0.5f;
147  cv::Mat distCoeffs;
148 
149  cv::Mat rvec(1,3, CV_64FC1);
150  cv::Mat tvec(1,3, CV_64FC1);
151  cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec);
152 
153  cv::Mat R;
154  cv::Rodrigues(rvec, R);
155  tf::Matrix3x3 rotationMatrix(
156  R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2),
157  R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2),
158  R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2));
159  rotationMatrix.getRotation(q);
160  transform.setOrigin(tf::Vector3(
161  tvec.at<double>(0)*(center3D.val[2]/tvec.at<double>(2)),
162  tvec.at<double>(1)*(center3D.val[2]/tvec.at<double>(2)),
163  tvec.at<double>(2)*(center3D.val[2]/tvec.at<double>(2))));
164  }
165  else
166  {
167  tf::Vector3 xAxis(axisEndX.val[0] - center3D.val[0], axisEndX.val[1] - center3D.val[1], axisEndX.val[2] - center3D.val[2]);
168  xAxis.normalize();
169  tf::Vector3 yAxis(axisEndY.val[0] - center3D.val[0], axisEndY.val[1] - center3D.val[1], axisEndY.val[2] - center3D.val[2]);
170  yAxis.normalize();
171  tf::Vector3 zAxis = xAxis.cross(yAxis);
172  zAxis.normalize();
173  tf::Matrix3x3 rotationMatrix(
174  xAxis.x(), yAxis.x() ,zAxis.x(),
175  xAxis.y(), yAxis.y(), zAxis.y(),
176  xAxis.z(), yAxis.z(), zAxis.z());
177  rotationMatrix.getRotation(q);
178  transform.setOrigin(tf::Vector3(center3D.val[0], center3D.val[1], center3D.val[2]));
179  }
180 
181  // set x axis going front of the object, with z up and z left
182  q *= tf::createQuaternionFromRPY(CV_PI/2.0, CV_PI/2.0, 0);
183  transform.setRotation(q.normalized());
184  transforms.push_back(transform);
185  }
186  else
187  {
188  ROS_WARN("Object %d detected, center 2D at (%f,%f), but invalid depth, cannot set frame \"%s\"! "
189  "(maybe object is too near of the camera or bad depth image)\n",
190  id,
191  center.x(), center.y(),
192  QString("%1_%2").arg(objFramePrefix_.c_str()).arg(id).toStdString().c_str());
193  }
194  }
195  if(transforms.size())
196  {
197  tfBroadcaster_.sendTransform(transforms);
198  }
199  }
200 
201  if(pub_.getNumSubscribers() || pubStamped_.getNumSubscribers() || pubInfo_.getNumSubscribers())
202  {
203  std_msgs::Float32MultiArray msg;
204  find_object_2d::ObjectsStamped msgStamped;
205  find_object_2d::DetectionInfo infoMsg;
206  if(pubInfo_.getNumSubscribers())
207  {
208  infoMsg.ids.resize(info.objDetected_.size());
209  infoMsg.widths.resize(info.objDetected_.size());
210  infoMsg.heights.resize(info.objDetected_.size());
211  infoMsg.file_paths.resize(info.objDetected_.size());
212  infoMsg.inliers.resize(info.objDetected_.size());
213  infoMsg.outliers.resize(info.objDetected_.size());
214  infoMsg.homographies.resize(info.objDetected_.size());
215  }
216  msg.data = std::vector<float>(info.objDetected_.size()*12);
217  msgStamped.objects.data = std::vector<float>(info.objDetected_.size()*12);
218 
219  ROS_ASSERT(info.objDetected_.size() == info.objDetectedSizes_.size() &&
220  info.objDetected_.size() == info.objDetectedFilePaths_.size() &&
221  info.objDetected_.size() == info.objDetectedInliersCount_.size() &&
222  info.objDetected_.size() == info.objDetectedOutliersCount_.size());
223 
224  int infoIndex=0;
225  int i=0;
226  QMultiMap<int, QSize>::const_iterator iterSizes=info.objDetectedSizes_.constBegin();
227  QMultiMap<int, QString>::const_iterator iterFilePaths=info.objDetectedFilePaths_.constBegin();
228  QMultiMap<int, int>::const_iterator iterInliers=info.objDetectedInliersCount_.constBegin();
229  QMultiMap<int, int>::const_iterator iterOutliers=info.objDetectedOutliersCount_.constBegin();
230  for(QMultiMap<int, QTransform>::const_iterator iter=info.objDetected_.constBegin();
231  iter!=info.objDetected_.constEnd();
232  ++iter, ++iterSizes, ++iterFilePaths, ++infoIndex, ++iterInliers, ++iterOutliers)
233  {
234  if(pub_.getNumSubscribers() || pubStamped_.getNumSubscribers())
235  {
236  msg.data[i] = msgStamped.objects.data[i] = iter.key(); ++i;
237  msg.data[i] = msgStamped.objects.data[i] = iterSizes->width(); ++i;
238  msg.data[i] = msgStamped.objects.data[i] = iterSizes->height(); ++i;
239  msg.data[i] = msgStamped.objects.data[i] = iter->m11(); ++i;
240  msg.data[i] = msgStamped.objects.data[i] = iter->m12(); ++i;
241  msg.data[i] = msgStamped.objects.data[i] = iter->m13(); ++i;
242  msg.data[i] = msgStamped.objects.data[i] = iter->m21(); ++i;
243  msg.data[i] = msgStamped.objects.data[i] = iter->m22(); ++i;
244  msg.data[i] = msgStamped.objects.data[i] = iter->m23(); ++i;
245  msg.data[i] = msgStamped.objects.data[i] = iter->m31(); ++i;// dx
246  msg.data[i] = msgStamped.objects.data[i] = iter->m32(); ++i;// dy
247  msg.data[i] = msgStamped.objects.data[i] = iter->m33(); ++i;
248  }
249 
250  if(pubInfo_.getNumSubscribers())
251  {
252  infoMsg.ids[infoIndex].data = iter.key();
253  infoMsg.widths[infoIndex].data = iterSizes->width();
254  infoMsg.heights[infoIndex].data = iterSizes->height();
255  infoMsg.file_paths[infoIndex].data = iterFilePaths.value().toStdString();
256  infoMsg.inliers[infoIndex].data = iterInliers.value();
257  infoMsg.outliers[infoIndex].data = iterOutliers.value();
258  infoMsg.homographies[infoIndex].data.resize(9);
259  infoMsg.homographies[infoIndex].data[0] = iter->m11();
260  infoMsg.homographies[infoIndex].data[1] = iter->m12();
261  infoMsg.homographies[infoIndex].data[2] = iter->m13();
262  infoMsg.homographies[infoIndex].data[3] = iter->m21();
263  infoMsg.homographies[infoIndex].data[4] = iter->m22();
264  infoMsg.homographies[infoIndex].data[5] = iter->m23();
265  infoMsg.homographies[infoIndex].data[6] = iter->m31();
266  infoMsg.homographies[infoIndex].data[7] = iter->m32();
267  infoMsg.homographies[infoIndex].data[8] = iter->m33();
268  }
269  }
270  if(pub_.getNumSubscribers())
271  {
272  pub_.publish(msg);
273  }
274  if(pubStamped_.getNumSubscribers())
275  {
276  // use same header as the input image (for synchronization and frame reference)
277  msgStamped.header.frame_id = header.frameId_.toStdString();
278  msgStamped.header.stamp.sec = header.sec_;
279  msgStamped.header.stamp.nsec = header.nsec_;
280  pubStamped_.publish(msgStamped);
281  }
282  if(pubInfo_.getNumSubscribers())
283  {
284  // use same header as the input image (for synchronization and frame reference)
285  infoMsg.header.frame_id = header.frameId_.toStdString();
286  infoMsg.header.stamp.sec = header.sec_;
287  infoMsg.header.stamp.nsec = header.nsec_;
288  pubInfo_.publish(infoMsg);
289  }
290  }
291 }
292 
293 cv::Vec3f FindObjectROS::getDepth(const cv::Mat & depthImage,
294  int x, int y,
295  float cx, float cy,
296  float fx, float fy)
297 {
298  if(!(x >=0 && x<depthImage.cols && y >=0 && y<depthImage.rows))
299  {
300  ROS_ERROR("Point must be inside the image (x=%d, y=%d), image size=(%d,%d)",
301  x, y,
302  depthImage.cols, depthImage.rows);
303  return cv::Vec3f(
304  std::numeric_limits<float>::quiet_NaN (),
305  std::numeric_limits<float>::quiet_NaN (),
306  std::numeric_limits<float>::quiet_NaN ());
307  }
308 
309 
310  cv::Vec3f pt;
311 
312  // Use correct principal point from calibration
313  float center_x = cx; //cameraInfo.K.at(2)
314  float center_y = cy; //cameraInfo.K.at(5)
315 
316  bool isInMM = depthImage.type() == CV_16UC1; // is in mm?
317 
318  // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
319  float unit_scaling = isInMM?0.001f:1.0f;
320  float constant_x = unit_scaling / fx; //cameraInfo.K.at(0)
321  float constant_y = unit_scaling / fy; //cameraInfo.K.at(4)
322  float bad_point = std::numeric_limits<float>::quiet_NaN ();
323 
324  float depth;
325  bool isValid;
326  if(isInMM)
327  {
328  depth = (float)depthImage.at<uint16_t>(y,x);
329  isValid = depth != 0.0f;
330  }
331  else
332  {
333  depth = depthImage.at<float>(y,x);
334  isValid = std::isfinite(depth) && depth > 0.0f;
335  }
336 
337  // Check for invalid measurements
338  if (!isValid)
339  {
340  pt.val[0] = pt.val[1] = pt.val[2] = bad_point;
341  }
342  else
343  {
344  // Fill in XYZ
345  pt.val[0] = (float(x) - center_x) * depth * constant_x;
346  pt.val[1] = (float(y) - center_y) * depth * constant_y;
347  pt.val[2] = depth*unit_scaling;
348  }
349  return pt;
350 }
tf::createQuaternionFromRPY
static tf::Quaternion createQuaternionFromRPY(double roll, double pitch, double yaw)
tf::Transform::setRotation
TFSIMD_FORCE_INLINE void setRotation(const Quaternion &q)
tf::Matrix3x3::getRotation
void getRotation(Quaternion &q) const
ros::NodeHandle
FindObjectROS::tfBroadcaster_
tf::TransformBroadcaster tfBroadcaster_
Definition: ros1/FindObjectROS.h:67
FindObjectROS::pubInfo_
ros::Publisher pubInfo_
Definition: ros1/FindObjectROS.h:63
FindObjectROS::pubStamped_
ros::Publisher pubStamped_
Definition: ros1/FindObjectROS.h:62
f
f
find_object::DetectionInfo::objDetectedFilePaths_
QMultiMap< int, QString > objDetectedFilePaths_
Definition: DetectionInfo.h:73
find_object::DetectionInfo::objDetectedSizes_
QMultiMap< int, QSize > objDetectedSizes_
Definition: DetectionInfo.h:72
find_object
Definition: Camera.h:38
FindObjectROS::pub_
ros::Publisher pub_
Definition: ros1/FindObjectROS.h:61
ROS_WARN
#define ROS_WARN(...)
tf::Transform::setIdentity
void setIdentity()
find_object::DetectionInfo
Definition: DetectionInfo.h:39
tf::StampedTransform::child_frame_id_
std::string child_frame_id_
ros::NodeHandle::param
bool param(const std::string &param_name, T &param_val, const T &default_val) const
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
find_object::DetectionInfo::objDetectedInliersCount_
QMultiMap< int, int > objDetectedInliersCount_
Definition: DetectionInfo.h:74
FindObjectROS::FindObjectROS
FindObjectROS(QObject *parent=0)
Definition: ros1/FindObjectROS.cpp:37
find_object::FindObject::objectsFound
void objectsFound(const find_object::DetectionInfo &, const find_object::Header &, const cv::Mat &, float)
ROS_INFO
#define ROS_INFO(...)
find_object::DetectionInfo::objDetected_
QMultiMap< int, QTransform > objDetected_
Definition: DetectionInfo.h:71
tf::TransformBroadcaster::sendTransform
void sendTransform(const StampedTransform &transform)
FindObjectROS::objFramePrefix_
std::string objFramePrefix_
Definition: ros1/FindObjectROS.h:65
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
find_object::FindObject
Definition: FindObject.h:53
FindObjectROS::publish
void publish(const find_object::DetectionInfo &info, const find_object::Header &header, const cv::Mat &depth, float depthConstant)
Definition: ros1/FindObjectROS.cpp:57
find_object::DetectionInfo::objDetectedOutliersCount_
QMultiMap< int, int > objDetectedOutliersCount_
Definition: DetectionInfo.h:75
FindObjectROS::getDepth
cv::Vec3f getDepth(const cv::Mat &depthImage, int x, int y, float cx, float cy, float fx, float fy)
Definition: ros1/FindObjectROS.cpp:293
ROS_ASSERT
#define ROS_ASSERT(cond)
find_object::Header::nsec_
uint64_t nsec_
Definition: Header.h:51
ros::Publisher::getNumSubscribers
uint32_t getNumSubscribers() const
find_object::Header::frameId_
QString frameId_
Definition: Header.h:49
ROS_ERROR
#define ROS_ERROR(...)
find_object::Header
Definition: Header.h:36
find_object::Header::sec_
uint64_t sec_
Definition: Header.h:50
tf::Transform::setOrigin
TFSIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
tf::StampedTransform::frame_id_
std::string frame_id_
tf::Quaternion::normalized
Quaternion normalized() const

find_object_2d
Author(s): Mathieu Labbe
autogenerated on Mon Dec 12 2022 03:20:09