From 9a004836b035d1e4d2bd3242b7062fa9df45fb04 Mon Sep 17 00:00:00 2001 From: ruipimentelfigueiredo Date: Thu, 8 Jul 2021 16:14:15 +0200 Subject: [PATCH 1/2] added support for 5 camera odometry --- src/nodelets/rgbd_odometry.cpp | 97 ++++++++++++++++++++++++++++++++-- 1 file changed, 94 insertions(+), 3 deletions(-) diff --git a/src/nodelets/rgbd_odometry.cpp b/src/nodelets/rgbd_odometry.cpp index 805b082b..989753e7 100644 --- a/src/nodelets/rgbd_odometry.cpp +++ b/src/nodelets/rgbd_odometry.cpp @@ -69,6 +69,8 @@ public: exactSync3_(0), approxSync4_(0), exactSync4_(0), + approxSync5_(0), + exactSync5_(0), queueSize_(5), keepColor_(false) { @@ -133,9 +135,9 @@ private: { rgbdCameras = 1; } - if(rgbdCameras > 4) + if(rgbdCameras > 5) { - NODELET_FATAL("Only 4 cameras maximum supported yet."); + NODELET_FATAL("Only 5 cameras maximum supported yet."); } pnh.param("keep_color", keepColor_, keepColor_); @@ -242,6 +244,39 @@ private: rgbd_image3_sub_.getTopic().c_str(), rgbd_image4_sub_.getTopic().c_str()); } + else if(rgbdCameras == 5) + { + if(approxSync) + { + approxSync5_ = new message_filters::Synchronizer( + MyApproxSync5Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_, + rgbd_image4_sub_, + rgbd_image5_sub_); + approxSync5_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD5, this, _1, _2, _3, _4, _5)); + } + else + { + exactSync5_ = new message_filters::Synchronizer( + MyExactSync5Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_, + rgbd_image4_sub_, + rgbd_image5_sub_); + exactSync5_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD5, this, _1, _2, _3, _4, _5)); + } + 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()); + } + } else { @@ -331,7 +366,6 @@ private: int depthHeight = depthImages[0]->image.rows; UASSERT_MSG( - imageWidth % depthWidth == 0 && imageHeight % depthHeight == 0 && imageWidth/depthWidth == imageHeight/depthHeight, uFormat("rgb=%dx%d depth=%dx%d", imageWidth, imageHeight, depthWidth, depthHeight).c_str()); @@ -554,6 +588,34 @@ private: } } + void callbackRGBD5( + const rtabmap_ros::RGBDImageConstPtr& image, + const rtabmap_ros::RGBDImageConstPtr& image2, + const rtabmap_ros::RGBDImageConstPtr& image3, + const rtabmap_ros::RGBDImageConstPtr& image4, + const rtabmap_ros::RGBDImageConstPtr& image5) + { + callbackCalled(); + if(!this->isPaused()) + { + std::vector imageMsgs(5); + std::vector depthMsgs(5); + 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]); + rtabmap_ros::toCvShare(image4, imageMsgs[4], depthMsgs[4]); + infoMsgs.push_back(image->rgb_camera_info); + infoMsgs.push_back(image2->rgb_camera_info); + infoMsgs.push_back(image3->rgb_camera_info); + infoMsgs.push_back(image4->rgb_camera_info); + infoMsgs.push_back(image5->rgb_camera_info); + + this->commonCallback(imageMsgs, depthMsgs, infoMsgs); + } + } + protected: virtual void flushCallbacks() { @@ -630,6 +692,30 @@ protected: rgbd_image4_sub_); exactSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, _1, _2, _3, _4)); } + if(approxSync5_) + { + delete approxSync5_; + approxSync5_ = new message_filters::Synchronizer( + MyApproxSync5Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_, + rgbd_image4_sub_, + rgbd_image5_sub_); + approxSync5_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD5, this, _1, _2, _3, _4, _5)); + } + if(exactSync5_) + { + delete exactSync5_; + exactSync5_ = new message_filters::Synchronizer( + MyExactSync5Policy(queueSize_), + rgbd_image1_sub_, + rgbd_image2_sub_, + rgbd_image3_sub_, + rgbd_image4_sub_, + rgbd_image5_sub_); + exactSync5_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD5, this, _1, _2, _3, _4, _5)); + } } private: @@ -642,6 +728,7 @@ private: message_filters::Subscriber rgbd_image2_sub_; message_filters::Subscriber rgbd_image3_sub_; message_filters::Subscriber rgbd_image4_sub_; + message_filters::Subscriber rgbd_image5_sub_; typedef message_filters::sync_policies::ApproximateTime MyApproxSyncPolicy; message_filters::Synchronizer * approxSync_; @@ -659,6 +746,10 @@ private: message_filters::Synchronizer * approxSync4_; typedef message_filters::sync_policies::ExactTime MyExactSync4Policy; message_filters::Synchronizer * exactSync4_; + typedef message_filters::sync_policies::ApproximateTime MyApproxSync5Policy; + message_filters::Synchronizer * approxSync5_; + typedef message_filters::sync_policies::ExactTime MyExactSync5Policy; + message_filters::Synchronizer * exactSync5_; int queueSize_; bool keepColor_; }; From f1ef801df2e6825f6e37bac13ad0cc3157e89f13 Mon Sep 17 00:00:00 2001 From: ruipimentelfigueiredo Date: Thu, 8 Jul 2021 17:01:44 +0200 Subject: [PATCH 2/2] bug fix --- src/nodelets/rgbd_odometry.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/nodelets/rgbd_odometry.cpp b/src/nodelets/rgbd_odometry.cpp index 989753e7..724b26c9 100644 --- a/src/nodelets/rgbd_odometry.cpp +++ b/src/nodelets/rgbd_odometry.cpp @@ -162,6 +162,10 @@ private: { rgbd_image4_sub_.subscribe(nh, "rgbd_image3", 1); } + if(rgbdCameras >= 5) + { + rgbd_image5_sub_.subscribe(nh, "rgbd_image4", 1); + } if(rgbdCameras == 2) { @@ -268,13 +272,14 @@ private: rgbd_image5_sub_); exactSync5_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD5, this, _1, _2, _3, _4, _5)); } - subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s \\\n %s", + subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s \\\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()); + rgbd_image4_sub_.getTopic().c_str(), + rgbd_image5_sub_.getTopic().c_str()); } } @@ -605,7 +610,7 @@ private: rtabmap_ros::toCvShare(image2, imageMsgs[1], depthMsgs[1]); rtabmap_ros::toCvShare(image3, imageMsgs[2], depthMsgs[2]); rtabmap_ros::toCvShare(image4, imageMsgs[3], depthMsgs[3]); - rtabmap_ros::toCvShare(image4, imageMsgs[4], depthMsgs[4]); + rtabmap_ros::toCvShare(image5, imageMsgs[4], depthMsgs[4]); infoMsgs.push_back(image->rgb_camera_info); infoMsgs.push_back(image2->rgb_camera_info); infoMsgs.push_back(image3->rgb_camera_info);