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

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