Timestamps, Camera Info, Depth Image and more

[mklingen]

EDIT: Looks like most of my problems were solved already in the unreleased "indigo" branch. That's confusing.

Hi guys, Today I was working on improving the ROS driver for some robotics experiments we've been running at Carnegie Mellon. First of all, thank you very much for getting this thing started. It really has sped up our development process.

Unfortunately, there are a few problems with the driver:

• Nothing is timestamped correctly. This is a big issue for synchronizing data with other stuff in ROS. Right now you are just using "ros::Time::now()". This causes the timestamps to be delayed by many milliseconds. The API provides timestamps for the capture time of the depth and color data. These need to be properly converted to ros::Time. According to the API, the timestamps correspond to microseconds as uint64_t, but it is not clear at all what these microseconds are relative to. Possibly they are relative to the start of the program, but they are definitely not UNIX standard time.
• The ROS driver is not publishing camera intrinsics or extrinsics for the sensors. These are also provided by the softkinetic API. In our branch, we've set it up using the proper image_transport CameraPublisher. This requires setting up different nodehandles for depth and RGB, and filling out the intrinsic parameters. For the extrinsics, we publish TF frames relative to "camera_link".
• Speaking of TF frames, your current setup is not correct. It places the RGB camera and Depth camera in the same frame, when they should be offset by some amount. This offset is given by the API in the depth callback.
• The point cloud is not projected according to the ROS standard. In the ROS standard, Z is forward, Y is down, and X is to the right (ie, its left handed). Right now, your point cloud is projected in a right-handed coordinate system, and so it will appear upside down and flipped from ROS perspective.
• The depth image is not even getting created! Somehow, a memcpy got left out of the code, and depth images were simply not getting filled with data. We've fixed that.

When we're done making our changes, we can create a pull request for this.

Regards, Matt

[gavanderhoorn]

@mklingen: sorry about that. The readme was just updated to avoid these kind of things in the future.

[corot]

Yes... apologies for the confusing situation. I suggest you to try indigo branch, but most of the problems you report are NOT fixed there, except for the depth image and for the camera intrinsics published on camera_info. Point by point:

• Nothing is timestamped correctly....
  • You are completely right. Needs to be fixed.
• The ROS driver is not publishing camera intrinsics or extrinsics....
  • We did the same on indigo. BUT we plainly ignore the extrinsic parameters provided for the camera. We make the tfs offsets "measuring by hand"
• Speaking of TF frames...
  • I think they are ok... at least there's the correct gap between RGB and depth. But we did that by hand! It seems to work ok: images and pointclouds seem properly aligned. But I'm not convinced at all about it, and it's different from the kinect tf tree.
• The point cloud is not projected according to the ROS standard...
  • Right, makes sense only if referenced by camera_link. Probably this needs to be fixed, together with the tf tree.
• The depth image is not even getting created...
  • At least this is fine on indigo :grin:

[mklingen]

Okay, thanks for clearing that up! We're going to merge in the indigo branch, and then try fixing the timestamps. Intrinsics, extrinsics, and depth image seem to be fixed in the indigo branch.

[mklingen]

Alright, we figured out how the timestamps work. The timestamps given by the driver are in microseconds of CLOCK_MONOTONIC time (ie, time since the machine started up). We made this little helper function to convert to ROS standard UTC time:

// Assuming timestamp is in microseconds since startup (CLOCK_MONOTONIC), and is earlier
// than the current time, calculates the UTC time in seconds when the timestamp occurred.
double timeStampToRosTime(uint64_t timestamp)
{
    timespec time;
    clock_gettime(CLOCK_MONOTONIC, &time);
    double monotonic_time = time.tv_sec  + time.tv_nsec * 1.0e-9;
    double ros_time = ros::Time::now().toSec();
    return ros_time - (monotonic_time - 1.0e-6 * timestamp);
}

This can be used to create a ROS timestamp in this way:

ros::Time stamp = ros::Time(timeStampToRosTime(data.timeOfCapture));

where "data" is a color or depth callback. Not sure what the best way to contribute this back to the codebase is. We could maybe make a branch off of indigo?

[corot]

Cool! PR on indigo or hydro would be great. We can cherry-pick to the other maintained branch.