anqixu / ueye_cam

A ROS nodelet and node that wraps the driver API for UEye cameras by IDS Imaging Development Systems GMBH.
Other
60 stars 102 forks source link

Multiple images with identical timestamp #37

Open anuppari opened 8 years ago

anuppari commented 8 years ago

My image callback is getting called multiple images with the same timestamp. This happens intermittently, about once a second, where the camera is publishing at ~80fps and my subscriber runs at ~40 hz with a queue length of 1.

To debug, I have been saving the last timestamp and image, as well as the current, and if they are the same, I save both images to a file, as well as an image of the difference scaled to the image depth.

Example previous image: lastimage0 Example current image: thisimage0 scaled difference: diffimagescaled0

More examples of the scaled image differences diffimagescaled1 diffimagescaled2

The black regions should indicate where the images are identical, so it appears that my callback is reading an image while a new image is being written. I am using cv_bridge::toCvCopy immediately in my callback, so I'm not sure why the image is being overwritten during reading, especially since I am NOT getting the IS_TIMED_OUT error. However, even if this is an issue with my callback not being fast enough, I'm not sure why my callback is being called twice with the same timestamp?

anuppari commented 8 years ago

Taking and saving the difference between every consecutive pair of images yields images like the one below, where there is no black region. This seems to indicate that the two issues of reading while writing and identical timestamps are related.

diffimagescaled10

anqixu commented 8 years ago

Let's try to address the 2 issues separately.

The "identical timestamp" issue:

I have a strong hypothesis for the source of the issue. ueye_cam uses is_getImageInfo() to set each frame's timestamp. The returned structure contains 2 sources of time, namely u64TimestampDevice, which is a 0.1us clock tick counter, and UEYETIME sub-structure, which contains a timestamp synchronized with the PC's system time, and has a resolution of 1ms.

ueye_cam currently uses the latter UEYETIME source to set the ROS frame's timestamp. The documentation warns that PC time synchronization occurs every 60 seconds, and "may cause minor time shifts in the value passed in UEYETIME (average time about 3 ms)." This seems to be consistent with the symptom that you described.

To resolve this issue, the code would need to switch to using u64TimestampDevice field for computing relative time with 0.1us precision. We would also need an initial source of absolute time (e.g. save UEYEIMAGEINFO and u64TimestampDevice fields for the very first frame).

I'll try to implement this soon, although I'm quite swamped right now, and I actually don't have readily access to a uEye camera. If you or someone else wants to implement this change, see: https://github.com/anqixu/ueye_cam/blob/master/src/ueye_cam_driver.cpp#L1290 https://github.com/anqixu/ueye_cam/blob/master/src/ueye_cam_nodelet.cpp#L1058

The "write-during-read" issue: For efficiency sakes, ueye_cam passes a raw buffer to the underlying IDS daemon. The buffer is filled by the IDS code, and in particular during free-run mode I'd presume it would update the buffer without waiting for clients.

Again, in free-run mode, ueye_cam waits for the IDS daemon to acknowledge a new frame event, and then ueye_cam copies the contents of the raw buffer into a ROS buffer. This second buffer is the one being published on the ROS topic.

I suspect that due to the mismatch between insufficient CPU cycles and excessive camera frame rate, the issue arises when IDS writes part of the next frame, while ueye_cam is in the process of std::copy()ing the buffer.

To resolve this issue, we would need to switch from using a single raw buffer, to a sequence of FIFO buffers. Unfortunately this would require code changes in a few places, such as make multiple calls to is_AddToSequence in reallocateCamBuffer, and maybe switching from is_WaitEvent to is_WaitForNextImage. Since these changes will affect both free-run and externally triggered modes, thorough testing will be required.

Unfortunately I cannot commit to this amount of code update/addition, due to an extremely tight schedule. Again, if you or anyone else wishes to contribute and test, I would gladly accept a code review + git pull request.

anuppari commented 8 years ago

I changed the timestamp source to the tick values as suggested, but this does not fix the issue.

During debugging, I noticed that sometimes the getImageTimestamp would get called and return the same timestamp twice. Is it possible that the is_WaitEvent does not pull down the IS_SET_EVENT_FRAME once that image has been captured, so on the next loop, is_WaitEvent returns success on the same event? Their timing diagram would suggest that it's possible for the event to get raised, the image buffer to get copied by ueye_cam, and then the subsequent call to is_WaitEvent also returns success just before the event is brought down and the next image is transferred, resulting in a read while write.

As I mentioned above, the read-while-write only seems to happen when there are identical timestamps. If I save all consecutive image differences, the only ones that have a black region are the ones that occur with identical timestamps, suggesting these problems are related.

anqixu commented 8 years ago

You do raise an interesting concern, regarding who is responsible for pulling down the FRAME event flag. I had assumed that IDS would do this automatically if is_WaitEvent returns successful.

I'll look into this issue later today. Nevertheless, I've not encountered this issue before, since ueye_cam_nodelet uses a blanket 200Hz spin rate, and with 15Hz camera frame rates, my bags did not show any repeated frames.

snakehaihai commented 5 years ago

change the image header.timestamp to ros::Time::now() ? Try this?