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

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