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

rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Tue Jan 24 2023 04:04:40