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

Adjust local transform RGBD data on depth stamp (while using rgb frame because of a zed issue where depth frame is not rgb frame when it should be)

Browse Source
This commit is contained in:
matlabbe
2017-05-17 10:24:10 -04:00
parent a9eeebf24e
commit 26b69581a4
1 changed files with 6 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/MsgConversion.cpp
+6 -5
View File
@@ -1142,26 +1142,27 @@ bool convertRGBDMsgs(
depthHeight,
depthMsgs[i]->image.rows).c_str());
rtabmap::Transform localTransform = rtabmap_ros::getTransform(frameId, cameraInfoMsgs[i].header.frame_id, cameraInfoMsgs[i].header.stamp, listener, waitForTransform);
// 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, depthMsgs[i]->header.stamp, listener, waitForTransform);
if(localTransform.isNull())
{
ROS_ERROR("TF of received depth image %d at time %fs is not set!", i, cameraInfoMsgs[i].header.stamp.toSec());
ROS_ERROR("TF of received depth image %d at time %fs is not set!", i, depthMsgs[i]->header.stamp.toSec());
return false;
}
// sync with odometry stamp
if(!odomFrameId.empty() && odomStamp != cameraInfoMsgs[i].header.stamp)
if(!odomFrameId.empty() && odomStamp != depthMsgs[i]->header.stamp)
{
rtabmap::Transform sensorT = getTransform(
frameId,
odomFrameId,
odomStamp,
cameraInfoMsgs[i].header.stamp,
depthMsgs[i]->header.stamp,
listener,
waitForTransform);
if(sensorT.isNull())
{
ROS_WARN("Could not get odometry value for depth image stamp (%fs). Latest odometry "
"stamp is %fs. The depth image pose will not be synchronized with odometry.", cameraInfoMsgs[i].header.stamp.toSec(), odomStamp.toSec());
"stamp is %fs. The depth image pose will not be synchronized with odometry.", depthMsgs[i]->header.stamp.toSec(), odomStamp.toSec());
}
else
{