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

Added approx_sync_max_interval parameter to odometry and sync nodes.

Browse Source
This commit is contained in:
matlabbe
2022-03-04 18:01:20 -05:00
parent 8570fe3179
commit 07bf207ec3
13 changed files with 244 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/data_throttle.cpp
+9 -3
View File
@@ -89,6 +89,7 @@ private:
int queueSize = 10;
bool approxSync = true;
double approxSyncMaxInterval = 0.0;
if(private_nh.getParam("max_rate", rate_))
{
NODELET_WARN("\"max_rate\" is now known as \"rate\".");
@@ -96,15 +97,20 @@ private:
private_nh.param("rate", rate_, rate_);
private_nh.param("queue_size", queueSize, queueSize);
private_nh.param("approx_sync", approxSync, approxSync);
private_nh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
private_nh.param("decimation", decimation_, decimation_);
ROS_ASSERT(decimation_ >= 1);
NODELET_INFO("Rate=%f Hz", rate_);
NODELET_INFO("Decimation=%d", decimation_);
NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false");
NODELET_INFO("rate=%f Hz", rate_);
NODELET_INFO("decimation=%d", decimation_);
NODELET_INFO("approx_sync = %s", approxSync?"true":"false");
if(approxSync)
NODELET_INFO("approx_sync_max_interval = %f", approxSyncMaxInterval);
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), image_sub_, image_depth_sub_, info_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxSync_->registerCallback(boost::bind(&DataThrottleNodelet::callback, this, _1, _2, _3));
}
else
src/nodelets/point_cloud_aggregator.cpp
+14 -3
View File
@@ -102,10 +102,12 @@ private:
int queueSize = 5;
int count = 2;
bool approx=true;
double approxSyncMaxInterval = 0.0;
pnh.param("queue_size", queueSize, queueSize);
pnh.param("frame_id", frameId_, frameId_);
pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_);
pnh.param("approx_sync", approx, approx);
pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
pnh.param("count", count, count);
pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_);
pnh.param("xyz_output", xyzOutput_, xyzOutput_);
@@ -121,6 +123,8 @@ private:
if(approx)
{
approxSync4_ = new message_filters::Synchronizer<ApproxSync4Policy>(ApproxSync4Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_);
if(approxSyncMaxInterval > 0.0)
approxSync4_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxSync4_->registerCallback(boost::bind(&rtabmap_ros::PointCloudAggregator::clouds4_callback, this, _1, _2, _3, _4));
}
else
@@ -128,9 +132,10 @@ private:
exactSync4_ = new message_filters::Synchronizer<ExactSync4Policy>(ExactSync4Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_);
exactSync4_->registerCallback(boost::bind(&rtabmap_ros::PointCloudAggregator::clouds4_callback, this, _1, _2, _3, _4));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s",
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s,\n %s",
getName().c_str(),
approx?"approx":"exact",
approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
cloudSub_1_.getTopic().c_str(),
cloudSub_2_.getTopic().c_str(),
cloudSub_3_.getTopic().c_str(),
@@ -142,6 +147,8 @@ private:
if(approx)
{
approxSync3_ = new message_filters::Synchronizer<ApproxSync3Policy>(ApproxSync3Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_);
if(approxSyncMaxInterval > 0.0)
approxSync3_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxSync3_->registerCallback(boost::bind(&rtabmap_ros::PointCloudAggregator::clouds3_callback, this, _1, _2, _3));
}
else
@@ -149,9 +156,10 @@ private:
exactSync3_ = new message_filters::Synchronizer<ExactSync3Policy>(ExactSync3Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_);
exactSync3_->registerCallback(boost::bind(&rtabmap_ros::PointCloudAggregator::clouds3_callback, this, _1, _2, _3));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s",
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s",
getName().c_str(),
approx?"approx":"exact",
approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
cloudSub_1_.getTopic().c_str(),
cloudSub_2_.getTopic().c_str(),
cloudSub_3_.getTopic().c_str());
@@ -161,6 +169,8 @@ private:
if(approx)
{
approxSync2_ = new message_filters::Synchronizer<ApproxSync2Policy>(ApproxSync2Policy(queueSize), cloudSub_1_, cloudSub_2_);
if(approxSyncMaxInterval > 0.0)
approxSync2_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxSync2_->registerCallback(boost::bind(&rtabmap_ros::PointCloudAggregator::clouds2_callback, this, _1, _2));
}
else
@@ -168,9 +178,10 @@ private:
exactSync2_ = new message_filters::Synchronizer<ExactSync2Policy>(ExactSync2Policy(queueSize), cloudSub_1_, cloudSub_2_);
exactSync2_->registerCallback(boost::bind(&rtabmap_ros::PointCloudAggregator::clouds2_callback, this, _1, _2));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s",
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s",
getName().c_str(),
approx?"approx":"exact",
approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
cloudSub_1_.getTopic().c_str(),
cloudSub_2_.getTopic().c_str());
}
src/nodelets/point_cloud_xyz.cpp
+6
View File
@@ -103,7 +103,9 @@ private:
int queueSize = 10;
bool approxSync = true;
std::string roiStr;
double approxSyncMaxInterval = 0.0;
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
pnh.param("queue_size", queueSize, queueSize);
pnh.param("max_depth", maxDepth_, maxDepth_);
pnh.param("min_depth", minDepth_, minDepth_);
@@ -170,9 +172,13 @@ private:
if(approxSync)
{
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_);
if(approxSyncMaxInterval > 0.0)
approxSyncDepth_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxSyncDepth_->registerCallback(boost::bind(&PointCloudXYZ::callback, this, _1, _2));
approxSyncDisparity_ = new message_filters::Synchronizer<MyApproxSyncDisparityPolicy>(MyApproxSyncDisparityPolicy(queueSize), disparitySub_, disparityCameraInfoSub_);
if(approxSyncMaxInterval > 0.0)
approxSyncDisparity_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZ::callbackDisparity, this, _1, _2));
}
else
src/nodelets/point_cloud_xyzrgb.cpp
+8 -1
View File
@@ -111,7 +111,9 @@ private:
int queueSize = 10;
bool approxSync = true;
std::string roiStr;
double approxSyncMaxInterval = 0.0;
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
pnh.param("queue_size", queueSize, queueSize);
pnh.param("max_depth", maxDepth_, maxDepth_);
pnh.param("min_depth", minDepth_, minDepth_);
@@ -195,14 +197,19 @@ private:
if(approxSync)
{
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
if(approxSyncMaxInterval > 0.0)
approxSyncDepth_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxSyncDepth_->registerCallback(boost::bind(&PointCloudXYZRGB::depthCallback, this, _1, _2, _3));
approxSyncDisparity_ = new message_filters::Synchronizer<MyApproxSyncDisparityPolicy>(MyApproxSyncDisparityPolicy(queueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_);
if(approxSyncMaxInterval > 0.0)
approxSyncDisparity_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZRGB::disparityCallback, this, _1, _2, _3));
approxSyncStereo_ = new message_filters::Synchronizer<MyApproxSyncStereoPolicy>(MyApproxSyncStereoPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
if(approxSyncMaxInterval > 0.0)
approxSyncStereo_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxSyncStereo_->registerCallback(boost::bind(&PointCloudXYZRGB::stereoCallback, this, _1, _2, _3, _4));
}
else
src/nodelets/rgb_sync.cpp
+8 -1
View File
@@ -88,11 +88,15 @@ private:
int queueSize = 10;
bool approxSync = false;
double approxSyncMaxInterval = 0.0;
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
pnh.param("queue_size", queueSize, queueSize);
pnh.param("compressed_rate", compressedRate_, compressedRate_);
NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false");
if(approxSync)
NODELET_INFO("%s: approx_sync_max_interval = %f", getName().c_str(), approxSyncMaxInterval);
NODELET_INFO("%s: queue_size = %d", getName().c_str(), queueSize);
NODELET_INFO("%s: compressed_rate = %f", getName().c_str(), compressedRate_);
@@ -102,6 +106,8 @@ private:
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageSub_, cameraInfoSub_);
if(approxSyncMaxInterval > 0.0)
approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxSync_->registerCallback(boost::bind(&RgbSync::callback, this, _1, _2));
}
else
@@ -118,9 +124,10 @@ private:
imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image_rect"), 1, hintsRgb);
cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s",
std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s",
getName().c_str(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=std::numeric_limits<double>::max()?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
imageSub_.getTopic().c_str(),
cameraInfoSub_.getTopic().c_str());
src/nodelets/rgbd_odometry.cpp
+64 -9
View File
@@ -123,7 +123,9 @@ private:
int rgbdCameras = 1;
bool approxSync = true;
bool subscribeRGBD = false;
double approxSyncMaxInterval = 0.0;
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
pnh.param("queue_size", queueSize_, queueSize_);
pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD);
if(pnh.hasParam("depth_cameras"))
@@ -142,6 +144,8 @@ private:
pnh.param("keep_color", keepColor_, keepColor_);
NODELET_INFO("RGBDOdometry: approx_sync = %s", approxSync?"true":"false");
if(approxSync)
NODELET_INFO("RGBDOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval);
NODELET_INFO("RGBDOdometry: queue_size = %d", queueSize_);
NODELET_INFO("RGBDOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
NODELET_INFO("RGBDOdometry: rgbd_cameras = %d", rgbdCameras);
@@ -175,6 +179,8 @@ private:
MyApproxSync2Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync2_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2));
}
else
@@ -185,9 +191,10 @@ private:
rgbd_image2_sub_);
exactSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s",
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s",
getName().c_str(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
rgbd_image1_sub_.getTopic().c_str(),
rgbd_image2_sub_.getTopic().c_str());
}
@@ -200,6 +207,8 @@ private:
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync3_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxSync3_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD3, this, _1, _2, _3));
}
else
@@ -211,9 +220,10 @@ private:
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",
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s",
getName().c_str(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
rgbd_image1_sub_.getTopic().c_str(),
rgbd_image2_sub_.getTopic().c_str(),
rgbd_image3_sub_.getTopic().c_str());
@@ -228,6 +238,8 @@ private:
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync4_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, _1, _2, _3, _4));
}
else
@@ -240,9 +252,10 @@ private:
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",
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s",
getName().c_str(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
rgbd_image1_sub_.getTopic().c_str(),
rgbd_image2_sub_.getTopic().c_str(),
rgbd_image3_sub_.getTopic().c_str(),
@@ -258,7 +271,9 @@ private:
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_,
rgbd_image5_sub_);
rgbd_image5_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync5_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxSync5_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD5, this, _1, _2, _3, _4, _5));
}
else
@@ -269,12 +284,13 @@ private:
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_,
rgbd_image5_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 \\\n %s",
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s \\\n %s",
getName().c_str(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
rgbd_image1_sub_.getTopic().c_str(),
rgbd_image2_sub_.getTopic().c_str(),
rgbd_image3_sub_.getTopic().c_str(),
@@ -311,6 +327,8 @@ private:
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
}
else
@@ -319,9 +337,10 @@ private:
exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s",
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s",
getName().c_str(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
image_mono_sub_.getTopic().c_str(),
image_depth_sub_.getTopic().c_str(),
info_sub_.getTopic().c_str());
@@ -379,6 +398,7 @@ private:
cv::Mat depth;
pcl::PointCloud<pcl::PointXYZ> scanCloud;
std::vector<CameraModel> cameraModels;
double stampDiff = 0;
for(unsigned int i=0; i<rgbImages.size(); ++i)
{
if(!(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
@@ -392,13 +412,13 @@ private:
!(depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
depthImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
{
{
NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8,bgra8,rgba8 and "
"image_depth=32FC1,16UC1,mono16. Current rgb=%s and depth=%s",
rgbImages[i]->encoding.c_str(),
depthImages[i]->encoding.c_str());
return;
}
}
UASSERT_MSG(rgbImages[i]->image.cols == imageWidth && rgbImages[i]->image.rows == imageHeight,
uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
imageWidth,
@@ -429,6 +449,29 @@ private:
return;
}
if(i>0)
{
double stampDiff = fabs(rgbImages[i]->header.stamp.toSec() - rgbImages[i-1]->header.stamp.toSec());
if(stampDiff > 1.0/60.0)
{
static bool warningShown = false;
if(!warningShown)
{
NODELET_WARN("The time difference between cameras %d and %d is "
"high (diff=%fs, cam%d=%fs, cam%d=%fs). You may want "
"to set approx_sync_max_interval to reject bad synchronizations or use "
"approx_sync=false if streams have all the exact same timestamp. This "
"message is only printed once.",
i-1, i,
stampDiff,
i-1, rgbImages[i-1]->header.stamp.toSec(),
i, rgbImages[i]->header.stamp.toSec());
warningShown = true;
}
}
}
cv_bridge::CvImageConstPtr ptrImage = rgbImages[i];
if(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
@@ -507,6 +550,18 @@ private:
depthMsgs[0] = cv_bridge::toCvShare(depth);
infoMsgs.push_back(*cameraInfo);
double stampDiff = fabs(image->header.stamp.toSec() - depth->header.stamp.toSec());
if(stampDiff > 0.010)
{
NODELET_WARN("The time difference between rgb and depth frames is "
"high (diff=%fs, rgb=%fs, depth=%fs). You may want "
"to set approx_sync_max_interval lower than 0.01s to reject spurious bad synchronizations or use "
"approx_sync=false if streams have all the exact same timestamp.",
stampDiff,
image->header.stamp.toSec(),
depth->header.stamp.toSec());
}
this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
}
}
src/nodelets/rgbd_sync.cpp
+20 -1
View File
@@ -92,7 +92,9 @@ private:
int queueSize = 10;
bool approxSync = true;
double approxSyncMaxInterval = 0.0;
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
pnh.param("queue_size", queueSize, queueSize);
pnh.param("depth_scale", depthScale_, depthScale_);
pnh.param("decimation", decimation_, decimation_);
@@ -104,6 +106,8 @@ private:
}
NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false");
if(approxSync)
NODELET_INFO("%s: approx_sync_max_interval = %f", getName().c_str(), approxSyncMaxInterval);
NODELET_INFO("%s: queue_size = %d", getName().c_str(), queueSize);
NODELET_INFO("%s: depth_scale = %f", getName().c_str(), depthScale_);
NODELET_INFO("%s: decimation = %d", getName().c_str(), decimation_);
@@ -115,6 +119,8 @@ private:
if(approxSync)
{
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
if(approxSyncMaxInterval > 0.0)
approxSyncDepth_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, _1, _2, _3));
}
else
@@ -136,9 +142,10 @@ private:
imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s",
std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s",
getName().c_str(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
imageSub_.getTopic().c_str(),
imageDepthSub_.getTopic().c_str(),
cameraInfoSub_.getTopic().c_str());
@@ -178,6 +185,18 @@ private:
double depthStamp = depth->header.stamp.toSec();
double infoStamp = cameraInfo->header.stamp.toSec();
double stampDiff = fabs(rgbStamp - depthStamp);
if(stampDiff > 0.010)
{
NODELET_WARN("The time difference between rgb and depth frames is "
"high (diff=%fs, rgb=%fs, depth=%fs). You may want "
"to set approx_sync_max_interval lower than 0.01s to reject spurious bad synchronizations or use "
"approx_sync=false if streams have all the exact same timestamp.",
stampDiff,
rgbStamp,
depthStamp);
}
rtabmap_ros::RGBDImage msg;
msg.header.frame_id = cameraInfo->header.frame_id;
msg.header.stamp = image->header.stamp>depth->header.stamp?image->header.stamp:depth->header.stamp;
src/nodelets/rgbdicp_odometry.cpp
+24 -2
View File
@@ -110,7 +110,9 @@ private:
bool approxSync = true;
bool subscribeScanCloud = false;
double approxSyncMaxInterval = 0.0;
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
pnh.param("queue_size", queueSize_, queueSize_);
pnh.param("subscribe_scan_cloud", subscribeScanCloud, subscribeScanCloud);
pnh.param("scan_cloud_max_points", scanCloudMaxPoints_, scanCloudMaxPoints_);
@@ -126,6 +128,8 @@ private:
pnh.param("keep_color", keepColor_, keepColor_);
NODELET_INFO("RGBDIcpOdometry: approx_sync = %s", approxSync?"true":"false");
if(approxSync)
NODELET_INFO("RGBDIcpOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval);
NODELET_INFO("RGBDIcpOdometry: queue_size = %d", queueSize_);
NODELET_INFO("RGBDIcpOdometry: subscribe_scan_cloud = %s", subscribeScanCloud?"true":"false");
NODELET_INFO("RGBDIcpOdometry: scan_cloud_max_points = %d", scanCloudMaxPoints_);
@@ -154,6 +158,8 @@ private:
if(approxSync)
{
approxCloudSync_ = new message_filters::Synchronizer<MyApproxCloudSyncPolicy>(MyApproxCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_);
if(approxSyncMaxInterval > 0.0)
approxCloudSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, _1, _2, _3, _4));
}
else
@@ -162,9 +168,10 @@ private:
exactCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, _1, _2, _3, _4));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s, \n %s",
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s, \n %s",
getName().c_str(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
image_mono_sub_.getTopic().c_str(),
image_depth_sub_.getTopic().c_str(),
info_sub_.getTopic().c_str(),
@@ -176,6 +183,8 @@ private:
if(approxSync)
{
approxScanSync_ = new message_filters::Synchronizer<MyApproxScanSyncPolicy>(MyApproxScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_);
if(approxSyncMaxInterval > 0.0)
approxScanSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, _1, _2, _3, _4));
}
else
@@ -184,9 +193,10 @@ private:
exactScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, _1, _2, _3, _4));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s \\\n %s",
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s",
getName().c_str(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
image_mono_sub_.getTopic().c_str(),
image_depth_sub_.getTopic().c_str(),
info_sub_.getTopic().c_str(),
@@ -277,6 +287,18 @@ private:
return;
}
double stampDiff = fabs(image->header.stamp.toSec() - depth->header.stamp.toSec());
if(stampDiff > 0.010)
{
NODELET_WARN("The time difference between rgb and depth frames is "
"high (diff=%fs, rgb=%fs, depth=%fs). You may want "
"to set approx_sync_max_interval lower than 0.01s to reject spurious bad synchronizations or use "
"approx_sync=false if streams have all the exact same timestamp.",
stampDiff,
image->header.stamp.toSec(),
depth->header.stamp.toSec());
}
if(image->data.size() && depth->data.size() && cameraInfo->K[4] != 0)
{
rtabmap::CameraModel rtabmapModel = rtabmap_ros::cameraModelFromROS(*cameraInfo, localTransform);
src/nodelets/rgbdx_sync.cpp
+34 -1
View File
@@ -78,11 +78,15 @@ private:
int queueSize = 10;
bool approxSync = true;
int rgbdCameras = 2;
double approxSyncMaxInterval = 0.0;
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
pnh.param("queue_size", queueSize, queueSize);
pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras);
NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false");
if(approxSync)
NODELET_INFO("%s: approx_sync_max_interval = %f", getName().c_str(), approxSyncMaxInterval);
NODELET_INFO("%s: queue_size = %d", getName().c_str(), queueSize);
NODELET_INFO("%s: rgbd_cameras = %d", getName().c_str(), rgbdCameras);
@@ -102,34 +106,63 @@ private:
if(rgbdCameras==2)
{
SYNC_DECL2(RGBDXSync, rgbd2, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
if(approxSync && approxSyncMaxInterval>0.0)
{
rgbd2ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
}
}
else if(rgbdCameras==3)
{
SYNC_DECL3(RGBDXSync, rgbd3, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
if(approxSync && approxSyncMaxInterval>0.0)
{
rgbd3ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
}
}
else if(rgbdCameras==4)
{
SYNC_DECL4(RGBDXSync, rgbd4, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
if(approxSync && approxSyncMaxInterval>0.0)
{
rgbd4ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
}
}
else if(rgbdCameras==5)
{
SYNC_DECL5(RGBDXSync, rgbd5, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]));
if(approxSync && approxSyncMaxInterval>0.0)
{
rgbd5ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
}
}
else if(rgbdCameras==6)
{
SYNC_DECL6(RGBDXSync, rgbd6, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
if(approxSync && approxSyncMaxInterval>0.0)
{
rgbd6ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
}
}
else if(rgbdCameras==7)
{
SYNC_DECL7(RGBDXSync, rgbd7, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), (*rgbdSubs_[6]));
if(approxSync && approxSyncMaxInterval>0.0)
{
rgbd7ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
}
}
else if(rgbdCameras==8)
{
SYNC_DECL8(RGBDXSync, rgbd8, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), (*rgbdSubs_[6]), (*rgbdSubs_[7]));
if(approxSync && approxSyncMaxInterval>0.0)
{
rgbd8ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
}
}
warningThread_ = new boost::thread(boost::bind(&RGBDXSync::warningLoop, this, subscribedTopicsMsg_, approxSync));
NODELET_INFO("%s", subscribedTopicsMsg_.c_str());
NODELET_INFO("%s%s", subscribedTopicsMsg_.c_str(),
approxSync&&approxSyncMaxInterval!=0.0?uFormat(" (approx sync max interval=%fs)", approxSyncMaxInterval).c_str():"");
}
void warningLoop(const std::string & subscribedTopicsMsg, bool approxSync)
src/nodelets/stereo_odometry.cpp
+20 -1
View File
@@ -88,12 +88,16 @@ private:
bool approxSync = false;
bool subscribeRGBD = false;
double approxSyncMaxInterval = 0.0;
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
pnh.param("queue_size", queueSize_, queueSize_);
pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD);
pnh.param("keep_color", keepColor_, keepColor_);
NODELET_INFO("StereoOdometry: approx_sync = %s", approxSync?"true":"false");
if(approxSync)
NODELET_INFO("StereoOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval);
NODELET_INFO("StereoOdometry: queue_size = %d", queueSize_);
NODELET_INFO("StereoOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
NODELET_INFO("StereoOdometry: keep_color = %s", keepColor_?"true":"false");
@@ -127,6 +131,8 @@ private:
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
if(approxSyncMaxInterval>0.0)
approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
}
else
@@ -136,9 +142,10 @@ private:
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s \\\n %s",
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s",
getName().c_str(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
imageRectLeft_.getTopic().c_str(),
imageRectRight_.getTopic().c_str(),
cameraInfoLeft_.getTopic().c_str(),
@@ -196,6 +203,18 @@ private:
return;
}
double stampDiff = fabs(imageRectLeft->header.stamp.toSec() - imageRectRight->header.stamp.toSec());
if(stampDiff > 0.010)
{
NODELET_WARN("The time difference between left and right frames is "
"high (diff=%fs, left=%fs, right=%fs). If your left and right cameras are hardware "
"synchronized, use approx_sync:=false. Otherwise, you may want "
"to set approx_sync_max_interval lower than 0.01s to reject spurious bad synchronizations.",
stampDiff,
imageRectLeft->header.stamp.toSec(),
imageRectRight->header.stamp.toSec());
}
int quality = -1;
if(imageRectLeft->data.size() && imageRectRight->data.size())
{
src/nodelets/stereo_sync.cpp
+20 -1
View File
@@ -88,11 +88,15 @@ private:
int queueSize = 10;
bool approxSync = false;
double approxSyncMaxInterval = 0.0;
pnh.param("approx_sync", approxSync, approxSync);
if(approxSync)
pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
pnh.param("queue_size", queueSize, queueSize);
pnh.param("compressed_rate", compressedRate_, compressedRate_);
NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false");
NODELET_INFO("%s: approx_sync_max_interval = %f", getName().c_str(), approxSyncMaxInterval);
NODELET_INFO("%s: queue_size = %d", getName().c_str(), queueSize);
NODELET_INFO("%s: compressed_rate = %f", getName().c_str(), compressedRate_);
@@ -102,6 +106,8 @@ private:
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageLeftSub_, imageRightSub_, cameraInfoLeftSub_, cameraInfoRightSub_);
if(approxSyncMaxInterval>0.0)
approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxSync_->registerCallback(boost::bind(&StereoSync::callback, this, _1, _2, _3, _4));
}
else
@@ -124,9 +130,10 @@ private:
cameraInfoLeftSub_.subscribe(left_nh, "camera_info", 1);
cameraInfoRightSub_.subscribe(right_nh, "camera_info", 1);
std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s \\\n %s",
std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s",
getName().c_str(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
imageLeftSub_.getTopic().c_str(),
imageRightSub_.getTopic().c_str(),
cameraInfoLeftSub_.getTopic().c_str(),
@@ -169,6 +176,18 @@ private:
double leftInfoStamp = cameraInfoLeft->header.stamp.toSec();
double rightInfoStamp = cameraInfoRight->header.stamp.toSec();
double stampDiff = fabs(leftStamp - rightStamp);
if(stampDiff > 0.010)
{
NODELET_WARN("The time difference between left and right frames is "
"high (diff=%fs, left=%fs, right=%fs). If your left and right cameras are hardware "
"synchronized, use approx_sync:=false. Otherwise, you may want "
"to set approx_sync_max_interval lower than 0.01s to reject spurious bad synchronizations.",
stampDiff,
leftStamp,
rightStamp);
}
rtabmap_ros::RGBDImage msg;
msg.header.frame_id = cameraInfoLeft->header.frame_id;
msg.header.stamp = imageLeft->header.stamp>imageRight->header.stamp?imageLeft->header.stamp:imageRight->header.stamp;
src/nodelets/stereo_throttle.cpp
+9 -3
View File
@@ -88,18 +88,24 @@ private:
int queueSize = 5;
bool approxSync = false;
double approxSyncMaxInterval = 0.0;
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
pnh.param("rate", rate_, rate_);
pnh.param("queue_size", queueSize, queueSize);
pnh.param("decimation", decimation_, decimation_);
ROS_ASSERT(decimation_ >= 1);
NODELET_INFO("Rate=%f Hz", rate_);
NODELET_INFO("Decimation=%d", decimation_);
NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false");
NODELET_INFO("rate=%f Hz", rate_);
NODELET_INFO("decimation=%d", decimation_);
NODELET_INFO("approx_sync = %s", approxSync?"true":"false");
if(approxSync)
NODELET_INFO("approx_sync_max_interval = %f", approxSyncMaxInterval);
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
if(approxSyncMaxInterval>0.0)
approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxSync_->registerCallback(boost::bind(&StereoThrottleNodelet::callback, this, _1, _2, _3, _4));
}
else