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

rtabmap: support rgbd_image without rgb/depth data (only features set).

Browse Source
This commit is contained in:
matlabbe
2021-06-27 10:50:17 -04:00
parent ed2451c351
commit 4304bae9f6
2 changed files with 77 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+3 -3
View File
@@ -1167,7 +1167,7 @@ void CoreWrapper::commonDepthCallback(
return; return;
} }
} }
else if(imageMsgs.size() == 0 || imageMsgs[0].get() == 0 || !odomUpdate(odomMsg, imageMsgs[0]->header.stamp)) else if(cameraInfoMsgs.size() == 0 || !odomUpdate(odomMsg, cameraInfoMsgs[0].header.stamp))
{ {
return; return;
} }
@@ -1186,7 +1186,7 @@ void CoreWrapper::commonDepthCallback(
return; return;
} }
} }
else if(imageMsgs.size() == 0 || imageMsgs[0].get() == 0 || !odomTFUpdate(imageMsgs[0]->header.stamp)) else if(cameraInfoMsgs.size() == 0 || !odomTFUpdate(cameraInfoMsgs[0].header.stamp))
{ {
return; return;
} }
@@ -1382,7 +1382,7 @@ void CoreWrapper::commonDepthCallbackImpl(
rgb, rgb,
depth, depth,
cameraModels, cameraModels,
lastPoseIntermediate_?-1:imageMsgs[0]->header.seq, lastPoseIntermediate_?-1:!cameraInfoMsgs.empty()?cameraInfoMsgs[0].header.seq:0,
rtabmap_ros::timestampFromROS(lastPoseStamp_), rtabmap_ros::timestampFromROS(lastPoseStamp_),
userData); userData);
src/MsgConversion.cpp
+74 -62
View File
@@ -1718,41 +1718,51 @@ bool convertRGBDMsgs(
std::vector<cv::Point3f> * localPoints3d, std::vector<cv::Point3f> * localPoints3d,
cv::Mat * localDescriptors) cv::Mat * localDescriptors)
{ {
UASSERT(imageMsgs.size()>0 && UASSERT(!cameraInfoMsgs.empty()>0 &&
(imageMsgs.size() == depthMsgs.size() || depthMsgs.empty()) && (cameraInfoMsgs.size() == imageMsgs.size() || imageMsgs.empty()) &&
imageMsgs.size() == cameraInfoMsgs.size()); (cameraInfoMsgs.size() == depthMsgs.size() || depthMsgs.empty()));
int imageWidth = imageMsgs[0]->image.cols; int imageWidth = imageMsgs.size()?imageMsgs[0]->image.cols:cameraInfoMsgs[0].width;
int imageHeight = imageMsgs[0]->image.rows; int imageHeight = imageMsgs.size()?imageMsgs[0]->image.rows:cameraInfoMsgs[0].height;
int depthWidth = depthMsgs.size()?depthMsgs[0]->image.cols:0; int depthWidth = depthMsgs.size()?depthMsgs[0]->image.cols:0;
int depthHeight = depthMsgs.size()?depthMsgs[0]->image.rows:0; int depthHeight = depthMsgs.size()?depthMsgs[0]->image.rows:0;
if(depthMsgs.size()) if(!depthMsgs.empty())
{ {
UASSERT_MSG( UASSERT_MSG(
imageWidth/depthWidth == imageHeight/depthHeight, imageWidth/depthWidth == imageHeight/depthHeight,
uFormat("rgb=%dx%d depth=%dx%d", imageWidth, imageHeight, depthWidth, depthHeight).c_str()); uFormat("rgb=%dx%d depth=%dx%d", imageWidth, imageHeight, depthWidth, depthHeight).c_str());
} }
int cameraCount = imageMsgs.size(); int cameraCount = cameraInfoMsgs.size();
for(unsigned int i=0; i<imageMsgs.size(); ++i) for(unsigned int i=0; i<cameraInfoMsgs.size(); ++i)
{ {
if(!(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 || if(!imageMsgs.empty())
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BAYER_RGGB8) == 0))
{ {
if(!(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BAYER_RGGB8) == 0))
{
ROS_ERROR("Input rgb type must be image=mono8,mono16,rgb8,bgr8,bgra8,rgba8. Current rgb=%s", ROS_ERROR("Input rgb type must be image=mono8,mono16,rgb8,bgr8,bgra8,rgba8. Current rgb=%s",
imageMsgs[i]->encoding.c_str()); imageMsgs[i]->encoding.c_str());
return false; return false;
}
UASSERT_MSG(imageMsgs[i]->image.cols == imageWidth && imageMsgs[i]->image.rows == imageHeight,
uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
imageWidth,
imageMsgs[i]->image.cols,
imageHeight,
imageMsgs[i]->image.rows).c_str());
} }
if(depthMsgs.size() && if(!depthMsgs.empty() &&
!(depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 || !(depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 || depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)) depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
@@ -1762,14 +1772,9 @@ bool convertRGBDMsgs(
return false; return false;
} }
UASSERT_MSG(imageMsgs[i]->image.cols == imageWidth && imageMsgs[i]->image.rows == imageHeight,
uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
imageWidth,
imageMsgs[i]->image.cols,
imageHeight,
imageMsgs[i]->image.rows).c_str());
ros::Time stamp; ros::Time stamp;
if(depthMsgs.size()) if(!depthMsgs.empty())
{ {
UASSERT_MSG(depthMsgs[i]->image.cols == depthWidth && depthMsgs[i]->image.rows == depthHeight, UASSERT_MSG(depthMsgs[i]->image.cols == depthWidth && depthMsgs[i]->image.rows == depthHeight,
uFormat("depthWidth=%d vs %d imageHeight=%d vs %d", uFormat("depthWidth=%d vs %d imageHeight=%d vs %d",
@@ -1779,13 +1784,17 @@ bool convertRGBDMsgs(
depthMsgs[i]->image.rows).c_str()); depthMsgs[i]->image.rows).c_str());
stamp = depthMsgs[i]->header.stamp; stamp = depthMsgs[i]->header.stamp;
} }
else else if(!imageMsgs.empty())
{ {
stamp = imageMsgs[i]->header.stamp; stamp = imageMsgs[i]->header.stamp;
} }
else
{
stamp = cameraInfoMsgs[i].header.stamp;
}
// use depth's stamp so that geometry is sync to odom, use rgb frame as we assume depth is registered (normally depth msg should have same frame than rgb) // use depth's stamp so that geometry is sync to odom, use rgb frame as we assume depth is registered (normally depth msg should have same frame than rgb)
rtabmap::Transform localTransform = rtabmap_ros::getTransform(frameId, imageMsgs[i]->header.frame_id, stamp, listener, waitForTransform); rtabmap::Transform localTransform = rtabmap_ros::getTransform(frameId, !imageMsgs.empty()?imageMsgs[i]->header.frame_id:cameraInfoMsgs[i].header.frame_id, stamp, listener, waitForTransform);
if(localTransform.isNull()) if(localTransform.isNull())
{ {
ROS_ERROR("TF of received image %d at time %fs is not set!", i, stamp.toSec()); ROS_ERROR("TF of received image %d at time %fs is not set!", i, stamp.toSec());
@@ -1813,38 +1822,41 @@ bool convertRGBDMsgs(
} }
} }
cv_bridge::CvImageConstPtr ptrImage = imageMsgs[i]; if(!imageMsgs.empty())
if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0)
{ {
// do nothing cv_bridge::CvImageConstPtr ptrImage = imageMsgs[i];
} if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0 ||
else if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0) imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
{ imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0)
ptrImage = cv_bridge::cvtColor(imageMsgs[i], "mono8"); {
} // do nothing
else }
{ else if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
ptrImage = cv_bridge::cvtColor(imageMsgs[i], "bgr8"); {
ptrImage = cv_bridge::cvtColor(imageMsgs[i], "mono8");
}
else
{
ptrImage = cv_bridge::cvtColor(imageMsgs[i], "bgr8");
}
// initialize
if(rgb.empty())
{
rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type());
}
if(ptrImage->image.type() == rgb.type())
{
ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
}
else
{
ROS_ERROR("Some RGB images are not the same type!");
return false;
}
} }
// initialize if(!depthMsgs.empty())
if(rgb.empty())
{
rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type());
}
if(ptrImage->image.type() == rgb.type())
{
ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
}
else
{
ROS_ERROR("Some RGB images are not the same type!");
return false;
}
if(depthMsgs.size())
{ {
cv_bridge::CvImageConstPtr ptrDepth = depthMsgs[i]; cv_bridge::CvImageConstPtr ptrDepth = depthMsgs[i];
cv::Mat subDepth = ptrDepth->image; cv::Mat subDepth = ptrDepth->image;
@@ -1867,16 +1879,16 @@ bool convertRGBDMsgs(
cameraModels.push_back(rtabmap_ros::cameraModelFromROS(cameraInfoMsgs[i], localTransform)); cameraModels.push_back(rtabmap_ros::cameraModelFromROS(cameraInfoMsgs[i], localTransform));
if(localKeyPoints && localKeyPointsMsgs.size() == imageMsgs.size()) if(localKeyPoints && localKeyPointsMsgs.size() == cameraInfoMsgs.size())
{ {
rtabmap_ros::keypointsFromROS(localKeyPointsMsgs[i], *localKeyPoints, imageWidth*i); rtabmap_ros::keypointsFromROS(localKeyPointsMsgs[i], *localKeyPoints, imageWidth*i);
} }
if(localPoints3d && localPoints3dMsgs.size() == imageMsgs.size()) if(localPoints3d && localPoints3dMsgs.size() == cameraInfoMsgs.size())
{ {
// Points should be in base frame // Points should be in base frame
rtabmap_ros::points3fFromROS(localPoints3dMsgs[i], *localPoints3d, localTransform); rtabmap_ros::points3fFromROS(localPoints3dMsgs[i], *localPoints3d, localTransform);
} }
if(localDescriptors && localDescriptorsMsgs.size() == imageMsgs.size()) if(localDescriptors && localDescriptorsMsgs.size() == cameraInfoMsgs.size())
{ {
localDescriptors->push_back(localDescriptorsMsgs[i]); localDescriptors->push_back(localDescriptorsMsgs[i]);
} }