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

Added multicam support for stereo_odometry

Browse Source
This commit is contained in:
matlabbe
2022-07-11 16:34:40 -04:00
parent eb932a86ee
commit 15d52fc0e0
2 changed files with 509 additions and 219 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/rgbd_odometry.cpp
+13 -8
View File
@@ -113,6 +113,14 @@ public:
{ {
delete exactSync4_; delete exactSync4_;
} }
if(approxSync5_)
{
delete approxSync5_;
}
if(exactSync5_)
{
delete exactSync5_;
}
} }
private: private:
@@ -397,9 +405,7 @@ private:
int cameraCount = rgbImages.size(); int cameraCount = rgbImages.size();
cv::Mat rgb; cv::Mat rgb;
cv::Mat depth; cv::Mat depth;
pcl::PointCloud<pcl::PointXYZ> scanCloud;
std::vector<rtabmap::CameraModel> cameraModels; std::vector<rtabmap::CameraModel> cameraModels;
double stampDiff = 0;
for(unsigned int i=0; i<rgbImages.size(); ++i) for(unsigned int i=0; i<rgbImages.size(); ++i)
{ {
if(!(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 || if(!(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
@@ -488,7 +494,6 @@ private:
} }
cv_bridge::CvImageConstPtr ptrDepth = depthImages[i]; cv_bridge::CvImageConstPtr ptrDepth = depthImages[i];
cv::Mat subDepth = ptrDepth->image;
// initialize // initialize
if(rgb.empty()) if(rgb.empty())
@@ -497,7 +502,7 @@ private:
} }
if(depth.empty()) 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()) if(ptrImage->image.type() == rgb.type())
@@ -506,17 +511,17 @@ private:
} }
else else
{ {
NODELET_ERROR("Some RGB images are not the same type!"); NODELET_ERROR("Some RGB images are not the same type! %d vs %d", ptrImage->image.type(), rgb.type());
return; 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 else
{ {
NODELET_ERROR("Some Depth images are not the same type! %d vs %d", subDepth.type(), depth.type()); NODELET_ERROR("Some Depth images are not the same type! %d vs %d", ptrDepth->image.type(), depth.type());
return; return;
} }
src/nodelets/stereo_odometry.cpp
+496 -211
View File
@@ -44,6 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <cv_bridge/cv_bridge.h> #include <cv_bridge/cv_bridge.h>
#include "rtabmap_ros/MsgConversion.h" #include "rtabmap_ros/MsgConversion.h"
#include <rtabmap_ros/RGBDImages.h>
#include <rtabmap/utilite/ULogger.h> #include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UTimer.h> #include <rtabmap/utilite/UTimer.h>
@@ -63,6 +64,12 @@ public:
rtabmap_ros::OdometryROS(true, true, false), rtabmap_ros::OdometryROS(true, true, false),
approxSync_(0), approxSync_(0),
exactSync_(0), exactSync_(0),
approxSync2_(0),
exactSync2_(0),
approxSync3_(0),
exactSync3_(0),
approxSync4_(0),
exactSync4_(0),
queueSize_(5), queueSize_(5),
keepColor_(false) keepColor_(false)
{ {
@@ -89,10 +96,12 @@ private:
bool approxSync = false; bool approxSync = false;
bool subscribeRGBD = false; bool subscribeRGBD = false;
double approxSyncMaxInterval = 0.0; double approxSyncMaxInterval = 0.0;
int rgbdCameras;
pnh.param("approx_sync", approxSync, approxSync); pnh.param("approx_sync", approxSync, approxSync);
pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval); pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
pnh.param("queue_size", queueSize_, queueSize_); pnh.param("queue_size", queueSize_, queueSize_);
pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD); pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD);
pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras);
pnh.param("keep_color", keepColor_, keepColor_); pnh.param("keep_color", keepColor_, keepColor_);
NODELET_INFO("StereoOdometry: approx_sync = %s", approxSync?"true":"false"); NODELET_INFO("StereoOdometry: approx_sync = %s", approxSync?"true":"false");
@@ -105,12 +114,133 @@ private:
std::string subscribedTopicsMsg; std::string subscribedTopicsMsg;
if(subscribeRGBD) if(subscribeRGBD)
{ {
rgbdSub_ = nh.subscribe("rgbd_image", 1, &StereoOdometry::callbackRGBD, this); if(rgbdCameras >= 2)
{
rgbd_image1_sub_.subscribe(nh, "rgbd_image0", 1);
rgbd_image2_sub_.subscribe(nh, "rgbd_image1", 1);
if(rgbdCameras >= 3)
{
rgbd_image3_sub_.subscribe(nh, "rgbd_image2", 1);
}
if(rgbdCameras >= 4)
{
rgbd_image4_sub_.subscribe(nh, "rgbd_image3", 1);
}
subscribedTopicsMsg = if(rgbdCameras == 2)
uFormat("\n%s subscribed to:\n %s", {
getName().c_str(), if(approxSync)
rgbdSub_.getTopic().c_str()); {
approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
MyApproxSync2Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync2_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxSync2_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2));
}
else
{
exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
MyExactSync2Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_);
exactSync2_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2));
}
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());
}
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(ros::Duration(approxSyncMaxInterval));
approxSync3_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD3, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
}
else
{
exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
MyExactSync3Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_);
exactSync3_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD3, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
}
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());
}
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(ros::Duration(approxSyncMaxInterval));
approxSync4_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD4, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::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(boost::bind(&StereoOdometry::callbackRGBD4, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
}
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(),
rgbd_image4_sub_.getTopic().c_str());
}
else
{
ROS_FATAL("%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.", getName().c_str(), rgbdCameras);
}
}
else if(rgbdCameras == 0)
{
rgbdxSub_ = nh.subscribe("rgbd_images", 1, &StereoOdometry::callbackRGBDX, this);
subscribedTopicsMsg =
uFormat("\n%s subscribed to:\n %s",
getName().c_str(),
rgbdxSub_.getTopic().c_str());
}
else
{
rgbdSub_ = nh.subscribe("rgbd_image", 1, &StereoOdometry::callbackRGBD, this);
subscribedTopicsMsg =
uFormat("\n%s subscribed to:\n %s",
getName().c_str(),
rgbdSub_.getTopic().c_str());
}
} }
else else
{ {
@@ -166,57 +296,90 @@ private:
uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "0")); uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "0"));
} }
void callback( void commonCallback(
const sensor_msgs::ImageConstPtr& imageRectLeft, const std::vector<cv_bridge::CvImageConstPtr> & leftImages,
const sensor_msgs::ImageConstPtr& imageRectRight, const std::vector<cv_bridge::CvImageConstPtr> & rightImages,
const sensor_msgs::CameraInfoConstPtr& cameraInfoLeft, const std::vector<sensor_msgs::CameraInfo>& leftCameraInfos,
const sensor_msgs::CameraInfoConstPtr& cameraInfoRight) const std::vector<sensor_msgs::CameraInfo>& rightCameraInfos)
{ {
callbackCalled(); ROS_ASSERT(rgbImages.size() > 0 && rgbImages.size() == depthImages.size() && rgbImages.size() == cameraInfos.size());
if(!this->isPaused()) ros::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 || if(!(leftImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || leftImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 || leftImages[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 || leftImages[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0) || leftImages[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0) ||
!(imageRectRight->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 || !(rightImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || rightImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || rightImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || rightImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 || rightImages[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 || rightImages[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0)) rightImages[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0))
{ {
NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8,rgba8,bgra8 (mono8 recommended), received types are %s (left) and %s (right)", NODELET_ERROR("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; return;
} }
ros::Time stamp = imageRectLeft->header.stamp>imageRectRight->header.stamp?imageRectLeft->header.stamp:imageRectRight->header.stamp; ros::Time stamp = leftImages[i]->header.stamp>rightImages[i]->header.stamp?leftImages[i]->header.stamp:rightImages[i]->header.stamp;
Transform localTransform = getTransform(this->frameId(), imageRectLeft->header.frame_id, stamp); if(i == 0)
{
higherStamp = stamp;
}
else if(stamp > higherStamp)
{
higherStamp = stamp;
}
Transform localTransform = getTransform(this->frameId(), leftImages[i]->header.frame_id, stamp);
if(localTransform.isNull()) if(localTransform.isNull())
{ {
return; return;
} }
double stampDiff = fabs(imageRectLeft->header.stamp.toSec() - imageRectRight->header.stamp.toSec()); if(i>0)
if(stampDiff > 0.010)
{ {
NODELET_WARN("The time difference between left and right frames is " double stampDiff = fabs(leftImages[i]->header.stamp.toSec() - leftImages[i-1]->header.stamp.toSec());
"high (diff=%fs, left=%fs, right=%fs). If your left and right cameras are hardware " if(stampDiff > 1.0/60.0)
"synchronized, use approx_sync:=false. Otherwise, you may want " {
"to set approx_sync_max_interval lower than 0.01s to reject spurious bad synchronizations.", static bool warningShown = false;
stampDiff, if(!warningShown)
imageRectLeft->header.stamp.toSec(), {
imageRectRight->header.stamp.toSec()); 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, leftImages[i-1]->header.stamp.toSec(),
i, leftImages[i]->header.stamp.toSec());
warningShown = true;
}
}
} }
int quality = -1; int quality = -1;
if(imageRectLeft->data.size() && imageRectRight->data.size()) if(!leftImages[i]->image.empty() && !rightImages[i]->image.empty())
{ {
bool alreadyRectified = true; bool alreadyRectified = true;
Parameters::parse(parameters(), Parameters::kRtabmapImagesAlreadyRectified(), alreadyRectified); Parameters::parse(parameters(), Parameters::kRtabmapImagesAlreadyRectified(), alreadyRectified);
@@ -224,15 +387,15 @@ private:
if(!alreadyRectified) if(!alreadyRectified)
{ {
stereoTransform = getTransform( stereoTransform = getTransform(
cameraInfoRight->header.frame_id, rightCameraInfos[i].header.frame_id,
cameraInfoLeft->header.frame_id, leftCameraInfos[i].header.frame_id,
cameraInfoLeft->header.stamp); leftCameraInfos[i].header.stamp);
if(stereoTransform.isNull()) if(stereoTransform.isNull())
{ {
NODELET_ERROR("Parameter %s is false but we cannot get TF between the two cameras! (between frames %s and %s)", NODELET_ERROR("Parameter %s is false but we cannot get TF between the two cameras! (between frames %s and %s)",
Parameters::kRtabmapImagesAlreadyRectified().c_str(), Parameters::kRtabmapImagesAlreadyRectified().c_str(),
cameraInfoRight->header.frame_id.c_str(), rightCameraInfos[i].header.frame_id.c_str(),
cameraInfoLeft->header.frame_id.c_str()); leftCameraInfos[i].header.frame_id.c_str());
return; return;
} }
else if(stereoTransform.isIdentity()) else if(stereoTransform.isIdentity())
@@ -241,20 +404,20 @@ private:
"Identity transform returned between left and right cameras. Verify that if TF between " "Identity transform returned between left and right cameras. Verify that if TF between "
"the cameras is valid: \"rosrun tf tf_echo %s %s\".", "the cameras is valid: \"rosrun tf tf_echo %s %s\".",
Parameters::kRtabmapImagesAlreadyRectified().c_str(), Parameters::kRtabmapImagesAlreadyRectified().c_str(),
cameraInfoRight->header.frame_id.c_str(), rightCameraInfos[i].header.frame_id.c_str(),
cameraInfoLeft->header.frame_id.c_str()); leftCameraInfos[i].header.frame_id.c_str());
return; 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) if(stereoModel.baseline() == 0 && alreadyRectified)
{ {
stereoTransform = getTransform( stereoTransform = getTransform(
cameraInfoLeft->header.frame_id, leftCameraInfos[i].header.frame_id,
cameraInfoRight->header.frame_id, rightCameraInfos[i].header.frame_id,
cameraInfoLeft->header.stamp); leftCameraInfos[i].header.stamp);
if(!stereoTransform.isNull() && stereoTransform.x()>0) if(!stereoTransform.isNull() && stereoTransform.x()>0)
{ {
@@ -265,7 +428,7 @@ private:
"recommended, we used TF to get the baseline (%s->%s = %fm) for convenience (e.g., D400 ir stereo issue). It is preferred to feed " "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...", "a valid right camera info if stereo images are already rectified. This message is only printed once...",
rtabmap::Parameters::kRtabmapImagesAlreadyRectified().c_str(), 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; warned = true;
} }
stereoModel = rtabmap::StereoCameraModel( stereoModel = rtabmap::StereoCameraModel(
@@ -298,34 +461,113 @@ private:
shown = true; shown = true;
} }
} }
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");
UTimer stepTimer; cv_bridge::CvImageConstPtr ptrLeft = leftImages[i];
// if(leftImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
UDEBUG("localTransform = %s", localTransform.prettyPrint().c_str()); leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
rtabmap::SensorData data( {
ptrImageLeft->image, if(keepColor_ && leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0)
ptrImageRight->image, {
stereoModel, ptrLeft = cv_bridge::cvtColor(leftImages[i], "bgr8");
0, }
rtabmap_ros::timestampFromROS(stamp)); else
{
ptrLeft = cv_bridge::cvtColor(leftImages[i], "mono8");
}
}
std_msgs::Header header; cv_bridge::CvImageConstPtr ptrRight = rightImages[i];
header.stamp = stamp; if(rightImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
header.frame_id = imageRectLeft->header.frame_id; rightImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
this->processData(data, header); {
ptrRight = cv_bridge::cvtColor(leftImages[i], "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());
}
if(ptrLeft->image.type() == left.type())
{
ptrLeft->image.copyTo(cv::Mat(left, cv::Rect(i*leftWidth, 0, leftWidth, leftHeight)));
}
else
{
NODELET_ERROR("Some left images are not the same type! %d vs %d", ptrLeft->image.type(), left.type());
return;
}
if(ptrRight->image.type() == right.type())
{
ptrRight->image.copyTo(cv::Mat(right, cv::Rect(i*rightWidth, 0, rightWidth, rightHeight)));
}
else
{
NODELET_ERROR("Some right images are not the same type! %d vs %d", ptrRight->image.type(), right.type());
return;
}
cameraModels.push_back(stereoModel);
} }
else else
{ {
NODELET_WARN("Odom: input images empty?!?"); NODELET_ERROR("Odom: input images empty?!?");
return;
} }
} }
//
rtabmap::SensorData data(
left,
right,
cameraModels,
0,
rtabmap_ros::timestampFromROS(higherStamp));
std_msgs::Header header;
header.stamp = higherStamp;
header.frame_id = leftImages.size()==1?leftImages[0]->header.frame_id:"";
this->processData(data, header);
}
void callback(
const sensor_msgs::ImageConstPtr& imageLeft,
const sensor_msgs::ImageConstPtr& imageRight,
const sensor_msgs::CameraInfoConstPtr& cameraInfoLeft,
const sensor_msgs::CameraInfoConstPtr& cameraInfoRight)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(1);
std::vector<cv_bridge::CvImageConstPtr> rightMsgs(1);
std::vector<sensor_msgs::CameraInfo> leftInfoMsgs;
std::vector<sensor_msgs::CameraInfo> rightInfoMsgs;
leftMsgs[0] = cv_bridge::toCvShare(imageLeft);
rightMsgs[0] = cv_bridge::toCvShare(imageRight);
leftInfoMsgs.push_back(*cameraInfoLeft);
rightInfoMsgs.push_back(*cameraInfoRight);
double stampDiff = fabs(imageLeft->header.stamp.toSec() - imageRight->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,
imageLeft->header.stamp.toSec(),
imageRight->header.stamp.toSec());
}
this->commonCallback(leftMsgs, leftMsgs, leftInfoMsgs, rightInfoMsgs);
}
} }
void callbackRGBD( void callbackRGBD(
@@ -334,157 +576,119 @@ private:
callbackCalled(); callbackCalled();
if(!this->isPaused()) if(!this->isPaused())
{ {
cv_bridge::CvImageConstPtr imageRectLeft, imageRectRight; std::vector<cv_bridge::CvImageConstPtr> leftMsgs(1);
rtabmap_ros::toCvShare(image, imageRectLeft, imageRectRight); std::vector<cv_bridge::CvImageConstPtr> rightMsgs(1);
std::vector<sensor_msgs::CameraInfo> leftInfoMsgs;
std::vector<sensor_msgs::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 || this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
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 || void callbackRGBDX(
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 || const rtabmap_ros::RGBDImagesConstPtr& images)
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0) || {
!(imageRectRight->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 || callbackCalled();
imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || if(!this->isPaused())
imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 || {
imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || if(images->rgbd_images.empty())
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))
{ {
NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8,rgba8,bgra8 (mono8 recommended), received types are %s (left) and %s (right)", NODELET_ERROR("Input topic \"%s\" doesn't contain any image(s)!", rgbdxSub_.getTopic().c_str());
imageRectLeft->encoding.c_str(), imageRectRight->encoding.c_str());
return; return;
} }
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(images->rgbd_images.size());
ros::Time stamp = imageRectLeft->header.stamp>imageRectRight->header.stamp?imageRectLeft->header.stamp:imageRectRight->header.stamp; std::vector<cv_bridge::CvImageConstPtr> rightMsgs(images->rgbd_images.size());
std::vector<sensor_msgs::CameraInfo> leftInfoMsgs;
Transform localTransform = getTransform(this->frameId(), imageRectLeft->header.frame_id, stamp); std::vector<sensor_msgs::CameraInfo> rightInfoMsgs;
if(localTransform.isNull()) 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);
} }
ros::WallTime time = ros::WallTime::now(); this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
}
}
int quality = -1; void callbackRGBD2(
if(!imageRectLeft->image.empty() && !imageRectRight->image.empty()) const rtabmap_ros::RGBDImageConstPtr& image,
{ const rtabmap_ros::RGBDImageConstPtr& image2)
bool alreadyRectified = true; {
Parameters::parse(parameters(), Parameters::kRtabmapImagesAlreadyRectified(), alreadyRectified); callbackCalled();
rtabmap::Transform stereoTransform; if(!this->isPaused())
if(!alreadyRectified) {
{ std::vector<cv_bridge::CvImageConstPtr> leftMsgs(2);
stereoTransform = getTransform( std::vector<cv_bridge::CvImageConstPtr> rightMsgs(2);
image->depth_camera_info.header.frame_id, std::vector<sensor_msgs::CameraInfo> leftInfoMsgs;
image->rgb_camera_info.header.frame_id, std::vector<sensor_msgs::CameraInfo> rightInfoMsgs;
image->rgb_camera_info.header.stamp); rtabmap_ros::toCvShare(image, leftMsgs[0], rightMsgs[0]);
if(stereoTransform.isNull()) rtabmap_ros::toCvShare(image2, leftMsgs[1], rightMsgs[1]);
{ leftInfoMsgs.push_back(image->rgb_camera_info);
NODELET_ERROR("Parameter %s is false but we cannot get TF between the two cameras!", Parameters::kRtabmapImagesAlreadyRectified().c_str()); leftInfoMsgs.push_back(image2->rgb_camera_info);
return; rightInfoMsgs.push_back(image->depth_camera_info);
} rightInfoMsgs.push_back(image2->depth_camera_info);
}
rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(image->rgb_camera_info, image->depth_camera_info, localTransform); this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
}
}
if(stereoModel.baseline() == 0 && alreadyRectified) void callbackRGBD3(
{ const rtabmap_ros::RGBDImageConstPtr& image,
stereoTransform = getTransform( const rtabmap_ros::RGBDImageConstPtr& image2,
image->rgb_camera_info.header.frame_id, const rtabmap_ros::RGBDImageConstPtr& image3)
image->depth_camera_info.header.frame_id, {
image->rgb_camera_info.header.stamp); callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(3);
std::vector<cv_bridge::CvImageConstPtr> rightMsgs(3);
std::vector<sensor_msgs::CameraInfo> leftInfoMsgs;
std::vector<sensor_msgs::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(!stereoTransform.isNull() && stereoTransform.x()>0) this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
{ }
static bool warned = false; }
if(!warned)
{
ROS_WARN("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());
}
}
if(alreadyRectified && stereoModel.baseline() <= 0) void callbackRGBD4(
{ const rtabmap_ros::RGBDImageConstPtr& image,
NODELET_ERROR("The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo " const rtabmap_ros::RGBDImageConstPtr& image2,
"setup where the Tx (or P(0,3)) is negative in the right camera info msg.", stereoModel.baseline()); const rtabmap_ros::RGBDImageConstPtr& image3,
return; const rtabmap_ros::RGBDImageConstPtr& image4)
} {
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(4);
std::vector<cv_bridge::CvImageConstPtr> rightMsgs(4);
std::vector<sensor_msgs::CameraInfo> leftInfoMsgs;
std::vector<sensor_msgs::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);
if(stereoModel.baseline() > 10.0) this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
{
static bool shown = false;
if(!shown)
{
NODELET_WARN("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;
}
}
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();
}
UTimer stepTimer;
//
UDEBUG("localTransform = %s", localTransform.prettyPrint().c_str());
rtabmap::SensorData data(
left,
right,
stereoModel,
0,
rtabmap_ros::timestampFromROS(stamp));
std_msgs::Header header;
header.stamp = stamp;
header.frame_id = image->header.frame_id;
this->processData(data, header);
}
else
{
NODELET_WARN("Odom: input images empty?!?");
}
} }
} }
@@ -504,6 +708,66 @@ protected:
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
} }
if(approxSync2_)
{
delete approxSync2_;
approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
MyApproxSync2Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_);
approxSync2_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2));
}
if(exactSync2_)
{
delete exactSync2_;
exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
MyExactSync2Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_);
exactSync2_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::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(boost::bind(&StereoOdometry::callbackRGBD3, this, boost::placeholders::_1, boost::placeholders::_2, boost::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(boost::bind(&StereoOdometry::callbackRGBD3, this, boost::placeholders::_1, boost::placeholders::_2, boost::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(boost::bind(&StereoOdometry::callbackRGBD4, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::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(boost::bind(&StereoOdometry::callbackRGBD4, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
}
} }
private: private:
@@ -511,11 +775,32 @@ private:
image_transport::SubscriberFilter imageRectRight_; image_transport::SubscriberFilter imageRectRight_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_; message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_; message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_;
ros::Subscriber rgbdSub_;
ros::Subscriber rgbdxSub_;
message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image1_sub_;
message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image2_sub_;
message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image3_sub_;
message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image4_sub_;
message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image5_sub_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyApproxSyncPolicy; typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_; message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncPolicy; typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncPolicy;
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_; message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
ros::Subscriber rgbdSub_; typedef message_filters::sync_policies::ApproximateTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyApproxSync2Policy;
message_filters::Synchronizer<MyApproxSync2Policy> * approxSync2_;
typedef message_filters::sync_policies::ExactTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyExactSync2Policy;
message_filters::Synchronizer<MyExactSync2Policy> * exactSync2_;
typedef message_filters::sync_policies::ApproximateTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyApproxSync3Policy;
message_filters::Synchronizer<MyApproxSync3Policy> * approxSync3_;
typedef message_filters::sync_policies::ExactTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyExactSync3Policy;
message_filters::Synchronizer<MyExactSync3Policy> * exactSync3_;
typedef message_filters::sync_policies::ApproximateTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyApproxSync4Policy;
message_filters::Synchronizer<MyApproxSync4Policy> * approxSync4_;
typedef message_filters::sync_policies::ExactTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyExactSync4Policy;
message_filters::Synchronizer<MyExactSync4Policy> * exactSync4_;
int queueSize_; int queueSize_;
bool keepColor_; bool keepColor_;
}; };