From 1058cafcd77237dc0c917909f898b524f791bb88 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 30 Oct 2017 21:45:49 +0100 Subject: [PATCH] Added rgbd_odometry support for up to 4 cameras --- src/CameraNode.cpp | 2 +- src/nodelets/rgbd_odometry.cpp | 227 ++++++++++++++++++++++++++++++--- 2 files changed, 208 insertions(+), 21 deletions(-) diff --git a/src/CameraNode.cpp b/src/CameraNode.cpp index df1f0530..10fff2f2 100644 --- a/src/CameraNode.cpp +++ b/src/CameraNode.cpp @@ -90,7 +90,7 @@ public: { if(cameraThread_) { - return cameraThread_->start(); + cameraThread_->start(); } } diff --git a/src/nodelets/rgbd_odometry.cpp b/src/nodelets/rgbd_odometry.cpp index f3141f80..fd9041cc 100644 --- a/src/nodelets/rgbd_odometry.cpp +++ b/src/nodelets/rgbd_odometry.cpp @@ -65,6 +65,10 @@ public: exactSync_(0), approxSync2_(0), exactSync2_(0), + approxSync3_(0), + exactSync3_(0), + approxSync4_(0), + exactSync4_(0), queueSize_(5) { } @@ -88,6 +92,22 @@ public: { delete exactSync2_; } + if(approxSync3_) + { + delete approxSync3_; + } + if(exactSync3_) + { + delete exactSync3_; + } + if(approxSync4_) + { + delete approxSync4_; + } + if(exactSync4_) + { + delete exactSync4_; + } } private: @@ -112,9 +132,9 @@ private: { rgbdCameras = 1; } - if(rgbdCameras > 2) + if(rgbdCameras > 4) { - NODELET_FATAL("Only 2 cameras maximum supported yet."); + NODELET_FATAL("Only 4 cameras maximum supported yet."); } NODELET_INFO("RGBDOdometry: approx_sync = %s", approxSync?"true":"false"); @@ -125,32 +145,100 @@ private: std::string subscribedTopicsMsg; if(subscribeRGBD) { - if(rgbdCameras == 2) + if(rgbdCameras >= 2) { rgbd_image1_sub_.subscribe(nh, "rgbd_image0", 1); rgbd_image2_sub_.subscribe(nh, "rgbd_image1", 1); + if(rgbdCameras >= 3) + { + rgbd_image3_sub_.subscribe(nh, "rgbd_image2", 1); + } + if(rgbdCameras >= 4) + { + rgbd_image4_sub_.subscribe(nh, "rgbd_image3", 1); + } - if(approxSync) + if(rgbdCameras == 2) { - approxSync2_ = new message_filters::Synchronizer( - MyApproxSync2Policy(queueSize_), - rgbd_image1_sub_, - rgbd_image2_sub_); - approxSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2)); + if(approxSync) + { + approxSync2_ = new message_filters::Synchronizer( + MyApproxSync2Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_); + approxSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2)); + } + else + { + exactSync2_ = new message_filters::Synchronizer( + MyExactSync2Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_); + exactSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2)); + } + subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s", + getName().c_str(), + approxSync?"approx":"exact", + rgbd_image1_sub_.getTopic().c_str(), + rgbd_image2_sub_.getTopic().c_str()); } - else + else if(rgbdCameras == 3) { - exactSync2_ = new message_filters::Synchronizer( - MyExactSync2Policy(queueSize_), - rgbd_image1_sub_, - rgbd_image2_sub_); - exactSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2)); + if(approxSync) + { + approxSync3_ = new message_filters::Synchronizer( + MyApproxSync3Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_); + approxSync3_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD3, this, _1, _2, _3)); + } + else + { + exactSync3_ = new message_filters::Synchronizer( + MyExactSync3Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_); + exactSync3_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD3, this, _1, _2, _3)); + } + subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s", + getName().c_str(), + approxSync?"approx":"exact", + rgbd_image1_sub_.getTopic().c_str(), + rgbd_image2_sub_.getTopic().c_str(), + rgbd_image3_sub_.getTopic().c_str()); + } + else if(rgbdCameras == 4) + { + if(approxSync) + { + approxSync4_ = new message_filters::Synchronizer( + MyApproxSync4Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_, + rgbd_image4_sub_); + approxSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, _1, _2, _3, _4)); + } + else + { + exactSync4_ = new message_filters::Synchronizer( + MyExactSync4Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_, + rgbd_image4_sub_); + exactSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, _1, _2, _3, _4)); + } + subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s", + getName().c_str(), + approxSync?"approx":"exact", + rgbd_image1_sub_.getTopic().c_str(), + rgbd_image2_sub_.getTopic().c_str(), + rgbd_image3_sub_.getTopic().c_str(), + rgbd_image4_sub_.getTopic().c_str()); } - subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s", - getName().c_str(), - approxSync?"approx":"exact", - rgbd_image1_sub_.getTopic().c_str(), - rgbd_image2_sub_.getTopic().c_str()); } else { @@ -406,6 +494,53 @@ private: } } + void callbackRGBD3( + const rtabmap_ros::RGBDImageConstPtr& image, + const rtabmap_ros::RGBDImageConstPtr& image2, + const rtabmap_ros::RGBDImageConstPtr& image3) + { + callbackCalled(); + if(!this->isPaused()) + { + std::vector imageMsgs(3); + std::vector depthMsgs(3); + std::vector infoMsgs; + rtabmap_ros::toCvShare(image, imageMsgs[0], depthMsgs[0]); + rtabmap_ros::toCvShare(image2, imageMsgs[1], depthMsgs[1]); + rtabmap_ros::toCvShare(image3, imageMsgs[2], depthMsgs[2]); + infoMsgs.push_back(image->cameraInfo); + infoMsgs.push_back(image2->cameraInfo); + infoMsgs.push_back(image3->cameraInfo); + + this->commonCallback(imageMsgs, depthMsgs, infoMsgs); + } + } + + void callbackRGBD4( + const rtabmap_ros::RGBDImageConstPtr& image, + const rtabmap_ros::RGBDImageConstPtr& image2, + const rtabmap_ros::RGBDImageConstPtr& image3, + const rtabmap_ros::RGBDImageConstPtr& image4) + { + callbackCalled(); + if(!this->isPaused()) + { + std::vector imageMsgs(4); + std::vector depthMsgs(4); + std::vector infoMsgs; + rtabmap_ros::toCvShare(image, imageMsgs[0], depthMsgs[0]); + rtabmap_ros::toCvShare(image2, imageMsgs[1], depthMsgs[1]); + rtabmap_ros::toCvShare(image3, imageMsgs[2], depthMsgs[2]); + rtabmap_ros::toCvShare(image4, imageMsgs[3], depthMsgs[3]); + infoMsgs.push_back(image->cameraInfo); + infoMsgs.push_back(image2->cameraInfo); + infoMsgs.push_back(image3->cameraInfo); + infoMsgs.push_back(image4->cameraInfo); + + this->commonCallback(imageMsgs, depthMsgs, infoMsgs); + } + } + protected: virtual void flushCallbacks() { @@ -440,6 +575,48 @@ protected: rgbd_image2_sub_); exactSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2)); } + if(approxSync3_) + { + delete approxSync3_; + approxSync3_ = new message_filters::Synchronizer( + MyApproxSync3Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_); + approxSync3_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD3, this, _1, _2, _3)); + } + if(exactSync3_) + { + delete exactSync3_; + exactSync3_ = new message_filters::Synchronizer( + MyExactSync3Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_); + exactSync3_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD3, this, _1, _2, _3)); + } + if(approxSync4_) + { + delete approxSync4_; + approxSync4_ = new message_filters::Synchronizer( + MyApproxSync4Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_, + rgbd_image4_sub_); + approxSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, _1, _2, _3, _4)); + } + if(exactSync4_) + { + delete exactSync4_; + exactSync4_ = new message_filters::Synchronizer( + MyExactSync4Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_, + rgbd_image4_sub_); + exactSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, _1, _2, _3, _4)); + } } private: @@ -450,6 +627,8 @@ private: ros::Subscriber rgbdSub_; message_filters::Subscriber rgbd_image1_sub_; message_filters::Subscriber rgbd_image2_sub_; + message_filters::Subscriber rgbd_image3_sub_; + message_filters::Subscriber rgbd_image4_sub_; typedef message_filters::sync_policies::ApproximateTime MyApproxSyncPolicy; message_filters::Synchronizer * approxSync_; @@ -459,6 +638,14 @@ private: message_filters::Synchronizer * approxSync2_; typedef message_filters::sync_policies::ExactTime MyExactSync2Policy; message_filters::Synchronizer * exactSync2_; + typedef message_filters::sync_policies::ApproximateTime MyApproxSync3Policy; + message_filters::Synchronizer * approxSync3_; + typedef message_filters::sync_policies::ExactTime MyExactSync3Policy; + message_filters::Synchronizer * exactSync3_; + typedef message_filters::sync_policies::ApproximateTime MyApproxSync4Policy; + message_filters::Synchronizer * approxSync4_; + typedef message_filters::sync_policies::ExactTime MyExactSync4Policy; + message_filters::Synchronizer * exactSync4_; int queueSize_; };