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

merged master->ros2

Browse Source
This commit is contained in:
matlabbe
2022-10-01 18:09:19 -07:00
parent 4fb73b4279 5024574e4a
commit 99b9891ab8
57 changed files with 1988 additions and 1096 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,16 +97,21 @@ 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_);
approxSync_->registerCallback(std::bind(&DataThrottleNodelet::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
if(approxSyncMaxInterval > 0.0)
approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
}
else
{
src/nodelets/point_cloud_aggregator.cpp
+115 -11
View File
@@ -49,7 +49,8 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
approxSync3_(0),
exactSync2_(0),
approxSync2_(0),
waitForTransform_(0.1)
waitForTransform_(0.1),
xyzOutput_(false)
{
tfBuffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
//auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
@@ -61,14 +62,17 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
int queueSize = 5;
int count = 2;
bool approx=true;
double approxSyncMaxInterval = 0.0;
int qos;
queueSize = this->declare_parameter("queue_size", queueSize);
qos = this->declare_parameter("qos", qos);
frameId_ = this->declare_parameter("frame_id", frameId_);
fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_);
approx = this->declare_parameter("approx_sync", approx);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
count = this->declare_parameter("count", count);
waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_);
xyzOutput_ = this->declare_parameter("xyz_output", xyzOutput_);
cloudPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("combined_cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
@@ -83,6 +87,8 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
if(approx)
{
approxSync4_ = new message_filters::Synchronizer<ApproxSync4Policy>(ApproxSync4Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_);
if(approxSyncMaxInterval > 0.0)
approxSync4_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync4_->registerCallback(std::bind(&rtabmap_ros::PointCloudAggregator::clouds4_callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
else
@@ -90,9 +96,10 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
exactSync4_ = new message_filters::Synchronizer<ExactSync4Policy>(ExactSync4Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_);
exactSync4_->registerCallback(std::bind(&rtabmap_ros::PointCloudAggregator::clouds4_callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_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",
get_name(),
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(),
@@ -104,6 +111,8 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
if(approx)
{
approxSync3_ = new message_filters::Synchronizer<ApproxSync3Policy>(ApproxSync3Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_);
if(approxSyncMaxInterval > 0.0)
approxSync3_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync3_->registerCallback(std::bind(&rtabmap_ros::PointCloudAggregator::clouds3_callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
else
@@ -111,9 +120,10 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
exactSync3_ = new message_filters::Synchronizer<ExactSync3Policy>(ExactSync3Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_);
exactSync3_->registerCallback(std::bind(&rtabmap_ros::PointCloudAggregator::clouds3_callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_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",
this->get_name(),
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());
@@ -123,6 +133,8 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
if(approx)
{
approxSync2_ = new message_filters::Synchronizer<ApproxSync2Policy>(ApproxSync2Policy(queueSize), cloudSub_1_, cloudSub_2_);
if(approxSyncMaxInterval > 0.0)
approxSync2_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync2_->registerCallback(std::bind(&rtabmap_ros::PointCloudAggregator::clouds2_callback, this, std::placeholders::_1, std::placeholders::_2));
}
else
@@ -130,9 +142,10 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
exactSync2_ = new message_filters::Synchronizer<ExactSync2Policy>(ExactSync2Policy(queueSize), cloudSub_1_, cloudSub_2_);
exactSync2_->registerCallback(std::bind(&rtabmap_ros::PointCloudAggregator::clouds2_callback, this, std::placeholders::_1, std::placeholders::_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",
this->get_name(),
approx?"approx":"exact",
approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
cloudSub_1_.getTopic().c_str(),
cloudSub_2_.getTopic().c_str());
}
@@ -233,6 +246,42 @@ void PointCloudAggregator::combineClouds(const std::vector<sensor_msgs::msg::Poi
frameId = cloudMsgs[0]->header.frame_id;
}
if(xyzOutput_ && !output->data.empty())
{
// convert only if not already XYZ cloud
bool hasField[4] = {false};
for(size_t i=0; i<output->fields.size(); ++i)
{
if(output->fields[i].name.compare("x") == 0)
{
hasField[0] = true;
}
else if(output->fields[i].name.compare("y") == 0)
{
hasField[1] = true;
}
else if(output->fields[i].name.compare("z") == 0)
{
hasField[2] = true;
}
else
{
hasField[3] = true; // other
break;
}
}
if(hasField[0] && hasField[1] && hasField[2] && !hasField[3])
{
// do nothing, already XYZ
}
else
{
pcl::PointCloud<pcl::PointXYZ> cloudxyz;
pcl::fromPCLPointCloud2(*output, cloudxyz);
pcl::toPCLPointCloud2(cloudxyz, *output);
}
}
for(unsigned int i=1; i<cloudMsgs.size(); ++i)
{
rtabmap::Transform cloudDisplacement;
@@ -286,15 +335,71 @@ void PointCloudAggregator::combineClouds(const std::vector<sensor_msgs::msg::Poi
cloud2 = rtabmap::util3d::removeNaNFromPointCloud(cloud2);
}
pcl::PCLPointCloud2::Ptr tmp_output(new pcl::PCLPointCloud2);
if(xyzOutput_ && !cloud2->data.empty())
{
// convert only if not already XYZ cloud
bool hasField[4] = {false};
for(size_t i=0; i<cloud2->fields.size(); ++i)
{
if(cloud2->fields[i].name.compare("x") == 0)
{
hasField[0] = true;
}
else if(cloud2->fields[i].name.compare("y") == 0)
{
hasField[1] = true;
}
else if(cloud2->fields[i].name.compare("z") == 0)
{
hasField[2] = true;
}
else
{
hasField[3] = true; // other
break;
}
}
if(hasField[0] && hasField[1] && hasField[2] && !hasField[3])
{
// do nothing, already XYZ
}
else
{
pcl::PointCloud<pcl::PointXYZ> cloudxyz;
pcl::fromPCLPointCloud2(*cloud2, cloudxyz);
pcl::toPCLPointCloud2(cloudxyz, *cloud2);
}
}
if(output->data.empty())
{
output = cloud2;
}
else if(!cloud2->data.empty())
{
if(output->fields.size() != cloud2->fields.size())
{
RCLCPP_WARN(this->get_logger(), "%s: Input topics don't have all the "
"same number of fields (cloud1=%d, cloud%d=%d), concatenation "
"may fails. You can enable \"xyz_output\" option "
"to convert all inputs to XYZ.",
get_name(),
(int)output->fields.size(),
i+1,
(int)output->fields.size());
}
pcl::PCLPointCloud2::Ptr tmp_output(new pcl::PCLPointCloud2);
#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
pcl::concatenate(*output, *cloud2, *tmp_output);
pcl::concatenate(*output, *cloud2, *tmp_output);
#else
pcl::concatenatePointCloud(*output, *cloud2, *tmp_output);
pcl::concatenatePointCloud(*output, *cloud2, *tmp_output);
#endif
//Make sure row_step is the sum of both
tmp_output->row_step = tmp_output->width * tmp_output->point_step;
output = tmp_output;
//Make sure row_step is the sum of both
tmp_output->row_step = tmp_output->width * tmp_output->point_step;
output = tmp_output;
}
}
sensor_msgs::msg::PointCloud2::UniquePtr rosCloud(new sensor_msgs::msg::PointCloud2);
@@ -304,7 +409,6 @@ void PointCloudAggregator::combineClouds(const std::vector<sensor_msgs::msg::Poi
cloudPub_->publish(std::move(rosCloud));
}
}
}
#include "rclcpp_components/register_node_macro.hpp"
src/nodelets/point_cloud_xyz.cpp
+62 -17
View File
@@ -66,7 +66,9 @@ PointCloudXYZ::PointCloudXYZ(const rclcpp::NodeOptions & options) :
int qos = 0;
bool approxSync = true;
std::string roiStr;
double approxSyncMaxInterval = 0.0;
approxSync = this->declare_parameter("approx_sync", approxSync);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
queueSize = this->declare_parameter("queue_size", queueSize);
qos = this->declare_parameter("qos", qos);
int qosCamInfo = this->declare_parameter("qos_camera_info", qos);
@@ -119,9 +121,13 @@ PointCloudXYZ::PointCloudXYZ(const rclcpp::NodeOptions & options) :
if(approxSync)
{
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_);
if(approxSyncMaxInterval > 0.0)
approxSyncDepth_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSyncDepth_->registerCallback(std::bind(&PointCloudXYZ::callback, this, std::placeholders::_1, std::placeholders::_2));
approxSyncDisparity_ = new message_filters::Synchronizer<MyApproxSyncDisparityPolicy>(MyApproxSyncDisparityPolicy(queueSize), disparitySub_, disparityCameraInfoSub_);
if(approxSyncMaxInterval > 0.0)
approxSyncDisparity_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSyncDisparity_->registerCallback(std::bind(&PointCloudXYZ::callbackDisparity, this, std::placeholders::_1, std::placeholders::_2));
}
else
@@ -151,12 +157,12 @@ PointCloudXYZ::~PointCloudXYZ()
delete exactSyncDisparity_;
}
void PointCloudXYZ::callback(
const sensor_msgs::msg::Image::ConstSharedPtr depth,
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
{
if(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 &&
depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0 &&
depth->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0)
if(depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 &&
depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0 &&
depthMsg->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0)
{
RCLCPP_ERROR(this->get_logger(), "Input type depth=32FC1,16UC1,MONO16");
return;
@@ -166,29 +172,68 @@ void PointCloudXYZ::callback(
{
rclcpp::Time time = now();
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth);
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depthMsg);
cv::Rect roi = rtabmap::util2d::computeRoi(imageDepthPtr->image, roiRatios_);
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfo);
rtabmap::CameraModel model = cameraModelFromROS(*cameraInfo);
pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
rtabmap::CameraModel m(
model.fx(),
model.fy(),
model.cx()-roiRatios_[0]*double(imageDepthPtr->image.cols),
model.cy()-roiRatios_[2]*double(imageDepthPtr->image.rows));
cv::Mat depth = imageDepthPtr->image;
if( roiRatios_.size() == 4 &&
((roiRatios_[0] > 0.0f && roiRatios_[0] <= 1.0f) ||
(roiRatios_[1] > 0.0f && roiRatios_[1] <= 1.0f) ||
(roiRatios_[2] > 0.0f && roiRatios_[2] <= 1.0f) ||
(roiRatios_[3] > 0.0f && roiRatios_[3] <= 1.0f)))
{
cv::Rect roiDepth = rtabmap::util2d::computeRoi(depth, roiRatios_);
cv::Rect roiRgb;
if(model.imageWidth() && model.imageHeight())
{
roiRgb = rtabmap::util2d::computeRoi(model.imageSize(), roiRatios_);
}
if( roiDepth.width%decimation_==0 &&
roiDepth.height%decimation_==0 &&
(roiRgb.width != 0 ||
(roiRgb.width%decimation_==0 &&
roiRgb.height%decimation_==0)))
{
depth = cv::Mat(depth, roiDepth);
if(model.imageWidth() != 0 && model.imageHeight() != 0)
{
model = model.roi(roiRgb);
}
else
{
model = model.roi(roiDepth);
}
}
else
{
RCLCPP_ERROR(this->get_logger(), "Cannot apply ROI ratios [%f,%f,%f,%f] because resulting "
"dimension (depth=%dx%d rgb=%dx%d) cannot be divided exactly "
"by decimation parameter (%d). Ignoring ROI ratios...",
roiRatios_[0],
roiRatios_[1],
roiRatios_[2],
roiRatios_[3],
roiDepth.width,
roiDepth.height,
roiRgb.width,
roiRgb.height,
decimation_);
}
}
pcl::IndicesPtr indices(new std::vector<int>);
pclCloud = rtabmap::util3d::cloudFromDepth(
cv::Mat(imageDepthPtr->image, roi),
m,
depth,
model,
decimation_,
maxDepth_,
minDepth_,
indices.get());
processAndPublish(pclCloud, indices, depth->header);
processAndPublish(pclCloud, indices, depthMsg->header);
RCLCPP_DEBUG(this->get_logger(), "point_cloud_xyz from depth time = %f s", (now() - time).seconds());
}
}
@@ -222,6 +267,7 @@ void PointCloudXYZ::callbackDisparity(
pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
rtabmap::CameraModel leftModel = rtabmap_ros::cameraModelFromROS(*cameraInfo);
UASSERT(disparity.cols == leftModel.imageWidth() && disparity.rows == leftModel.imageHeight());
rtabmap::StereoCameraModel stereoModel(disparityMsg->f, disparityMsg->f, leftModel.cx()-roiRatios_[0]*double(disparity.cols), leftModel.cy()-roiRatios_[2]*double(disparity.rows), disparityMsg->t);
pcl::IndicesPtr indices(new std::vector<int>);
pclCloud = rtabmap::util3d::cloudFromDisparity(
@@ -233,7 +279,6 @@ void PointCloudXYZ::callbackDisparity(
indices.get());
processAndPublish(pclCloud, indices, disparityMsg->header);
RCLCPP_DEBUG(this->get_logger(), "point_cloud_xyz from disparity time = %f s", (now() - time).seconds());
}
}
src/nodelets/point_cloud_xyzrgb.cpp
+52 -16
View File
@@ -69,7 +69,9 @@ PointCloudXYZRGB::PointCloudXYZRGB(const rclcpp::NodeOptions & options) :
std::string roiStr;
int queueSize = 10;
int qos = 0;
double approxSyncMaxInterval = 0.0;
approxSync = this->declare_parameter("approx_sync", approxSync);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
queueSize = this->declare_parameter("queue_size", queueSize);
qos = this->declare_parameter("qos", qos);
int qosCamInfo = this->declare_parameter("qos_camera_info", qos);
@@ -139,12 +141,18 @@ PointCloudXYZRGB::PointCloudXYZRGB(const rclcpp::NodeOptions & options) :
{
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
if(approxSyncMaxInterval > 0.0)
approxSyncDepth_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSyncDepth_->registerCallback(std::bind(&PointCloudXYZRGB::depthCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
approxSyncDisparity_ = new message_filters::Synchronizer<MyApproxSyncDisparityPolicy>(MyApproxSyncDisparityPolicy(queueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_);
if(approxSyncMaxInterval > 0.0)
approxSyncDisparity_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSyncDisparity_->registerCallback(std::bind(&PointCloudXYZRGB::disparityCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
approxSyncStereo_ = new message_filters::Synchronizer<MyApproxSyncStereoPolicy>(MyApproxSyncStereoPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
if(approxSyncMaxInterval > 0.0)
approxSyncStereo_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSyncStereo_->registerCallback(std::bind(&PointCloudXYZRGB::stereoCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
else
@@ -224,31 +232,56 @@ void PointCloudXYZRGB::depthCallback(
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth);
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfo);
UASSERT(imageDepthPtr->image.cols == imagePtr->image.cols);
UASSERT(imageDepthPtr->image.rows == imagePtr->image.rows);
rtabmap::CameraModel model = cameraModelFromROS(*cameraInfo);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
cv::Rect roi = rtabmap::util2d::computeRoi(imageDepthPtr->image, roiRatios_);
rtabmap::CameraModel m(
model.fx(),
model.fy(),
model.cx()-roiRatios_[0]*double(imageDepthPtr->image.cols),
model.cy()-roiRatios_[2]*double(imageDepthPtr->image.rows));
cv::Mat rgb = imagePtr->image;
cv::Mat depth = imageDepthPtr->image;
if( roiRatios_.size() == 4 &&
((roiRatios_[0] > 0.0f && roiRatios_[0] <= 1.0f) ||
(roiRatios_[1] > 0.0f && roiRatios_[1] <= 1.0f) ||
(roiRatios_[2] > 0.0f && roiRatios_[2] <= 1.0f) ||
(roiRatios_[3] > 0.0f && roiRatios_[3] <= 1.0f)))
{
cv::Rect roiDepth = rtabmap::util2d::computeRoi(depth, roiRatios_);
cv::Rect roiRgb = rtabmap::util2d::computeRoi(rgb, roiRatios_);
if( roiDepth.width%decimation_==0 &&
roiDepth.height%decimation_==0 &&
roiRgb.width%decimation_==0 &&
roiRgb.height%decimation_==0)
{
depth = cv::Mat(depth, roiDepth);
rgb = cv::Mat(rgb, roiRgb);
model = model.roi(roiRgb);
}
else
{
RCLCPP_ERROR(this->get_logger(), "Cannot apply ROI ratios [%f,%f,%f,%f] because resulting "
"dimension (depth=%dx%d rgb=%dx%d) cannot be divided exactly "
"by decimation parameter (%d). Ignoring ROI ratios...",
roiRatios_[0],
roiRatios_[1],
roiRatios_[2],
roiRatios_[3],
roiDepth.width,
roiDepth.height,
roiRgb.width,
roiRgb.height,
decimation_);
}
}
pcl::IndicesPtr indices(new std::vector<int>);
pclCloud = rtabmap::util3d::cloudFromDepthRGB(
cv::Mat(imagePtr->image, roi),
cv::Mat(imageDepthPtr->image, roi),
m,
rgb,
depth,
model,
decimation_,
maxDepth_,
minDepth_,
indices.get());
processAndPublish(pclCloud, indices, imagePtr->header);
RCLCPP_DEBUG(this->get_logger(), "point_cloud_xyzrgb from RGB-D time = %f s", (now() - time).seconds());
@@ -300,6 +333,8 @@ void PointCloudXYZRGB::disparityCallback(
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
rtabmap::CameraModel leftModel = rtabmap_ros::cameraModelFromROS(*cameraInfo);
UASSERT(disparity.cols == leftModel.imageWidth() && disparity.rows == leftModel.imageHeight());
UASSERT(imagePtr->image.cols == leftModel.imageWidth() && imagePtr->image.rows == leftModel.imageHeight());
rtabmap::StereoCameraModel stereoModel(imageDisparity->f, imageDisparity->f, leftModel.cx()-roiRatios_[0]*double(disparity.cols), leftModel.cy()-roiRatios_[2]*double(disparity.rows), imageDisparity->t);
pcl::IndicesPtr indices(new std::vector<int>);
pclCloud = rtabmap::util3d::cloudFromDisparityRGB(
@@ -397,7 +432,8 @@ void PointCloudXYZRGB::rgbdImageCallback(
maxDepth_,
minDepth_,
indices.get(),
stereoBMParameters_);
stereoBMParameters_,
roiRatios_);
processAndPublish(pclCloud, indices, image->header);
}
src/nodelets/pointcloud_to_depthimage.cpp
+14 -2
View File
@@ -95,6 +95,8 @@ PointCloudToDepthImage::PointCloudToDepthImage(const rclcpp::NodeOptions & optio
depthImage16Pub_ = image_transport::create_camera_publisher(node.get(), "image_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); // 16 bits unsigned in mm
depthImage32Pub_ = image_transport::create_camera_publisher(node.get(), "image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());// 32 bits float in meters
pointCloudTransformedPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("cloud_transformed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
cameraInfo16Pub_ = create_publisher<sensor_msgs::msg::CameraInfo>(depthImage16Pub_.getTopic()+"/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo));
cameraInfo32Pub_ = create_publisher<sensor_msgs::msg::CameraInfo>(depthImage32Pub_.getTopic()+"/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo));
if(approx)
{
@@ -160,12 +162,15 @@ void PointCloudToDepthImage::callback(
rtabmap::Transform localTransform = cloudDisplacement*cloudToCamera;
rtabmap::CameraModel model = rtabmap_ros::cameraModelFromROS(*cameraInfoMsg, localTransform);
sensor_msgs::msg::CameraInfo cameraInfoMsgOut = *cameraInfoMsg;
if(decimation_ > 1)
{
if(model.imageWidth()%decimation_ == 0 && model.imageHeight()%decimation_ == 0)
{
model = model.scaled(1.0f/float(decimation_));
float scale = 1.0f/float(decimation_);
model = model.scaled(scale);
rtabmap_ros::cameraModelToROS(model, cameraInfoMsgOut);
}
else
{
@@ -216,6 +221,10 @@ void PointCloudToDepthImage::callback(
{
depthImage.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
depthImage32Pub_.publish(depthImage.toImageMsg(), cameraInfoMsg);
if(cameraInfo32Pub_->get_subscription_count())
{
cameraInfo32Pub_->publish(cameraInfoMsgOut);
}
}
if(depthImage16Pub_.getNumSubscribers())
@@ -223,6 +232,10 @@ void PointCloudToDepthImage::callback(
depthImage.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
depthImage.image = rtabmap::util2d::cvtDepthFromFloat(depthImage.image);
depthImage16Pub_.publish(depthImage.toImageMsg(), cameraInfoMsg);
if(cameraInfo16Pub_->get_subscription_count())
{
cameraInfo16Pub_->publish(cameraInfoMsgOut);
}
}
if( cloudStamp != timestampFromROS(pointCloud2Msg->header.stamp) ||
@@ -237,7 +250,6 @@ void PointCloudToDepthImage::callback(
}
}
}
}
#include "rclcpp_components/register_node_macro.hpp"
src/nodelets/rgb_sync.cpp
+8 -1
View File
@@ -52,13 +52,17 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) :
int queueSize = 10;
bool approxSync = true;
int qos = 0;
double approxSyncMaxInterval = 0.0;
approxSync = this->declare_parameter("approx_sync", approxSync);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
queueSize = this->declare_parameter("queue_size", queueSize);
qos = this->declare_parameter("qos", qos);
int qosCaminfo = this->declare_parameter("qos_camera_info", qos);
compressedRate_ = this->declare_parameter("compressed_rate", compressedRate_);
RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false");
if(approxSync)
RCLCPP_INFO(this->get_logger(), "%s: approx_sync_max_interval = %f", get_name(), approxSyncMaxInterval);
RCLCPP_INFO(this->get_logger(), "%s: queue_size = %d", get_name(), queueSize);
RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos);
RCLCPP_INFO(this->get_logger(), "%s: qos_camera_info = %d", get_name(), qosCaminfo);
@@ -70,6 +74,8 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) :
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageSub_, cameraInfoSub_);
if(approxSyncMaxInterval > 0.0)
approxSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync_->registerCallback(std::bind(&RGBSync::callback, this, std::placeholders::_1, std::placeholders::_2));
}
else
@@ -82,9 +88,10 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) :
imageSub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cameraInfoSub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCaminfo).get_rmw_qos_profile());
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",
get_name(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=std::numeric_limits<double>::max()?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
imageSub_.getSubscriber().getTopic().c_str(),
cameraInfoSub_.getSubscriber()->get_topic_name());
src/nodelets/rgbd_odometry.cpp
+109 -23
View File
@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <sensor_msgs/image_encodings.hpp>
#include "rtabmap_ros/MsgConversion.h"
#include <rtabmap_ros/msg/rgbd_images.hpp>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util2d.h>
@@ -72,6 +73,8 @@ RGBDOdometry::~RGBDOdometry()
delete exactSync3_;
delete approxSync4_;
delete exactSync4_;
delete approxSync5_;
delete exactSync5_;
}
void RGBDOdometry::onOdomInit()
@@ -79,7 +82,9 @@ void RGBDOdometry::onOdomInit()
int rgbdCameras = 1;
bool approxSync = true;
bool subscribeRGBD = false;
double approxSyncMaxInterval = 0.0;
approxSync = this->declare_parameter("approx_sync", approxSync);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
queueSize_ = this->declare_parameter("queue_size", queueSize_);
int qosCamInfo = this->declare_parameter("qos_camera_info", (int)qos());
subscribeRGBD = this->declare_parameter("subscribe_rgbd", subscribeRGBD);
@@ -90,11 +95,13 @@ void RGBDOdometry::onOdomInit()
}
if(rgbdCameras > 5)
{
RCLCPP_FATAL(this->get_logger(), "Only 5 cameras maximum supported yet.");
RCLCPP_FATAL(this->get_logger(), "Only 5 cameras maximum supported yet. Set 0 to use rgbd_images input (for which rgbdx_sync node can sync up to 8 cameras).");
}
keepColor_ = this->declare_parameter("keep_color", keepColor_);
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: approx_sync = %s", approxSync?"true":"false");
if(approxSync)
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval);
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: queue_size = %d", queueSize_);
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: qos = %d", (int)qos());
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: qos_camera_info = %d", qosCamInfo);
@@ -107,19 +114,19 @@ void RGBDOdometry::onOdomInit()
{
if(rgbdCameras >= 2)
{
rgbd_image1_sub_.subscribe(this, "rgbd_image0", rclcpp::QoS(queueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(queueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
rgbd_image1_sub_.subscribe(this, "rgbd_image0", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
if(rgbdCameras >= 3)
{
rgbd_image3_sub_.subscribe(this, "rgbd_image2", rclcpp::QoS(queueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
rgbd_image3_sub_.subscribe(this, "rgbd_image2", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
}
if(rgbdCameras >= 4)
{
rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(queueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
}
if(rgbdCameras >= 5)
{
rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(queueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
}
if(rgbdCameras == 2)
@@ -130,6 +137,8 @@ void RGBDOdometry::onOdomInit()
MyApproxSync2Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync2_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync2_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
}
else
@@ -140,9 +149,10 @@ void RGBDOdometry::onOdomInit()
rgbd_image2_sub_);
exactSync2_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_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",
get_name(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
rgbd_image1_sub_.getSubscriber()->get_topic_name(),
rgbd_image2_sub_.getSubscriber()->get_topic_name());
}
@@ -155,6 +165,8 @@ void RGBDOdometry::onOdomInit()
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync3_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync3_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD3, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
else
@@ -166,9 +178,10 @@ void RGBDOdometry::onOdomInit()
rgbd_image3_sub_);
exactSync3_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD3, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_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",
get_name(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
rgbd_image1_sub_.getSubscriber()->get_topic_name(),
rgbd_image2_sub_.getSubscriber()->get_topic_name(),
rgbd_image3_sub_.getSubscriber()->get_topic_name());
@@ -183,6 +196,8 @@ void RGBDOdometry::onOdomInit()
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync4_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync4_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD4, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
else
@@ -195,9 +210,10 @@ void RGBDOdometry::onOdomInit()
rgbd_image4_sub_);
exactSync4_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD4, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_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",
get_name(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
rgbd_image1_sub_.getSubscriber()->get_topic_name(),
rgbd_image2_sub_.getSubscriber()->get_topic_name(),
rgbd_image3_sub_.getSubscriber()->get_topic_name(),
@@ -214,6 +230,8 @@ void RGBDOdometry::onOdomInit()
rgbd_image3_sub_,
rgbd_image4_sub_,
rgbd_image5_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync5_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync5_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD5, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5));
}
else
@@ -227,9 +245,10 @@ void RGBDOdometry::onOdomInit()
rgbd_image5_sub_);
exactSync5_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD5, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_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",
get_name(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
rgbd_image1_sub_.getSubscriber()->get_topic_name(),
rgbd_image2_sub_.getSubscriber()->get_topic_name(),
rgbd_image3_sub_.getSubscriber()->get_topic_name(),
@@ -237,9 +256,17 @@ void RGBDOdometry::onOdomInit()
rgbd_image5_sub_.getSubscriber()->get_topic_name());
}
}
else if(rgbdCameras == 0)
{
rgbdxSub_ = create_subscription<rtabmap_ros::msg::RGBDImages>("rgbd_images", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBDX, this, std::placeholders::_1));
subscribedTopicsMsg = uFormat("\n%s subscribed to:\n %s",
get_name(),
rgbdxSub_->get_topic_name());
}
else
{
rgbdSub_ = create_subscription<rtabmap_ros::msg::RGBDImage>("rgbd_image", rclcpp::QoS(queueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBD, this, std::placeholders::_1));
rgbdSub_ = create_subscription<rtabmap_ros::msg::RGBDImage>("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBD, this, std::placeholders::_1));
subscribedTopicsMsg =
uFormat("\n%s subscribed to:\n %s",
@@ -250,13 +277,15 @@ void RGBDOdometry::onOdomInit()
else
{
image_transport::TransportHints hints(this);
image_mono_sub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(queueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
image_depth_sub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(queueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
info_sub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(queueSize_).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
image_mono_sub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
image_depth_sub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
info_sub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync_->registerCallback(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
else
@@ -265,9 +294,10 @@ void RGBDOdometry::onOdomInit()
exactSync_->registerCallback(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_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",
get_name(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
image_mono_sub_.getSubscriber().getTopic().c_str(),
image_depth_sub_.getSubscriber().getTopic().c_str(),
info_sub_.getSubscriber()->get_topic_name());
@@ -322,8 +352,7 @@ void RGBDOdometry::commonCallback(
int cameraCount = rgbImages.size();
cv::Mat rgb;
cv::Mat depth;
pcl::PointCloud<pcl::PointXYZ> scanCloud;
std::vector<CameraModel> cameraModels;
std::vector<rtabmap::CameraModel> cameraModels;
for(unsigned int i=0; i<rgbImages.size(); ++i)
{
if(!(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
@@ -374,6 +403,28 @@ void RGBDOdometry::commonCallback(
return;
}
if(i>0)
{
double stampDiff = fabs(timestampFromROS(rgbImages[i]->header.stamp) - timestampFromROS(rgbImages[i-1]->header.stamp));
if(stampDiff > 1.0/60.0)
{
static bool warningShown = false;
if(!warningShown)
{
RCLCPP_WARN(this->get_logger(), "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, timestampFromROS(rgbImages[i-1]->header.stamp),
i, timestampFromROS(rgbImages[i]->header.stamp));
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)
@@ -389,7 +440,6 @@ void RGBDOdometry::commonCallback(
}
cv_bridge::CvImageConstPtr ptrDepth = depthImages[i];
cv::Mat subDepth = ptrDepth->image;
// initialize
if(rgb.empty())
@@ -398,7 +448,7 @@ void RGBDOdometry::commonCallback(
}
if(depth.empty())
{
depth = cv::Mat(depthHeight, depthWidth*cameraCount, subDepth.type());
depth = cv::Mat(depthHeight, depthWidth*cameraCount, ptrDepth->image.type());
}
if(ptrImage->image.type() == rgb.type())
@@ -407,17 +457,17 @@ void RGBDOdometry::commonCallback(
}
else
{
RCLCPP_ERROR(this->get_logger(), "Some RGB images are not the same type!");
RCLCPP_ERROR(this->get_logger(), "Some RGB images are not the same type! %d vs %d", ptrImage->image.type(), rgb.type());
return;
}
if(subDepth.type() == depth.type())
if(ptrDepth->image.type() == depth.type())
{
subDepth.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight)));
ptrDepth->image.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight)));
}
else
{
RCLCPP_ERROR(this->get_logger(), "Some Depth images are not the same type! %d vs %d", subDepth.type(), depth.type());
RCLCPP_ERROR(this->get_logger(), "Some Depth images are not the same type! %d vs %d", ptrDepth->image.type(), depth.type());
return;
}
@@ -452,6 +502,42 @@ void RGBDOdometry::callback(
depthMsgs[0] = cv_bridge::toCvShare(depth);
infoMsgs.push_back(*cameraInfo);
double stampDiff = fabs(timestampFromROS(image->header.stamp) - timestampFromROS(depth->header.stamp));
if(stampDiff > 0.010)
{
RCLCPP_WARN(this->get_logger(), "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,
timestampFromROS(image->header.stamp),
timestampFromROS(depth->header.stamp));
}
this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
}
}
void RGBDOdometry::callbackRGBDX(
const rtabmap_ros::msg::RGBDImages::ConstSharedPtr images)
{
callbackCalled();
if(!this->isPaused())
{
if(images->rgbd_images.empty())
{
RCLCPP_ERROR(this->get_logger(), "Input topic \"%s\" doesn't contain any image(s)!", rgbdxSub_->get_topic_name());
return;
}
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(images->rgbd_images.size());
std::vector<cv_bridge::CvImageConstPtr> depthMsgs(images->rgbd_images.size());
std::vector<sensor_msgs::msg::CameraInfo> infoMsgs;
for(size_t i=0; i<images->rgbd_images.size(); ++i)
{
rtabmap_ros::toCvShare(images->rgbd_images[i], images, imageMsgs[i], depthMsgs[i]);
infoMsgs.push_back(images->rgbd_images[i].rgb_camera_info);
}
this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
}
}
src/nodelets/rgbd_sync.cpp
+21 -1
View File
@@ -53,8 +53,10 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
{
int queueSize = 10;
bool approxSync = true;
double approxSyncMaxInterval = 0.0;
int qos = 0;
approxSync = this->declare_parameter("approx_sync", approxSync);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
queueSize = this->declare_parameter("queue_size", queueSize);
qos = this->declare_parameter("qos", qos);
int qosCamInfo = this->declare_parameter("qos_camera_info", qos);
@@ -68,6 +70,8 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
}
RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false");
if(approxSync)
RCLCPP_INFO(this->get_logger(), "%s: approx_sync_max_interval = %f", get_name(), approxSyncMaxInterval);
RCLCPP_INFO(this->get_logger(), "%s: queue_size = %d", get_name(), queueSize);
RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos);
RCLCPP_INFO(this->get_logger(), "%s: qos_camera_info = %d", get_name(), qosCamInfo);
@@ -81,6 +85,8 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
if(approxSync)
{
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
if(approxSyncMaxInterval > 0.0)
approxSyncDepth_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSyncDepth_->registerCallback(std::bind(&RGBDSync::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
else
@@ -94,9 +100,10 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
imageDepthSub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cameraInfoSub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
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",
get_name(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
imageSub_.getSubscriber().getTopic().c_str(),
imageDepthSub_.getSubscriber().getTopic().c_str(),
cameraInfoSub_.getSubscriber()->get_topic_name());
@@ -142,6 +149,19 @@ void RGBDSync::callback(
{
double rgbStamp = timestampFromROS(image->header.stamp);
double depthStamp = timestampFromROS(depth->header.stamp);
double infoStamp = timestampFromROS(cameraInfo->header.stamp);
double stampDiff = fabs(rgbStamp - depthStamp);
if(stampDiff > 0.010)
{
RCLCPP_WARN(this->get_logger(), "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::msg::RGBDImage::UniquePtr msg(new rtabmap_ros::msg::RGBDImage);
msg->header.frame_id = cameraInfo->header.frame_id;
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(std::bind(&RGBDICPOdometry::callbackCloud, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
else
@@ -162,9 +168,10 @@ private:
exactCloudSync_->registerCallback(std::bind(&RGBDICPOdometry::callbackCloud, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_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_.getSubscriber().getTopic().c_str(),
image_depth_sub_.getSubscriber().getTopic().c_str(),
info_sub_.getSubscriber()->get_topic_name(),
@@ -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(std::bind(&RGBDICPOdometry::callbackScan, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
else
@@ -184,9 +193,10 @@ private:
exactScanSync_->registerCallback(std::bind(&RGBDICPOdometry::callbackScan, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_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_.getSubscriber().getTopic().c_str(),
image_depth_sub_.getSubscriber().getTopic().c_str(),
info_sub_.getSubscriber()->get_topic_name(),
@@ -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
@@ -47,13 +47,17 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) :
int queueSize = 10;
bool approxSync = true;
int rgbdCameras = 2;
double approxSyncMaxInterval = 0.0;
int qos = 0;
approxSync = this->declare_parameter("approx_sync", approxSync);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
queueSize = this->declare_parameter("queue_size", queueSize);
qos = this->declare_parameter("qos", qos);
rgbdCameras = this->declare_parameter("rgbd_cameras", rgbdCameras);
RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false");
if(approxSync)
RCLCPP_INFO(this->get_logger(), "%s: approx_sync_max_interval = %f", get_name(), approxSyncMaxInterval);
RCLCPP_INFO(this->get_logger(), "%s: queue_size = %d", get_name(), queueSize);
RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos);
RCLCPP_INFO(this->get_logger(), "%s: rgbd_cameras = %d", get_name(), rgbdCameras);
@@ -74,33 +78,62 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) :
if(rgbdCameras==2)
{
SYNC_DECL2(RGBDXSync, rgbd2, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
if(approxSync && approxSyncMaxInterval>0.0)
{
rgbd2ApproximateSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
}
}
else if(rgbdCameras==3)
{
SYNC_DECL3(RGBDXSync, rgbd3, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
if(approxSync && approxSyncMaxInterval>0.0)
{
rgbd3ApproximateSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(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(rclcpp::Duration::from_seconds(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(rclcpp::Duration::from_seconds(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(rclcpp::Duration::from_seconds(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(rclcpp::Duration::from_seconds(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(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
}
}
RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg_.c_str());
RCLCPP_INFO(this->get_logger(), "%s%s", subscribedTopicsMsg_.c_str(),
approxSync&&approxSyncMaxInterval!=0.0?uFormat(" (approx sync max interval=%fs)", approxSyncMaxInterval).c_str():"");
warningThread_ = new std::thread([&](){
rclcpp::Rate r(1/5.0);
src/nodelets/stereo_odometry.cpp
+492 -211
View File
@@ -31,9 +31,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <image_geometry/stereo_camera_model.h>
#include <cv_bridge/cv_bridge.h>
#include "rtabmap_ros/MsgConversion.h"
#include <rtabmap_ros/msg/rgbd_images.hpp>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UTimer.h>
@@ -50,6 +49,12 @@ StereoOdometry::StereoOdometry(const rclcpp::NodeOptions & options) :
rtabmap_ros::OdometryROS("stereo_odometry", options),
approxSync_(0),
exactSync_(0),
approxSync2_(0),
exactSync2_(0),
approxSync3_(0),
exactSync3_(0),
approxSync4_(0),
exactSync4_(0),
queueSize_(5),
keepColor_(false)
{
@@ -66,13 +71,19 @@ void StereoOdometry::onOdomInit()
{
bool approxSync = false;
bool subscribeRGBD = false;
double approxSyncMaxInterval = 0.0;
int rgbdCameras = 1;
approxSync = this->declare_parameter("approx_sync", approxSync);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
queueSize_ = this->declare_parameter("queue_size", queueSize_);
int qosCamInfo = this->declare_parameter("qos_camera_info", (int)qos());
subscribeRGBD = this->declare_parameter("subscribe_rgbd", subscribeRGBD);
rgbdCameras = this->declare_parameter("rgbd_cameras", rgbdCameras);
keepColor_ = this->declare_parameter("keep_color", keepColor_);
RCLCPP_INFO(this->get_logger(), "StereoOdometry: approx_sync = %s", approxSync?"true":"false");
if(approxSync)
RCLCPP_INFO(this->get_logger(), "StereoOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval);
RCLCPP_INFO(this->get_logger(), "StereoOdometry: queue_size = %d", queueSize_);
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: qos = %d", (int)qos());
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: qos_camera_info = %d", qosCamInfo);
@@ -82,12 +93,133 @@ void StereoOdometry::onOdomInit()
std::string subscribedTopicsMsg;
if(subscribeRGBD)
{
rgbdSub_ = create_subscription<rtabmap_ros::msg::RGBDImage>("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBD, this, std::placeholders::_1));
if(rgbdCameras >= 2)
{
rgbd_image1_sub_.subscribe(this, "rgbd_image0", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
if(rgbdCameras >= 3)
{
rgbd_image3_sub_.subscribe(this, "rgbd_image2", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
}
if(rgbdCameras >= 4)
{
rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
}
subscribedTopicsMsg =
uFormat("\n%s subscribed to:\n %s",
get_name(),
rgbdSub_->get_topic_name());
if(rgbdCameras == 2)
{
if(approxSync)
{
approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
MyApproxSync2Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync2_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync2_->registerCallback(std::bind(&StereoOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
}
else
{
exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
MyExactSync2Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_);
exactSync2_->registerCallback(std::bind(&StereoOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s",
get_name(),
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());
}
else if(rgbdCameras == 3)
{
if(approxSync)
{
approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
MyApproxSync3Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync3_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync3_->registerCallback(std::bind(&StereoOdometry::callbackRGBD3, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
else
{
exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
MyExactSync3Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_);
exactSync3_->registerCallback(std::bind(&StereoOdometry::callbackRGBD3, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s",
get_name(),
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());
}
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_);
if(approxSyncMaxInterval > 0.0)
approxSync4_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync4_->registerCallback(std::bind(&StereoOdometry::callbackRGBD4, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
else
{
exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
MyExactSync4Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_);
exactSync4_->registerCallback(std::bind(&StereoOdometry::callbackRGBD4, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s",
get_name(),
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(),
rgbd_image4_sub_.getTopic().c_str());
}
else
{
RCLCPP_FATAL(this->get_logger(), "%s doesn't support more than 4 cameras (rgbd_cameras=%d) with internal synchronization interface, set rgbd_cameras=0 and use rgbd_images input topic instead for more cameras.", get_name(), rgbdCameras);
}
}
else if(rgbdCameras == 0)
{
rgbdxSub_ = create_subscription<rtabmap_ros::msg::RGBDImages>("rgbd_images", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBDX, this, std::placeholders::_1));
subscribedTopicsMsg =
uFormat("\n%s subscribed to:\n %s",
get_name(),
rgbdxSub_->get_topic_name());
}
else
{
rgbdSub_ = create_subscription<rtabmap_ros::msg::RGBDImage>("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBD, this, std::placeholders::_1));
subscribedTopicsMsg =
uFormat("\n%s subscribed to:\n %s",
get_name(),
rgbdSub_->get_topic_name());
}
}
else
{
@@ -100,6 +232,8 @@ void StereoOdometry::onOdomInit()
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
if(approxSyncMaxInterval>0.0)
approxSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync_->registerCallback(std::bind(&StereoOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
else
@@ -108,13 +242,14 @@ void StereoOdometry::onOdomInit()
exactSync_->registerCallback(std::bind(&StereoOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_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",
get_name(),
approxSync?"approx":"exact",
imageRectLeft_.getSubscriber().getTopic().c_str(),
imageRectRight_.getSubscriber().getTopic().c_str(),
cameraInfoLeft_.getSubscriber()->get_topic_name(),
cameraInfoRight_.getSubscriber()->get_topic_name());
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
imageRectLeft_.getTopic().c_str(),
imageRectRight_.getTopic().c_str(),
cameraInfoLeft_.getTopic().c_str(),
cameraInfoRight_.getTopic().c_str());
}
this->startWarningThread(subscribedTopicsMsg, approxSync);
@@ -131,62 +266,111 @@ void StereoOdometry::updateParameters(ParametersMap & parameters)
uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "0"));
}
void StereoOdometry::callback(
const sensor_msgs::msg::Image::ConstSharedPtr imageRectLeft,
const sensor_msgs::msg::Image::ConstSharedPtr imageRectRight,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoLeft,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoRight)
void StereoOdometry::commonCallback(
const std::vector<cv_bridge::CvImageConstPtr> & leftImages,
const std::vector<cv_bridge::CvImageConstPtr> & rightImages,
const std::vector<sensor_msgs::msg::CameraInfo>& leftCameraInfos,
const std::vector<sensor_msgs::msg::CameraInfo>& rightCameraInfos)
{
callbackCalled();
if(!this->isPaused())
UASSERT(leftImages.size() > 0 &&
leftImages.size() == rightImages.size() &&
leftImages.size() == leftCameraInfos.size() &&
rightImages.size() == rightCameraInfos.size());
rclcpp::Time higherStamp;
int leftWidth = leftImages[0]->image.cols;
int leftHeight = leftImages[0]->image.rows;
int rightWidth = rightImages[0]->image.cols;
int rightHeight = rightImages[0]->image.rows;
UASSERT_MSG(
leftWidth == rightWidth && leftHeight == rightHeight,
uFormat("left=%dx%d right=%dx%d", leftWidth, leftHeight, rightWidth, rightHeight).c_str());
int cameraCount = leftImages.size();
cv::Mat left;
cv::Mat right;
std::vector<rtabmap::StereoCameraModel> cameraModels;
for(unsigned int i=0; i<leftImages.size(); ++i)
{
if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0) ||
!(imageRectRight->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0))
if(!(leftImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
leftImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
leftImages[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
leftImages[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
leftImages[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0) ||
!(rightImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
rightImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
rightImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
rightImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
rightImages[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
rightImages[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
rightImages[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0))
{
RCLCPP_ERROR(this->get_logger(), "Input type must be image=mono8,mono16,rgb8,bgr8,rgba8,bgra8 (mono8 recommended), received types are %s (left) and %s (right)",
imageRectLeft->encoding.c_str(), imageRectRight->encoding.c_str());
leftImages[i]->encoding.c_str(), rightImages[i]->encoding.c_str());
return;
}
rclcpp::Time stamp = timestampFromROS(imageRectLeft->header.stamp)>timestampFromROS(imageRectRight->header.stamp)?imageRectLeft->header.stamp:imageRectRight->header.stamp;
rclcpp::Time stamp = timestampFromROS(leftImages[i]->header.stamp)>timestampFromROS(rightImages[i]->header.stamp)?leftImages[i]->header.stamp:rightImages[i]->header.stamp;
Transform localTransform = getTransform(this->frameId(), imageRectLeft->header.frame_id, stamp, tfBuffer(), waitForTransform());
if(i == 0)
{
higherStamp = stamp;
}
else if(stamp > higherStamp)
{
higherStamp = stamp;
}
Transform localTransform = rtabmap_ros::getTransform(this->frameId(), leftImages[i]->header.frame_id, stamp, tfBuffer(), waitForTransform());
if(localTransform.isNull())
{
return;
}
if(imageRectLeft->data.size() && imageRectRight->data.size())
if(i>0)
{
double stampDiff = fabs(timestampFromROS(leftImages[i]->header.stamp) - timestampFromROS(leftImages[i-1]->header.stamp));
if(stampDiff > 1.0/60.0)
{
static bool warningShown = false;
if(!warningShown)
{
RCLCPP_WARN(this->get_logger(), "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, timestampFromROS(leftImages[i-1]->header.stamp),
i, timestampFromROS(leftImages[i]->header.stamp));
warningShown = true;
}
}
}
int quality = -1;
if(!leftImages[i]->image.empty() && !rightImages[i]->image.empty())
{
bool alreadyRectified = true;
Parameters::parse(parameters(), Parameters::kRtabmapImagesAlreadyRectified(), alreadyRectified);
rtabmap::Transform stereoTransform;
if(!alreadyRectified)
{
stereoTransform = getTransform(
cameraInfoRight->header.frame_id,
cameraInfoLeft->header.frame_id,
cameraInfoLeft->header.stamp,
stereoTransform = rtabmap_ros::getTransform(
rightCameraInfos[i].header.frame_id,
leftCameraInfos[i].header.frame_id,
leftCameraInfos[i].header.stamp,
tfBuffer(),
waitForTransform());
if(stereoTransform.isNull())
{
RCLCPP_ERROR(this->get_logger(), "Parameter %s is false but we cannot get TF between the two cameras! (between frames %s and %s)",
Parameters::kRtabmapImagesAlreadyRectified().c_str(),
cameraInfoRight->header.frame_id.c_str(),
cameraInfoLeft->header.frame_id.c_str());
rightCameraInfos[i].header.frame_id.c_str(),
leftCameraInfos[i].header.frame_id.c_str());
return;
}
else if(stereoTransform.isIdentity())
@@ -195,20 +379,20 @@ void StereoOdometry::callback(
"Identity transform returned between left and right cameras. Verify that if TF between "
"the cameras is valid: \"rosrun tf tf_echo %s %s\".",
Parameters::kRtabmapImagesAlreadyRectified().c_str(),
cameraInfoRight->header.frame_id.c_str(),
cameraInfoLeft->header.frame_id.c_str());
rightCameraInfos[i].header.frame_id.c_str(),
leftCameraInfos[i].header.frame_id.c_str());
return;
}
}
rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*cameraInfoLeft, *cameraInfoRight, localTransform, stereoTransform);
rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(leftCameraInfos[i], rightCameraInfos[i], localTransform, stereoTransform);
if(stereoModel.baseline() == 0 && alreadyRectified)
{
stereoTransform = getTransform(
cameraInfoLeft->header.frame_id,
cameraInfoRight->header.frame_id,
cameraInfoLeft->header.stamp,
stereoTransform = rtabmap_ros::getTransform(
leftCameraInfos[i].header.frame_id,
rightCameraInfos[i].header.frame_id,
leftCameraInfos[i].header.stamp,
tfBuffer(),
waitForTransform());
@@ -221,7 +405,7 @@ void StereoOdometry::callback(
"recommended, we used TF to get the baseline (%s->%s = %fm) for convenience (e.g., D400 ir stereo issue). It is preferred to feed "
"a valid right camera info if stereo images are already rectified. This message is only printed once...",
rtabmap::Parameters::kRtabmapImagesAlreadyRectified().c_str(),
cameraInfoRight->header.frame_id.c_str(), cameraInfoLeft->header.frame_id.c_str(), stereoTransform.x());
rightCameraInfos[i].header.frame_id.c_str(), leftCameraInfos[i].header.frame_id.c_str(), stereoTransform.x());
warned = true;
}
stereoModel = rtabmap::StereoCameraModel(
@@ -234,7 +418,6 @@ void StereoOdometry::callback(
stereoModel.left().imageSize());
}
}
if(alreadyRectified && stereoModel.baseline() <= 0)
{
RCLCPP_ERROR(this->get_logger(), "The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo "
@@ -254,35 +437,111 @@ void StereoOdometry::callback(
shown = true;
}
}
cv_bridge::CvImageConstPtr ptrLeft = leftImages[i];
if(leftImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
{
if(keepColor_ && leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0)
{
ptrLeft = cv_bridge::cvtColor(leftImages[i], "bgr8");
}
else
{
ptrLeft = cv_bridge::cvtColor(leftImages[i], "mono8");
}
}
cv_bridge::CvImageConstPtr ptrRight = rightImages[i];
if(rightImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
rightImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
{
ptrRight = cv_bridge::cvtColor(rightImages[i], "mono8");
}
cv_bridge::CvImagePtr ptrImageLeft = cv_bridge::toCvCopy(imageRectLeft,
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8)==0?"":
keepColor_ && imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0?"bgr8":"mono8");
cv_bridge::CvImagePtr ptrImageRight = cv_bridge::toCvCopy(imageRectRight,
imageRectRight->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8)==0?"":"mono8");
// initialize
if(left.empty())
{
left = cv::Mat(leftHeight, leftWidth*cameraCount, ptrLeft->image.type());
}
if(right.empty())
{
right = cv::Mat(rightHeight, rightWidth*cameraCount, ptrRight->image.type());
}
UTimer stepTimer;
//
UDEBUG("localTransform = %s", localTransform.prettyPrint().c_str());
rtabmap::SensorData data(
ptrImageLeft->image,
ptrImageRight->image,
stereoModel,
0,
rtabmap_ros::timestampFromROS(stamp));
if(ptrLeft->image.type() == left.type())
{
ptrLeft->image.copyTo(cv::Mat(left, cv::Rect(i*leftWidth, 0, leftWidth, leftHeight)));
}
else
{
RCLCPP_ERROR(this->get_logger(), "Some left images are not the same type! %d vs %d", ptrLeft->image.type(), left.type());
return;
}
std_msgs::msg::Header header;
header.stamp = stamp;
header.frame_id = imageRectLeft->header.frame_id;
this->processData(data, header);
if(ptrRight->image.type() == right.type())
{
ptrRight->image.copyTo(cv::Mat(right, cv::Rect(i*rightWidth, 0, rightWidth, rightHeight)));
}
else
{
RCLCPP_ERROR(this->get_logger(), "Some right images are not the same type! %d vs %d", ptrRight->image.type(), right.type());
return;
}
cameraModels.push_back(stereoModel);
}
else
{
RCLCPP_WARN(this->get_logger(), "Odom: input images empty?!?");
RCLCPP_ERROR(this->get_logger(), "Odom: input images empty?!?");
return;
}
}
//
rtabmap::SensorData data(
left,
right,
cameraModels,
0,
rtabmap_ros::timestampFromROS(higherStamp));
std_msgs::msg::Header header;
header.stamp = higherStamp;
header.frame_id = leftImages.size()==1?leftImages[0]->header.frame_id:"";
this->processData(data, header);
}
void StereoOdometry::callback(
const sensor_msgs::msg::Image::ConstSharedPtr imageRectLeft,
const sensor_msgs::msg::Image::ConstSharedPtr imageRectRight,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoLeft,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoRight)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(1);
std::vector<cv_bridge::CvImageConstPtr> rightMsgs(1);
std::vector<sensor_msgs::msg::CameraInfo> leftInfoMsgs;
std::vector<sensor_msgs::msg::CameraInfo> rightInfoMsgs;
leftMsgs[0] = cv_bridge::toCvShare(imageRectLeft);
rightMsgs[0] = cv_bridge::toCvShare(imageRectRight);
leftInfoMsgs.push_back(*cameraInfoLeft);
rightInfoMsgs.push_back(*cameraInfoRight);
double stampDiff = fabs(timestampFromROS(imageRectLeft->header.stamp) - timestampFromROS(imageRectRight->header.stamp));
if(stampDiff > 0.010)
{
RCLCPP_WARN(this->get_logger(), "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,
timestampFromROS(imageRectLeft->header.stamp),
timestampFromROS(imageRectRight->header.stamp));
}
this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
}
}
void StereoOdometry::callbackRGBD(
@@ -291,157 +550,119 @@ void StereoOdometry::callbackRGBD(
callbackCalled();
if(!this->isPaused())
{
cv_bridge::CvImageConstPtr imageRectLeft, imageRectRight;
rtabmap_ros::toCvShare(image, imageRectLeft, imageRectRight);
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(1);
std::vector<cv_bridge::CvImageConstPtr> rightMsgs(1);
std::vector<sensor_msgs::msg::CameraInfo> leftInfoMsgs;
std::vector<sensor_msgs::msg::CameraInfo> rightInfoMsgs;
rtabmap_ros::toCvShare(image, leftMsgs[0], rightMsgs[0]);
leftInfoMsgs.push_back(image->rgb_camera_info);
rightInfoMsgs.push_back(image->depth_camera_info);
if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0) ||
!(imageRectRight->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0))
this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
}
}
void StereoOdometry::callbackRGBDX(
const rtabmap_ros::msg::RGBDImages::ConstSharedPtr images)
{
callbackCalled();
if(!this->isPaused())
{
if(images->rgbd_images.empty())
{
RCLCPP_ERROR(this->get_logger(), "Input type must be image=mono8,mono16,rgb8,bgr8,rgba8,bgra8 (mono8 recommended), received types are %s (left) and %s (right)",
imageRectLeft->encoding.c_str(), imageRectRight->encoding.c_str());
RCLCPP_ERROR(this->get_logger(), "Input topic \"%s\" doesn't contain any image(s)!", rgbdxSub_->get_topic_name());
return;
}
rclcpp::Time stamp = timestampFromROS(imageRectLeft->header.stamp)>timestampFromROS(imageRectRight->header.stamp)?imageRectLeft->header.stamp:imageRectRight->header.stamp;
Transform localTransform = getTransform(this->frameId(), imageRectLeft->header.frame_id, stamp, tfBuffer(), waitForTransform());
if(localTransform.isNull())
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(images->rgbd_images.size());
std::vector<cv_bridge::CvImageConstPtr> rightMsgs(images->rgbd_images.size());
std::vector<sensor_msgs::msg::CameraInfo> leftInfoMsgs;
std::vector<sensor_msgs::msg::CameraInfo> rightInfoMsgs;
for(size_t i=0; i<images->rgbd_images.size(); ++i)
{
return;
rtabmap_ros::toCvShare(images->rgbd_images[i], images, leftMsgs[i], rightMsgs[i]);
leftInfoMsgs.push_back(images->rgbd_images[i].rgb_camera_info);
rightInfoMsgs.push_back(images->rgbd_images[i].depth_camera_info);
}
if(!imageRectLeft->image.empty() && !imageRectRight->image.empty())
{
bool alreadyRectified = true;
Parameters::parse(parameters(), Parameters::kRtabmapImagesAlreadyRectified(), alreadyRectified);
rtabmap::Transform stereoTransform;
if(!alreadyRectified)
{
stereoTransform = getTransform(
image->depth_camera_info.header.frame_id,
image->rgb_camera_info.header.frame_id,
image->rgb_camera_info.header.stamp,
tfBuffer(),
waitForTransform());
if(stereoTransform.isNull())
{
RCLCPP_ERROR(this->get_logger(), "Parameter %s is false but we cannot get TF between the two cameras!", Parameters::kRtabmapImagesAlreadyRectified().c_str());
return;
}
}
this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
}
}
rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(image->rgb_camera_info, image->depth_camera_info, localTransform);
void StereoOdometry::callbackRGBD2(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(2);
std::vector<cv_bridge::CvImageConstPtr> rightMsgs(2);
std::vector<sensor_msgs::msg::CameraInfo> leftInfoMsgs;
std::vector<sensor_msgs::msg::CameraInfo> rightInfoMsgs;
rtabmap_ros::toCvShare(image, leftMsgs[0], rightMsgs[0]);
rtabmap_ros::toCvShare(image2, leftMsgs[1], rightMsgs[1]);
leftInfoMsgs.push_back(image->rgb_camera_info);
leftInfoMsgs.push_back(image2->rgb_camera_info);
rightInfoMsgs.push_back(image->depth_camera_info);
rightInfoMsgs.push_back(image2->depth_camera_info);
if(stereoModel.baseline() == 0 && alreadyRectified)
{
stereoTransform = getTransform(
image->rgb_camera_info.header.frame_id,
image->depth_camera_info.header.frame_id,
image->rgb_camera_info.header.stamp,
tfBuffer(),
waitForTransform());
this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
}
}
if(!stereoTransform.isNull() && stereoTransform.x()>0)
{
static bool warned = false;
if(!warned)
{
RCLCPP_WARN(get_logger(), "Right camera info doesn't have Tx set but we are assuming that stereo images are already rectified (see %s parameter). While not "
"recommended, we used TF to get the baseline (%s->%s = %fm) for convenience (e.g., D400 ir stereo issue). It is preferred to feed "
"a valid right camera info if stereo images are already rectified. This message is only printed once...",
rtabmap::Parameters::kRtabmapImagesAlreadyRectified().c_str(),
image->depth_camera_info.header.frame_id.c_str(), image->rgb_camera_info.header.frame_id.c_str(), stereoTransform.x());
warned = true;
}
stereoModel = rtabmap::StereoCameraModel(
stereoModel.left().fx(),
stereoModel.left().fy(),
stereoModel.left().cx(),
stereoModel.left().cy(),
stereoTransform.x(),
stereoModel.localTransform(),
stereoModel.left().imageSize());
}
}
void StereoOdometry::callbackRGBD3(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(3);
std::vector<cv_bridge::CvImageConstPtr> rightMsgs(3);
std::vector<sensor_msgs::msg::CameraInfo> leftInfoMsgs;
std::vector<sensor_msgs::msg::CameraInfo> rightInfoMsgs;
rtabmap_ros::toCvShare(image, leftMsgs[0], rightMsgs[0]);
rtabmap_ros::toCvShare(image2, leftMsgs[1], rightMsgs[1]);
rtabmap_ros::toCvShare(image3, leftMsgs[2], rightMsgs[2]);
leftInfoMsgs.push_back(image->rgb_camera_info);
leftInfoMsgs.push_back(image2->rgb_camera_info);
leftInfoMsgs.push_back(image3->rgb_camera_info);
rightInfoMsgs.push_back(image->depth_camera_info);
rightInfoMsgs.push_back(image2->depth_camera_info);
rightInfoMsgs.push_back(image3->depth_camera_info);
if(alreadyRectified && stereoModel.baseline() <= 0)
{
RCLCPP_FATAL(this->get_logger(), "The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo "
"setup where the Tx (or P(0,3)) is negative in the right camera info msg.", stereoModel.baseline());
return;
}
this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
}
}
if(stereoModel.baseline() > 10.0)
{
static bool shown = false;
if(!shown)
{
RCLCPP_WARN(this->get_logger(), "Detected baseline (%f m) is quite large! Is your "
"right camera_info P(0,3) correctly set? Note that "
"baseline=-P(0,3)/P(0,0). This warning is printed only once.",
stereoModel.baseline());
shown = true;
}
}
void StereoOdometry::callbackRGBD4(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(4);
std::vector<cv_bridge::CvImageConstPtr> rightMsgs(4);
std::vector<sensor_msgs::msg::CameraInfo> leftInfoMsgs;
std::vector<sensor_msgs::msg::CameraInfo> rightInfoMsgs;
rtabmap_ros::toCvShare(image, leftMsgs[0], rightMsgs[0]);
rtabmap_ros::toCvShare(image2, leftMsgs[1], rightMsgs[1]);
rtabmap_ros::toCvShare(image3, leftMsgs[2], rightMsgs[2]);
rtabmap_ros::toCvShare(image4, leftMsgs[3], rightMsgs[3]);
leftInfoMsgs.push_back(image->rgb_camera_info);
leftInfoMsgs.push_back(image2->rgb_camera_info);
leftInfoMsgs.push_back(image3->rgb_camera_info);
leftInfoMsgs.push_back(image4->rgb_camera_info);
rightInfoMsgs.push_back(image->depth_camera_info);
rightInfoMsgs.push_back(image2->depth_camera_info);
rightInfoMsgs.push_back(image3->depth_camera_info);
rightInfoMsgs.push_back(image4->depth_camera_info);
cv::Mat left;
if(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
{
if(keepColor_ && imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0)
{
left = cv_bridge::cvtColor(imageRectLeft, "bgr8")->image;
}
else
{
left = cv_bridge::cvtColor(imageRectLeft, "mono8")->image;
}
}
else
{
left = imageRectLeft->image.clone();
}
cv::Mat right;
if(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
{
right = cv_bridge::cvtColor(imageRectRight, "mono8")->image;
}
else
{
right = imageRectRight->image.clone();
}
//
UDEBUG("localTransform = %s", localTransform.prettyPrint().c_str());
rtabmap::SensorData data(
left,
right,
stereoModel,
0,
rtabmap_ros::timestampFromROS(stamp));
std_msgs::msg::Header header;
header.stamp = stamp;
header.frame_id = image->header.frame_id;
this->processData(data, header);
}
else
{
RCLCPP_WARN(this->get_logger(), "Odom: input images empty?!?");
}
this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
}
}
@@ -460,6 +681,66 @@ void StereoOdometry::flushCallbacks()
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
exactSync_->registerCallback(std::bind(&StereoOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
if(approxSync2_)
{
delete approxSync2_;
approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
MyApproxSync2Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_);
approxSync2_->registerCallback(std::bind(&StereoOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
}
if(exactSync2_)
{
delete exactSync2_;
exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
MyExactSync2Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_);
exactSync2_->registerCallback(std::bind(&StereoOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
}
if(approxSync3_)
{
delete approxSync3_;
approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
MyApproxSync3Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_);
approxSync3_->registerCallback(std::bind(&StereoOdometry::callbackRGBD3, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
if(exactSync3_)
{
delete exactSync3_;
exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
MyExactSync3Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_);
exactSync3_->registerCallback(std::bind(&StereoOdometry::callbackRGBD3, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_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(std::bind(&StereoOdometry::callbackRGBD4, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_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(std::bind(&StereoOdometry::callbackRGBD4, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
}
}
src/nodelets/stereo_sync.cpp
+19 -1
View File
@@ -50,14 +50,17 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) :
{
int queueSize = 10;
bool approxSync = false;
double approxSyncMaxInterval = 0.0;
int qos = 0;
approxSync = this->declare_parameter("approx_sync", approxSync);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
queueSize = this->declare_parameter("queue_size", queueSize);
qos = this->declare_parameter("qos", qos);
int qosCamInfo = this->declare_parameter("qos_camera_info", qos);
compressedRate_ = this->declare_parameter("compressed_rate", compressedRate_);
RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false");
RCLCPP_INFO(this->get_logger(), "%s: approx_sync_max_interval = %f", get_name(), approxSyncMaxInterval);
RCLCPP_INFO(this->get_logger(), "%s: queue_size = %d", get_name(), queueSize);
RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos);
RCLCPP_INFO(this->get_logger(), "%s: qos_camera_info = %d", get_name(), qosCamInfo);
@@ -69,6 +72,8 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) :
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageLeftSub_, imageRightSub_, cameraInfoLeftSub_, cameraInfoRightSub_);
if(approxSyncMaxInterval>0.0)
approxSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync_->registerCallback(std::bind(&StereoSync::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
else
@@ -83,9 +88,10 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) :
cameraInfoLeftSub_.subscribe(this, "left/camera_info"), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile();
cameraInfoRightSub_.subscribe(this, "right/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
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",
get_name(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
imageLeftSub_.getSubscriber().getTopic().c_str(),
imageRightSub_.getSubscriber().getTopic().c_str(),
cameraInfoLeftSub_.getSubscriber()->get_topic_name(),
@@ -135,6 +141,18 @@ void StereoSync::callback(
double leftStamp = timestampFromROS(imageLeft->header.stamp);
double rightStamp = timestampFromROS(imageRight->header.stamp);
double stampDiff = fabs(leftStamp - rightStamp);
if(stampDiff > 0.010)
{
RCLCPP_WARN(this->get_logger(), "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::msg::RGBDImage::UniquePtr msg(new rtabmap_ros::msg::RGBDImage);
msg->header.frame_id = cameraInfoLeft->header.frame_id;
msg->header.stamp = leftStamp>rightStamp?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(std::bind(&StereoThrottleNodelet::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
else