When I tested IVT on ARMAR-6, I noticed that the provided object poses were not really stable but moved similar to the platform motion. This PR should help to investigate whether using the new memory to retrieve the robot state eliminates these issues. Previously, the robot was just synchronized to the current timestamp but the object detections are from the past (eventually ~200ms) and therefore, the transformation robot->global or camera->global is incorrect.
The major change is thus l.194 in Segment.cpp: ARMARX_CHECK(virtualRobotReader->synchronizeRobot(*robot, timestamp));
Here, now the provided timestamp is used.
ToDo:
as in most cases, the timestamps of all provided object poses in reportObjectPoses() might be similar, the robot state only needs to be updated once and not for each object pose individually.
Fabian Reister authored