rgbd_odometry.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, 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 <rtabmap_ros/OdometryROS.h>
29 
30 #include <pluginlib/class_list_macros.h>
31 #include <nodelet/nodelet.h>
32 
33 #include <message_filters/subscriber.h>
34 #include <message_filters/time_synchronizer.h>
35 #include <message_filters/sync_policies/approximate_time.h>
36 
37 #include <image_transport/image_transport.h>
38 #include <image_transport/subscriber_filter.h>
39 
40 #include <image_geometry/stereo_camera_model.h>
41 
42 #include <sensor_msgs/Image.h>
43 #include <sensor_msgs/image_encodings.h>
44 #include <cv_bridge/cv_bridge.h>
45 
46 #include "rtabmap_ros/MsgConversion.h"
47 
48 #include <rtabmap/core/util3d.h>
49 #include <rtabmap/core/util2d.h>
50 #include <rtabmap/utilite/ULogger.h>
51 #include <rtabmap/utilite/UConversion.h>
52 #include <rtabmap/utilite/UStl.h>
53 
54 using namespace rtabmap;
55 
56 namespace rtabmap_ros
57 {
58 
59 class RGBDOdometry : public rtabmap_ros::OdometryROS
60 {
61 public:
62  RGBDOdometry() :
63  OdometryROS(false, true, false),
64  approxSync_(0),
65  exactSync_(0),
66  approxSync2_(0),
67  exactSync2_(0),
68  approxSync3_(0),
69  exactSync3_(0),
70  approxSync4_(0),
71  exactSync4_(0),
72  queueSize_(5),
73  keepColor_(false)
74  {
75  }
76 
77  virtual ~RGBDOdometry()
78  {
79  rgbdSub_.shutdown();
80  if(approxSync_)
81  {
82  delete approxSync_;
83  }
84  if(exactSync_)
85  {
86  delete exactSync_;
87  }
88  if(approxSync2_)
89  {
90  delete approxSync2_;
91  }
92  if(exactSync2_)
93  {
94  delete exactSync2_;
95  }
96  if(approxSync3_)
97  {
98  delete approxSync3_;
99  }
100  if(exactSync3_)
101  {
102  delete exactSync3_;
103  }
104  if(approxSync4_)
105  {
106  delete approxSync4_;
107  }
108  if(exactSync4_)
109  {
110  delete exactSync4_;
111  }
112  }
113 
114 private:
115 
116  virtual void onOdomInit()
117  {
118  ros::NodeHandle & nh = getNodeHandle();
119  ros::NodeHandle & pnh = getPrivateNodeHandle();
120 
121  int rgbdCameras = 1;
122  bool approxSync = true;
123  bool subscribeRGBD = false;
124  pnh.param("approx_sync", approxSync, approxSync);
125  pnh.param("queue_size", queueSize_, queueSize_);
126  pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD);
127  if(pnh.hasParam("depth_cameras"))
128  {
129  ROS_ERROR("\"depth_cameras\" parameter doesn't exist anymore. It is replaced by \"rgbd_cameras\" with the \"rgbd_image\" input topics. \"subscribe_rgbd\" should be also set to true.");
130  }
131  pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras);
132  if(rgbdCameras <= 0)
133  {
134  rgbdCameras = 1;
135  }
136  if(rgbdCameras > 4)
137  {
138  NODELET_FATAL("Only 4 cameras maximum supported yet.");
139  }
140  pnh.param("keep_color", keepColor_, keepColor_);
141 
142  NODELET_INFO("RGBDOdometry: approx_sync = %s", approxSync?"true":"false");
143  NODELET_INFO("RGBDOdometry: queue_size = %d", queueSize_);
144  NODELET_INFO("RGBDOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
145  NODELET_INFO("RGBDOdometry: rgbd_cameras = %d", rgbdCameras);
146  NODELET_INFO("RGBDOdometry: keep_color = %s", keepColor_?"true":"false");
147 
148  std::string subscribedTopicsMsg;
149  if(subscribeRGBD)
150  {
151  if(rgbdCameras >= 2)
152  {
153  rgbd_image1_sub_.subscribe(nh, "rgbd_image0", 1);
154  rgbd_image2_sub_.subscribe(nh, "rgbd_image1", 1);
155  if(rgbdCameras >= 3)
156  {
157  rgbd_image3_sub_.subscribe(nh, "rgbd_image2", 1);
158  }
159  if(rgbdCameras >= 4)
160  {
161  rgbd_image4_sub_.subscribe(nh, "rgbd_image3", 1);
162  }
163 
164  if(rgbdCameras == 2)
165  {
166  if(approxSync)
167  {
168  approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
169  MyApproxSync2Policy(queueSize_),
170  rgbd_image1_sub_,
171  rgbd_image2_sub_);
172  approxSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2));
173  }
174  else
175  {
176  exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
177  MyExactSync2Policy(queueSize_),
178  rgbd_image1_sub_,
179  rgbd_image2_sub_);
180  exactSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2));
181  }
182  subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s",
183  getName().c_str(),
184  approxSync?"approx":"exact",
185  rgbd_image1_sub_.getTopic().c_str(),
186  rgbd_image2_sub_.getTopic().c_str());
187  }
188  else if(rgbdCameras == 3)
189  {
190  if(approxSync)
191  {
192  approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
193  MyApproxSync3Policy(queueSize_),
194  rgbd_image1_sub_,
195  rgbd_image2_sub_,
196  rgbd_image3_sub_);
197  approxSync3_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD3, this, _1, _2, _3));
198  }
199  else
200  {
201  exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
202  MyExactSync3Policy(queueSize_),
203  rgbd_image1_sub_,
204  rgbd_image2_sub_,
205  rgbd_image3_sub_);
206  exactSync3_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD3, this, _1, _2, _3));
207  }
208  subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s",
209  getName().c_str(),
210  approxSync?"approx":"exact",
211  rgbd_image1_sub_.getTopic().c_str(),
212  rgbd_image2_sub_.getTopic().c_str(),
213  rgbd_image3_sub_.getTopic().c_str());
214  }
215  else if(rgbdCameras == 4)
216  {
217  if(approxSync)
218  {
219  approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
220  MyApproxSync4Policy(queueSize_),
221  rgbd_image1_sub_,
222  rgbd_image2_sub_,
223  rgbd_image3_sub_,
224  rgbd_image4_sub_);
225  approxSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, _1, _2, _3, _4));
226  }
227  else
228  {
229  exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
230  MyExactSync4Policy(queueSize_),
231  rgbd_image1_sub_,
232  rgbd_image2_sub_,
233  rgbd_image3_sub_,
234  rgbd_image4_sub_);
235  exactSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, _1, _2, _3, _4));
236  }
237  subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s \\\n %s",
238  getName().c_str(),
239  approxSync?"approx":"exact",
240  rgbd_image1_sub_.getTopic().c_str(),
241  rgbd_image2_sub_.getTopic().c_str(),
242  rgbd_image3_sub_.getTopic().c_str(),
243  rgbd_image4_sub_.getTopic().c_str());
244  }
245  }
246  else
247  {
248  rgbdSub_ = nh.subscribe("rgbd_image", 1, &RGBDOdometry::callbackRGBD, this);
249 
250  subscribedTopicsMsg =
251  uFormat("\n%s subscribed to:\n %s",
252  getName().c_str(),
253  rgbdSub_.getTopic().c_str());
254  }
255  }
256  else
257  {
258  ros::NodeHandle rgb_nh(nh, "rgb");
259  ros::NodeHandle depth_nh(nh, "depth");
260  ros::NodeHandle rgb_pnh(pnh, "rgb");
261  ros::NodeHandle depth_pnh(pnh, "depth");
262  image_transport::ImageTransport rgb_it(rgb_nh);
263  image_transport::ImageTransport depth_it(depth_nh);
264  image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
265  image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
266 
267  image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
268  image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
269  info_sub_.subscribe(rgb_nh, "camera_info", 1);
270 
271  if(approxSync)
272  {
273  approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
274  approxSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
275  }
276  else
277  {
278  exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
279  exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
280  }
281 
282  subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s",
283  getName().c_str(),
284  approxSync?"approx":"exact",
285  image_mono_sub_.getTopic().c_str(),
286  image_depth_sub_.getTopic().c_str(),
287  info_sub_.getTopic().c_str());
288  }
289  this->startWarningThread(subscribedTopicsMsg, approxSync);
290  }
291 
292  virtual void updateParameters(ParametersMap & parameters)
293  {
294  //make sure we are using Reg/Strategy=0
295  ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy());
296  if(iter != parameters.end() && iter->second.compare("0") != 0)
297  {
298  ROS_WARN("RGBD odometry works only with \"Reg/Strategy\"=0. Ignoring value %s.", iter->second.c_str());
299  }
300  uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "0"));
301 
302  int estimationType = Parameters::defaultVisEstimationType();
303  Parameters::parse(parameters, Parameters::kVisEstimationType(), estimationType);
304  ros::NodeHandle & pnh = getPrivateNodeHandle();
305  int rgbdCameras = 1;
306  bool subscribeRGBD = false;
307  pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD);
308  pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras);
309  if(subscribeRGBD && rgbdCameras> 1 && estimationType>0)
310  {
311  NODELET_WARN("Setting \"%s\" parameter to 0 (%d is not supported "
312  "for multi-cameras) as \"subscribe_rgbd\" is "
313  "true and \"rgbd_cameras\">1. Set \"%s\" to 0 to suppress this warning.",
314  Parameters::kVisEstimationType().c_str(),
315  estimationType,
316  Parameters::kVisEstimationType().c_str());
317  uInsert(parameters, ParametersPair(Parameters::kVisEstimationType(), "0"));
318  }
319  }
320 
321  void commonCallback(
322  const std::vector<cv_bridge::CvImageConstPtr> & rgbImages,
323  const std::vector<cv_bridge::CvImageConstPtr> & depthImages,
324  const std::vector<sensor_msgs::CameraInfo>& cameraInfos)
325  {
326  ROS_ASSERT(rgbImages.size() > 0 && rgbImages.size() == depthImages.size() && rgbImages.size() == cameraInfos.size());
327  ros::Time higherStamp;
328  int imageWidth = rgbImages[0]->image.cols;
329  int imageHeight = rgbImages[0]->image.rows;
330  int depthWidth = depthImages[0]->image.cols;
331  int depthHeight = depthImages[0]->image.rows;
332 
333  UASSERT_MSG(
334  imageWidth % depthWidth == 0 && imageHeight % depthHeight == 0 &&
335  imageWidth/depthWidth == imageHeight/depthHeight,
336  uFormat("rgb=%dx%d depth=%dx%d", imageWidth, imageHeight, depthWidth, depthHeight).c_str());
337 
338  int cameraCount = rgbImages.size();
339  cv::Mat rgb;
340  cv::Mat depth;
341  pcl::PointCloud<pcl::PointXYZ> scanCloud;
342  std::vector<CameraModel> cameraModels;
343  for(unsigned int i=0; i<rgbImages.size(); ++i)
344  {
345  if(!(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
346  rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
347  rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
348  rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
349  rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
350  rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
351  rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
352  rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0) ||
353  !(depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
354  depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
355  depthImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
356  {
357  NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8,bgra8,rgba8 and "
358  "image_depth=32FC1,16UC1,mono16. Current rgb=%s and depth=%s",
359  rgbImages[i]->encoding.c_str(),
360  depthImages[i]->encoding.c_str());
361  return;
362  }
363  UASSERT_MSG(rgbImages[i]->image.cols == imageWidth && rgbImages[i]->image.rows == imageHeight,
364  uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
365  imageWidth,
366  rgbImages[i]->image.cols,
367  imageHeight,
368  rgbImages[i]->image.rows).c_str());
369  UASSERT_MSG(depthImages[i]->image.cols == depthWidth && depthImages[i]->image.rows == depthHeight,
370  uFormat("depthWidth=%d vs %d depthHeight=%d vs %d",
371  depthWidth,
372  depthImages[i]->image.cols,
373  depthHeight,
374  depthImages[i]->image.rows).c_str());
375 
376  ros::Time stamp = rgbImages[i]->header.stamp>depthImages[i]->header.stamp?rgbImages[i]->header.stamp:depthImages[i]->header.stamp;
377 
378  if(i == 0)
379  {
380  higherStamp = stamp;
381  }
382  else if(stamp > higherStamp)
383  {
384  higherStamp = stamp;
385  }
386 
387  Transform localTransform = getTransform(this->frameId(), rgbImages[i]->header.frame_id, stamp);
388  if(localTransform.isNull())
389  {
390  return;
391  }
392 
393  cv_bridge::CvImageConstPtr ptrImage = rgbImages[i];
394  if(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
395  rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
396  {
397  if(keepColor_ && rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0)
398  {
399  ptrImage = cv_bridge::cvtColor(rgbImages[i], "bgr8");
400  }
401  else
402  {
403  ptrImage = cv_bridge::cvtColor(rgbImages[i], "mono8");
404  }
405  }
406 
407  cv_bridge::CvImageConstPtr ptrDepth = depthImages[i];
408  cv::Mat subDepth = ptrDepth->image;
409 
410  // initialize
411  if(rgb.empty())
412  {
413  rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type());
414  }
415  if(depth.empty())
416  {
417  depth = cv::Mat(depthHeight, depthWidth*cameraCount, subDepth.type());
418  }
419 
420  if(ptrImage->image.type() == rgb.type())
421  {
422  ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
423  }
424  else
425  {
426  NODELET_ERROR("Some RGB images are not the same type!");
427  return;
428  }
429 
430  if(subDepth.type() == depth.type())
431  {
432  subDepth.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight)));
433  }
434  else
435  {
436  NODELET_ERROR("Some Depth images are not the same type! %d vs %d", subDepth.type(), depth.type());
437  return;
438  }
439 
440  cameraModels.push_back(rtabmap_ros::cameraModelFromROS(cameraInfos[i], localTransform));
441  }
442 
443  rtabmap::SensorData data(
444  rgb,
445  depth,
446  cameraModels,
447  0,
448  rtabmap_ros::timestampFromROS(higherStamp));
449 
450  this->processData(data, higherStamp, rgbImages.size()==1?rgbImages[0]->header.frame_id:"");
451  }
452 
453  void callback(
454  const sensor_msgs::ImageConstPtr& image,
455  const sensor_msgs::ImageConstPtr& depth,
456  const sensor_msgs::CameraInfoConstPtr& cameraInfo)
457  {
458  callbackCalled();
459  if(!this->isPaused())
460  {
461  std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);
462  std::vector<cv_bridge::CvImageConstPtr> depthMsgs(1);
463  std::vector<sensor_msgs::CameraInfo> infoMsgs;
464  imageMsgs[0] = cv_bridge::toCvShare(image);
465  depthMsgs[0] = cv_bridge::toCvShare(depth);
466  infoMsgs.push_back(*cameraInfo);
467 
468  this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
469  }
470  }
471 
472  void callbackRGBD(
473  const rtabmap_ros::RGBDImageConstPtr& image)
474  {
475  callbackCalled();
476  if(!this->isPaused())
477  {
478  std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);
479  std::vector<cv_bridge::CvImageConstPtr> depthMsgs(1);
480  std::vector<sensor_msgs::CameraInfo> infoMsgs;
481  rtabmap_ros::toCvShare(image, imageMsgs[0], depthMsgs[0]);
482  infoMsgs.push_back(image->rgb_camera_info);
483 
484  this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
485  }
486  }
487 
488  void callbackRGBD2(
489  const rtabmap_ros::RGBDImageConstPtr& image,
490  const rtabmap_ros::RGBDImageConstPtr& image2)
491  {
492  callbackCalled();
493  if(!this->isPaused())
494  {
495  std::vector<cv_bridge::CvImageConstPtr> imageMsgs(2);
496  std::vector<cv_bridge::CvImageConstPtr> depthMsgs(2);
497  std::vector<sensor_msgs::CameraInfo> infoMsgs;
498  rtabmap_ros::toCvShare(image, imageMsgs[0], depthMsgs[0]);
499  rtabmap_ros::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
500  infoMsgs.push_back(image->rgb_camera_info);
501  infoMsgs.push_back(image2->rgb_camera_info);
502 
503  this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
504  }
505  }
506 
507  void callbackRGBD3(
508  const rtabmap_ros::RGBDImageConstPtr& image,
509  const rtabmap_ros::RGBDImageConstPtr& image2,
510  const rtabmap_ros::RGBDImageConstPtr& image3)
511  {
512  callbackCalled();
513  if(!this->isPaused())
514  {
515  std::vector<cv_bridge::CvImageConstPtr> imageMsgs(3);
516  std::vector<cv_bridge::CvImageConstPtr> depthMsgs(3);
517  std::vector<sensor_msgs::CameraInfo> infoMsgs;
518  rtabmap_ros::toCvShare(image, imageMsgs[0], depthMsgs[0]);
519  rtabmap_ros::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
520  rtabmap_ros::toCvShare(image3, imageMsgs[2], depthMsgs[2]);
521  infoMsgs.push_back(image->rgb_camera_info);
522  infoMsgs.push_back(image2->rgb_camera_info);
523  infoMsgs.push_back(image3->rgb_camera_info);
524 
525  this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
526  }
527  }
528 
529  void callbackRGBD4(
530  const rtabmap_ros::RGBDImageConstPtr& image,
531  const rtabmap_ros::RGBDImageConstPtr& image2,
532  const rtabmap_ros::RGBDImageConstPtr& image3,
533  const rtabmap_ros::RGBDImageConstPtr& image4)
534  {
535  callbackCalled();
536  if(!this->isPaused())
537  {
538  std::vector<cv_bridge::CvImageConstPtr> imageMsgs(4);
539  std::vector<cv_bridge::CvImageConstPtr> depthMsgs(4);
540  std::vector<sensor_msgs::CameraInfo> infoMsgs;
541  rtabmap_ros::toCvShare(image, imageMsgs[0], depthMsgs[0]);
542  rtabmap_ros::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
543  rtabmap_ros::toCvShare(image3, imageMsgs[2], depthMsgs[2]);
544  rtabmap_ros::toCvShare(image4, imageMsgs[3], depthMsgs[3]);
545  infoMsgs.push_back(image->rgb_camera_info);
546  infoMsgs.push_back(image2->rgb_camera_info);
547  infoMsgs.push_back(image3->rgb_camera_info);
548  infoMsgs.push_back(image4->rgb_camera_info);
549 
550  this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
551  }
552  }
553 
554 protected:
555  virtual void flushCallbacks()
556  {
557  // flush callbacks
558  if(approxSync_)
559  {
560  delete approxSync_;
561  approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
562  approxSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
563  }
564  if(exactSync_)
565  {
566  delete exactSync_;
567  exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
568  exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
569  }
570  if(approxSync2_)
571  {
572  delete approxSync2_;
573  approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
574  MyApproxSync2Policy(queueSize_),
575  rgbd_image1_sub_,
576  rgbd_image2_sub_);
577  approxSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2));
578  }
579  if(exactSync2_)
580  {
581  delete exactSync2_;
582  exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
583  MyExactSync2Policy(queueSize_),
584  rgbd_image1_sub_,
585  rgbd_image2_sub_);
586  exactSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2));
587  }
588  if(approxSync3_)
589  {
590  delete approxSync3_;
591  approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
592  MyApproxSync3Policy(queueSize_),
593  rgbd_image1_sub_,
594  rgbd_image2_sub_,
595  rgbd_image3_sub_);
596  approxSync3_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD3, this, _1, _2, _3));
597  }
598  if(exactSync3_)
599  {
600  delete exactSync3_;
601  exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
602  MyExactSync3Policy(queueSize_),
603  rgbd_image1_sub_,
604  rgbd_image2_sub_,
605  rgbd_image3_sub_);
606  exactSync3_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD3, this, _1, _2, _3));
607  }
608  if(approxSync4_)
609  {
610  delete approxSync4_;
611  approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
612  MyApproxSync4Policy(queueSize_),
613  rgbd_image1_sub_,
614  rgbd_image2_sub_,
615  rgbd_image3_sub_,
616  rgbd_image4_sub_);
617  approxSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, _1, _2, _3, _4));
618  }
619  if(exactSync4_)
620  {
621  delete exactSync4_;
622  exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
623  MyExactSync4Policy(queueSize_),
624  rgbd_image1_sub_,
625  rgbd_image2_sub_,
626  rgbd_image3_sub_,
627  rgbd_image4_sub_);
628  exactSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, _1, _2, _3, _4));
629  }
630  }
631 
632 private:
633  image_transport::SubscriberFilter image_mono_sub_;
634  image_transport::SubscriberFilter image_depth_sub_;
635  message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
636 
637  ros::Subscriber rgbdSub_;
638  message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image1_sub_;
639  message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image2_sub_;
640  message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image3_sub_;
641  message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image4_sub_;
642 
643  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
644  message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
645  typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncPolicy;
646  message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
647  typedef message_filters::sync_policies::ApproximateTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyApproxSync2Policy;
648  message_filters::Synchronizer<MyApproxSync2Policy> * approxSync2_;
649  typedef message_filters::sync_policies::ExactTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyExactSync2Policy;
650  message_filters::Synchronizer<MyExactSync2Policy> * exactSync2_;
651  typedef message_filters::sync_policies::ApproximateTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyApproxSync3Policy;
652  message_filters::Synchronizer<MyApproxSync3Policy> * approxSync3_;
653  typedef message_filters::sync_policies::ExactTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyExactSync3Policy;
654  message_filters::Synchronizer<MyExactSync3Policy> * exactSync3_;
655  typedef message_filters::sync_policies::ApproximateTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyApproxSync4Policy;
656  message_filters::Synchronizer<MyApproxSync4Policy> * approxSync4_;
657  typedef message_filters::sync_policies::ExactTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyExactSync4Policy;
658  message_filters::Synchronizer<MyExactSync4Policy> * exactSync4_;
659  int queueSize_;
660  bool keepColor_;
661 };
662 
663 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::RGBDOdometry, nodelet::Nodelet);
664 
665 }
cv_bridge::toCvShare
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
sensor_msgs::image_encodings::BAYER_GRBG8
const std::string BAYER_GRBG8
rtabmap_ros::RGBDOdometry::rgbd_image2_sub_
message_filters::Subscriber< rtabmap_ros::RGBDImage > rgbd_image2_sub_
Definition: rgbd_odometry.cpp:639
rtabmap_ros::RGBDOdometry::info_sub_
message_filters::Subscriber< sensor_msgs::CameraInfo > info_sub_
Definition: rgbd_odometry.cpp:635
message_filters::Synchronizer
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
image_transport::SubscriberFilter
sensor_msgs::image_encodings::BGR8
const std::string BGR8
rtabmap_ros::RGBDOdometry::callbackRGBD4
void callbackRGBD4(const rtabmap_ros::RGBDImageConstPtr &image, const rtabmap_ros::RGBDImageConstPtr &image2, const rtabmap_ros::RGBDImageConstPtr &image3, const rtabmap_ros::RGBDImageConstPtr &image4)
Definition: rgbd_odometry.cpp:529
uFormat
std::string uFormat(const char *fmt,...)
ros::NodeHandle
NODELET_ERROR
#define NODELET_ERROR(...)
sensor_msgs::image_encodings::RGB8
const std::string RGB8
NODELET_WARN
#define NODELET_WARN(...)
subscriber_filter.h
rtabmap_ros::RGBDOdometry::MyApproxSyncPolicy
message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo > MyApproxSyncPolicy
Definition: rgbd_odometry.cpp:643
rtabmap_ros::RGBDOdometry::commonCallback
void commonCallback(const std::vector< cv_bridge::CvImageConstPtr > &rgbImages, const std::vector< cv_bridge::CvImageConstPtr > &depthImages, const std::vector< sensor_msgs::CameraInfo > &cameraInfos)
Definition: rgbd_odometry.cpp:321
rtabmap_ros::RGBDOdometry::exactSync2_
message_filters::Synchronizer< MyExactSync2Policy > * exactSync2_
Definition: rgbd_odometry.cpp:650
rtabmap_ros::RGBDOdometry::MyExactSyncPolicy
message_filters::sync_policies::ExactTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo > MyExactSyncPolicy
Definition: rgbd_odometry.cpp:645
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
rtabmap_ros::RGBDOdometry::rgbd_image1_sub_
message_filters::Subscriber< rtabmap_ros::RGBDImage > rgbd_image1_sub_
Definition: rgbd_odometry.cpp:638
message_filters::sync_policies::ExactTime
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
ros::NodeHandle::resolveName
std::string resolveName(const std::string &name, bool remap=true) const
rtabmap_ros::RGBDOdometry::MyApproxSync2Policy
message_filters::sync_policies::ApproximateTime< rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage > MyApproxSync2Policy
Definition: rgbd_odometry.cpp:647
image_transport.h
rtabmap_ros::RGBDOdometry::rgbd_image3_sub_
message_filters::Subscriber< rtabmap_ros::RGBDImage > rgbd_image3_sub_
Definition: rgbd_odometry.cpp:640
rtabmap_ros::RGBDOdometry::exactSync_
message_filters::Synchronizer< MyExactSyncPolicy > * exactSync_
Definition: rgbd_odometry.cpp:646
getName
std::string getName(void *handle)
rtabmap_ros::RGBDOdometry::approxSync3_
message_filters::Synchronizer< MyApproxSync3Policy > * approxSync3_
Definition: rgbd_odometry.cpp:652
sensor_msgs::image_encodings::TYPE_8UC1
const std::string TYPE_8UC1
rtabmap_ros::RGBDOdometry::MyExactSync2Policy
message_filters::sync_policies::ExactTime< rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage > MyExactSync2Policy
Definition: rgbd_odometry.cpp:649
rtabmap_ros::RGBDOdometry::keepColor_
bool keepColor_
Definition: rgbd_odometry.cpp:660
rtabmap_ros::RGBDOdometry::approxSync4_
message_filters::Synchronizer< MyApproxSync4Policy > * approxSync4_
Definition: rgbd_odometry.cpp:656
rtabmap_ros::RGBDOdometry::onOdomInit
virtual void onOdomInit()
Definition: rgbd_odometry.cpp:116
rtabmap_ros
Definition: CommonDataSubscriber.h:56
ROS_WARN
#define ROS_WARN(...)
rtabmap_ros::getTransform
rtabmap::Transform getTransform(const std::string &fromFrameId, const std::string &toFrameId, const ros::Time &stamp, tf::TransformListener &listener, double waitForTransform)
Definition: MsgConversion.cpp:1614
rtabmap_ros::RGBDOdometry::image_depth_sub_
image_transport::SubscriberFilter image_depth_sub_
Definition: rgbd_odometry.cpp:634
rtabmap_ros::RGBDOdometry::callbackRGBD
void callbackRGBD(const rtabmap_ros::RGBDImageConstPtr &image)
Definition: rgbd_odometry.cpp:472
boost::shared_ptr
rtabmap_ros::RGBDOdometry::callbackRGBD2
void callbackRGBD2(const rtabmap_ros::RGBDImageConstPtr &image, const rtabmap_ros::RGBDImageConstPtr &image2)
Definition: rgbd_odometry.cpp:488
rtabmap_ros::RGBDOdometry::MyApproxSync3Policy
message_filters::sync_policies::ApproximateTime< rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage > MyApproxSync3Policy
Definition: rgbd_odometry.cpp:651
time_synchronizer.h
stereo_camera_model.h
rtabmap_ros::RGBDOdometry::callback
void callback(const sensor_msgs::ImageConstPtr &image, const sensor_msgs::ImageConstPtr &depth, const sensor_msgs::CameraInfoConstPtr &cameraInfo)
Definition: rgbd_odometry.cpp:453
sensor_msgs::image_encodings::RGBA8
const std::string RGBA8
true
#define true
rtabmap_ros::RGBDOdometry::approxSync2_
message_filters::Synchronizer< MyApproxSync2Policy > * approxSync2_
Definition: rgbd_odometry.cpp:648
rtabmap::Transform::isNull
bool isNull() const
cv_bridge.h
message_filters::Synchronizer::registerCallback
Connection registerCallback(C &callback)
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
cv_bridge::cvtColor
CvImagePtr cvtColor(const CvImageConstPtr &source, const std::string &encoding)
class_list_macros.h
rtabmap_ros::RGBDOdometry::approxSync_
message_filters::Synchronizer< MyApproxSyncPolicy > * approxSync_
Definition: rgbd_odometry.cpp:644
ros::NodeHandle::param
bool param(const std::string &param_name, T &param_val, const T &default_val) const
sensor_msgs::image_encodings::TYPE_32FC1
const std::string TYPE_32FC1
rtabmap_ros::RGBDOdometry
Definition: rgbd_odometry.cpp:59
rtabmap_ros::cameraModelFromROS
rtabmap::CameraModel cameraModelFromROS(const sensor_msgs::CameraInfo &camInfo, const rtabmap::Transform &localTransform=rtabmap::Transform::getIdentity())
Definition: MsgConversion.cpp:767
sensor_msgs::image_encodings::MONO8
const std::string MONO8
rtabmap_ros::RGBDOdometry::rgbdSub_
ros::Subscriber rgbdSub_
Definition: rgbd_odometry.cpp:637
sensor_msgs::image_encodings::TYPE_16UC1
const std::string TYPE_16UC1
approximate_time.h
sensor_msgs::image_encodings::BGRA8
const std::string BGRA8
ParametersMap
QMap< QString, QVariant > ParametersMap
sensor_msgs::image_encodings::MONO16
const std::string MONO16
rtabmap_ros::RGBDOdometry::image_mono_sub_
image_transport::SubscriberFilter image_mono_sub_
Definition: rgbd_odometry.cpp:633
gt_tf_broadcaster.stamp
float stamp
Definition: gt_tf_broadcaster.py:28
rtabmap_ros::RGBDOdometry::MyApproxSync4Policy
message_filters::sync_policies::ApproximateTime< rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage > MyApproxSync4Policy
Definition: rgbd_odometry.cpp:655
rtabmap_ros::RGBDOdometry::RGBDOdometry
RGBDOdometry()
Definition: rgbd_odometry.cpp:62
patrol.frameId
string frameId
Definition: patrol.py:11
rtabmap_ros::RGBDOdometry::~RGBDOdometry
virtual ~RGBDOdometry()
Definition: rgbd_odometry.cpp:77
false
#define false
callback
void callback(rtabmap_ros::CameraConfig &config, uint32_t level)
Definition: CameraNode.cpp:259
rtabmap_ros::RGBDOdometry::MyExactSync4Policy
message_filters::sync_policies::ExactTime< rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage > MyExactSync4Policy
Definition: rgbd_odometry.cpp:657
rtabmap_ros::RGBDOdometry::rgbd_image4_sub_
message_filters::Subscriber< rtabmap_ros::RGBDImage > rgbd_image4_sub_
Definition: rgbd_odometry.cpp:641
message_filters::sync_policies::ApproximateTime
NODELET_INFO
#define NODELET_INFO(...)
rtabmap_ros::PLUGINLIB_EXPORT_CLASS
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::RGBDOdometry, nodelet::Nodelet)
rtabmap_ros::RGBDOdometry::updateParameters
virtual void updateParameters(ParametersMap &parameters)
Definition: rgbd_odometry.cpp:292
rtabmap_ros::RGBDOdometry::exactSync4_
message_filters::Synchronizer< MyExactSync4Policy > * exactSync4_
Definition: rgbd_odometry.cpp:658
image_transport::TransportHints
rtabmap_ros::toCvShare
void toCvShare(const rtabmap_ros::RGBDImageConstPtr &image, cv_bridge::CvImageConstPtr &rgb, cv_bridge::CvImageConstPtr &depth)
Definition: MsgConversion.cpp:164
rtabmap_ros::RGBDOdometry::callbackRGBD3
void callbackRGBD3(const rtabmap_ros::RGBDImageConstPtr &image, const rtabmap_ros::RGBDImageConstPtr &image2, const rtabmap_ros::RGBDImageConstPtr &image3)
Definition: rgbd_odometry.cpp:507
rtabmap_ros::timestampFromROS
double timestampFromROS(const ros::Time &stamp)
Definition: MsgConversion.h:187
image_transport::ImageTransport
image_encodings.h
rtabmap_ros::RGBDOdometry::MyExactSync3Policy
message_filters::sync_policies::ExactTime< rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage > MyExactSync3Policy
Definition: rgbd_odometry.cpp:653
ros::NodeHandle::hasParam
bool hasParam(const std::string &key) const
rtabmap_ros::RGBDOdometry::exactSync3_
message_filters::Synchronizer< MyExactSync3Policy > * exactSync3_
Definition: rgbd_odometry.cpp:654
message_filters::Subscriber< sensor_msgs::CameraInfo >
NODELET_FATAL
#define NODELET_FATAL(...)
ROS_ASSERT
#define ROS_ASSERT(cond)
ROS_ERROR
#define ROS_ERROR(...)
rtabmap_ros::RGBDOdometry::queueSize_
int queueSize_
Definition: rgbd_odometry.cpp:659
rtabmap_ros::OdometryROS
Definition: OdometryROS.h:53
rtabmap_ros::RGBDOdometry::flushCallbacks
virtual void flushCallbacks()
Definition: rgbd_odometry.cpp:555
uInsert
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)

rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:42:19