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_odom/OdometryROS.h>
29 
30 #include <pluginlib/class_list_macros.hpp>
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_conversions/MsgConversion.h"
47 #include <rtabmap_msgs/RGBDImages.h>
48 
49 #include <rtabmap/core/util3d.h>
50 #include <rtabmap/core/util2d.h>
51 #include <rtabmap/utilite/ULogger.h>
52 #include <rtabmap/utilite/UConversion.h>
53 #include <rtabmap/utilite/UStl.h>
54 
55 using namespace rtabmap;
56 
57 namespace rtabmap_odom
58 {
59 
60 class RGBDOdometry : public OdometryROS
61 {
62 public:
63  RGBDOdometry() :
64  OdometryROS(false, true, false),
65  approxSync_(0),
66  exactSync_(0),
67  approxSync2_(0),
68  exactSync2_(0),
69  approxSync3_(0),
70  exactSync3_(0),
71  approxSync4_(0),
72  exactSync4_(0),
73  approxSync5_(0),
74  exactSync5_(0),
75  approxSync6_(0),
76  exactSync6_(0),
77  topicQueueSize_(1),
78  syncQueueSize_(5),
79  keepColor_(false)
80  {
81  }
82 
83  virtual ~RGBDOdometry()
84  {
85  rgbdSub_.shutdown();
86  rgbdxSub_.shutdown();
87  delete approxSync_;
88  delete exactSync_;
89  delete approxSync2_;
90  delete exactSync2_;
91  delete approxSync3_;
92  delete exactSync3_;
93  delete approxSync4_;
94  delete exactSync4_;
95  delete approxSync5_;
96  delete exactSync5_;
97  delete approxSync6_;
98  delete exactSync6_;
99  }
100 
101 private:
102 
103  virtual void onOdomInit()
104  {
105  ros::NodeHandle & nh = getNodeHandle();
106  ros::NodeHandle & pnh = getPrivateNodeHandle();
107 
108  int rgbdCameras = 1;
109  bool approxSync = true;
110  bool subscribeRGBD = false;
111  double approxSyncMaxInterval = 0.0;
112  pnh.param("approx_sync", approxSync, approxSync);
113  pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
114  pnh.param("topic_queue_size", topicQueueSize_, topicQueueSize_);
115  if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size"))
116  {
117  pnh.param("queue_size", syncQueueSize_, syncQueueSize_);
118  ROS_WARN("Parameter \"queue_size\" has been renamed "
119  "to \"sync_queue_size\" and will be removed "
120  "in future versions! The value (%d) is still copied to "
121  "\"sync_queue_size\".", syncQueueSize_);
122  }
123  else
124  {
125  pnh.param("sync_queue_size", syncQueueSize_, syncQueueSize_);
126  }
127  pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD);
128  if(pnh.hasParam("depth_cameras"))
129  {
130  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.");
131  }
132  pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras);
133  if(rgbdCameras < 0)
134  {
135  rgbdCameras = 0;
136  }
137  pnh.param("keep_color", keepColor_, keepColor_);
138 
139  NODELET_INFO("RGBDOdometry: approx_sync = %s", approxSync?"true":"false");
140  if(approxSync)
141  NODELET_INFO("RGBDOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval);
142  NODELET_INFO("RGBDOdometry: topic_queue_size = %d", topicQueueSize_);
143  NODELET_INFO("RGBDOdometry: sync_queue_size = %d", syncQueueSize_);
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 subscribedTopic;
149  std::string subscribedTopicsMsg;
150  if(subscribeRGBD)
151  {
152  if(rgbdCameras >= 2)
153  {
154  rgbd_image1_sub_.subscribe(nh, "rgbd_image0", topicQueueSize_);
155  rgbd_image2_sub_.subscribe(nh, "rgbd_image1", topicQueueSize_);
156  if(rgbdCameras >= 3)
157  {
158  rgbd_image3_sub_.subscribe(nh, "rgbd_image2", topicQueueSize_);
159  }
160  if(rgbdCameras >= 4)
161  {
162  rgbd_image4_sub_.subscribe(nh, "rgbd_image3", topicQueueSize_);
163  }
164  if(rgbdCameras >= 5)
165  {
166  rgbd_image5_sub_.subscribe(nh, "rgbd_image4", topicQueueSize_);
167  }
168  if(rgbdCameras >= 6)
169  {
170  rgbd_image6_sub_.subscribe(nh, "rgbd_image5", topicQueueSize_);
171  }
172 
173  if(rgbdCameras == 2)
174  {
175  if(approxSync)
176  {
177  approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
178  MyApproxSync2Policy(syncQueueSize_),
179  rgbd_image1_sub_,
180  rgbd_image2_sub_);
181  if(approxSyncMaxInterval > 0.0)
182  approxSync2_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
183  approxSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2));
184  }
185  else
186  {
187  exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
188  MyExactSync2Policy(syncQueueSize_),
189  rgbd_image1_sub_,
190  rgbd_image2_sub_);
191  exactSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2));
192  }
193  subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s",
194  getName().c_str(),
195  approxSync?"approx":"exact",
196  approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
197  rgbd_image1_sub_.getTopic().c_str(),
198  rgbd_image2_sub_.getTopic().c_str());
199  }
200  else if(rgbdCameras == 3)
201  {
202  if(approxSync)
203  {
204  approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
205  MyApproxSync3Policy(syncQueueSize_),
206  rgbd_image1_sub_,
207  rgbd_image2_sub_,
208  rgbd_image3_sub_);
209  if(approxSyncMaxInterval > 0.0)
210  approxSync3_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
211  approxSync3_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD3, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
212  }
213  else
214  {
215  exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
216  MyExactSync3Policy(syncQueueSize_),
217  rgbd_image1_sub_,
218  rgbd_image2_sub_,
219  rgbd_image3_sub_);
220  exactSync3_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD3, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
221  }
222  subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s",
223  getName().c_str(),
224  approxSync?"approx":"exact",
225  approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
226  rgbd_image1_sub_.getTopic().c_str(),
227  rgbd_image2_sub_.getTopic().c_str(),
228  rgbd_image3_sub_.getTopic().c_str());
229  }
230  else if(rgbdCameras == 4)
231  {
232  if(approxSync)
233  {
234  approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
235  MyApproxSync4Policy(syncQueueSize_),
236  rgbd_image1_sub_,
237  rgbd_image2_sub_,
238  rgbd_image3_sub_,
239  rgbd_image4_sub_);
240  if(approxSyncMaxInterval > 0.0)
241  approxSync4_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
242  approxSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
243  }
244  else
245  {
246  exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
247  MyExactSync4Policy(syncQueueSize_),
248  rgbd_image1_sub_,
249  rgbd_image2_sub_,
250  rgbd_image3_sub_,
251  rgbd_image4_sub_);
252  exactSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
253  }
254  subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s",
255  getName().c_str(),
256  approxSync?"approx":"exact",
257  approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
258  rgbd_image1_sub_.getTopic().c_str(),
259  rgbd_image2_sub_.getTopic().c_str(),
260  rgbd_image3_sub_.getTopic().c_str(),
261  rgbd_image4_sub_.getTopic().c_str());
262  }
263  else if(rgbdCameras == 5)
264  {
265  if(approxSync)
266  {
267  approxSync5_ = new message_filters::Synchronizer<MyApproxSync5Policy>(
268  MyApproxSync5Policy(syncQueueSize_),
269  rgbd_image1_sub_,
270  rgbd_image2_sub_,
271  rgbd_image3_sub_,
272  rgbd_image4_sub_,
273  rgbd_image5_sub_);
274  if(approxSyncMaxInterval > 0.0)
275  approxSync5_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
276  approxSync5_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD5, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5));
277  }
278  else
279  {
280  exactSync5_ = new message_filters::Synchronizer<MyExactSync5Policy>(
281  MyExactSync5Policy(syncQueueSize_),
282  rgbd_image1_sub_,
283  rgbd_image2_sub_,
284  rgbd_image3_sub_,
285  rgbd_image4_sub_,
286  rgbd_image5_sub_);
287  exactSync5_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD5, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5));
288  }
289  subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s \\\n %s",
290  getName().c_str(),
291  approxSync?"approx":"exact",
292  approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
293  rgbd_image1_sub_.getTopic().c_str(),
294  rgbd_image2_sub_.getTopic().c_str(),
295  rgbd_image3_sub_.getTopic().c_str(),
296  rgbd_image4_sub_.getTopic().c_str(),
297  rgbd_image5_sub_.getTopic().c_str());
298  }
299  else if(rgbdCameras == 6)
300  {
301  if(approxSync)
302  {
303  approxSync6_ = new message_filters::Synchronizer<MyApproxSync6Policy>(
304  MyApproxSync6Policy(syncQueueSize_),
305  rgbd_image1_sub_,
306  rgbd_image2_sub_,
307  rgbd_image3_sub_,
308  rgbd_image4_sub_,
309  rgbd_image5_sub_,
310  rgbd_image6_sub_);
311  if(approxSyncMaxInterval > 0.0)
312  approxSync6_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
313  approxSync6_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD6, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6));
314  }
315  else
316  {
317  exactSync6_ = new message_filters::Synchronizer<MyExactSync6Policy>(
318  MyExactSync6Policy(syncQueueSize_),
319  rgbd_image1_sub_,
320  rgbd_image2_sub_,
321  rgbd_image3_sub_,
322  rgbd_image4_sub_,
323  rgbd_image5_sub_,
324  rgbd_image6_sub_);
325  exactSync6_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD6, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6));
326  }
327  subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s",
328  getName().c_str(),
329  approxSync?"approx":"exact",
330  approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
331  rgbd_image1_sub_.getTopic().c_str(),
332  rgbd_image2_sub_.getTopic().c_str(),
333  rgbd_image3_sub_.getTopic().c_str(),
334  rgbd_image4_sub_.getTopic().c_str(),
335  rgbd_image5_sub_.getTopic().c_str(),
336  rgbd_image6_sub_.getTopic().c_str());
337  }
338  else
339  {
340  NODELET_FATAL("%s doesn't support more than 6 cameras (rgbd_cameras=%d) with "
341  "internal synchronization interface, set rgbd_cameras=0 and use "
342  "rgbd_images input topic instead for more cameras (for which "
343  "rgbdx_sync node can sync up to 8 cameras).",
344  getName().c_str(), rgbdCameras);
345  }
346 
347  }
348  else if(rgbdCameras == 0)
349  {
350  rgbdxSub_ = nh.subscribe("rgbd_images", topicQueueSize_, &RGBDOdometry::callbackRGBDX, this);
351 
352  subscribedTopicsMsg =
353  uFormat("\n%s subscribed to:\n %s",
354  getName().c_str(),
355  rgbdxSub_.getTopic().c_str());
356  }
357  else
358  {
359  rgbdSub_ = nh.subscribe("rgbd_image", topicQueueSize_, &RGBDOdometry::callbackRGBD, this);
360 
361  subscribedTopicsMsg =
362  uFormat("\n%s subscribed to:\n %s",
363  getName().c_str(),
364  rgbdSub_.getTopic().c_str());
365  }
366  }
367  else
368  {
369  ros::NodeHandle rgb_nh(nh, "rgb");
370  ros::NodeHandle depth_nh(nh, "depth");
371  ros::NodeHandle rgb_pnh(pnh, "rgb");
372  ros::NodeHandle depth_pnh(pnh, "depth");
373  image_transport::ImageTransport rgb_it(rgb_nh);
374  image_transport::ImageTransport depth_it(depth_nh);
375  image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
376  image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
377 
378  image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), topicQueueSize_, hintsRgb);
379  image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), topicQueueSize_, hintsDepth);
380  info_sub_.subscribe(rgb_nh, "camera_info", topicQueueSize_);
381 
382  if(approxSync)
383  {
384  approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
385  if(approxSyncMaxInterval > 0.0)
386  approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
387  approxSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
388  }
389  else
390  {
391  exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
392  exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
393  }
394 
395  subscribedTopic = rgb_nh.resolveName("image");
396  subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s",
397  getName().c_str(),
398  approxSync?"approx":"exact",
399  approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
400  image_mono_sub_.getTopic().c_str(),
401  image_depth_sub_.getTopic().c_str(),
402  info_sub_.getTopic().c_str());
403  }
404  initDiagnosticMsg(subscribedTopicsMsg, approxSync, subscribedTopic);
405  }
406 
407  virtual void updateParameters(ParametersMap & parameters)
408  {
409  //make sure we are using Reg/Strategy=0
410  ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy());
411  if(iter != parameters.end() && iter->second.compare("0") != 0)
412  {
413  ROS_WARN("RGBD odometry works only with \"Reg/Strategy\"=0. Ignoring value %s.", iter->second.c_str());
414  }
415  uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "0"));
416 
417  int estimationType = Parameters::defaultVisEstimationType();
418  Parameters::parse(parameters, Parameters::kVisEstimationType(), estimationType);
419  ros::NodeHandle & pnh = getPrivateNodeHandle();
420  int rgbdCameras = 1;
421  bool subscribeRGBD = false;
422  pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD);
423  pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras);
424  }
425 
426  void commonCallback(
427  const std::vector<cv_bridge::CvImageConstPtr> & rgbImages,
428  const std::vector<cv_bridge::CvImageConstPtr> & depthImages,
429  const std::vector<sensor_msgs::CameraInfo>& cameraInfos)
430  {
431  ROS_ASSERT(rgbImages.size() > 0 && rgbImages.size() == depthImages.size() && rgbImages.size() == cameraInfos.size());
432  ros::Time higherStamp;
433  int imageWidth = rgbImages[0]->image.cols;
434  int imageHeight = rgbImages[0]->image.rows;
435  int depthWidth = depthImages[0]->image.cols;
436  int depthHeight = depthImages[0]->image.rows;
437 
438  UASSERT_MSG(
439  imageWidth/depthWidth == imageHeight/depthHeight,
440  uFormat("rgb=%dx%d depth=%dx%d", imageWidth, imageHeight, depthWidth, depthHeight).c_str());
441 
442  int cameraCount = rgbImages.size();
443  cv::Mat rgb;
444  cv::Mat depth;
445  std::vector<rtabmap::CameraModel> cameraModels;
446  for(unsigned int i=0; i<rgbImages.size(); ++i)
447  {
448  if(!(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
449  rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
450  rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
451  rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
452  rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
453  rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
454  rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
455  rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0) ||
456  !(depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
457  depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
458  depthImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
459  {
460  NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8,bgra8,rgba8 and "
461  "image_depth=32FC1,16UC1,mono16. Current rgb=%s and depth=%s",
462  rgbImages[i]->encoding.c_str(),
463  depthImages[i]->encoding.c_str());
464  return;
465  }
466  UASSERT_MSG(rgbImages[i]->image.cols == imageWidth && rgbImages[i]->image.rows == imageHeight,
467  uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
468  imageWidth,
469  rgbImages[i]->image.cols,
470  imageHeight,
471  rgbImages[i]->image.rows).c_str());
472  UASSERT_MSG(depthImages[i]->image.cols == depthWidth && depthImages[i]->image.rows == depthHeight,
473  uFormat("depthWidth=%d vs %d depthHeight=%d vs %d",
474  depthWidth,
475  depthImages[i]->image.cols,
476  depthHeight,
477  depthImages[i]->image.rows).c_str());
478 
479  ros::Time stamp = rgbImages[i]->header.stamp>depthImages[i]->header.stamp?rgbImages[i]->header.stamp:depthImages[i]->header.stamp;
480 
481  if(i == 0)
482  {
483  higherStamp = stamp;
484  }
485  else if(stamp > higherStamp)
486  {
487  higherStamp = stamp;
488  }
489 
490  Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), rgbImages[i]->header.frame_id, stamp, this->tfListener(), this->waitForTransformDuration());
491  if(localTransform.isNull())
492  {
493  return;
494  }
495 
496  if(i>0)
497  {
498  double stampDiff = fabs(rgbImages[i]->header.stamp.toSec() - rgbImages[i-1]->header.stamp.toSec());
499  if(stampDiff > 1.0/60.0)
500  {
501  static bool warningShown = false;
502  if(!warningShown)
503  {
504  NODELET_WARN("The time difference between cameras %d and %d is "
505  "high (diff=%fs, cam%d=%fs, cam%d=%fs). You may want "
506  "to set approx_sync_max_interval to reject bad synchronizations or use "
507  "approx_sync=false if streams have all the exact same timestamp. This "
508  "message is only printed once.",
509  i-1, i,
510  stampDiff,
511  i-1, rgbImages[i-1]->header.stamp.toSec(),
512  i, rgbImages[i]->header.stamp.toSec());
513  warningShown = true;
514  }
515  }
516  }
517 
518 
519  cv_bridge::CvImageConstPtr ptrImage = rgbImages[i];
520  if(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
521  rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
522  {
523  if(keepColor_ && rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0)
524  {
525  ptrImage = cv_bridge::cvtColor(rgbImages[i], "bgr8");
526  }
527  else
528  {
529  ptrImage = cv_bridge::cvtColor(rgbImages[i], "mono8");
530  }
531  }
532 
533  cv_bridge::CvImageConstPtr ptrDepth = depthImages[i];
534 
535  // initialize
536  if(rgb.empty())
537  {
538  rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type());
539  }
540  if(depth.empty())
541  {
542  depth = cv::Mat(depthHeight, depthWidth*cameraCount, ptrDepth->image.type());
543  }
544 
545  if(ptrImage->image.type() == rgb.type())
546  {
547  ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
548  }
549  else
550  {
551  NODELET_ERROR("Some RGB images are not the same type! %d vs %d", ptrImage->image.type(), rgb.type());
552  return;
553  }
554 
555  if(ptrDepth->image.type() == depth.type())
556  {
557  ptrDepth->image.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight)));
558  }
559  else
560  {
561  NODELET_ERROR("Some Depth images are not the same type! %d vs %d", ptrDepth->image.type(), depth.type());
562  return;
563  }
564 
565  cameraModels.push_back(rtabmap_conversions::cameraModelFromROS(cameraInfos[i], localTransform));
566  }
567 
568  rtabmap::SensorData data(
569  rgb,
570  depth,
571  cameraModels,
572  0,
573  rtabmap_conversions::timestampFromROS(higherStamp));
574 
575  std_msgs::Header header;
576  header.stamp = higherStamp;
577  header.frame_id = rgbImages.size()==1?rgbImages[0]->header.frame_id:"";
578  this->processData(data, header);
579  }
580 
581  void callback(
582  const sensor_msgs::ImageConstPtr& image,
583  const sensor_msgs::ImageConstPtr& depth,
584  const sensor_msgs::CameraInfoConstPtr& cameraInfo)
585  {
586  if(!this->isPaused())
587  {
588  std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);
589  std::vector<cv_bridge::CvImageConstPtr> depthMsgs(1);
590  std::vector<sensor_msgs::CameraInfo> infoMsgs;
591  imageMsgs[0] = cv_bridge::toCvShare(image);
592  depthMsgs[0] = cv_bridge::toCvShare(depth);
593  infoMsgs.push_back(*cameraInfo);
594 
595  double stampDiff = fabs(image->header.stamp.toSec() - depth->header.stamp.toSec());
596  if(stampDiff > 0.020)
597  {
598  NODELET_WARN("The time difference between rgb and depth frames is "
599  "high (diff=%fs, rgb=%fs, depth=%fs). You may want "
600  "to set approx_sync_max_interval lower than 0.02s to reject spurious bad synchronizations or use "
601  "approx_sync=false if streams have all the exact same timestamp.",
602  stampDiff,
603  image->header.stamp.toSec(),
604  depth->header.stamp.toSec());
605  }
606 
607  this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
608  }
609  }
610 
611  void callbackRGBD(
612  const rtabmap_msgs::RGBDImageConstPtr& image)
613  {
614  if(!this->isPaused())
615  {
616  std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);
617  std::vector<cv_bridge::CvImageConstPtr> depthMsgs(1);
618  std::vector<sensor_msgs::CameraInfo> infoMsgs;
619  rtabmap_conversions::toCvShare(image, imageMsgs[0], depthMsgs[0]);
620  infoMsgs.push_back(image->rgb_camera_info);
621 
622  this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
623  }
624  }
625 
626  void callbackRGBDX(
627  const rtabmap_msgs::RGBDImagesConstPtr& images)
628  {
629  if(!this->isPaused())
630  {
631  if(images->rgbd_images.empty())
632  {
633  NODELET_ERROR("Input topic \"%s\" doesn't contain any image(s)!", rgbdxSub_.getTopic().c_str());
634  return;
635  }
636  std::vector<cv_bridge::CvImageConstPtr> imageMsgs(images->rgbd_images.size());
637  std::vector<cv_bridge::CvImageConstPtr> depthMsgs(images->rgbd_images.size());
638  std::vector<sensor_msgs::CameraInfo> infoMsgs;
639  for(size_t i=0; i<images->rgbd_images.size(); ++i)
640  {
641  rtabmap_conversions::toCvShare(images->rgbd_images[i], images, imageMsgs[i], depthMsgs[i]);
642  infoMsgs.push_back(images->rgbd_images[i].rgb_camera_info);
643  }
644 
645  this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
646  }
647  }
648 
649  void callbackRGBD2(
650  const rtabmap_msgs::RGBDImageConstPtr& image,
651  const rtabmap_msgs::RGBDImageConstPtr& image2)
652  {
653  if(!this->isPaused())
654  {
655  std::vector<cv_bridge::CvImageConstPtr> imageMsgs(2);
656  std::vector<cv_bridge::CvImageConstPtr> depthMsgs(2);
657  std::vector<sensor_msgs::CameraInfo> infoMsgs;
658  rtabmap_conversions::toCvShare(image, imageMsgs[0], depthMsgs[0]);
659  rtabmap_conversions::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
660  infoMsgs.push_back(image->rgb_camera_info);
661  infoMsgs.push_back(image2->rgb_camera_info);
662 
663  this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
664  }
665  }
666 
667  void callbackRGBD3(
668  const rtabmap_msgs::RGBDImageConstPtr& image,
669  const rtabmap_msgs::RGBDImageConstPtr& image2,
670  const rtabmap_msgs::RGBDImageConstPtr& image3)
671  {
672  if(!this->isPaused())
673  {
674  std::vector<cv_bridge::CvImageConstPtr> imageMsgs(3);
675  std::vector<cv_bridge::CvImageConstPtr> depthMsgs(3);
676  std::vector<sensor_msgs::CameraInfo> infoMsgs;
677  rtabmap_conversions::toCvShare(image, imageMsgs[0], depthMsgs[0]);
678  rtabmap_conversions::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
679  rtabmap_conversions::toCvShare(image3, imageMsgs[2], depthMsgs[2]);
680  infoMsgs.push_back(image->rgb_camera_info);
681  infoMsgs.push_back(image2->rgb_camera_info);
682  infoMsgs.push_back(image3->rgb_camera_info);
683 
684  this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
685  }
686  }
687 
688  void callbackRGBD4(
689  const rtabmap_msgs::RGBDImageConstPtr& image,
690  const rtabmap_msgs::RGBDImageConstPtr& image2,
691  const rtabmap_msgs::RGBDImageConstPtr& image3,
692  const rtabmap_msgs::RGBDImageConstPtr& image4)
693  {
694  if(!this->isPaused())
695  {
696  std::vector<cv_bridge::CvImageConstPtr> imageMsgs(4);
697  std::vector<cv_bridge::CvImageConstPtr> depthMsgs(4);
698  std::vector<sensor_msgs::CameraInfo> infoMsgs;
699  rtabmap_conversions::toCvShare(image, imageMsgs[0], depthMsgs[0]);
700  rtabmap_conversions::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
701  rtabmap_conversions::toCvShare(image3, imageMsgs[2], depthMsgs[2]);
702  rtabmap_conversions::toCvShare(image4, imageMsgs[3], depthMsgs[3]);
703  infoMsgs.push_back(image->rgb_camera_info);
704  infoMsgs.push_back(image2->rgb_camera_info);
705  infoMsgs.push_back(image3->rgb_camera_info);
706  infoMsgs.push_back(image4->rgb_camera_info);
707 
708  this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
709  }
710  }
711 
712  void callbackRGBD5(
713  const rtabmap_msgs::RGBDImageConstPtr& image,
714  const rtabmap_msgs::RGBDImageConstPtr& image2,
715  const rtabmap_msgs::RGBDImageConstPtr& image3,
716  const rtabmap_msgs::RGBDImageConstPtr& image4,
717  const rtabmap_msgs::RGBDImageConstPtr& image5)
718  {
719  if(!this->isPaused())
720  {
721  std::vector<cv_bridge::CvImageConstPtr> imageMsgs(5);
722  std::vector<cv_bridge::CvImageConstPtr> depthMsgs(5);
723  std::vector<sensor_msgs::CameraInfo> infoMsgs;
724  rtabmap_conversions::toCvShare(image, imageMsgs[0], depthMsgs[0]);
725  rtabmap_conversions::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
726  rtabmap_conversions::toCvShare(image3, imageMsgs[2], depthMsgs[2]);
727  rtabmap_conversions::toCvShare(image4, imageMsgs[3], depthMsgs[3]);
728  rtabmap_conversions::toCvShare(image5, imageMsgs[4], depthMsgs[4]);
729  infoMsgs.push_back(image->rgb_camera_info);
730  infoMsgs.push_back(image2->rgb_camera_info);
731  infoMsgs.push_back(image3->rgb_camera_info);
732  infoMsgs.push_back(image4->rgb_camera_info);
733  infoMsgs.push_back(image5->rgb_camera_info);
734 
735  this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
736  }
737  }
738  void callbackRGBD6(
739  const rtabmap_msgs::RGBDImageConstPtr& image,
740  const rtabmap_msgs::RGBDImageConstPtr& image2,
741  const rtabmap_msgs::RGBDImageConstPtr& image3,
742  const rtabmap_msgs::RGBDImageConstPtr& image4,
743  const rtabmap_msgs::RGBDImageConstPtr& image5,
744  const rtabmap_msgs::RGBDImageConstPtr& image6)
745  {
746  if(!this->isPaused())
747  {
748  std::vector<cv_bridge::CvImageConstPtr> imageMsgs(6);
749  std::vector<cv_bridge::CvImageConstPtr> depthMsgs(6);
750  std::vector<sensor_msgs::CameraInfo> infoMsgs;
751  rtabmap_conversions::toCvShare(image, imageMsgs[0], depthMsgs[0]);
752  rtabmap_conversions::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
753  rtabmap_conversions::toCvShare(image3, imageMsgs[2], depthMsgs[2]);
754  rtabmap_conversions::toCvShare(image4, imageMsgs[3], depthMsgs[3]);
755  rtabmap_conversions::toCvShare(image5, imageMsgs[4], depthMsgs[4]);
756  rtabmap_conversions::toCvShare(image6, imageMsgs[5], depthMsgs[5]);
757  infoMsgs.push_back(image->rgb_camera_info);
758  infoMsgs.push_back(image2->rgb_camera_info);
759  infoMsgs.push_back(image3->rgb_camera_info);
760  infoMsgs.push_back(image4->rgb_camera_info);
761  infoMsgs.push_back(image5->rgb_camera_info);
762  infoMsgs.push_back(image6->rgb_camera_info);
763 
764  this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
765  }
766  }
767 
768 protected:
769  virtual void flushCallbacks()
770  {
771  // flush callbacks
772  if(approxSync_)
773  {
774  delete approxSync_;
775  approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
776  approxSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
777  }
778  if(exactSync_)
779  {
780  delete exactSync_;
781  exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
782  exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
783  }
784  if(approxSync2_)
785  {
786  delete approxSync2_;
787  approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
788  MyApproxSync2Policy(syncQueueSize_),
789  rgbd_image1_sub_,
790  rgbd_image2_sub_);
791  approxSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2));
792  }
793  if(exactSync2_)
794  {
795  delete exactSync2_;
796  exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
797  MyExactSync2Policy(syncQueueSize_),
798  rgbd_image1_sub_,
799  rgbd_image2_sub_);
800  exactSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2));
801  }
802  if(approxSync3_)
803  {
804  delete approxSync3_;
805  approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
806  MyApproxSync3Policy(syncQueueSize_),
807  rgbd_image1_sub_,
808  rgbd_image2_sub_,
809  rgbd_image3_sub_);
810  approxSync3_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD3, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
811  }
812  if(exactSync3_)
813  {
814  delete exactSync3_;
815  exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
816  MyExactSync3Policy(syncQueueSize_),
817  rgbd_image1_sub_,
818  rgbd_image2_sub_,
819  rgbd_image3_sub_);
820  exactSync3_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD3, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
821  }
822  if(approxSync4_)
823  {
824  delete approxSync4_;
825  approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
826  MyApproxSync4Policy(syncQueueSize_),
827  rgbd_image1_sub_,
828  rgbd_image2_sub_,
829  rgbd_image3_sub_,
830  rgbd_image4_sub_);
831  approxSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
832  }
833  if(exactSync4_)
834  {
835  delete exactSync4_;
836  exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
837  MyExactSync4Policy(syncQueueSize_),
838  rgbd_image1_sub_,
839  rgbd_image2_sub_,
840  rgbd_image3_sub_,
841  rgbd_image4_sub_);
842  exactSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
843  }
844  if(approxSync5_)
845  {
846  delete approxSync5_;
847  approxSync5_ = new message_filters::Synchronizer<MyApproxSync5Policy>(
848  MyApproxSync5Policy(syncQueueSize_),
849  rgbd_image1_sub_,
850  rgbd_image2_sub_,
851  rgbd_image3_sub_,
852  rgbd_image4_sub_,
853  rgbd_image5_sub_);
854  approxSync5_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD5, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5));
855  }
856  if(exactSync5_)
857  {
858  delete exactSync5_;
859  exactSync5_ = new message_filters::Synchronizer<MyExactSync5Policy>(
860  MyExactSync5Policy(syncQueueSize_),
861  rgbd_image1_sub_,
862  rgbd_image2_sub_,
863  rgbd_image3_sub_,
864  rgbd_image4_sub_,
865  rgbd_image5_sub_);
866  exactSync5_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD5, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5));
867  }
868  if(approxSync6_)
869  {
870  delete approxSync6_;
871  approxSync6_ = new message_filters::Synchronizer<MyApproxSync6Policy>(
872  MyApproxSync6Policy(syncQueueSize_),
873  rgbd_image1_sub_,
874  rgbd_image2_sub_,
875  rgbd_image3_sub_,
876  rgbd_image4_sub_,
877  rgbd_image5_sub_,
878  rgbd_image6_sub_);
879  approxSync6_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD6, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6));
880  }
881  if(exactSync6_)
882  {
883  delete exactSync6_;
884  exactSync6_ = new message_filters::Synchronizer<MyExactSync6Policy>(
885  MyExactSync6Policy(syncQueueSize_),
886  rgbd_image1_sub_,
887  rgbd_image2_sub_,
888  rgbd_image3_sub_,
889  rgbd_image4_sub_,
890  rgbd_image5_sub_,
891  rgbd_image6_sub_);
892  exactSync6_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD6, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6));
893  }
894  }
895 
896 private:
897  image_transport::SubscriberFilter image_mono_sub_;
898  image_transport::SubscriberFilter image_depth_sub_;
899  message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
900 
901  ros::Subscriber rgbdSub_;
902  ros::Subscriber rgbdxSub_;
903  message_filters::Subscriber<rtabmap_msgs::RGBDImage> rgbd_image1_sub_;
904  message_filters::Subscriber<rtabmap_msgs::RGBDImage> rgbd_image2_sub_;
905  message_filters::Subscriber<rtabmap_msgs::RGBDImage> rgbd_image3_sub_;
906  message_filters::Subscriber<rtabmap_msgs::RGBDImage> rgbd_image4_sub_;
907  message_filters::Subscriber<rtabmap_msgs::RGBDImage> rgbd_image5_sub_;
908  message_filters::Subscriber<rtabmap_msgs::RGBDImage> rgbd_image6_sub_;
909 
910  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
911  message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
912  typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncPolicy;
913  message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
914  typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyApproxSync2Policy;
915  message_filters::Synchronizer<MyApproxSync2Policy> * approxSync2_;
916  typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyExactSync2Policy;
917  message_filters::Synchronizer<MyExactSync2Policy> * exactSync2_;
918  typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyApproxSync3Policy;
919  message_filters::Synchronizer<MyApproxSync3Policy> * approxSync3_;
920  typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyExactSync3Policy;
921  message_filters::Synchronizer<MyExactSync3Policy> * exactSync3_;
922  typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyApproxSync4Policy;
923  message_filters::Synchronizer<MyApproxSync4Policy> * approxSync4_;
924  typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyExactSync4Policy;
925  message_filters::Synchronizer<MyExactSync4Policy> * exactSync4_;
926  typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyApproxSync5Policy;
927  message_filters::Synchronizer<MyApproxSync5Policy> * approxSync5_;
928  typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyExactSync5Policy;
929  message_filters::Synchronizer<MyExactSync5Policy> * exactSync5_;
930  typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyApproxSync6Policy;
931  message_filters::Synchronizer<MyApproxSync6Policy> * approxSync6_;
932  typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyExactSync6Policy;
933  message_filters::Synchronizer<MyExactSync6Policy> * exactSync6_;
934  int topicQueueSize_;
935  int syncQueueSize_;
936  bool keepColor_;
937 };
938 
939 PLUGINLIB_EXPORT_CLASS(rtabmap_odom::RGBDOdometry, nodelet::Nodelet);
940 
941 }
rtabmap::SensorData
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
rtabmap_odom
Definition: OdometryROS.h:57
image_transport::SubscriberFilter
NODELET_FATAL
#define NODELET_FATAL(...)
sensor_msgs::image_encodings::TYPE_8UC1
const std::string TYPE_8UC1
NODELET_ERROR
#define NODELET_ERROR(...)
rtabmap_odom::RGBDOdometry::MyApproxSyncPolicy
message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo > MyApproxSyncPolicy
Definition: rgbd_odometry.cpp:910
rtabmap_odom::RGBDOdometry::approxSync2_
message_filters::Synchronizer< MyApproxSync2Policy > * approxSync2_
Definition: rgbd_odometry.cpp:915
rtabmap_odom::RGBDOdometry::rgbdSub_
ros::Subscriber rgbdSub_
Definition: rgbd_odometry.cpp:901
image_encodings.h
rtabmap_odom::RGBDOdometry::callbackRGBD2
void callbackRGBD2(const rtabmap_msgs::RGBDImageConstPtr &image, const rtabmap_msgs::RGBDImageConstPtr &image2)
Definition: rgbd_odometry.cpp:649
image_transport::ImageTransport
rtabmap_odom::RGBDOdometry
Definition: rgbd_odometry.cpp:60
encoding
encoding
message_filters::Synchronizer
boost::shared_ptr
rtabmap_odom::RGBDOdometry::updateParameters
virtual void updateParameters(ParametersMap &parameters)
Definition: rgbd_odometry.cpp:407
rtabmap_conversions::getTransform
rtabmap::Transform getTransform(const std::string &fromFrameId, const std::string &toFrameId, const ros::Time &stamp, tf::TransformListener &listener, double waitForTransform)
rtabmap_odom::RGBDOdometry::exactSync5_
message_filters::Synchronizer< MyExactSync5Policy > * exactSync5_
Definition: rgbd_odometry.cpp:929
rtabmap_odom::RGBDOdometry::callbackRGBDX
void callbackRGBDX(const rtabmap_msgs::RGBDImagesConstPtr &images)
Definition: rgbd_odometry.cpp:626
uFormat
std::string uFormat(const char *fmt,...)
this
this
rtabmap_odom::RGBDOdometry::image_mono_sub_
image_transport::SubscriberFilter image_mono_sub_
Definition: rgbd_odometry.cpp:897
rtabmap_odom::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:426
getName
ROSCONSOLE_CONSOLE_IMPL_DECL std::string getName(void *handle)
rtabmap_odom::RGBDOdometry::exactSync3_
message_filters::Synchronizer< MyExactSync3Policy > * exactSync3_
Definition: rgbd_odometry.cpp:921
sensor_msgs::image_encodings::RGBA8
const std::string RGBA8
frameId
string frameId
rtabmap_conversions::cameraModelFromROS
rtabmap::CameraModel cameraModelFromROS(const sensor_msgs::CameraInfo &camInfo, const rtabmap::Transform &localTransform=rtabmap::Transform::getIdentity())
ros::TransportHints
UStl.h
NODELET_WARN
#define NODELET_WARN(...)
rtabmap_odom::RGBDOdometry::rgbd_image4_sub_
message_filters::Subscriber< rtabmap_msgs::RGBDImage > rgbd_image4_sub_
Definition: rgbd_odometry.cpp:906
rtabmap_odom::RGBDOdometry::topicQueueSize_
int topicQueueSize_
Definition: rgbd_odometry.cpp:934
time_synchronizer.h
true
#define true
rtabmap_odom::RGBDOdometry::syncQueueSize_
int syncQueueSize_
Definition: rgbd_odometry.cpp:935
rtabmap_odom::RGBDOdometry::MyExactSync6Policy
message_filters::sync_policies::ExactTime< rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage > MyExactSync6Policy
Definition: rgbd_odometry.cpp:932
rtabmap_odom::RGBDOdometry::rgbd_image5_sub_
message_filters::Subscriber< rtabmap_msgs::RGBDImage > rgbd_image5_sub_
Definition: rgbd_odometry.cpp:907
sensor_msgs::image_encodings::RGB8
const std::string RGB8
ParametersMap
std::map< std::string, std::string > ParametersMap
rtabmap::Transform::isNull
bool isNull() const
rtabmap_odom::RGBDOdometry::approxSync5_
message_filters::Synchronizer< MyApproxSync5Policy > * approxSync5_
Definition: rgbd_odometry.cpp:927
stereo_camera_model.h
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
data
data
rtabmap_odom::RGBDOdometry::callback
void callback(const sensor_msgs::ImageConstPtr &image, const sensor_msgs::ImageConstPtr &depth, const sensor_msgs::CameraInfoConstPtr &cameraInfo)
Definition: rgbd_odometry.cpp:581
fabs
Real fabs(const Real &a)
rtabmap_odom::RGBDOdometry::info_sub_
message_filters::Subscriber< sensor_msgs::CameraInfo > info_sub_
Definition: rgbd_odometry.cpp:899
sensor_msgs::image_encodings::TYPE_16UC1
const std::string TYPE_16UC1
message_filters::Subscriber< sensor_msgs::CameraInfo >
rtabmap_odom::RGBDOdometry::rgbd_image6_sub_
message_filters::Subscriber< rtabmap_msgs::RGBDImage > rgbd_image6_sub_
Definition: rgbd_odometry.cpp:908
rtabmap_odom::RGBDOdometry::rgbd_image3_sub_
message_filters::Subscriber< rtabmap_msgs::RGBDImage > rgbd_image3_sub_
Definition: rgbd_odometry.cpp:905
ros::NodeHandle::resolveName
std::string resolveName(const std::string &name, bool remap=true) const
rtabmap_odom::RGBDOdometry::exactSync_
message_filters::Synchronizer< MyExactSyncPolicy > * exactSync_
Definition: rgbd_odometry.cpp:913
message_filters::sync_policies::ApproximateTime
rtabmap_odom::RGBDOdometry::keepColor_
bool keepColor_
Definition: rgbd_odometry.cpp:936
rtabmap_odom::RGBDOdometry::MyApproxSync6Policy
message_filters::sync_policies::ApproximateTime< rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage > MyApproxSync6Policy
Definition: rgbd_odometry.cpp:930
uInsert
void uInsert(std::map< K, V > &map, const std::map< K, V > &items)
rtabmap_odom::RGBDOdometry::rgbd_image1_sub_
message_filters::Subscriber< rtabmap_msgs::RGBDImage > rgbd_image1_sub_
Definition: rgbd_odometry.cpp:903
subscriber_filter.h
rtabmap_odom::RGBDOdometry::approxSync_
message_filters::Synchronizer< MyApproxSyncPolicy > * approxSync_
Definition: rgbd_odometry.cpp:911
rtabmap_odom::RGBDOdometry::approxSync4_
message_filters::Synchronizer< MyApproxSync4Policy > * approxSync4_
Definition: rgbd_odometry.cpp:923
rtabmap_odom::RGBDOdometry::exactSync4_
message_filters::Synchronizer< MyExactSync4Policy > * exactSync4_
Definition: rgbd_odometry.cpp:925
subscriber.h
rtabmap_odom::PLUGINLIB_EXPORT_CLASS
PLUGINLIB_EXPORT_CLASS(rtabmap_odom::RGBDOdometry, nodelet::Nodelet)
rtabmap_odom::RGBDOdometry::callbackRGBD3
void callbackRGBD3(const rtabmap_msgs::RGBDImageConstPtr &image, const rtabmap_msgs::RGBDImageConstPtr &image2, const rtabmap_msgs::RGBDImageConstPtr &image3)
Definition: rgbd_odometry.cpp:667
rtabmap_odom::RGBDOdometry::MyApproxSync4Policy
message_filters::sync_policies::ApproximateTime< rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage > MyApproxSync4Policy
Definition: rgbd_odometry.cpp:922
ros::NodeHandle::hasParam
bool hasParam(const std::string &key) const
ROS_WARN
#define ROS_WARN(...)
rtabmap_odom::RGBDOdometry::MyExactSync4Policy
message_filters::sync_policies::ExactTime< rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage > MyExactSync4Policy
Definition: rgbd_odometry.cpp:924
rtabmap_odom::RGBDOdometry::callbackRGBD6
void callbackRGBD6(const rtabmap_msgs::RGBDImageConstPtr &image, const rtabmap_msgs::RGBDImageConstPtr &image2, const rtabmap_msgs::RGBDImageConstPtr &image3, const rtabmap_msgs::RGBDImageConstPtr &image4, const rtabmap_msgs::RGBDImageConstPtr &image5, const rtabmap_msgs::RGBDImageConstPtr &image6)
Definition: rgbd_odometry.cpp:738
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, const boost::function< void(C)> &callback, const VoidConstPtr &tracked_object=VoidConstPtr(), const TransportHints &transport_hints=TransportHints())
rtabmap_odom::RGBDOdometry::callbackRGBD5
void callbackRGBD5(const rtabmap_msgs::RGBDImageConstPtr &image, const rtabmap_msgs::RGBDImageConstPtr &image2, const rtabmap_msgs::RGBDImageConstPtr &image3, const rtabmap_msgs::RGBDImageConstPtr &image4, const rtabmap_msgs::RGBDImageConstPtr &image5)
Definition: rgbd_odometry.cpp:712
rtabmap_odom::RGBDOdometry::MyApproxSync5Policy
message_filters::sync_policies::ApproximateTime< rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage > MyApproxSync5Policy
Definition: rgbd_odometry.cpp:926
rtabmap_odom::RGBDOdometry::exactSync6_
message_filters::Synchronizer< MyExactSync6Policy > * exactSync6_
Definition: rgbd_odometry.cpp:933
rtabmap_odom::RGBDOdometry::RGBDOdometry
RGBDOdometry()
Definition: rgbd_odometry.cpp:63
rtabmap_odom::RGBDOdometry::rgbd_image2_sub_
message_filters::Subscriber< rtabmap_msgs::RGBDImage > rgbd_image2_sub_
Definition: rgbd_odometry.cpp:904
rtabmap_odom::RGBDOdometry::onOdomInit
virtual void onOdomInit()
Definition: rgbd_odometry.cpp:103
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
rtabmap_odom::RGBDOdometry::MyApproxSync2Policy
message_filters::sync_policies::ApproximateTime< rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage > MyApproxSync2Policy
Definition: rgbd_odometry.cpp:914
image_transport.h
rtabmap_odom::OdometryROS
Definition: OdometryROS.h:59
rtabmap_odom::RGBDOdometry::MyExactSync5Policy
message_filters::sync_policies::ExactTime< rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage > MyExactSync5Policy
Definition: rgbd_odometry.cpp:928
NODELET_INFO
#define NODELET_INFO(...)
rtabmap::Transform
rtabmap_conversions::timestampFromROS
double timestampFromROS(const ros::Time &stamp)
nodelet::Nodelet
ros::Time
sensor_msgs::image_encodings::BGRA8
const std::string BGRA8
rtabmap_odom::RGBDOdometry::approxSync3_
message_filters::Synchronizer< MyApproxSync3Policy > * approxSync3_
Definition: rgbd_odometry.cpp:919
sensor_msgs::image_encodings::TYPE_32FC1
const std::string TYPE_32FC1
iter
iterator iter(handle obj)
nodelet.h
sensor_msgs::image_encodings::MONO8
const std::string MONO8
rtabmap_odom::RGBDOdometry::MyExactSync2Policy
message_filters::sync_policies::ExactTime< rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage > MyExactSync2Policy
Definition: rgbd_odometry.cpp:916
c_str
const char * c_str(Args &&...args)
cv_bridge.h
sensor_msgs::image_encodings::MONO16
const std::string MONO16
ROS_ERROR
#define ROS_ERROR(...)
rtabmap_odom::RGBDOdometry::image_depth_sub_
image_transport::SubscriberFilter image_depth_sub_
Definition: rgbd_odometry.cpp:898
rtabmap_odom::RGBDOdometry::flushCallbacks
virtual void flushCallbacks()
Definition: rgbd_odometry.cpp:769
class_list_macros.hpp
rtabmap_odom::RGBDOdometry::rgbdxSub_
ros::Subscriber rgbdxSub_
Definition: rgbd_odometry.cpp:902
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
rtabmap_odom::RGBDOdometry::MyExactSync3Policy
message_filters::sync_policies::ExactTime< rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage > MyExactSync3Policy
Definition: rgbd_odometry.cpp:920
rtabmap_odom::RGBDOdometry::~RGBDOdometry
virtual ~RGBDOdometry()
Definition: rgbd_odometry.cpp:83
ULogger.h
approximate_time.h
sensor_msgs::image_encodings::BGR8
const std::string BGR8
sensor_msgs::image_encodings::BAYER_GRBG8
const std::string BAYER_GRBG8
rtabmap_odom::RGBDOdometry::MyApproxSync3Policy
message_filters::sync_policies::ApproximateTime< rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage > MyApproxSync3Policy
Definition: rgbd_odometry.cpp:918
false
#define false
image_transport::TransportHints
cv_bridge::toCvShare
CvImageConstPtr toCvShare(const sensor_msgs::Image &source, const boost::shared_ptr< void const > &tracked_object, const std::string &encoding=std::string())
message_filters::sync_policies::ExactTime
header
const std::string header
UConversion.h
OdometryROS.h
ROS_ASSERT
#define ROS_ASSERT(cond)
MsgConversion.h
rtabmap_odom::RGBDOdometry::callbackRGBD4
void callbackRGBD4(const rtabmap_msgs::RGBDImageConstPtr &image, const rtabmap_msgs::RGBDImageConstPtr &image2, const rtabmap_msgs::RGBDImageConstPtr &image3, const rtabmap_msgs::RGBDImageConstPtr &image4)
Definition: rgbd_odometry.cpp:688
rtabmap
ros::Duration
rtabmap_odom::RGBDOdometry::MyExactSyncPolicy
message_filters::sync_policies::ExactTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo > MyExactSyncPolicy
Definition: rgbd_odometry.cpp:912
i
int i
rtabmap_odom::RGBDOdometry::approxSync6_
message_filters::Synchronizer< MyApproxSync6Policy > * approxSync6_
Definition: rgbd_odometry.cpp:931
util3d.h
rtabmap_conversions::toCvShare
void toCvShare(const rtabmap_msgs::RGBDImage &image, const boost::shared_ptr< void const > &trackedObject, cv_bridge::CvImageConstPtr &rgb, cv_bridge::CvImageConstPtr &depth)
ros::NodeHandle
ros::Subscriber
rtabmap_odom::RGBDOdometry::callbackRGBD
void callbackRGBD(const rtabmap_msgs::RGBDImageConstPtr &image)
Definition: rgbd_odometry.cpp:611
util2d.h
cv_bridge::cvtColor
CvImagePtr cvtColor(const CvImageConstPtr &source, const std::string &encoding)
rtabmap_odom::RGBDOdometry::exactSync2_
message_filters::Synchronizer< MyExactSync2Policy > * exactSync2_
Definition: rgbd_odometry.cpp:917

rtabmap_odom
Author(s): Mathieu Labbe
autogenerated on Mon Apr 28 2025 02:42:24