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

rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Fri Jun 7 2019 21:55:04