i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added rgbd_odometry support for up to 4 cameras

Browse Source
This commit is contained in:
matlabbe
2017-10-30 21:45:49 +01:00
parent d7db848dad
commit 1058cafcd7
2 changed files with 208 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CameraNode.cpp
+1 -1
View File
@@ -90,7 +90,7 @@ public:
{
if(cameraThread_)
{
return cameraThread_->start();
cameraThread_->start();
}
}
src/nodelets/rgbd_odometry.cpp
+207 -20
View File
@@ -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>(
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>(
MyApproxSync2Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_);
approxSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2));
}
else
{
exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
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>(
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>(
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>(
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>(
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>(
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<cv_bridge::CvImageConstPtr> imageMsgs(3);
std::vector<cv_bridge::CvImageConstPtr> depthMsgs(3);
std::vector<sensor_msgs::CameraInfo> 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<cv_bridge::CvImageConstPtr> imageMsgs(4);
std::vector<cv_bridge::CvImageConstPtr> depthMsgs(4);
std::vector<sensor_msgs::CameraInfo> 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>(
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>(
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>(
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>(
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<rtabmap_ros::RGBDImage> rgbd_image1_sub_;
message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image2_sub_;
message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image3_sub_;
message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image4_sub_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
@@ -459,6 +638,14 @@ private:
message_filters::Synchronizer<MyApproxSync2Policy> * approxSync2_;
typedef message_filters::sync_policies::ExactTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyExactSync2Policy;
message_filters::Synchronizer<MyExactSync2Policy> * exactSync2_;
typedef message_filters::sync_policies::ApproximateTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyApproxSync3Policy;
message_filters::Synchronizer<MyApproxSync3Policy> * approxSync3_;
typedef message_filters::sync_policies::ExactTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyExactSync3Policy;
message_filters::Synchronizer<MyExactSync3Policy> * exactSync3_;
typedef message_filters::sync_policies::ApproximateTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyApproxSync4Policy;
message_filters::Synchronizer<MyApproxSync4Policy> * approxSync4_;
typedef message_filters::sync_policies::ExactTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyExactSync4Policy;
message_filters::Synchronizer<MyExactSync4Policy> * exactSync4_;
int queueSize_;
};