00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include <rtabmap_ros/OdometryROS.h>
00029
00030 #include <pluginlib/class_list_macros.h>
00031 #include <nodelet/nodelet.h>
00032
00033 #include <message_filters/subscriber.h>
00034 #include <message_filters/time_synchronizer.h>
00035 #include <message_filters/sync_policies/approximate_time.h>
00036
00037 #include <image_transport/image_transport.h>
00038 #include <image_transport/subscriber_filter.h>
00039
00040 #include <image_geometry/stereo_camera_model.h>
00041
00042 #include <sensor_msgs/Image.h>
00043 #include <sensor_msgs/image_encodings.h>
00044 #include <cv_bridge/cv_bridge.h>
00045
00046 #include "rtabmap_ros/MsgConversion.h"
00047
00048 #include <rtabmap/core/util3d.h>
00049 #include <rtabmap/core/util2d.h>
00050 #include <rtabmap/utilite/ULogger.h>
00051 #include <rtabmap/utilite/UConversion.h>
00052 #include <rtabmap/utilite/UStl.h>
00053
00054 using namespace rtabmap;
00055
00056 namespace rtabmap_ros
00057 {
00058
00059 class RGBDOdometry : public rtabmap_ros::OdometryROS
00060 {
00061 public:
00062 RGBDOdometry() :
00063 OdometryROS(false, true, false),
00064 approxSync_(0),
00065 exactSync_(0),
00066 approxSync2_(0),
00067 exactSync2_(0),
00068 approxSync3_(0),
00069 exactSync3_(0),
00070 approxSync4_(0),
00071 exactSync4_(0),
00072 queueSize_(5)
00073 {
00074 }
00075
00076 virtual ~RGBDOdometry()
00077 {
00078 rgbdSub_.shutdown();
00079 if(approxSync_)
00080 {
00081 delete approxSync_;
00082 }
00083 if(exactSync_)
00084 {
00085 delete exactSync_;
00086 }
00087 if(approxSync2_)
00088 {
00089 delete approxSync2_;
00090 }
00091 if(exactSync2_)
00092 {
00093 delete exactSync2_;
00094 }
00095 if(approxSync3_)
00096 {
00097 delete approxSync3_;
00098 }
00099 if(exactSync3_)
00100 {
00101 delete exactSync3_;
00102 }
00103 if(approxSync4_)
00104 {
00105 delete approxSync4_;
00106 }
00107 if(exactSync4_)
00108 {
00109 delete exactSync4_;
00110 }
00111 }
00112
00113 private:
00114
00115 virtual void onOdomInit()
00116 {
00117 ros::NodeHandle & nh = getNodeHandle();
00118 ros::NodeHandle & pnh = getPrivateNodeHandle();
00119
00120 int rgbdCameras = 1;
00121 bool approxSync = true;
00122 bool subscribeRGBD = false;
00123 pnh.param("approx_sync", approxSync, approxSync);
00124 pnh.param("queue_size", queueSize_, queueSize_);
00125 pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD);
00126 if(pnh.hasParam("depth_cameras"))
00127 {
00128 ROS_ERROR("\"depth_cameras\" parameter doesn't exist anymore. It is replaced by \"rgbd_cameras\" with the \"rgbd_image\" input topics. \"subscribe_rgbd\" should be also set to true.");
00129 }
00130 pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras);
00131 if(rgbdCameras <= 0)
00132 {
00133 rgbdCameras = 1;
00134 }
00135 if(rgbdCameras > 4)
00136 {
00137 NODELET_FATAL("Only 4 cameras maximum supported yet.");
00138 }
00139
00140 NODELET_INFO("RGBDOdometry: approx_sync = %s", approxSync?"true":"false");
00141 NODELET_INFO("RGBDOdometry: queue_size = %d", queueSize_);
00142 NODELET_INFO("RGBDOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
00143 NODELET_INFO("RGBDOdometry: rgbd_cameras = %d", rgbdCameras);
00144
00145 std::string subscribedTopicsMsg;
00146 if(subscribeRGBD)
00147 {
00148 if(rgbdCameras >= 2)
00149 {
00150 rgbd_image1_sub_.subscribe(nh, "rgbd_image0", 1);
00151 rgbd_image2_sub_.subscribe(nh, "rgbd_image1", 1);
00152 if(rgbdCameras >= 3)
00153 {
00154 rgbd_image3_sub_.subscribe(nh, "rgbd_image2", 1);
00155 }
00156 if(rgbdCameras >= 4)
00157 {
00158 rgbd_image4_sub_.subscribe(nh, "rgbd_image3", 1);
00159 }
00160
00161 if(rgbdCameras == 2)
00162 {
00163 if(approxSync)
00164 {
00165 approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
00166 MyApproxSync2Policy(queueSize_),
00167 rgbd_image1_sub_,
00168 rgbd_image2_sub_);
00169 approxSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2));
00170 }
00171 else
00172 {
00173 exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
00174 MyExactSync2Policy(queueSize_),
00175 rgbd_image1_sub_,
00176 rgbd_image2_sub_);
00177 exactSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2));
00178 }
00179 subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s",
00180 getName().c_str(),
00181 approxSync?"approx":"exact",
00182 rgbd_image1_sub_.getTopic().c_str(),
00183 rgbd_image2_sub_.getTopic().c_str());
00184 }
00185 else if(rgbdCameras == 3)
00186 {
00187 if(approxSync)
00188 {
00189 approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
00190 MyApproxSync3Policy(queueSize_),
00191 rgbd_image1_sub_,
00192 rgbd_image2_sub_,
00193 rgbd_image3_sub_);
00194 approxSync3_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD3, this, _1, _2, _3));
00195 }
00196 else
00197 {
00198 exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
00199 MyExactSync3Policy(queueSize_),
00200 rgbd_image1_sub_,
00201 rgbd_image2_sub_,
00202 rgbd_image3_sub_);
00203 exactSync3_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD3, this, _1, _2, _3));
00204 }
00205 subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s",
00206 getName().c_str(),
00207 approxSync?"approx":"exact",
00208 rgbd_image1_sub_.getTopic().c_str(),
00209 rgbd_image2_sub_.getTopic().c_str(),
00210 rgbd_image3_sub_.getTopic().c_str());
00211 }
00212 else if(rgbdCameras == 4)
00213 {
00214 if(approxSync)
00215 {
00216 approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
00217 MyApproxSync4Policy(queueSize_),
00218 rgbd_image1_sub_,
00219 rgbd_image2_sub_,
00220 rgbd_image3_sub_,
00221 rgbd_image4_sub_);
00222 approxSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, _1, _2, _3, _4));
00223 }
00224 else
00225 {
00226 exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
00227 MyExactSync4Policy(queueSize_),
00228 rgbd_image1_sub_,
00229 rgbd_image2_sub_,
00230 rgbd_image3_sub_,
00231 rgbd_image4_sub_);
00232 exactSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, _1, _2, _3, _4));
00233 }
00234 subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s",
00235 getName().c_str(),
00236 approxSync?"approx":"exact",
00237 rgbd_image1_sub_.getTopic().c_str(),
00238 rgbd_image2_sub_.getTopic().c_str(),
00239 rgbd_image3_sub_.getTopic().c_str(),
00240 rgbd_image4_sub_.getTopic().c_str());
00241 }
00242 }
00243 else
00244 {
00245 rgbdSub_ = nh.subscribe("rgbd_image", 1, &RGBDOdometry::callbackRGBD, this);
00246
00247 subscribedTopicsMsg =
00248 uFormat("\n%s subscribed to:\n %s",
00249 getName().c_str(),
00250 rgbdSub_.getTopic().c_str());
00251 }
00252 }
00253 else
00254 {
00255 ros::NodeHandle rgb_nh(nh, "rgb");
00256 ros::NodeHandle depth_nh(nh, "depth");
00257 ros::NodeHandle rgb_pnh(pnh, "rgb");
00258 ros::NodeHandle depth_pnh(pnh, "depth");
00259 image_transport::ImageTransport rgb_it(rgb_nh);
00260 image_transport::ImageTransport depth_it(depth_nh);
00261 image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
00262 image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
00263
00264 image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
00265 image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
00266 info_sub_.subscribe(rgb_nh, "camera_info", 1);
00267
00268 if(approxSync)
00269 {
00270 approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
00271 approxSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
00272 }
00273 else
00274 {
00275 exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
00276 exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
00277 }
00278
00279 subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s",
00280 getName().c_str(),
00281 approxSync?"approx":"exact",
00282 image_mono_sub_.getTopic().c_str(),
00283 image_depth_sub_.getTopic().c_str(),
00284 info_sub_.getTopic().c_str());
00285 }
00286 this->startWarningThread(subscribedTopicsMsg, approxSync);
00287 }
00288
00289 virtual void updateParameters(ParametersMap & parameters)
00290 {
00291
00292 ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy());
00293 if(iter != parameters.end() && iter->second.compare("0") != 0)
00294 {
00295 ROS_WARN("RGBD odometry works only with \"Reg/Strategy\"=0. Ignoring value %s.", iter->second.c_str());
00296 }
00297 uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "0"));
00298
00299 int estimationType = Parameters::defaultVisEstimationType();
00300 Parameters::parse(parameters, Parameters::kVisEstimationType(), estimationType);
00301 ros::NodeHandle & pnh = getPrivateNodeHandle();
00302 int rgbdCameras = 1;
00303 bool subscribeRGBD = false;
00304 pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD);
00305 pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras);
00306 if(subscribeRGBD && rgbdCameras> 1 && estimationType>0)
00307 {
00308 NODELET_WARN("Setting \"%s\" parameter to 0 (%d is not supported "
00309 "for multi-cameras) as \"subscribe_rgbd\" is "
00310 "true and \"rgbd_cameras\">1. Set \"%s\" to 0 to suppress this warning.",
00311 Parameters::kVisEstimationType().c_str(),
00312 estimationType,
00313 Parameters::kVisEstimationType().c_str());
00314 uInsert(parameters, ParametersPair(Parameters::kVisEstimationType(), "0"));
00315 }
00316 }
00317
00318 void commonCallback(
00319 const std::vector<cv_bridge::CvImageConstPtr> & rgbImages,
00320 const std::vector<cv_bridge::CvImageConstPtr> & depthImages,
00321 const std::vector<sensor_msgs::CameraInfo>& cameraInfos)
00322 {
00323 ROS_ASSERT(rgbImages.size() > 0 && rgbImages.size() == depthImages.size() && rgbImages.size() == cameraInfos.size());
00324 ros::Time higherStamp;
00325 int imageWidth = rgbImages[0]->image.cols;
00326 int imageHeight = rgbImages[0]->image.rows;
00327 int depthWidth = depthImages[0]->image.cols;
00328 int depthHeight = depthImages[0]->image.rows;
00329
00330 UASSERT_MSG(
00331 imageWidth % depthWidth == 0 && imageHeight % depthHeight == 0 &&
00332 imageWidth/depthWidth == imageHeight/depthHeight,
00333 uFormat("rgb=%dx%d depth=%dx%d", imageWidth, imageHeight, depthWidth, depthHeight).c_str());
00334
00335 int cameraCount = rgbImages.size();
00336 cv::Mat rgb;
00337 cv::Mat depth;
00338 pcl::PointCloud<pcl::PointXYZ> scanCloud;
00339 std::vector<CameraModel> cameraModels;
00340 for(unsigned int i=0; i<rgbImages.size(); ++i)
00341 {
00342 if(!(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
00343 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
00344 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
00345 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
00346 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
00347 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
00348 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
00349 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0) ||
00350 !(depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
00351 depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
00352 depthImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
00353 {
00354 NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8,bgra8,rgba8 and "
00355 "image_depth=32FC1,16UC1,mono16. Current rgb=%s and depth=%s",
00356 rgbImages[i]->encoding.c_str(),
00357 depthImages[i]->encoding.c_str());
00358 return;
00359 }
00360 UASSERT_MSG(rgbImages[i]->image.cols == imageWidth && rgbImages[i]->image.rows == imageHeight,
00361 uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
00362 imageWidth,
00363 rgbImages[i]->image.cols,
00364 imageHeight,
00365 rgbImages[i]->image.rows).c_str());
00366 UASSERT_MSG(depthImages[i]->image.cols == depthWidth && depthImages[i]->image.rows == depthHeight,
00367 uFormat("depthWidth=%d vs %d depthHeight=%d vs %d",
00368 depthWidth,
00369 depthImages[i]->image.cols,
00370 depthHeight,
00371 depthImages[i]->image.rows).c_str());
00372
00373 ros::Time stamp = rgbImages[i]->header.stamp>depthImages[i]->header.stamp?rgbImages[i]->header.stamp:depthImages[i]->header.stamp;
00374
00375 if(i == 0)
00376 {
00377 higherStamp = stamp;
00378 }
00379 else if(stamp > higherStamp)
00380 {
00381 higherStamp = stamp;
00382 }
00383
00384 Transform localTransform = getTransform(this->frameId(), rgbImages[i]->header.frame_id, stamp);
00385 if(localTransform.isNull())
00386 {
00387 return;
00388 }
00389
00390 cv_bridge::CvImageConstPtr ptrImage = rgbImages[i];
00391 if(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
00392 rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
00393 {
00394 ptrImage = cv_bridge::cvtColor(rgbImages[i], "mono8");
00395 }
00396
00397 cv_bridge::CvImageConstPtr ptrDepth = depthImages[i];
00398 cv::Mat subDepth = ptrDepth->image;
00399
00400
00401 if(rgb.empty())
00402 {
00403 rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type());
00404 }
00405 if(depth.empty())
00406 {
00407 depth = cv::Mat(depthHeight, depthWidth*cameraCount, subDepth.type());
00408 }
00409
00410 if(ptrImage->image.type() == rgb.type())
00411 {
00412 ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
00413 }
00414 else
00415 {
00416 NODELET_ERROR("Some RGB images are not the same type!");
00417 return;
00418 }
00419
00420 if(subDepth.type() == depth.type())
00421 {
00422 subDepth.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight)));
00423 }
00424 else
00425 {
00426 NODELET_ERROR("Some Depth images are not the same type!");
00427 return;
00428 }
00429
00430 cameraModels.push_back(rtabmap_ros::cameraModelFromROS(cameraInfos[i], localTransform));
00431 }
00432
00433 rtabmap::SensorData data(
00434 rgb,
00435 depth,
00436 cameraModels,
00437 0,
00438 rtabmap_ros::timestampFromROS(higherStamp));
00439
00440 this->processData(data, higherStamp);
00441 }
00442
00443 void callback(
00444 const sensor_msgs::ImageConstPtr& image,
00445 const sensor_msgs::ImageConstPtr& depth,
00446 const sensor_msgs::CameraInfoConstPtr& cameraInfo)
00447 {
00448 callbackCalled();
00449 if(!this->isPaused())
00450 {
00451 std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);
00452 std::vector<cv_bridge::CvImageConstPtr> depthMsgs(1);
00453 std::vector<sensor_msgs::CameraInfo> infoMsgs;
00454 imageMsgs[0] = cv_bridge::toCvShare(image);
00455 depthMsgs[0] = cv_bridge::toCvShare(depth);
00456 infoMsgs.push_back(*cameraInfo);
00457
00458 this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
00459 }
00460 }
00461
00462 void callbackRGBD(
00463 const rtabmap_ros::RGBDImageConstPtr& image)
00464 {
00465 callbackCalled();
00466 if(!this->isPaused())
00467 {
00468 std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);
00469 std::vector<cv_bridge::CvImageConstPtr> depthMsgs(1);
00470 std::vector<sensor_msgs::CameraInfo> infoMsgs;
00471 rtabmap_ros::toCvShare(image, imageMsgs[0], depthMsgs[0]);
00472 infoMsgs.push_back(image->rgbCameraInfo);
00473
00474 this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
00475 }
00476 }
00477
00478 void callbackRGBD2(
00479 const rtabmap_ros::RGBDImageConstPtr& image,
00480 const rtabmap_ros::RGBDImageConstPtr& image2)
00481 {
00482 callbackCalled();
00483 if(!this->isPaused())
00484 {
00485 std::vector<cv_bridge::CvImageConstPtr> imageMsgs(2);
00486 std::vector<cv_bridge::CvImageConstPtr> depthMsgs(2);
00487 std::vector<sensor_msgs::CameraInfo> infoMsgs;
00488 rtabmap_ros::toCvShare(image, imageMsgs[0], depthMsgs[0]);
00489 rtabmap_ros::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
00490 infoMsgs.push_back(image->rgbCameraInfo);
00491 infoMsgs.push_back(image2->rgbCameraInfo);
00492
00493 this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
00494 }
00495 }
00496
00497 void callbackRGBD3(
00498 const rtabmap_ros::RGBDImageConstPtr& image,
00499 const rtabmap_ros::RGBDImageConstPtr& image2,
00500 const rtabmap_ros::RGBDImageConstPtr& image3)
00501 {
00502 callbackCalled();
00503 if(!this->isPaused())
00504 {
00505 std::vector<cv_bridge::CvImageConstPtr> imageMsgs(3);
00506 std::vector<cv_bridge::CvImageConstPtr> depthMsgs(3);
00507 std::vector<sensor_msgs::CameraInfo> infoMsgs;
00508 rtabmap_ros::toCvShare(image, imageMsgs[0], depthMsgs[0]);
00509 rtabmap_ros::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
00510 rtabmap_ros::toCvShare(image3, imageMsgs[2], depthMsgs[2]);
00511 infoMsgs.push_back(image->rgbCameraInfo);
00512 infoMsgs.push_back(image2->rgbCameraInfo);
00513 infoMsgs.push_back(image3->rgbCameraInfo);
00514
00515 this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
00516 }
00517 }
00518
00519 void callbackRGBD4(
00520 const rtabmap_ros::RGBDImageConstPtr& image,
00521 const rtabmap_ros::RGBDImageConstPtr& image2,
00522 const rtabmap_ros::RGBDImageConstPtr& image3,
00523 const rtabmap_ros::RGBDImageConstPtr& image4)
00524 {
00525 callbackCalled();
00526 if(!this->isPaused())
00527 {
00528 std::vector<cv_bridge::CvImageConstPtr> imageMsgs(4);
00529 std::vector<cv_bridge::CvImageConstPtr> depthMsgs(4);
00530 std::vector<sensor_msgs::CameraInfo> infoMsgs;
00531 rtabmap_ros::toCvShare(image, imageMsgs[0], depthMsgs[0]);
00532 rtabmap_ros::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
00533 rtabmap_ros::toCvShare(image3, imageMsgs[2], depthMsgs[2]);
00534 rtabmap_ros::toCvShare(image4, imageMsgs[3], depthMsgs[3]);
00535 infoMsgs.push_back(image->rgbCameraInfo);
00536 infoMsgs.push_back(image2->rgbCameraInfo);
00537 infoMsgs.push_back(image3->rgbCameraInfo);
00538 infoMsgs.push_back(image4->rgbCameraInfo);
00539
00540 this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
00541 }
00542 }
00543
00544 protected:
00545 virtual void flushCallbacks()
00546 {
00547
00548 if(approxSync_)
00549 {
00550 delete approxSync_;
00551 approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
00552 approxSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
00553 }
00554 if(exactSync_)
00555 {
00556 delete exactSync_;
00557 exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
00558 exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
00559 }
00560 if(approxSync2_)
00561 {
00562 delete approxSync2_;
00563 approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
00564 MyApproxSync2Policy(queueSize_),
00565 rgbd_image1_sub_,
00566 rgbd_image2_sub_);
00567 approxSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2));
00568 }
00569 if(exactSync2_)
00570 {
00571 delete exactSync2_;
00572 exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
00573 MyExactSync2Policy(queueSize_),
00574 rgbd_image1_sub_,
00575 rgbd_image2_sub_);
00576 exactSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2));
00577 }
00578 if(approxSync3_)
00579 {
00580 delete approxSync3_;
00581 approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
00582 MyApproxSync3Policy(queueSize_),
00583 rgbd_image1_sub_,
00584 rgbd_image2_sub_,
00585 rgbd_image3_sub_);
00586 approxSync3_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD3, this, _1, _2, _3));
00587 }
00588 if(exactSync3_)
00589 {
00590 delete exactSync3_;
00591 exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
00592 MyExactSync3Policy(queueSize_),
00593 rgbd_image1_sub_,
00594 rgbd_image2_sub_,
00595 rgbd_image3_sub_);
00596 exactSync3_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD3, this, _1, _2, _3));
00597 }
00598 if(approxSync4_)
00599 {
00600 delete approxSync4_;
00601 approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
00602 MyApproxSync4Policy(queueSize_),
00603 rgbd_image1_sub_,
00604 rgbd_image2_sub_,
00605 rgbd_image3_sub_,
00606 rgbd_image4_sub_);
00607 approxSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, _1, _2, _3, _4));
00608 }
00609 if(exactSync4_)
00610 {
00611 delete exactSync4_;
00612 exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
00613 MyExactSync4Policy(queueSize_),
00614 rgbd_image1_sub_,
00615 rgbd_image2_sub_,
00616 rgbd_image3_sub_,
00617 rgbd_image4_sub_);
00618 exactSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, _1, _2, _3, _4));
00619 }
00620 }
00621
00622 private:
00623 image_transport::SubscriberFilter image_mono_sub_;
00624 image_transport::SubscriberFilter image_depth_sub_;
00625 message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
00626
00627 ros::Subscriber rgbdSub_;
00628 message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image1_sub_;
00629 message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image2_sub_;
00630 message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image3_sub_;
00631 message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image4_sub_;
00632
00633 typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
00634 message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
00635 typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncPolicy;
00636 message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
00637 typedef message_filters::sync_policies::ApproximateTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyApproxSync2Policy;
00638 message_filters::Synchronizer<MyApproxSync2Policy> * approxSync2_;
00639 typedef message_filters::sync_policies::ExactTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyExactSync2Policy;
00640 message_filters::Synchronizer<MyExactSync2Policy> * exactSync2_;
00641 typedef message_filters::sync_policies::ApproximateTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyApproxSync3Policy;
00642 message_filters::Synchronizer<MyApproxSync3Policy> * approxSync3_;
00643 typedef message_filters::sync_policies::ExactTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyExactSync3Policy;
00644 message_filters::Synchronizer<MyExactSync3Policy> * exactSync3_;
00645 typedef message_filters::sync_policies::ApproximateTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyApproxSync4Policy;
00646 message_filters::Synchronizer<MyApproxSync4Policy> * approxSync4_;
00647 typedef message_filters::sync_policies::ExactTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyExactSync4Policy;
00648 message_filters::Synchronizer<MyExactSync4Policy> * exactSync4_;
00649 int queueSize_;
00650 };
00651
00652 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::RGBDOdometry, nodelet::Nodelet);
00653
00654 }