stereo_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 #include "pluginlib/class_list_macros.hpp"
30 #include "nodelet/nodelet.h"
31 
32 #include <message_filters/subscriber.h>
33 #include <message_filters/time_synchronizer.h>
34 #include <message_filters/sync_policies/approximate_time.h>
35 
36 #include <image_transport/image_transport.h>
37 #include <image_transport/subscriber_filter.h>
38 
39 #include <sensor_msgs/Image.h>
40 #include <sensor_msgs/image_encodings.h>
41 
42 #include <image_geometry/stereo_camera_model.h>
43 
44 #include <cv_bridge/cv_bridge.h>
45 
46 #include "rtabmap_conversions/MsgConversion.h"
47 #include <rtabmap_msgs/RGBDImages.h>
48 
49 #include <rtabmap/utilite/ULogger.h>
50 #include <rtabmap/utilite/UTimer.h>
51 #include <rtabmap/utilite/UStl.h>
52 #include <rtabmap/utilite/UConversion.h>
53 #include <rtabmap/core/Odometry.h>
54 
55 using namespace rtabmap;
56 
57 namespace rtabmap_odom
58 {
59 
60 class StereoOdometry : public OdometryROS
61 {
62 public:
63  StereoOdometry() :
64  OdometryROS(true, 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 ~StereoOdometry()
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  virtual void onOdomInit()
103  {
104  ros::NodeHandle & nh = getNodeHandle();
105  ros::NodeHandle & pnh = getPrivateNodeHandle();
106 
107  bool approxSync = false;
108  bool subscribeRGBD = false;
109  double approxSyncMaxInterval = 0.0;
110  int rgbdCameras = 1;
111  pnh.param("approx_sync", approxSync, approxSync);
112  pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
113  pnh.param("topic_queue_size", topicQueueSize_, topicQueueSize_);
114  if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size"))
115  {
116  pnh.param("queue_size", syncQueueSize_, syncQueueSize_);
117  ROS_WARN("Parameter \"queue_size\" has been renamed "
118  "to \"sync_queue_size\" and will be removed "
119  "in future versions! The value (%d) is still copied to "
120  "\"sync_queue_size\".", syncQueueSize_);
121  }
122  else
123  {
124  pnh.param("sync_queue_size", syncQueueSize_, syncQueueSize_);
125  }
126  pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD);
127  pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras);
128  pnh.param("keep_color", keepColor_, keepColor_);
129 
130  NODELET_INFO("StereoOdometry: approx_sync = %s", approxSync?"true":"false");
131  if(approxSync)
132  NODELET_INFO("StereoOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval);
133  NODELET_INFO("StereoOdometry: topic_queue_size = %d", topicQueueSize_);
134  NODELET_INFO("StereoOdometry: sync_queue_size = %d", syncQueueSize_);
135  NODELET_INFO("StereoOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
136  NODELET_INFO("StereoOdometry: keep_color = %s", keepColor_?"true":"false");
137 
138  std::string subscribedTopic;
139  std::string subscribedTopicsMsg;
140  if(subscribeRGBD)
141  {
142  if(rgbdCameras >= 2)
143  {
144  rgbd_image1_sub_.subscribe(nh, "rgbd_image0", topicQueueSize_);
145  rgbd_image2_sub_.subscribe(nh, "rgbd_image1", topicQueueSize_);
146  if(rgbdCameras >= 3)
147  {
148  rgbd_image3_sub_.subscribe(nh, "rgbd_image2", topicQueueSize_);
149  }
150  if(rgbdCameras >= 4)
151  {
152  rgbd_image4_sub_.subscribe(nh, "rgbd_image3", topicQueueSize_);
153  }
154  if(rgbdCameras >= 5)
155  {
156  rgbd_image5_sub_.subscribe(nh, "rgbd_image4", topicQueueSize_);
157  }
158  if(rgbdCameras >= 6)
159  {
160  rgbd_image6_sub_.subscribe(nh, "rgbd_image5", topicQueueSize_);
161  }
162 
163  if(rgbdCameras == 2)
164  {
165  if(approxSync)
166  {
167  approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
168  MyApproxSync2Policy(syncQueueSize_),
169  rgbd_image1_sub_,
170  rgbd_image2_sub_);
171  if(approxSyncMaxInterval > 0.0)
172  approxSync2_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
173  approxSync2_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2));
174  }
175  else
176  {
177  exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
178  MyExactSync2Policy(syncQueueSize_),
179  rgbd_image1_sub_,
180  rgbd_image2_sub_);
181  exactSync2_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2));
182  }
183  subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s",
184  getName().c_str(),
185  approxSync?"approx":"exact",
186  approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
187  rgbd_image1_sub_.getTopic().c_str(),
188  rgbd_image2_sub_.getTopic().c_str());
189  }
190  else if(rgbdCameras == 3)
191  {
192  if(approxSync)
193  {
194  approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
195  MyApproxSync3Policy(syncQueueSize_),
196  rgbd_image1_sub_,
197  rgbd_image2_sub_,
198  rgbd_image3_sub_);
199  if(approxSyncMaxInterval > 0.0)
200  approxSync3_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
201  approxSync3_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD3, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
202  }
203  else
204  {
205  exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
206  MyExactSync3Policy(syncQueueSize_),
207  rgbd_image1_sub_,
208  rgbd_image2_sub_,
209  rgbd_image3_sub_);
210  exactSync3_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD3, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
211  }
212  subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s",
213  getName().c_str(),
214  approxSync?"approx":"exact",
215  approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
216  rgbd_image1_sub_.getTopic().c_str(),
217  rgbd_image2_sub_.getTopic().c_str(),
218  rgbd_image3_sub_.getTopic().c_str());
219  }
220  else if(rgbdCameras == 4)
221  {
222  if(approxSync)
223  {
224  approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
225  MyApproxSync4Policy(syncQueueSize_),
226  rgbd_image1_sub_,
227  rgbd_image2_sub_,
228  rgbd_image3_sub_,
229  rgbd_image4_sub_);
230  if(approxSyncMaxInterval > 0.0)
231  approxSync4_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
232  approxSync4_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD4, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
233  }
234  else
235  {
236  exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
237  MyExactSync4Policy(syncQueueSize_),
238  rgbd_image1_sub_,
239  rgbd_image2_sub_,
240  rgbd_image3_sub_,
241  rgbd_image4_sub_);
242  exactSync4_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD4, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
243  }
244  subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s",
245  getName().c_str(),
246  approxSync?"approx":"exact",
247  approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
248  rgbd_image1_sub_.getTopic().c_str(),
249  rgbd_image2_sub_.getTopic().c_str(),
250  rgbd_image3_sub_.getTopic().c_str(),
251  rgbd_image4_sub_.getTopic().c_str());
252  }
253  else if(rgbdCameras == 5)
254  {
255  if(approxSync)
256  {
257  approxSync5_ = new message_filters::Synchronizer<MyApproxSync5Policy>(
258  MyApproxSync5Policy(syncQueueSize_),
259  rgbd_image1_sub_,
260  rgbd_image2_sub_,
261  rgbd_image3_sub_,
262  rgbd_image4_sub_,
263  rgbd_image5_sub_);
264  if(approxSyncMaxInterval > 0.0)
265  approxSync5_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
266  approxSync5_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD5, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5));
267  }
268  else
269  {
270  exactSync5_ = new message_filters::Synchronizer<MyExactSync5Policy>(
271  MyExactSync5Policy(syncQueueSize_),
272  rgbd_image1_sub_,
273  rgbd_image2_sub_,
274  rgbd_image3_sub_,
275  rgbd_image4_sub_,
276  rgbd_image5_sub_);
277  exactSync5_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD5, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5));
278  }
279  subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s \\\n %s",
280  getName().c_str(),
281  approxSync?"approx":"exact",
282  approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
283  rgbd_image1_sub_.getTopic().c_str(),
284  rgbd_image2_sub_.getTopic().c_str(),
285  rgbd_image3_sub_.getTopic().c_str(),
286  rgbd_image4_sub_.getTopic().c_str(),
287  rgbd_image5_sub_.getTopic().c_str());
288  }
289  else if(rgbdCameras == 6)
290  {
291  if(approxSync)
292  {
293  approxSync6_ = new message_filters::Synchronizer<MyApproxSync6Policy>(
294  MyApproxSync6Policy(syncQueueSize_),
295  rgbd_image1_sub_,
296  rgbd_image2_sub_,
297  rgbd_image3_sub_,
298  rgbd_image4_sub_,
299  rgbd_image5_sub_,
300  rgbd_image6_sub_);
301  if(approxSyncMaxInterval > 0.0)
302  approxSync6_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
303  approxSync6_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD6, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6));
304  }
305  else
306  {
307  exactSync6_ = new message_filters::Synchronizer<MyExactSync6Policy>(
308  MyExactSync6Policy(syncQueueSize_),
309  rgbd_image1_sub_,
310  rgbd_image2_sub_,
311  rgbd_image3_sub_,
312  rgbd_image4_sub_,
313  rgbd_image5_sub_,
314  rgbd_image6_sub_);
315  exactSync6_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD6, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6));
316  }
317  subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s",
318  getName().c_str(),
319  approxSync?"approx":"exact",
320  approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
321  rgbd_image1_sub_.getTopic().c_str(),
322  rgbd_image2_sub_.getTopic().c_str(),
323  rgbd_image3_sub_.getTopic().c_str(),
324  rgbd_image4_sub_.getTopic().c_str(),
325  rgbd_image5_sub_.getTopic().c_str(),
326  rgbd_image6_sub_.getTopic().c_str());
327  }
328  else
329  {
330  NODELET_FATAL("%s doesn't support more than 6 cameras (rgbd_cameras=%d) with "
331  "internal synchronization interface, set rgbd_cameras=0 and use "
332  "rgbd_images input topic instead for more cameras (for which "
333  "rgbdx_sync node can sync up to 8 cameras).",
334  getName().c_str(), rgbdCameras);
335  }
336 
337  }
338  else if(rgbdCameras == 0)
339  {
340  rgbdxSub_ = nh.subscribe("rgbd_images", topicQueueSize_, &StereoOdometry::callbackRGBDX, this);
341 
342  subscribedTopicsMsg =
343  uFormat("\n%s subscribed to:\n %s",
344  getName().c_str(),
345  rgbdxSub_.getTopic().c_str());
346  }
347  else
348  {
349  rgbdSub_ = nh.subscribe("rgbd_image", topicQueueSize_, &StereoOdometry::callbackRGBD, this);
350 
351  subscribedTopicsMsg =
352  uFormat("\n%s subscribed to:\n %s",
353  getName().c_str(),
354  rgbdSub_.getTopic().c_str());
355  }
356  }
357  else
358  {
359  ros::NodeHandle left_nh(nh, "left");
360  ros::NodeHandle right_nh(nh, "right");
361  ros::NodeHandle left_pnh(pnh, "left");
362  ros::NodeHandle right_pnh(pnh, "right");
363  image_transport::ImageTransport left_it(left_nh);
364  image_transport::ImageTransport right_it(right_nh);
365  image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh);
366  image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh);
367 
368  imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), topicQueueSize_, hintsLeft);
369  imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), topicQueueSize_, hintsRight);
370  cameraInfoLeft_.subscribe(left_nh, "camera_info", topicQueueSize_);
371  cameraInfoRight_.subscribe(right_nh, "camera_info", topicQueueSize_);
372 
373  if(approxSync)
374  {
375  approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(syncQueueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
376  if(approxSyncMaxInterval>0.0)
377  approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
378  approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
379  }
380  else
381  {
382  exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(syncQueueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
383  exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
384  }
385 
386  subscribedTopic = left_nh.resolveName("image_rect");
387  subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s",
388  getName().c_str(),
389  approxSync?"approx":"exact",
390  approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
391  imageRectLeft_.getTopic().c_str(),
392  imageRectRight_.getTopic().c_str(),
393  cameraInfoLeft_.getTopic().c_str(),
394  cameraInfoRight_.getTopic().c_str());
395  }
396 
397  initDiagnosticMsg(subscribedTopicsMsg, approxSync, subscribedTopic);
398  }
399 
400  virtual void updateParameters(ParametersMap & parameters)
401  {
402  //make sure we are using Reg/Strategy=0
403  ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy());
404  if(iter != parameters.end() && iter->second.compare("0") != 0)
405  {
406  ROS_WARN("Stereo odometry works only with \"Reg/Strategy\"=0. Ignoring value %s.", iter->second.c_str());
407  }
408  uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "0"));
409  }
410 
411  void commonCallback(
412  const std::vector<cv_bridge::CvImageConstPtr> & leftImages,
413  const std::vector<cv_bridge::CvImageConstPtr> & rightImages,
414  const std::vector<sensor_msgs::CameraInfo>& leftCameraInfos,
415  const std::vector<sensor_msgs::CameraInfo>& rightCameraInfos)
416  {
417  UASSERT(leftImages.size() > 0 &&
418  leftImages.size() == rightImages.size() &&
419  leftImages.size() == leftCameraInfos.size() &&
420  rightImages.size() == rightCameraInfos.size());
421  ros::Time higherStamp;
422  int leftWidth = leftImages[0]->image.cols;
423  int leftHeight = leftImages[0]->image.rows;
424  int rightWidth = rightImages[0]->image.cols;
425  int rightHeight = rightImages[0]->image.rows;
426 
427  UASSERT_MSG(
428  leftWidth == rightWidth && leftHeight == rightHeight,
429  uFormat("left=%dx%d right=%dx%d", leftWidth, leftHeight, rightWidth, rightHeight).c_str());
430 
431  int cameraCount = leftImages.size();
432  cv::Mat left;
433  cv::Mat right;
434  std::vector<rtabmap::StereoCameraModel> cameraModels;
435  for(unsigned int i=0; i<leftImages.size(); ++i)
436  {
437  if(!(leftImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
438  leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
439  leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
440  leftImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
441  leftImages[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
442  leftImages[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
443  leftImages[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0) ||
444  !(rightImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
445  rightImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
446  rightImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
447  rightImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
448  rightImages[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
449  rightImages[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
450  rightImages[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0))
451  {
452  NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8,rgba8,bgra8 (mono8 recommended), received types are %s (left) and %s (right)",
453  leftImages[i]->encoding.c_str(), rightImages[i]->encoding.c_str());
454  return;
455  }
456 
457  ros::Time stamp = leftImages[i]->header.stamp>rightImages[i]->header.stamp?leftImages[i]->header.stamp:rightImages[i]->header.stamp;
458 
459  if(i == 0)
460  {
461  higherStamp = stamp;
462  }
463  else if(stamp > higherStamp)
464  {
465  higherStamp = stamp;
466  }
467 
468  Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), leftImages[i]->header.frame_id, stamp, this->tfListener(), this->waitForTransformDuration());
469  if(localTransform.isNull())
470  {
471  return;
472  }
473 
474  if(i>0)
475  {
476  double stampDiff = fabs(leftImages[i]->header.stamp.toSec() - leftImages[i-1]->header.stamp.toSec());
477  if(stampDiff > 1.0/60.0)
478  {
479  static bool warningShown = false;
480  if(!warningShown)
481  {
482  NODELET_WARN("The time difference between cameras %d and %d is "
483  "high (diff=%fs, cam%d=%fs, cam%d=%fs). You may want "
484  "to set approx_sync_max_interval to reject bad synchronizations or use "
485  "approx_sync=false if streams have all the exact same timestamp. This "
486  "message is only printed once.",
487  i-1, i,
488  stampDiff,
489  i-1, leftImages[i-1]->header.stamp.toSec(),
490  i, leftImages[i]->header.stamp.toSec());
491  warningShown = true;
492  }
493  }
494  }
495 
496  int quality = -1;
497  if(!leftImages[i]->image.empty() && !rightImages[i]->image.empty())
498  {
499  bool alreadyRectified = true;
500  Parameters::parse(parameters(), Parameters::kRtabmapImagesAlreadyRectified(), alreadyRectified);
501  rtabmap::Transform stereoTransform;
502  if(!alreadyRectified)
503  {
504  if(rightCameraInfos[i].header.frame_id.empty() || leftCameraInfos[i].header.frame_id.empty())
505  {
506  if(rightCameraInfos[i].P[3] == 0.0 && leftCameraInfos[i].P[3] == 0)
507  {
508  NODELET_ERROR("Parameter %s is false but the frame_id in one of the camera_info "
509  "topic is empty, so TF between the cameras cannot be computed!",
510  Parameters::kRtabmapImagesAlreadyRectified().c_str());
511  return;
512  }
513  else
514  {
515  static bool warned = false;
516  if(!warned)
517  {
518  NODELET_WARN("Parameter %s is false but the frame_id in one of the "
519  "camera_info topic is empty, so TF between the cameras cannot be "
520  "computed! However, the baseline can be computed from the calibration, "
521  "we will use this one instead of TF. This message is only printed once...",
522  Parameters::kRtabmapImagesAlreadyRectified().c_str());
523  warned = true;
524  }
525  }
526  }
527  else
528  {
529  stereoTransform = rtabmap_conversions::getTransform(
530  rightCameraInfos[i].header.frame_id,
531  leftCameraInfos[i].header.frame_id,
532  leftCameraInfos[i].header.stamp,
533  this->tfListener(),
534  this->waitForTransformDuration());
535  if(stereoTransform.isNull())
536  {
537  NODELET_ERROR("Parameter %s is false but we cannot get TF between the two cameras! (between frames %s and %s)",
538  Parameters::kRtabmapImagesAlreadyRectified().c_str(),
539  rightCameraInfos[i].header.frame_id.c_str(),
540  leftCameraInfos[i].header.frame_id.c_str());
541  return;
542  }
543  else if(stereoTransform.isIdentity())
544  {
545  NODELET_ERROR("Parameter %s is false but we cannot get a valid TF between the two cameras! "
546  "Identity transform returned between left and right cameras. Verify that if TF between "
547  "the cameras is valid: \"rosrun tf tf_echo %s %s\".",
548  Parameters::kRtabmapImagesAlreadyRectified().c_str(),
549  rightCameraInfos[i].header.frame_id.c_str(),
550  leftCameraInfos[i].header.frame_id.c_str());
551  return;
552  }
553  }
554  }
555 
556  rtabmap::StereoCameraModel stereoModel = rtabmap_conversions::stereoCameraModelFromROS(leftCameraInfos[i], rightCameraInfos[i], localTransform, stereoTransform);
557 
558  if( stereoModel.baseline() == 0 &&
559  alreadyRectified &&
560  !rightCameraInfos[i].header.frame_id.empty() &&
561  !leftCameraInfos[i].header.frame_id.empty())
562  {
563  stereoTransform = rtabmap_conversions::getTransform(
564  leftCameraInfos[i].header.frame_id,
565  rightCameraInfos[i].header.frame_id,
566  leftCameraInfos[i].header.stamp,
567  this->tfListener(),
568  this->waitForTransformDuration());
569 
570  if(!stereoTransform.isNull() && stereoTransform.x()>0)
571  {
572  static bool warned = false;
573  if(!warned)
574  {
575  ROS_WARN("Right camera info doesn't have Tx set but we are assuming that stereo images are already rectified (see %s parameter). While not "
576  "recommended, we used TF to get the baseline (%s->%s = %fm) for convenience (e.g., D400 ir stereo issue). It is preferred to feed "
577  "a valid right camera info if stereo images are already rectified. This message is only printed once...",
578  rtabmap::Parameters::kRtabmapImagesAlreadyRectified().c_str(),
579  rightCameraInfos[i].header.frame_id.c_str(), leftCameraInfos[i].header.frame_id.c_str(), stereoTransform.x());
580  warned = true;
581  }
582  stereoModel = rtabmap::StereoCameraModel(
583  stereoModel.left().fx(),
584  stereoModel.left().fy(),
585  stereoModel.left().cx(),
586  stereoModel.left().cy(),
587  stereoTransform.x(),
588  stereoModel.localTransform(),
589  stereoModel.left().imageSize());
590  }
591  }
592 
593  if(alreadyRectified && stereoModel.baseline() <= 0)
594  {
595  NODELET_ERROR("The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo "
596  "setup where the Tx (or P(0,3)) is negative in the right camera info msg.", stereoModel.baseline());
597  return;
598  }
599 
600  if(stereoModel.baseline() > 10.0)
601  {
602  static bool shown = false;
603  if(!shown)
604  {
605  NODELET_WARN("Detected baseline (%f m) is quite large! Is your "
606  "right camera_info P(0,3) correctly set? Note that "
607  "baseline=-P(0,3)/P(0,0). This warning is printed only once.",
608  stereoModel.baseline());
609  shown = true;
610  }
611  }
612 
613  cv_bridge::CvImageConstPtr ptrLeft = leftImages[i];
614  if(leftImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
615  leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
616  {
617  if(keepColor_ && leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0)
618  {
619  ptrLeft = cv_bridge::cvtColor(leftImages[i], "bgr8");
620  }
621  else
622  {
623  ptrLeft = cv_bridge::cvtColor(leftImages[i], "mono8");
624  }
625  }
626 
627  cv_bridge::CvImageConstPtr ptrRight = rightImages[i];
628  if(rightImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
629  rightImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
630  {
631  ptrRight = cv_bridge::cvtColor(rightImages[i], "mono8");
632  }
633 
634  // initialize
635  if(left.empty())
636  {
637  left = cv::Mat(leftHeight, leftWidth*cameraCount, ptrLeft->image.type());
638  }
639  if(right.empty())
640  {
641  right = cv::Mat(rightHeight, rightWidth*cameraCount, ptrRight->image.type());
642  }
643 
644  if(ptrLeft->image.type() == left.type())
645  {
646  ptrLeft->image.copyTo(cv::Mat(left, cv::Rect(i*leftWidth, 0, leftWidth, leftHeight)));
647  }
648  else
649  {
650  NODELET_ERROR("Some left images are not the same type! %d vs %d", ptrLeft->image.type(), left.type());
651  return;
652  }
653 
654  if(ptrRight->image.type() == right.type())
655  {
656  ptrRight->image.copyTo(cv::Mat(right, cv::Rect(i*rightWidth, 0, rightWidth, rightHeight)));
657  }
658  else
659  {
660  NODELET_ERROR("Some right images are not the same type! %d vs %d", ptrRight->image.type(), right.type());
661  return;
662  }
663 
664  cameraModels.push_back(stereoModel);
665  }
666  else
667  {
668  NODELET_ERROR("Odom: input images empty?!?");
669  return;
670  }
671  }
672 
673  //
674  rtabmap::SensorData data(
675  left,
676  right,
677  cameraModels,
678  0,
679  rtabmap_conversions::timestampFromROS(higherStamp));
680 
681  std_msgs::Header header;
682  header.stamp = higherStamp;
683  header.frame_id = leftImages.size()==1?leftImages[0]->header.frame_id:"";
684  this->processData(data, header);
685  }
686 
687  void callback(
688  const sensor_msgs::ImageConstPtr& imageLeft,
689  const sensor_msgs::ImageConstPtr& imageRight,
690  const sensor_msgs::CameraInfoConstPtr& cameraInfoLeft,
691  const sensor_msgs::CameraInfoConstPtr& cameraInfoRight)
692  {
693  if(!this->isPaused())
694  {
695  std::vector<cv_bridge::CvImageConstPtr> leftMsgs(1);
696  std::vector<cv_bridge::CvImageConstPtr> rightMsgs(1);
697  std::vector<sensor_msgs::CameraInfo> leftInfoMsgs;
698  std::vector<sensor_msgs::CameraInfo> rightInfoMsgs;
699  leftMsgs[0] = cv_bridge::toCvShare(imageLeft);
700  rightMsgs[0] = cv_bridge::toCvShare(imageRight);
701  leftInfoMsgs.push_back(*cameraInfoLeft);
702  rightInfoMsgs.push_back(*cameraInfoRight);
703 
704  double stampDiff = fabs(imageLeft->header.stamp.toSec() - imageRight->header.stamp.toSec());
705  if(stampDiff > 0.010)
706  {
707  NODELET_WARN("The time difference between left and right frames is "
708  "high (diff=%fs, left=%fs, right=%fs). If your left and right cameras are hardware "
709  "synchronized, use approx_sync:=false. Otherwise, you may want "
710  "to set approx_sync_max_interval lower than 0.01s to reject spurious bad synchronizations.",
711  stampDiff,
712  imageLeft->header.stamp.toSec(),
713  imageRight->header.stamp.toSec());
714  }
715 
716  this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
717  }
718  }
719 
720  void callbackRGBD(
721  const rtabmap_msgs::RGBDImageConstPtr& image)
722  {
723  if(!this->isPaused())
724  {
725  std::vector<cv_bridge::CvImageConstPtr> leftMsgs(1);
726  std::vector<cv_bridge::CvImageConstPtr> rightMsgs(1);
727  std::vector<sensor_msgs::CameraInfo> leftInfoMsgs;
728  std::vector<sensor_msgs::CameraInfo> rightInfoMsgs;
729  rtabmap_conversions::toCvShare(image, leftMsgs[0], rightMsgs[0]);
730  leftInfoMsgs.push_back(image->rgb_camera_info);
731  rightInfoMsgs.push_back(image->depth_camera_info);
732 
733  this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
734  }
735  }
736 
737  void callbackRGBDX(
738  const rtabmap_msgs::RGBDImagesConstPtr& images)
739  {
740  if(!this->isPaused())
741  {
742  if(images->rgbd_images.empty())
743  {
744  NODELET_ERROR("Input topic \"%s\" doesn't contain any image(s)!", rgbdxSub_.getTopic().c_str());
745  return;
746  }
747  std::vector<cv_bridge::CvImageConstPtr> leftMsgs(images->rgbd_images.size());
748  std::vector<cv_bridge::CvImageConstPtr> rightMsgs(images->rgbd_images.size());
749  std::vector<sensor_msgs::CameraInfo> leftInfoMsgs;
750  std::vector<sensor_msgs::CameraInfo> rightInfoMsgs;
751  for(size_t i=0; i<images->rgbd_images.size(); ++i)
752  {
753  rtabmap_conversions::toCvShare(images->rgbd_images[i], images, leftMsgs[i], rightMsgs[i]);
754  leftInfoMsgs.push_back(images->rgbd_images[i].rgb_camera_info);
755  rightInfoMsgs.push_back(images->rgbd_images[i].depth_camera_info);
756  }
757 
758  this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
759  }
760  }
761 
762  void callbackRGBD2(
763  const rtabmap_msgs::RGBDImageConstPtr& image,
764  const rtabmap_msgs::RGBDImageConstPtr& image2)
765  {
766  if(!this->isPaused())
767  {
768  std::vector<cv_bridge::CvImageConstPtr> leftMsgs(2);
769  std::vector<cv_bridge::CvImageConstPtr> rightMsgs(2);
770  std::vector<sensor_msgs::CameraInfo> leftInfoMsgs;
771  std::vector<sensor_msgs::CameraInfo> rightInfoMsgs;
772  rtabmap_conversions::toCvShare(image, leftMsgs[0], rightMsgs[0]);
773  rtabmap_conversions::toCvShare(image2, leftMsgs[1], rightMsgs[1]);
774  leftInfoMsgs.push_back(image->rgb_camera_info);
775  leftInfoMsgs.push_back(image2->rgb_camera_info);
776  rightInfoMsgs.push_back(image->depth_camera_info);
777  rightInfoMsgs.push_back(image2->depth_camera_info);
778 
779  this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
780  }
781  }
782 
783  void callbackRGBD3(
784  const rtabmap_msgs::RGBDImageConstPtr& image,
785  const rtabmap_msgs::RGBDImageConstPtr& image2,
786  const rtabmap_msgs::RGBDImageConstPtr& image3)
787  {
788  if(!this->isPaused())
789  {
790  std::vector<cv_bridge::CvImageConstPtr> leftMsgs(3);
791  std::vector<cv_bridge::CvImageConstPtr> rightMsgs(3);
792  std::vector<sensor_msgs::CameraInfo> leftInfoMsgs;
793  std::vector<sensor_msgs::CameraInfo> rightInfoMsgs;
794  rtabmap_conversions::toCvShare(image, leftMsgs[0], rightMsgs[0]);
795  rtabmap_conversions::toCvShare(image2, leftMsgs[1], rightMsgs[1]);
796  rtabmap_conversions::toCvShare(image3, leftMsgs[2], rightMsgs[2]);
797  leftInfoMsgs.push_back(image->rgb_camera_info);
798  leftInfoMsgs.push_back(image2->rgb_camera_info);
799  leftInfoMsgs.push_back(image3->rgb_camera_info);
800  rightInfoMsgs.push_back(image->depth_camera_info);
801  rightInfoMsgs.push_back(image2->depth_camera_info);
802  rightInfoMsgs.push_back(image3->depth_camera_info);
803 
804  this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
805  }
806  }
807 
808  void callbackRGBD4(
809  const rtabmap_msgs::RGBDImageConstPtr& image,
810  const rtabmap_msgs::RGBDImageConstPtr& image2,
811  const rtabmap_msgs::RGBDImageConstPtr& image3,
812  const rtabmap_msgs::RGBDImageConstPtr& image4)
813  {
814  if(!this->isPaused())
815  {
816  std::vector<cv_bridge::CvImageConstPtr> leftMsgs(4);
817  std::vector<cv_bridge::CvImageConstPtr> rightMsgs(4);
818  std::vector<sensor_msgs::CameraInfo> leftInfoMsgs;
819  std::vector<sensor_msgs::CameraInfo> rightInfoMsgs;
820  rtabmap_conversions::toCvShare(image, leftMsgs[0], rightMsgs[0]);
821  rtabmap_conversions::toCvShare(image2, leftMsgs[1], rightMsgs[1]);
822  rtabmap_conversions::toCvShare(image3, leftMsgs[2], rightMsgs[2]);
823  rtabmap_conversions::toCvShare(image4, leftMsgs[3], rightMsgs[3]);
824  leftInfoMsgs.push_back(image->rgb_camera_info);
825  leftInfoMsgs.push_back(image2->rgb_camera_info);
826  leftInfoMsgs.push_back(image3->rgb_camera_info);
827  leftInfoMsgs.push_back(image4->rgb_camera_info);
828  rightInfoMsgs.push_back(image->depth_camera_info);
829  rightInfoMsgs.push_back(image2->depth_camera_info);
830  rightInfoMsgs.push_back(image3->depth_camera_info);
831  rightInfoMsgs.push_back(image4->depth_camera_info);
832 
833  this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
834  }
835  }
836 
837  void callbackRGBD5(
838  const rtabmap_msgs::RGBDImageConstPtr& image,
839  const rtabmap_msgs::RGBDImageConstPtr& image2,
840  const rtabmap_msgs::RGBDImageConstPtr& image3,
841  const rtabmap_msgs::RGBDImageConstPtr& image4,
842  const rtabmap_msgs::RGBDImageConstPtr& image5)
843  {
844  if(!this->isPaused())
845  {
846  std::vector<cv_bridge::CvImageConstPtr> leftMsgs(5);
847  std::vector<cv_bridge::CvImageConstPtr> rightMsgs(5);
848  std::vector<sensor_msgs::CameraInfo> leftInfoMsgs;
849  std::vector<sensor_msgs::CameraInfo> rightInfoMsgs;
850  rtabmap_conversions::toCvShare(image, leftMsgs[0], rightMsgs[0]);
851  rtabmap_conversions::toCvShare(image2, leftMsgs[1], rightMsgs[1]);
852  rtabmap_conversions::toCvShare(image3, leftMsgs[2], rightMsgs[2]);
853  rtabmap_conversions::toCvShare(image4, leftMsgs[3], rightMsgs[3]);
854  rtabmap_conversions::toCvShare(image5, leftMsgs[4], rightMsgs[4]);
855  leftInfoMsgs.push_back(image->rgb_camera_info);
856  leftInfoMsgs.push_back(image2->rgb_camera_info);
857  leftInfoMsgs.push_back(image3->rgb_camera_info);
858  leftInfoMsgs.push_back(image4->rgb_camera_info);
859  leftInfoMsgs.push_back(image5->rgb_camera_info);
860  rightInfoMsgs.push_back(image->depth_camera_info);
861  rightInfoMsgs.push_back(image2->depth_camera_info);
862  rightInfoMsgs.push_back(image3->depth_camera_info);
863  rightInfoMsgs.push_back(image4->depth_camera_info);
864  rightInfoMsgs.push_back(image5->depth_camera_info);
865 
866  this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
867  }
868  }
869 
870  void callbackRGBD6(
871  const rtabmap_msgs::RGBDImageConstPtr& image,
872  const rtabmap_msgs::RGBDImageConstPtr& image2,
873  const rtabmap_msgs::RGBDImageConstPtr& image3,
874  const rtabmap_msgs::RGBDImageConstPtr& image4,
875  const rtabmap_msgs::RGBDImageConstPtr& image5,
876  const rtabmap_msgs::RGBDImageConstPtr& image6)
877  {
878  if(!this->isPaused())
879  {
880  std::vector<cv_bridge::CvImageConstPtr> leftMsgs(6);
881  std::vector<cv_bridge::CvImageConstPtr> rightMsgs(6);
882  std::vector<sensor_msgs::CameraInfo> leftInfoMsgs;
883  std::vector<sensor_msgs::CameraInfo> rightInfoMsgs;
884  rtabmap_conversions::toCvShare(image, leftMsgs[0], rightMsgs[0]);
885  rtabmap_conversions::toCvShare(image2, leftMsgs[1], rightMsgs[1]);
886  rtabmap_conversions::toCvShare(image3, leftMsgs[2], rightMsgs[2]);
887  rtabmap_conversions::toCvShare(image4, leftMsgs[3], rightMsgs[3]);
888  rtabmap_conversions::toCvShare(image5, leftMsgs[4], rightMsgs[4]);
889  rtabmap_conversions::toCvShare(image6, leftMsgs[5], rightMsgs[5]);
890  leftInfoMsgs.push_back(image->rgb_camera_info);
891  leftInfoMsgs.push_back(image2->rgb_camera_info);
892  leftInfoMsgs.push_back(image3->rgb_camera_info);
893  leftInfoMsgs.push_back(image4->rgb_camera_info);
894  leftInfoMsgs.push_back(image5->rgb_camera_info);
895  leftInfoMsgs.push_back(image6->rgb_camera_info);
896  rightInfoMsgs.push_back(image->depth_camera_info);
897  rightInfoMsgs.push_back(image2->depth_camera_info);
898  rightInfoMsgs.push_back(image3->depth_camera_info);
899  rightInfoMsgs.push_back(image4->depth_camera_info);
900  rightInfoMsgs.push_back(image5->depth_camera_info);
901  rightInfoMsgs.push_back(image6->depth_camera_info);
902 
903  this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
904  }
905  }
906 
907 protected:
908  virtual void flushCallbacks()
909  {
910  //flush callbacks
911  if(approxSync_)
912  {
913  delete approxSync_;
914  approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(syncQueueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
915  approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
916  }
917  if(exactSync_)
918  {
919  delete exactSync_;
920  exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(syncQueueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
921  exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
922  }
923  if(approxSync2_)
924  {
925  delete approxSync2_;
926  approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
927  MyApproxSync2Policy(syncQueueSize_),
928  rgbd_image1_sub_,
929  rgbd_image2_sub_);
930  approxSync2_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2));
931  }
932  if(exactSync2_)
933  {
934  delete exactSync2_;
935  exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
936  MyExactSync2Policy(syncQueueSize_),
937  rgbd_image1_sub_,
938  rgbd_image2_sub_);
939  exactSync2_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2));
940  }
941  if(approxSync3_)
942  {
943  delete approxSync3_;
944  approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
945  MyApproxSync3Policy(syncQueueSize_),
946  rgbd_image1_sub_,
947  rgbd_image2_sub_,
948  rgbd_image3_sub_);
949  approxSync3_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD3, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
950  }
951  if(exactSync3_)
952  {
953  delete exactSync3_;
954  exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
955  MyExactSync3Policy(syncQueueSize_),
956  rgbd_image1_sub_,
957  rgbd_image2_sub_,
958  rgbd_image3_sub_);
959  exactSync3_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD3, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
960  }
961  if(approxSync4_)
962  {
963  delete approxSync4_;
964  approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
965  MyApproxSync4Policy(syncQueueSize_),
966  rgbd_image1_sub_,
967  rgbd_image2_sub_,
968  rgbd_image3_sub_,
969  rgbd_image4_sub_);
970  approxSync4_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD4, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
971  }
972  if(exactSync4_)
973  {
974  delete exactSync4_;
975  exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
976  MyExactSync4Policy(syncQueueSize_),
977  rgbd_image1_sub_,
978  rgbd_image2_sub_,
979  rgbd_image3_sub_,
980  rgbd_image4_sub_);
981  exactSync4_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD4, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
982  }
983  if(approxSync5_)
984  {
985  delete approxSync5_;
986  approxSync5_ = new message_filters::Synchronizer<MyApproxSync5Policy>(
987  MyApproxSync5Policy(syncQueueSize_),
988  rgbd_image1_sub_,
989  rgbd_image2_sub_,
990  rgbd_image3_sub_,
991  rgbd_image4_sub_,
992  rgbd_image5_sub_);
993  approxSync5_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD5, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5));
994  }
995  if(exactSync5_)
996  {
997  delete exactSync5_;
998  exactSync5_ = new message_filters::Synchronizer<MyExactSync5Policy>(
999  MyExactSync5Policy(syncQueueSize_),
1000  rgbd_image1_sub_,
1001  rgbd_image2_sub_,
1002  rgbd_image3_sub_,
1003  rgbd_image4_sub_,
1004  rgbd_image5_sub_);
1005  exactSync5_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD5, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5));
1006  }
1007  if(approxSync6_)
1008  {
1009  delete approxSync6_;
1010  approxSync6_ = new message_filters::Synchronizer<MyApproxSync6Policy>(
1011  MyApproxSync6Policy(syncQueueSize_),
1012  rgbd_image1_sub_,
1013  rgbd_image2_sub_,
1014  rgbd_image3_sub_,
1015  rgbd_image4_sub_,
1016  rgbd_image5_sub_,
1017  rgbd_image6_sub_);
1018  approxSync6_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD6, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6));
1019  }
1020  if(exactSync6_)
1021  {
1022  delete exactSync6_;
1023  exactSync6_ = new message_filters::Synchronizer<MyExactSync6Policy>(
1024  MyExactSync6Policy(syncQueueSize_),
1025  rgbd_image1_sub_,
1026  rgbd_image2_sub_,
1027  rgbd_image3_sub_,
1028  rgbd_image4_sub_,
1029  rgbd_image5_sub_,
1030  rgbd_image6_sub_);
1031  exactSync6_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD6, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6));
1032  }
1033  }
1034 
1035 private:
1036  image_transport::SubscriberFilter imageRectLeft_;
1037  image_transport::SubscriberFilter imageRectRight_;
1038  message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
1039  message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_;
1040 
1041  ros::Subscriber rgbdSub_;
1042  ros::Subscriber rgbdxSub_;
1043  message_filters::Subscriber<rtabmap_msgs::RGBDImage> rgbd_image1_sub_;
1044  message_filters::Subscriber<rtabmap_msgs::RGBDImage> rgbd_image2_sub_;
1045  message_filters::Subscriber<rtabmap_msgs::RGBDImage> rgbd_image3_sub_;
1046  message_filters::Subscriber<rtabmap_msgs::RGBDImage> rgbd_image4_sub_;
1047  message_filters::Subscriber<rtabmap_msgs::RGBDImage> rgbd_image5_sub_;
1048  message_filters::Subscriber<rtabmap_msgs::RGBDImage> rgbd_image6_sub_;
1049 
1050  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
1051  message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
1052  typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncPolicy;
1053  message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
1054  typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyApproxSync2Policy;
1055  message_filters::Synchronizer<MyApproxSync2Policy> * approxSync2_;
1056  typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyExactSync2Policy;
1057  message_filters::Synchronizer<MyExactSync2Policy> * exactSync2_;
1058  typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyApproxSync3Policy;
1059  message_filters::Synchronizer<MyApproxSync3Policy> * approxSync3_;
1060  typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyExactSync3Policy;
1061  message_filters::Synchronizer<MyExactSync3Policy> * exactSync3_;
1062  typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyApproxSync4Policy;
1063  message_filters::Synchronizer<MyApproxSync4Policy> * approxSync4_;
1064  typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyExactSync4Policy;
1065  message_filters::Synchronizer<MyExactSync4Policy> * exactSync4_;
1066  typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyApproxSync5Policy;
1067  message_filters::Synchronizer<MyApproxSync5Policy> * approxSync5_;
1068  typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyExactSync5Policy;
1069  message_filters::Synchronizer<MyExactSync5Policy> * exactSync5_;
1070  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;
1071  message_filters::Synchronizer<MyApproxSync6Policy> * approxSync6_;
1072  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;
1073  message_filters::Synchronizer<MyExactSync6Policy> * exactSync6_;
1074 
1075  int topicQueueSize_;
1076  int syncQueueSize_;
1077  bool keepColor_;
1078 };
1079 
1080 PLUGINLIB_EXPORT_CLASS(rtabmap_odom::StereoOdometry, nodelet::Nodelet);
1081 
1082 }
1083 
rtabmap::SensorData
rtabmap_odom::StereoOdometry::exactSync5_
message_filters::Synchronizer< MyExactSync5Policy > * exactSync5_
Definition: stereo_odometry.cpp:1069
rtabmap_odom::StereoOdometry::rgbd_image6_sub_
message_filters::Subscriber< rtabmap_msgs::RGBDImage > rgbd_image6_sub_
Definition: stereo_odometry.cpp:1048
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
rtabmap::CameraModel::fx
double fx() const
rtabmap_odom
Definition: OdometryROS.h:57
image_transport::SubscriberFilter
rtabmap::CameraModel::cx
double cx() const
NODELET_FATAL
#define NODELET_FATAL(...)
rtabmap_odom::StereoOdometry::MyApproxSync4Policy
message_filters::sync_policies::ApproximateTime< rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage > MyApproxSync4Policy
Definition: stereo_odometry.cpp:1062
rtabmap_odom::StereoOdometry::callback
void callback(const sensor_msgs::ImageConstPtr &imageLeft, const sensor_msgs::ImageConstPtr &imageRight, const sensor_msgs::CameraInfoConstPtr &cameraInfoLeft, const sensor_msgs::CameraInfoConstPtr &cameraInfoRight)
Definition: stereo_odometry.cpp:687
sensor_msgs::image_encodings::TYPE_8UC1
const std::string TYPE_8UC1
NODELET_ERROR
#define NODELET_ERROR(...)
rtabmap::StereoCameraModel
image_encodings.h
image_transport::ImageTransport
encoding
encoding
message_filters::Synchronizer
boost::shared_ptr
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::StereoOdometry::StereoOdometry
StereoOdometry()
Definition: stereo_odometry.cpp:63
rtabmap_odom::StereoOdometry::keepColor_
bool keepColor_
Definition: stereo_odometry.cpp:1077
rtabmap::CameraModel::cy
double cy() const
uFormat
std::string uFormat(const char *fmt,...)
this
this
rtabmap_odom::StereoOdometry::exactSync2_
message_filters::Synchronizer< MyExactSync2Policy > * exactSync2_
Definition: stereo_odometry.cpp:1057
rtabmap_odom::StereoOdometry::~StereoOdometry
virtual ~StereoOdometry()
Definition: stereo_odometry.cpp:83
getName
ROSCONSOLE_CONSOLE_IMPL_DECL std::string getName(void *handle)
sensor_msgs::image_encodings::RGBA8
const std::string RGBA8
frameId
string frameId
ros::TransportHints
UStl.h
NODELET_WARN
#define NODELET_WARN(...)
rtabmap::StereoCameraModel::baseline
double baseline() const
rtabmap_odom::StereoOdometry::topicQueueSize_
int topicQueueSize_
Definition: stereo_odometry.cpp:1075
rtabmap_odom::StereoOdometry
Definition: stereo_odometry.cpp:60
time_synchronizer.h
true
#define true
rtabmap_odom::StereoOdometry::callbackRGBD
void callbackRGBD(const rtabmap_msgs::RGBDImageConstPtr &image)
Definition: stereo_odometry.cpp:720
rtabmap_odom::StereoOdometry::MyExactSync2Policy
message_filters::sync_policies::ExactTime< rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage > MyExactSync2Policy
Definition: stereo_odometry.cpp:1056
rtabmap_odom::StereoOdometry::MyApproxSync2Policy
message_filters::sync_policies::ApproximateTime< rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage > MyApproxSync2Policy
Definition: stereo_odometry.cpp:1054
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::StereoOdometry::updateParameters
virtual void updateParameters(ParametersMap &parameters)
Definition: stereo_odometry.cpp:400
rtabmap_odom::PLUGINLIB_EXPORT_CLASS
PLUGINLIB_EXPORT_CLASS(rtabmap_odom::StereoOdometry, nodelet::Nodelet)
rtabmap_odom::StereoOdometry::approxSync2_
message_filters::Synchronizer< MyApproxSync2Policy > * approxSync2_
Definition: stereo_odometry.cpp:1055
stereo_camera_model.h
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
data
data
rtabmap_odom::StereoOdometry::rgbd_image4_sub_
message_filters::Subscriber< rtabmap_msgs::RGBDImage > rgbd_image4_sub_
Definition: stereo_odometry.cpp:1046
fabs
Real fabs(const Real &a)
message_filters::Subscriber< sensor_msgs::CameraInfo >
rtabmap_odom::StereoOdometry::exactSync3_
message_filters::Synchronizer< MyExactSync3Policy > * exactSync3_
Definition: stereo_odometry.cpp:1061
ros::NodeHandle::resolveName
std::string resolveName(const std::string &name, bool remap=true) const
rtabmap_odom::StereoOdometry::exactSync4_
message_filters::Synchronizer< MyExactSync4Policy > * exactSync4_
Definition: stereo_odometry.cpp:1065
rtabmap_conversions::stereoCameraModelFromROS
rtabmap::StereoCameraModel stereoCameraModelFromROS(const sensor_msgs::CameraInfo &leftCamInfo, const sensor_msgs::CameraInfo &rightCamInfo, const rtabmap::Transform &localTransform=rtabmap::Transform::getIdentity(), const rtabmap::Transform &stereoTransform=rtabmap::Transform())
rtabmap_odom::StereoOdometry::syncQueueSize_
int syncQueueSize_
Definition: stereo_odometry.cpp:1076
message_filters::sync_policies::ApproximateTime
rtabmap_odom::StereoOdometry::MyExactSync3Policy
message_filters::sync_policies::ExactTime< rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage > MyExactSync3Policy
Definition: stereo_odometry.cpp:1060
rtabmap_odom::StereoOdometry::onOdomInit
virtual void onOdomInit()
Definition: stereo_odometry.cpp:102
uInsert
void uInsert(std::map< K, V > &map, const std::map< K, V > &items)
rtabmap_odom::StereoOdometry::MyExactSync4Policy
message_filters::sync_policies::ExactTime< rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage > MyExactSync4Policy
Definition: stereo_odometry.cpp:1064
rtabmap_odom::StereoOdometry::callbackRGBDX
void callbackRGBDX(const rtabmap_msgs::RGBDImagesConstPtr &images)
Definition: stereo_odometry.cpp:737
rtabmap_odom::StereoOdometry::rgbd_image1_sub_
message_filters::Subscriber< rtabmap_msgs::RGBDImage > rgbd_image1_sub_
Definition: stereo_odometry.cpp:1043
subscriber_filter.h
rtabmap_odom::StereoOdometry::imageRectLeft_
image_transport::SubscriberFilter imageRectLeft_
Definition: stereo_odometry.cpp:1036
subscriber.h
rtabmap::Transform::x
float & x()
rtabmap_odom::StereoOdometry::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: stereo_odometry.cpp:1070
Odometry.h
rtabmap_odom::StereoOdometry::approxSync6_
message_filters::Synchronizer< MyApproxSync6Policy > * approxSync6_
Definition: stereo_odometry.cpp:1071
rtabmap_odom::StereoOdometry::flushCallbacks
virtual void flushCallbacks()
Definition: stereo_odometry.cpp:908
rtabmap::StereoCameraModel::localTransform
const Transform & localTransform() const
rtabmap_odom::StereoOdometry::commonCallback
void commonCallback(const std::vector< cv_bridge::CvImageConstPtr > &leftImages, const std::vector< cv_bridge::CvImageConstPtr > &rightImages, const std::vector< sensor_msgs::CameraInfo > &leftCameraInfos, const std::vector< sensor_msgs::CameraInfo > &rightCameraInfos)
Definition: stereo_odometry.cpp:411
rtabmap::CameraModel::fy
double fy() const
rtabmap::StereoCameraModel::left
const CameraModel & left() const
ros::NodeHandle::hasParam
bool hasParam(const std::string &key) const
UASSERT
#define UASSERT(condition)
rtabmap_odom::StereoOdometry::approxSync_
message_filters::Synchronizer< MyApproxSyncPolicy > * approxSync_
Definition: stereo_odometry.cpp:1051
ROS_WARN
#define ROS_WARN(...)
rtabmap_odom::StereoOdometry::approxSync5_
message_filters::Synchronizer< MyApproxSync5Policy > * approxSync5_
Definition: stereo_odometry.cpp:1067
rtabmap_odom::StereoOdometry::MyExactSyncPolicy
message_filters::sync_policies::ExactTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo > MyExactSyncPolicy
Definition: stereo_odometry.cpp:1052
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::StereoOdometry::rgbd_image2_sub_
message_filters::Subscriber< rtabmap_msgs::RGBDImage > rgbd_image2_sub_
Definition: stereo_odometry.cpp:1044
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
image_transport.h
rtabmap_odom::StereoOdometry::cameraInfoRight_
message_filters::Subscriber< sensor_msgs::CameraInfo > cameraInfoRight_
Definition: stereo_odometry.cpp:1039
rtabmap_odom::OdometryROS
Definition: OdometryROS.h:59
rtabmap_odom::StereoOdometry::cameraInfoLeft_
message_filters::Subscriber< sensor_msgs::CameraInfo > cameraInfoLeft_
Definition: stereo_odometry.cpp:1038
NODELET_INFO
#define NODELET_INFO(...)
rtabmap::Transform
rtabmap_odom::StereoOdometry::approxSync3_
message_filters::Synchronizer< MyApproxSync3Policy > * approxSync3_
Definition: stereo_odometry.cpp:1059
rtabmap_conversions::timestampFromROS
double timestampFromROS(const ros::Time &stamp)
nodelet::Nodelet
ros::Time
rtabmap_odom::StereoOdometry::MyApproxSync3Policy
message_filters::sync_policies::ApproximateTime< rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage > MyApproxSync3Policy
Definition: stereo_odometry.cpp:1058
sensor_msgs::image_encodings::BGRA8
const std::string BGRA8
PointMatcherSupport::Parametrizable
iter
iterator iter(handle obj)
rtabmap_odom::StereoOdometry::callbackRGBD3
void callbackRGBD3(const rtabmap_msgs::RGBDImageConstPtr &image, const rtabmap_msgs::RGBDImageConstPtr &image2, const rtabmap_msgs::RGBDImageConstPtr &image3)
Definition: stereo_odometry.cpp:783
rtabmap_odom::StereoOdometry::MyExactSync5Policy
message_filters::sync_policies::ExactTime< rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage > MyExactSync5Policy
Definition: stereo_odometry.cpp:1068
nodelet.h
rtabmap_odom::StereoOdometry::exactSync6_
message_filters::Synchronizer< MyExactSync6Policy > * exactSync6_
Definition: stereo_odometry.cpp:1073
sensor_msgs::image_encodings::MONO8
const std::string MONO8
rtabmap_odom::StereoOdometry::exactSync_
message_filters::Synchronizer< MyExactSyncPolicy > * exactSync_
Definition: stereo_odometry.cpp:1053
rtabmap::Transform::isIdentity
bool isIdentity() const
c_str
const char * c_str(Args &&...args)
cv_bridge.h
sensor_msgs::image_encodings::MONO16
const std::string MONO16
rtabmap_odom::StereoOdometry::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: stereo_odometry.cpp:1072
class_list_macros.hpp
rtabmap::CameraModel::imageSize
const cv::Size & imageSize() const
rtabmap_odom::StereoOdometry::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: stereo_odometry.cpp:870
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
ULogger.h
rtabmap_odom::StereoOdometry::MyApproxSync5Policy
message_filters::sync_policies::ApproximateTime< rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage > MyApproxSync5Policy
Definition: stereo_odometry.cpp:1066
approximate_time.h
rtabmap_odom::StereoOdometry::imageRectRight_
image_transport::SubscriberFilter imageRectRight_
Definition: stereo_odometry.cpp:1037
sensor_msgs::image_encodings::BGR8
const std::string BGR8
rtabmap_odom::StereoOdometry::approxSync4_
message_filters::Synchronizer< MyApproxSync4Policy > * approxSync4_
Definition: stereo_odometry.cpp:1063
false
#define false
image_transport::TransportHints
rtabmap_odom::StereoOdometry::rgbdSub_
ros::Subscriber rgbdSub_
Definition: stereo_odometry.cpp:1041
cv_bridge::toCvShare
CvImageConstPtr toCvShare(const sensor_msgs::Image &source, const boost::shared_ptr< void const > &tracked_object, const std::string &encoding=std::string())
UTimer.h
message_filters::sync_policies::ExactTime
header
const std::string header
rtabmap_odom::StereoOdometry::callbackRGBD4
void callbackRGBD4(const rtabmap_msgs::RGBDImageConstPtr &image, const rtabmap_msgs::RGBDImageConstPtr &image2, const rtabmap_msgs::RGBDImageConstPtr &image3, const rtabmap_msgs::RGBDImageConstPtr &image4)
Definition: stereo_odometry.cpp:808
UConversion.h
OdometryROS.h
rtabmap_odom::StereoOdometry::MyApproxSyncPolicy
message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo > MyApproxSyncPolicy
Definition: stereo_odometry.cpp:1050
MsgConversion.h
rtabmap
rtabmap_odom::StereoOdometry::rgbdxSub_
ros::Subscriber rgbdxSub_
Definition: stereo_odometry.cpp:1042
ros::Duration
rtabmap_odom::StereoOdometry::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: stereo_odometry.cpp:837
rtabmap_odom::StereoOdometry::rgbd_image3_sub_
message_filters::Subscriber< rtabmap_msgs::RGBDImage > rgbd_image3_sub_
Definition: stereo_odometry.cpp:1045
i
int i
rtabmap_odom::StereoOdometry::rgbd_image5_sub_
message_filters::Subscriber< rtabmap_msgs::RGBDImage > rgbd_image5_sub_
Definition: stereo_odometry.cpp:1047
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)
rtabmap_odom::StereoOdometry::callbackRGBD2
void callbackRGBD2(const rtabmap_msgs::RGBDImageConstPtr &image, const rtabmap_msgs::RGBDImageConstPtr &image2)
Definition: stereo_odometry.cpp:762
ros::NodeHandle
ros::Subscriber
cv_bridge::cvtColor
CvImagePtr cvtColor(const CvImageConstPtr &source, const std::string &encoding)

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