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

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, depthHeight,
depthMsgs[i]->image.rows).c_str()); 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()) 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; return false;
} }
// sync with odometry stamp // sync with odometry stamp
if(!odomFrameId.empty() && odomStamp != cameraInfoMsgs[i].header.stamp) if(!odomFrameId.empty() && odomStamp != depthMsgs[i]->header.stamp)
{ {
rtabmap::Transform sensorT = getTransform( rtabmap::Transform sensorT = getTransform(
frameId, frameId,
odomFrameId, odomFrameId,
odomStamp, odomStamp,
cameraInfoMsgs[i].header.stamp, depthMsgs[i]->header.stamp,
listener, listener,
waitForTransform); waitForTransform);
if(sensorT.isNull()) if(sensorT.isNull())
{ {
ROS_WARN("Could not get odometry value for depth image stamp (%fs). Latest odometry " 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 else
{ {