Skip to content

Predict current 6d pose to compensate SLAM processing delay

Compute and publish extrapolated transform compensating latency induced by SLAM processing duration.

Predicted latency-compensated transform is extrapolated from the 2 last poses ('current' and 'previous'), with the prior of constant speed : 'previous' pose at time 0. and 'current' pose at time 1. are used to predict pose 'pred' a time 1. + dt.

Translation part is linearly extrapolated, and rotation part is extrapolated using SLERP.

Merge request reports