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

Rtabmap/AlreadyRectified=false: handling a case where camera_info topic doesn't have frame_id set but calibration is fine.

Browse Source
This commit is contained in:
matlabbe
2022-12-10 14:06:04 -08:00
parent e34ea3e0b3
commit d9996a03f8
2 changed files with 98 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/MsgConversion.cpp
+50 -12
View File
@@ -2062,16 +2062,42 @@ bool convertRGBDMsgs(
rtabmap::Transform stereoTransform;
if(!alreadRectifiedImages)
{
stereoTransform = getTransform(
depthCameraInfoMsgs[i].header.frame_id,
cameraInfoMsgs[i].header.frame_id,
cameraInfoMsgs[i].header.stamp,
listener,
waitForTransform);
if(stereoTransform.isNull())
if(depthCameraInfoMsgs[i].header.frame_id.empty() || cameraInfoMsgs[i].header.frame_id.empty())
{
ROS_ERROR("Parameter %s is false but we cannot get TF between the two cameras!", rtabmap::Parameters::kRtabmapImagesAlreadyRectified().c_str());
return false;
if(depthCameraInfoMsgs[i].P[3] == 0.0 && cameraInfoMsgs[i].P[3] == 0)
{
ROS_ERROR("Parameter %s is false but the frame_id in one of the camera_info "
"topic is empty, so TF between the cameras cannot be computed!",
rtabmap::Parameters::kRtabmapImagesAlreadyRectified().c_str());
return false;
}
else
{
static bool warned = false;
if(!warned)
{
ROS_WARN("Parameter %s is false but the frame_id in one of the "
"camera_info topic is empty, so TF between the cameras cannot be "
"computed! However, the baseline can be computed from the calibration, "
"we will use this one instead of TF. This message is only printed once...",
rtabmap::Parameters::kRtabmapImagesAlreadyRectified().c_str());
warned = true;
}
}
}
else
{
stereoTransform = getTransform(
depthCameraInfoMsgs[i].header.frame_id,
cameraInfoMsgs[i].header.frame_id,
cameraInfoMsgs[i].header.stamp,
listener,
waitForTransform);
if(stereoTransform.isNull())
{
ROS_ERROR("Parameter %s is false but we cannot get TF between the two cameras!", rtabmap::Parameters::kRtabmapImagesAlreadyRectified().c_str());
return false;
}
}
}
@@ -2092,16 +2118,28 @@ bool convertRGBDMsgs(
}
else if(stereoModel.baseline() == 0 && alreadRectifiedImages)
{
rtabmap::Transform stereoTransform = getTransform(
rtabmap::Transform stereoTransform;
if( !cameraInfoMsgs[i].header.frame_id.empty() &&
!depthCameraInfoMsgs[i].header.frame_id.empty())
{
stereoTransform = getTransform(
cameraInfoMsgs[i].header.frame_id,
depthCameraInfoMsgs[i].header.frame_id,
cameraInfoMsgs[i].header.stamp,
listener,
waitForTransform);
}
if(stereoTransform.isNull() || stereoTransform.x()<=0)
{
ROS_WARN("We cannot estimated the baseline of the rectified images with tf! (%s->%s = %s)",
depthCameraInfoMsgs[i].header.frame_id.c_str(), cameraInfoMsgs[i].header.frame_id.c_str(), stereoTransform.prettyPrint().c_str());
if(cameraInfoMsgs[i].header.frame_id.empty() || depthCameraInfoMsgs[i].header.frame_id.empty())
{
ROS_WARN("We cannot estimated the baseline of the rectified images with tf! (camera_info topics have empty frame_id)");
}
else
{
ROS_WARN("We cannot estimated the baseline of the rectified images with tf! (%s->%s = %s)",
depthCameraInfoMsgs[i].header.frame_id.c_str(), cameraInfoMsgs[i].header.frame_id.c_str(), stereoTransform.prettyPrint().c_str());
}
}
else
{
src/nodelets/stereo_odometry.cpp
+48 -19
View File
@@ -389,33 +389,62 @@ private:
rtabmap::Transform stereoTransform;
if(!alreadyRectified)
{
stereoTransform = getTransform(
rightCameraInfos[i].header.frame_id,
leftCameraInfos[i].header.frame_id,
leftCameraInfos[i].header.stamp);
if(stereoTransform.isNull())
if(rightCameraInfos[i].header.frame_id.empty() || leftCameraInfos[i].header.frame_id.empty())
{
NODELET_ERROR("Parameter %s is false but we cannot get TF between the two cameras! (between frames %s and %s)",
Parameters::kRtabmapImagesAlreadyRectified().c_str(),
rightCameraInfos[i].header.frame_id.c_str(),
leftCameraInfos[i].header.frame_id.c_str());
return;
if(rightCameraInfos[i].P[3] == 0.0 && leftCameraInfos[i].P[3] == 0)
{
NODELET_ERROR("Parameter %s is false but the frame_id in one of the camera_info "
"topic is empty, so TF between the cameras cannot be computed!",
Parameters::kRtabmapImagesAlreadyRectified().c_str());
return;
}
else
{
static bool warned = false;
if(!warned)
{
NODELET_WARN("Parameter %s is false but the frame_id in one of the "
"camera_info topic is empty, so TF between the cameras cannot be "
"computed! However, the baseline can be computed from the calibration, "
"we will use this one instead of TF. This message is only printed once...",
Parameters::kRtabmapImagesAlreadyRectified().c_str());
warned = true;
}
}
}
else if(stereoTransform.isIdentity())
else
{
NODELET_ERROR("Parameter %s is false but we cannot get a valid TF between the two cameras! "
"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(),
rightCameraInfos[i].header.frame_id.c_str(),
leftCameraInfos[i].header.frame_id.c_str());
return;
stereoTransform = getTransform(
rightCameraInfos[i].header.frame_id,
leftCameraInfos[i].header.frame_id,
leftCameraInfos[i].header.stamp);
if(stereoTransform.isNull())
{
NODELET_ERROR("Parameter %s is false but we cannot get TF between the two cameras! (between frames %s and %s)",
Parameters::kRtabmapImagesAlreadyRectified().c_str(),
rightCameraInfos[i].header.frame_id.c_str(),
leftCameraInfos[i].header.frame_id.c_str());
return;
}
else if(stereoTransform.isIdentity())
{
NODELET_ERROR("Parameter %s is false but we cannot get a valid TF between the two cameras! "
"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(),
rightCameraInfos[i].header.frame_id.c_str(),
leftCameraInfos[i].header.frame_id.c_str());
return;
}
}
}
rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(leftCameraInfos[i], rightCameraInfos[i], localTransform, stereoTransform);
if(stereoModel.baseline() == 0 && alreadyRectified)
if( stereoModel.baseline() == 0 &&
alreadyRectified &&
!rightCameraInfos[i].header.frame_id.empty() &&
!leftCameraInfos[i].header.frame_id.empty())
{
stereoTransform = getTransform(
leftCameraInfos[i].header.frame_id,