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

OdometryROS: refactored imu sync (#331). Using tf to compute baseline if Rtabmap/ImagesAlreadyRectified is true (for convenience with realsense rectified IR images but right camera info Tx not set)

Browse Source
This commit is contained in:
matlabbe
2020-07-30 12:43:44 -04:00
parent 0f7044f6ff
commit ae854dfb4c
10 changed files with 134 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/MsgConversion.cpp
+37 -1
View File
@@ -1767,7 +1767,8 @@ bool convertStereoMsg(
cv::Mat & right,
rtabmap::StereoCameraModel & stereoModel,
tf::TransformListener & listener,
double waitForTransform)
double waitForTransform,
bool alreadyRectified)
{
UASSERT(leftImageMsg.get() && rightImageMsg.get());
@@ -1843,6 +1844,41 @@ bool convertStereoMsg(
shown = true;
}
}
else if(stereoModel.baseline() == 0 && alreadyRectified)
{
rtabmap::Transform stereoTransform = getTransform(
leftCamInfoMsg.header.frame_id,
rightCamInfoMsg.header.frame_id,
leftCamInfoMsg.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)",
rightCamInfoMsg.header.frame_id.c_str(), leftCamInfoMsg.header.frame_id.c_str(), stereoTransform.prettyPrint().c_str());
}
else
{
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(),
rightCamInfoMsg.header.frame_id.c_str(), leftCamInfoMsg.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());
}
}
return true;
}