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.h"
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 
48 #include <rtabmap/utilite/ULogger.h>
49 #include <rtabmap/utilite/UTimer.h>
50 #include <rtabmap/utilite/UStl.h>
51 #include <rtabmap/utilite/UConversion.h>
52 #include <rtabmap/core/Odometry.h>
53 
54 using namespace rtabmap;
55 
56 namespace rtabmap_ros
57 {
58 
59 class StereoOdometry : public rtabmap_ros::OdometryROS
60 {
61 public:
62  StereoOdometry() :
63  rtabmap_ros::OdometryROS(true, true, false),
64  approxSync_(0),
65  exactSync_(0),
66  queueSize_(5),
67  keepColor_(false)
68  {
69  }
70 
71  virtual ~StereoOdometry()
72  {
73  if(approxSync_)
74  {
75  delete approxSync_;
76  }
77  if(exactSync_)
78  {
79  delete exactSync_;
80  }
81  }
82 
83 private:
84  virtual void onOdomInit()
85  {
86  ros::NodeHandle & nh = getNodeHandle();
87  ros::NodeHandle & pnh = getPrivateNodeHandle();
88 
89  bool approxSync = false;
90  bool subscribeRGBD = false;
91  pnh.param("approx_sync", approxSync, approxSync);
92  pnh.param("queue_size", queueSize_, queueSize_);
93  pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD);
94  pnh.param("keep_color", keepColor_, keepColor_);
95 
96  NODELET_INFO("StereoOdometry: approx_sync = %s", approxSync?"true":"false");
97  NODELET_INFO("StereoOdometry: queue_size = %d", queueSize_);
98  NODELET_INFO("StereoOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
99  NODELET_INFO("StereoOdometry: keep_color = %s", keepColor_?"true":"false");
100 
101  std::string subscribedTopicsMsg;
102  if(subscribeRGBD)
103  {
104  rgbdSub_ = nh.subscribe("rgbd_image", 1, &StereoOdometry::callbackRGBD, this);
105 
106  subscribedTopicsMsg =
107  uFormat("\n%s subscribed to:\n %s",
108  getName().c_str(),
109  rgbdSub_.getTopic().c_str());
110  }
111  else
112  {
113  ros::NodeHandle left_nh(nh, "left");
114  ros::NodeHandle right_nh(nh, "right");
115  ros::NodeHandle left_pnh(pnh, "left");
116  ros::NodeHandle right_pnh(pnh, "right");
117  image_transport::ImageTransport left_it(left_nh);
118  image_transport::ImageTransport right_it(right_nh);
119  image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh);
120  image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh);
121 
122  imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), 1, hintsLeft);
123  imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), 1, hintsRight);
124  cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
125  cameraInfoRight_.subscribe(right_nh, "camera_info", 1);
126 
127  if(approxSync)
128  {
129  approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
130  approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
131  }
132  else
133  {
134  exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
135  exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
136  }
137 
138 
139  subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s \\\n %s",
140  getName().c_str(),
141  approxSync?"approx":"exact",
142  imageRectLeft_.getTopic().c_str(),
143  imageRectRight_.getTopic().c_str(),
144  cameraInfoLeft_.getTopic().c_str(),
145  cameraInfoRight_.getTopic().c_str());
146  }
147 
148  this->startWarningThread(subscribedTopicsMsg, approxSync);
149  }
150 
151  virtual void updateParameters(ParametersMap & parameters)
152  {
153  //make sure we are using Reg/Strategy=0
154  ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy());
155  if(iter != parameters.end() && iter->second.compare("0") != 0)
156  {
157  ROS_WARN("Stereo odometry works only with \"Reg/Strategy\"=0. Ignoring value %s.", iter->second.c_str());
158  }
159  uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "0"));
160  }
161 
162  void callback(
163  const sensor_msgs::ImageConstPtr& imageRectLeft,
164  const sensor_msgs::ImageConstPtr& imageRectRight,
165  const sensor_msgs::CameraInfoConstPtr& cameraInfoLeft,
166  const sensor_msgs::CameraInfoConstPtr& cameraInfoRight)
167  {
168  callbackCalled();
169  if(!this->isPaused())
170  {
171  if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
172  imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
173  imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
174  imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
175  imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
176  imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0) ||
177  !(imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
178  imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
179  imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
180  imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
181  imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
182  imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0))
183  {
184  NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8,rgba8,bgra8 (mono8 recommended), received types are %s (left) and %s (right)",
185  imageRectLeft->encoding.c_str(), imageRectRight->encoding.c_str());
186  return;
187  }
188 
189  ros::Time stamp = imageRectLeft->header.stamp>imageRectRight->header.stamp?imageRectLeft->header.stamp:imageRectRight->header.stamp;
190 
191  Transform localTransform = getTransform(this->frameId(), imageRectLeft->header.frame_id, stamp);
192  if(localTransform.isNull())
193  {
194  return;
195  }
196 
197  int quality = -1;
198  if(imageRectLeft->data.size() && imageRectRight->data.size())
199  {
200  bool alreadyRectified = true;
201  Parameters::parse(parameters(), Parameters::kRtabmapImagesAlreadyRectified(), alreadyRectified);
202  rtabmap::Transform stereoTransform;
203  if(!alreadyRectified)
204  {
205  stereoTransform = getTransform(
206  cameraInfoRight->header.frame_id,
207  cameraInfoLeft->header.frame_id,
208  cameraInfoLeft->header.stamp);
209  if(stereoTransform.isNull())
210  {
211  NODELET_ERROR("Parameter %s is false but we cannot get TF between the two cameras!", Parameters::kRtabmapImagesAlreadyRectified().c_str());
212  return;
213  }
214  }
215 
216  rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*cameraInfoLeft, *cameraInfoRight, localTransform, stereoTransform);
217 
218  if(stereoModel.baseline() == 0 && alreadyRectified)
219  {
220  stereoTransform = getTransform(
221  cameraInfoLeft->header.frame_id,
222  cameraInfoRight->header.frame_id,
223  cameraInfoLeft->header.stamp);
224 
225  if(!stereoTransform.isNull() && stereoTransform.x()>0)
226  {
227  static bool warned = false;
228  if(!warned)
229  {
230  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 "
231  "recommended, we used TF to get the baseline (%s->%s = %fm) for convenience (e.g., D400 ir stereo issue). It is preferred to feed "
232  "a valid right camera info if stereo images are already rectified. This message is only printed once...",
233  rtabmap::Parameters::kRtabmapImagesAlreadyRectified().c_str(),
234  cameraInfoRight->header.frame_id.c_str(), cameraInfoLeft->header.frame_id.c_str(), stereoTransform.x());
235  warned = true;
236  }
237  stereoModel = rtabmap::StereoCameraModel(
238  stereoModel.left().fx(),
239  stereoModel.left().fy(),
240  stereoModel.left().cx(),
241  stereoModel.left().cy(),
242  stereoTransform.x(),
243  stereoModel.localTransform(),
244  stereoModel.left().imageSize());
245  }
246  }
247 
248  if(alreadyRectified && stereoModel.baseline() <= 0)
249  {
250  NODELET_ERROR("The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo "
251  "setup where the Tx (or P(0,3)) is negative in the right camera info msg.", stereoModel.baseline());
252  return;
253  }
254 
255  if(stereoModel.baseline() > 10.0)
256  {
257  static bool shown = false;
258  if(!shown)
259  {
260  NODELET_WARN("Detected baseline (%f m) is quite large! Is your "
261  "right camera_info P(0,3) correctly set? Note that "
262  "baseline=-P(0,3)/P(0,0). This warning is printed only once.",
263  stereoModel.baseline());
264  shown = true;
265  }
266  }
267  cv_bridge::CvImagePtr ptrImageLeft = cv_bridge::toCvCopy(imageRectLeft,
268  imageRectLeft->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0 ||
269  imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8)==0?"":
270  keepColor_ && imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0?"bgr8":"mono8");
271  cv_bridge::CvImagePtr ptrImageRight = cv_bridge::toCvCopy(imageRectRight, "mono8");
272 
273  UTimer stepTimer;
274  //
275  UDEBUG("localTransform = %s", localTransform.prettyPrint().c_str());
276  rtabmap::SensorData data(
277  ptrImageLeft->image,
278  ptrImageRight->image,
279  stereoModel,
280  0,
281  rtabmap_ros::timestampFromROS(stamp));
282 
283  this->processData(data, stamp, imageRectLeft->header.frame_id);
284  }
285  else
286  {
287  NODELET_WARN("Odom: input images empty?!?");
288  }
289  }
290  }
291 
292  void callbackRGBD(
293  const rtabmap_ros::RGBDImageConstPtr& image)
294  {
295  callbackCalled();
296  if(!this->isPaused())
297  {
298  cv_bridge::CvImageConstPtr imageRectLeft, imageRectRight;
299  rtabmap_ros::toCvShare(image, imageRectLeft, imageRectRight);
300 
301  if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
302  imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
303  imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
304  imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
305  imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
306  imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0) ||
307  !(imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
308  imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
309  imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
310  imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
311  imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
312  imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0))
313  {
314  NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8,rgba8,bgra8 (mono8 recommended), received types are %s (left) and %s (right)",
315  imageRectLeft->encoding.c_str(), imageRectRight->encoding.c_str());
316  return;
317  }
318 
319  ros::Time stamp = imageRectLeft->header.stamp>imageRectRight->header.stamp?imageRectLeft->header.stamp:imageRectRight->header.stamp;
320 
321  Transform localTransform = getTransform(this->frameId(), imageRectLeft->header.frame_id, stamp);
322  if(localTransform.isNull())
323  {
324  return;
325  }
326 
327  ros::WallTime time = ros::WallTime::now();
328 
329  int quality = -1;
330  if(!imageRectLeft->image.empty() && !imageRectRight->image.empty())
331  {
332  bool alreadyRectified = true;
333  Parameters::parse(parameters(), Parameters::kRtabmapImagesAlreadyRectified(), alreadyRectified);
334  rtabmap::Transform stereoTransform;
335  if(!alreadyRectified)
336  {
337  stereoTransform = getTransform(
338  image->depth_camera_info.header.frame_id,
339  image->rgb_camera_info.header.frame_id,
340  image->rgb_camera_info.header.stamp);
341  if(stereoTransform.isNull())
342  {
343  NODELET_ERROR("Parameter %s is false but we cannot get TF between the two cameras!", Parameters::kRtabmapImagesAlreadyRectified().c_str());
344  return;
345  }
346  }
347 
348  rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(image->rgb_camera_info, image->depth_camera_info, localTransform);
349 
350  if(stereoModel.baseline() == 0 && alreadyRectified)
351  {
352  stereoTransform = getTransform(
353  image->rgb_camera_info.header.frame_id,
354  image->depth_camera_info.header.frame_id,
355  image->rgb_camera_info.header.stamp);
356 
357  if(!stereoTransform.isNull() && stereoTransform.x()>0)
358  {
359  static bool warned = false;
360  if(!warned)
361  {
362  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 "
363  "recommended, we used TF to get the baseline (%s->%s = %fm) for convenience (e.g., D400 ir stereo issue). It is preferred to feed "
364  "a valid right camera info if stereo images are already rectified. This message is only printed once...",
365  rtabmap::Parameters::kRtabmapImagesAlreadyRectified().c_str(),
366  image->depth_camera_info.header.frame_id.c_str(), image->rgb_camera_info.header.frame_id.c_str(), stereoTransform.x());
367  warned = true;
368  }
369  stereoModel = rtabmap::StereoCameraModel(
370  stereoModel.left().fx(),
371  stereoModel.left().fy(),
372  stereoModel.left().cx(),
373  stereoModel.left().cy(),
374  stereoTransform.x(),
375  stereoModel.localTransform(),
376  stereoModel.left().imageSize());
377  }
378  }
379 
380  if(alreadyRectified && stereoModel.baseline() <= 0)
381  {
382  NODELET_ERROR("The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo "
383  "setup where the Tx (or P(0,3)) is negative in the right camera info msg.", stereoModel.baseline());
384  return;
385  }
386 
387  if(stereoModel.baseline() > 10.0)
388  {
389  static bool shown = false;
390  if(!shown)
391  {
392  NODELET_WARN("Detected baseline (%f m) is quite large! Is your "
393  "right camera_info P(0,3) correctly set? Note that "
394  "baseline=-P(0,3)/P(0,0). This warning is printed only once.",
395  stereoModel.baseline());
396  shown = true;
397  }
398  }
399 
400  cv_bridge::CvImageConstPtr ptrImageLeft = imageRectLeft;
401  if(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
402  imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
403  {
404  if(keepColor_ && imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0)
405  {
406  ptrImageLeft = cv_bridge::cvtColor(imageRectLeft, "bgr8");
407  }
408  else
409  {
410  ptrImageLeft = cv_bridge::cvtColor(imageRectLeft, "mono8");
411  }
412  }
413  cv_bridge::CvImagePtr ptrImageRight = cv_bridge::cvtColor(imageRectRight, "mono8");
414 
415  UTimer stepTimer;
416  //
417  UDEBUG("localTransform = %s", localTransform.prettyPrint().c_str());
418  rtabmap::SensorData data(
419  ptrImageLeft->image,
420  ptrImageRight->image,
421  stereoModel,
422  0,
423  rtabmap_ros::timestampFromROS(stamp));
424 
425  this->processData(data, stamp, image->header.frame_id);
426  }
427  else
428  {
429  NODELET_WARN("Odom: input images empty?!?");
430  }
431  }
432  }
433 
434 protected:
435  virtual void flushCallbacks()
436  {
437  //flush callbacks
438  if(approxSync_)
439  {
440  delete approxSync_;
441  approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
442  approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
443  }
444  if(exactSync_)
445  {
446  delete exactSync_;
447  exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
448  exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
449  }
450  }
451 
452 private:
453  image_transport::SubscriberFilter imageRectLeft_;
454  image_transport::SubscriberFilter imageRectRight_;
455  message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
456  message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_;
457  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
458  message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
459  typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncPolicy;
460  message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
461  ros::Subscriber rgbdSub_;
462  int queueSize_;
463  bool keepColor_;
464 };
465 
466 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::StereoOdometry, nodelet::Nodelet);
467 
468 }
469 
message_filters::Synchronizer
rtabmap_ros::StereoOdometry::keepColor_
bool keepColor_
Definition: stereo_odometry.cpp:463
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
rtabmap::StereoCameraModel::baseline
double baseline() const
image_transport::SubscriberFilter
sensor_msgs::image_encodings::BGR8
const std::string BGR8
uFormat
std::string uFormat(const char *fmt,...)
ros::NodeHandle
NODELET_ERROR
#define NODELET_ERROR(...)
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
sensor_msgs::image_encodings::RGB8
const std::string RGB8
rtabmap::CameraModel::imageSize
const cv::Size & imageSize() const
NODELET_WARN
#define NODELET_WARN(...)
subscriber_filter.h
rtabmap_ros::StereoOdometry::flushCallbacks
virtual void flushCallbacks()
Definition: stereo_odometry.cpp:435
rtabmap_ros::StereoOdometry::imageRectLeft_
image_transport::SubscriberFilter imageRectLeft_
Definition: stereo_odometry.cpp:453
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
message_filters::sync_policies::ExactTime
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
ros::NodeHandle::resolveName
std::string resolveName(const std::string &name, bool remap=true) const
rtabmap_ros::StereoOdometry::callback
void callback(const sensor_msgs::ImageConstPtr &imageRectLeft, const sensor_msgs::ImageConstPtr &imageRectRight, const sensor_msgs::CameraInfoConstPtr &cameraInfoLeft, const sensor_msgs::CameraInfoConstPtr &cameraInfoRight)
Definition: stereo_odometry.cpp:162
image_transport.h
getName
std::string getName(void *handle)
sensor_msgs::image_encodings::TYPE_8UC1
const std::string TYPE_8UC1
rtabmap_ros::StereoOdometry::~StereoOdometry
virtual ~StereoOdometry()
Definition: stereo_odometry.cpp:71
rtabmap_ros
Definition: CommonDataSubscriber.h:56
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:1614
boost::shared_ptr
rtabmap_ros::StereoOdometry::cameraInfoLeft_
message_filters::Subscriber< sensor_msgs::CameraInfo > cameraInfoLeft_
Definition: stereo_odometry.cpp:455
time_synchronizer.h
stereo_camera_model.h
sensor_msgs::image_encodings::RGBA8
const std::string RGBA8
rtabmap_ros::PLUGINLIB_EXPORT_CLASS
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::StereoOdometry, nodelet::Nodelet)
rtabmap::StereoCameraModel::left
const CameraModel & left() const
rtabmap_ros::StereoOdometry::imageRectRight_
image_transport::SubscriberFilter imageRectRight_
Definition: stereo_odometry.cpp:454
rtabmap::CameraModel::cx
double cx() const
true
#define true
rtabmap::Transform::isNull
bool isNull() const
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:459
cv_bridge::cvtColor
CvImagePtr cvtColor(const CvImageConstPtr &source, const std::string &encoding)
rtabmap::CameraModel::fy
double fy() const
class_list_macros.h
rtabmap_ros::StereoOdometry::StereoOdometry
StereoOdometry()
Definition: stereo_odometry.cpp:62
ros::NodeHandle::param
bool param(const std::string &param_name, T &param_val, const T &default_val) const
sensor_msgs::image_encodings::MONO8
const std::string MONO8
cv_bridge::toCvCopy
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
approximate_time.h
sensor_msgs::image_encodings::BGRA8
const std::string BGRA8
ParametersMap
QMap< QString, QVariant > ParametersMap
rtabmap_ros::StereoOdometry::exactSync_
message_filters::Synchronizer< MyExactSyncPolicy > * exactSync_
Definition: stereo_odometry.cpp:460
rtabmap_ros::StereoOdometry::cameraInfoRight_
message_filters::Subscriber< sensor_msgs::CameraInfo > cameraInfoRight_
Definition: stereo_odometry.cpp:456
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:461
rtabmap::CameraModel::cy
double cy() const
rtabmap_ros::StereoOdometry::updateParameters
virtual void updateParameters(ParametersMap &parameters)
Definition: stereo_odometry.cpp:151
patrol.frameId
string frameId
Definition: patrol.py:11
false
#define false
rtabmap_ros::StereoOdometry::queueSize_
int queueSize_
Definition: stereo_odometry.cpp:462
callback
void callback(rtabmap_ros::CameraConfig &config, uint32_t level)
Definition: CameraNode.cpp:259
message_filters::sync_policies::ApproximateTime
NODELET_INFO
#define NODELET_INFO(...)
UDEBUG
#define UDEBUG(...)
rtabmap::CameraModel::fx
double fx() const
ros::WallTime::now
static WallTime now()
image_transport::TransportHints
rtabmap_ros::toCvShare
void toCvShare(const rtabmap_ros::RGBDImageConstPtr &image, cv_bridge::CvImageConstPtr &rgb, cv_bridge::CvImageConstPtr &depth)
Definition: MsgConversion.cpp:164
rtabmap_ros::StereoOdometry
Definition: stereo_odometry.cpp:59
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:886
rtabmap_ros::timestampFromROS
double timestampFromROS(const ros::Time &stamp)
Definition: MsgConversion.h:187
image_transport::ImageTransport
rtabmap_ros::StereoOdometry::callbackRGBD
void callbackRGBD(const rtabmap_ros::RGBDImageConstPtr &image)
Definition: stereo_odometry.cpp:292
image_encodings.h
message_filters::Subscriber< sensor_msgs::CameraInfo >
rtabmap_ros::StereoOdometry::onOdomInit
virtual void onOdomInit()
Definition: stereo_odometry.cpp:84
rtabmap::StereoCameraModel::localTransform
const Transform & localTransform() const
rtabmap_ros::OdometryROS
Definition: OdometryROS.h:53
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:457
rtabmap_ros::StereoOdometry::approxSync_
message_filters::Synchronizer< MyApproxSyncPolicy > * approxSync_
Definition: stereo_odometry.cpp:458
uInsert
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)

rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:42:19