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

stereo_odometry: added baseline=0 workaround for rgbd_image input

Browse Source
This commit is contained in:
matlabbe
2020-10-12 16:09:30 -04:00
parent f7f9e82cb1
commit 10ed0109b8
2 changed files with 51 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/PreferencesDialogROS.cpp
+1
View File
@@ -131,6 +131,7 @@ bool PreferencesDialogROS::readCoreSettings(const QString & filePath)
}
else
{
ROS_WARN("Parameter %s not found", i->first.c_str());
validParameters = false;
}
}
src/nodelets/stereo_odometry.cpp
+50 -3
View File
@@ -324,10 +324,57 @@ private:
int quality = -1;
if(!imageRectLeft->image.empty() && !imageRectRight->image.empty())
{
rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(image->rgb_camera_info, image->depth_camera_info, localTransform);
if(stereoModel.baseline() <= 0)
bool alreadyRectified = true;
Parameters::parse(parameters(), Parameters::kRtabmapImagesAlreadyRectified(), alreadyRectified);
rtabmap::Transform stereoTransform;
if(!alreadyRectified)
{
NODELET_FATAL("The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo "
stereoTransform = getTransform(
image->depth_camera_info.header.frame_id,
image->rgb_camera_info.header.frame_id,
image->rgb_camera_info.header.stamp);
if(stereoTransform.isNull())
{
NODELET_ERROR("Parameter %s is false but we cannot get TF between the two cameras!", Parameters::kRtabmapImagesAlreadyRectified().c_str());
return;
}
}
rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(image->rgb_camera_info, image->depth_camera_info, localTransform);
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);
if(!stereoTransform.isNull() && stereoTransform.x()>0)
{
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)
{
NODELET_ERROR("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;
}