Comments (6)
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:
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.
from rs4se.
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]
from rs4se.
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.
from rs4se.
-
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.
from rs4se.
-
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.
-
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
from rs4se.
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;
from rs4se.
Related Issues (4)
Recommend Projects
-
React
A declarative, efficient, and flexible JavaScript library for building user interfaces.
-
Vue.js
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
-
Typescript
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
-
TensorFlow
An Open Source Machine Learning Framework for Everyone
-
Django
The Web framework for perfectionists with deadlines.
-
Laravel
A PHP framework for web artisans
-
D3
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
-
Recommend Topics
-
javascript
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
-
web
Some thing interesting about web. New door for the world.
-
server
A server is a program made to process requests and deliver data to clients.
-
Machine learning
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
-
Visualization
Some thing interesting about visualization, use data art
-
Game
Some thing interesting about game, make everyone happy.
Recommend Org
-
Facebook
We are working to build community through open source technology. NB: members must have two-factor auth.
-
Microsoft
Open source projects and samples from Microsoft.
-
Google
Google ❤️ Open Source for everyone.
-
Alibaba
Alibaba Open Source for everyone
-
D3
Data-Driven Documents codes.
-
Tencent
China tencent open source team.
from rs4se.