odom_sensor_sync

[AD]

hi,

I was trying to get odomSync working here, after reading the code, I think there might be a problem with the calculation of localTransform: https://github.com/introlab/rtabmap_ros/blob/master/rtabmap_conversions/src/MsgConversion.cpp#L2196

According to the code above, in ROS convension, we got:
localTransform = camlink@imageStamp -> baselink@imageStamp; // according to this
sensorT = baselink@odomStamp -> baselink@imageStamp; // according to this

I understand that we are using rtabmap convensions while doing the final calculation, but it seems the two getTransform functions above does not match each other.
It seems to me we need to switch the odomStamp and stamp here.
Can you help with this?

thanks.

[matlabbe]

Based on the current code:

localTransform = baselink@imageStamp -> camlink@imageStamp;
sensorT = baselink@imageStamp -> baselink@odomStamp;

so that final localTransform is

localTransform = sensorT * localTransform;
or
localTransform = baselink@imageStamp -> baselink@odomStamp * baselink@imageStamp -> camlink@imageStamp;


Based on the original code, it was:

odomT = odom -> baselink@odomStamp;
localTransform = baselink -> camlink@imageStamp;
sensorT = odom -> baselink@imageStamp;

localTransform = odomT.inv() * sensorT * localTransform
or
localTransform = baselink@odomStamp -> odom * odom -> baselink@imageStamp * baselink@imageStamp -> camlink@imageStamp;
or with new code it would translate to:
localTransform = baselink@odomStamp -> baselink@imageStamp * baselink@imageStamp -> camlink@imageStamp;

with odomStamp and stamp inverted like you pointed out here. That commit indeed broke it.

I renamed the getTransform() with fixed frame to getMovingTransform() and changed the order of the input stamps in that commit. So it is inverting the odomStamp and odom implicitly.

I tested odom_sensor_sync in simulation with both images or scans not at the same rate than odom, and it seems to work correctly now.

Thanks to have reported this!
Mathieu