Frame timestamping issue

[chutsu]

Frame timestamping issue

[chutsu]

Isn't this the wrong way of timestamping the image data?

So my original intent was to get the timestamp at camera frame mid-exposure (hence I used the RS2_FRAME_METADATA_SENSOR_TIMESTAMP key), because following this paper:

Nikolic, Janosch, et al. "A synchronized visual-inertial sensor system with FPGA pre-processing for
accurate real-time SLAM." 2014 IEEE international conference on robotics and automation (ICRA).
IEEE, 2014.

It claims that

It is an established fact in photogrammetry, that images should be timestamped by their mid-exposure
time, and in previous work [12], it could be shown that neglecting image exposure time in timestamping data has an observable effect, which suggests that it could adversely affect image-based state estimation.

---

The timestamp outputted from that key will give you a timestamp that is not synced up with the get_timestamp() that the gyro and accel are using.

I'm aware of this issue. I'll have to think a little more on how best to handle this.

---

I had also tried to run Kalibr with just using the get_timestamp() from both the frame and IMU data, but I get a projection error of an order of 90px+. I'm thinking it's a timestamp issue.

Looking at the Kalibr ROS Bag wiki page, it could be that they're expecting a proper UNIX timestamp in nano seconds, as supposed to from when the realsense was first turned on. Might have to change the timestamp to reflect that.

[MoBaT]

Yeah, the timestamping was the issue! It works well now. For the camera frame timestamping, I did:

const auto sensor_ts = vf.get_frame_metadata(RS2_FRAME_METADATA_SENSOR_TIMESTAMP);
const auto frame_ts = vf.get_frame_metadata(RS2_FRAME_METADATA_FRAME_TIMESTAMP);

const auto exposure_time_ms = (frame_ts - sensor_ts) / 1000.0;
const auto timestamp = vf.get_timestamp() - exposure_time_ms;

Here's my Kalibr output with the changes:

Calibration results
===================
Normalized Residuals
----------------------------
Reprojection error (cam0):     mean 0.103512759369, median 0.0866073840209, std: 0.0777440308941
Reprojection error (cam1):     mean 0.105192409174, median 0.0887114010436, std: 0.0809908657543
Gyroscope error (imu0):        mean 0.183580027215, median 0.14337365282, std: 0.161935769698
Accelerometer error (imu0):    mean 0.261783018834, median 0.226140625274, std: 0.183251748362

Residuals
----------------------------
Reprojection error (cam0) [px]:     mean 0.103512759369, median 0.0866073840209, std: 0.0777440308941
Reprojection error (cam1) [px]:     mean 0.105192409174, median 0.0887114010436, std: 0.0809908657543
Gyroscope error (imu0) [rad/s]:     mean 0.0114118282805, median 0.00891249190209, std: 0.0100663630152
Accelerometer error (imu0) [m/s^2]: mean 0.147511885618, median 0.127427784268, std: 0.103260368316

Transformation (cam0):
-----------------------
T_ci:  (imu0 to cam0): 
[[ 0.99998508 -0.00162086  0.00521714  0.00262298]
 [ 0.00158255  0.99997182  0.00733798  0.00217727]
 [-0.00522888 -0.00732961  0.99995947 -0.01850177]
 [ 0.          0.          0.          1.        ]]

T_ic:  (cam0 to imu0): 
[[ 0.99998508  0.00158255 -0.00522888 -0.00272313]
 [-0.00162086  0.99997182 -0.00732961 -0.00230857]
 [ 0.00521714  0.00733798  0.99995947  0.01847136]
 [ 0.          0.          0.          1.        ]]

timeshift cam0 to imu0: [s] (t_imu = t_cam + shift)
0.00801364326483467


Transformation (cam1):
-----------------------
T_ci:  (imu0 to cam1): 
[[ 0.99998021 -0.00189472  0.00599863 -0.04771077]
 [ 0.00185091  0.99997164  0.00729978  0.002146  ]
 [-0.00601229 -0.00728854  0.99995536 -0.01857657]
 [ 0.          0.          0.          1.        ]]

T_ic:  (cam1 to imu0): 
[[ 0.99998021  0.00185091 -0.00601229  0.04759417]
 [-0.00189472  0.99997164 -0.00728854 -0.00237173]
 [ 0.00599863  0.00729978  0.99995536  0.01884627]
 [ 0.          0.          0.          1.        ]]

timeshift cam1 to imu0: [s] (t_imu = t_cam + shift)
0.008011714256880343

Baselines:
----------
Baseline (cam0 to cam1): 
[[ 0.99999966 -0.00026813  0.00078349 -0.05031867]
 [ 0.00026816  0.99999996 -0.0000396  -0.00003271]
 [-0.00078348  0.00003981  0.99999969 -0.00007283]
 [ 0.          0.          0.          1.        ]]
baseline norm:  0.050318734134735144 [m]


Gravity vector in target coords: [m/s^2]
[-0.02223012 -9.74645326 -1.0837793 ]


Calibration configuration
=========================

cam0
-----
  Camera model: pinhole
  Focal length: [384.35386606462447, 384.9560729180638]
  Principal point: [319.28590839603237, 239.87334366520707]
  Distortion model: radtan
  Distortion coefficients: [0.003911246673083202, -0.0034005376214794316, 0.0004081791451489365, 0.0007181727055388892]
  Type: aprilgrid
  Tags: 
    Rows: 6
    Cols: 6
    Size: 0.048 [m]
    Spacing 0.0144 [m]


cam1
-----
  Camera model: pinhole
  Focal length: [384.32186243851334, 384.9214612504451]
  Principal point: [319.08615697476637, 239.98593476287843]
  Distortion model: radtan
  Distortion coefficients: [0.004378427247850674, -0.0041355241799928425, 0.0005900340360341945, 0.0002999052406894697]
  Type: aprilgrid
  Tags: 
    Rows: 6
    Cols: 6
    Size: 0.048 [m]
    Spacing 0.0144 [m]



IMU configuration
=================

IMU0:
----------------------------
  Model: calibrated
  Update rate: 500.0
  Accelerometer:
    Noise density: 0.0252 
    Noise density (discrete): 0.56348913033 
    Random walk: 0.000441
  Gyroscope:
    Noise density: 0.00278
    Noise density (discrete): 0.0621626897745 
    Random walk: 1.65e-05
  T_i_b
    [[1. 0. 0. 0.]
     [0. 1. 0. 0.]
     [0. 0. 1. 0.]
     [0. 0. 0. 1.]]
  time offset with respect to IMU0: 0.0 [s]

[chutsu]

Thanks @MoBaT ! Will check it out soon. But first glance I agree with your logic.

For future reference the RS2_FRAME_METADATA_SENSOR_TIMESTAMP gives time at mid exposure relative to device start time (i.e. 0 s), and NOT relative to when the frame capture started. To get that @MoBaT caculated the half exposure time (aka mid exposure time) relatve to the frame capture time by doing the following:

const auto half_exposure_time_ms = (frame_ts - sensor_ts) / 1000.0;

Then he used it to correct the actual timestamp since EPOCH via

const auto timestamp_ms = vf.get_timestamp() - half_exposure_time_ms;

where vf.get_timestamp() is assumed to return the timestamp at the end of frame capture. The documentation is a bit ambiguous on this.

[chutsu]

@MoBaT

• Was wondering if you could upload your Kalibr report as well for future reference? Details such as reprojection error plot as well as the predicted vs measured angular velocities and forces would be a good indicator of the calibration quality. Looking at your reprojection errors they look really good though at 0.1px RMSE.

• The IMU update rate should be 450Hz instead of 500Hz as you specified, right?

• Checkout PR #2 , let me know if this is good enough for you.

[MoBaT]

@chutsu

• The IMU rate should be 450Hz. I think I put it incorrectly into my YAML file for the IMU sensor when putting it into Kalibr. I don't think this should affect the calibration.

• Here's a copy of my PDF calibration file

• The only difference with your PR and what I did was that instead of:
  const auto ts_s = (vf.get_timestamp() - half_exposure_time_ms) * 1e-3;
  I did:
  const auto ts = (vf.get_timestamp() - half_exposure_time_ms) * 1e6.
  Kalibr wanted the timestamps in nanoseconds and I think the realsense ROS repo also outputs them in nanoseconds. You will also need to change the IMU's timestamp to let it be in nanoseconds.

Maybe a better way of doing things if you're going to keep the data outputted in nanoseconds is to not multiple by 1e-3 when getting the half_expsour_time_ms so we don't lose resolution, convert get_timestamp() to microseconds and then do the subtraction. Then at the end, multiply the result by 1e3 to get it into nanoseconds. I think this is what I should've done.

So what I propose is:

static double vframe2ts(const rs2::video_frame &vf) {
  // Calculate half of the exposure time
  const auto frame_ts_us = vf.get_frame_metadata(RS2_FRAME_METADATA_FRAME_TIMESTAMP);
  const auto sensor_ts_us = vf.get_frame_metadata(RS2_FRAME_METADATA_SENSOR_TIMESTAMP);
  const auto half_exposure_time_us = (frame_ts_us - sensor_ts_us);

  // Calculate corrected timestamp
  const auto ts_ns = (vf.get_timestamp() * 1e3 - half_exposure_time_us) * 1e3;

  return ts_ns;
}

My data looked something like this at the end (Just showing an example of how my timestamps looked):

#timestamp [ns], w_RS_S_x [rad s^-1], w_RS_S_y [rad s^-1], w_RS_S_z [rad s^-1], a_RS_S_x [m s^-2], a_RS_S_y [m s^-2], a_RS_S_z [m s^-2]
1564084459562103808, 0.031416, 0.111701, 0.055851, -0.144963, -9.767424, 0.318506
1564084459567111168, 0.024435, 0.120428, 0.068068, -0.201402, -9.784768, 0.234628
1564084459564606720, 0.027925, 0.116937, 0.062832, -0.176800, -9.772468, 0.283832
1564084459569614080, 0.017453, 0.122173, 0.075049, -0.286131, -9.746941, 0.175433

[chutsu]

Actually I may have found an even better way? Full code example: http://cpp.sh/9pb6l

So the problem we have is that vf.get_timestamp() returns a double of the timestamp in miliseconds. We could convert ms to ns like you suggested with vf.get_timestamp() * 1e3, however, that introduces rounding errors.

printf("frame timestamp [ms]: %f\n", ts_ms);
printf("frame timestamp [us]: %f\n", ts_ms * 1e3);
printf("frame timestamp [ns]: %f\n", ts_ms * 1e6);

// Example output:
// frame timestamp [ms]: 1564436052531.301514
// frame timestamp [us]: 1564436052531301.500000
// frame timestamp [ns]: 1564436052531301632.000000

So in-order to fix this I did it the dumb way by converting the number to a std::string() and went through the base 10s and reconstructed the number in integer form.

static inline uint64_t str2ts(const std::string &s) {
  uint64_t ts = 0;
  size_t end = s.length() - 1;

  int idx = 0;
  for (int i = 0; i <= end; i++) {
    const char c = s.at(end - i);

    if (c != '.') {
      const uint64_t base = static_cast<uint64_t>(pow(10, idx));
      ts += std::atoi(&c) * base;
      idx++;
    }
  }

  return ts;
}

And so the function would be:

// Calculate half of the exposure time
// -- Frame meta timestamp
const auto frame_meta_key = RS2_FRAME_METADATA_FRAME_TIMESTAMP;
const auto frame_ts_us = vf.get_frame_metadata(frame_meta_key);
const uint64_t frame_ts_ns = static_cast<uint64_t>(frame_ts_us) * 1000;
// -- Sensor meta timestamp
const auto sensor_meta_key = RS2_FRAME_METADATA_SENSOR_TIMESTAMP;
const auto sensor_ts_us = vf.get_frame_metadata(sensor_meta_key);
const uint64_t sensor_ts_ns = static_cast<uint64_t>(sensor_ts_us) * 1000;
// -- Half exposure time
const uint64_t half_exposure_time_ns = frame_ts_ns - sensor_ts_ns;
    
// Calculate corrected timestamp
const double ts_ms = vf.get_timestamp();
const uint64_t ts_ns = str2ts(std::to_string(ts_ms));
const uint64_t ts_corrected_ns = ts_ns - half_exposure_time_ns;