Modernize TimestampExtrapolator to use correct units

* Add unit tests
* Use TimestampUnwrapper
* Follow style guide

Change-Id: I057b05faba0aeafb2830a45007474be0eca1c6e0
Bug: webrtc:13756
Reviewed-on: https://webrtc-review.googlesource.com/c/src/+/256261
Reviewed-by: Stefan Holmer <stefan@webrtc.org>
Commit-Queue: Evan Shrubsole <eshr@webrtc.org>
Cr-Commit-Position: refs/heads/main@{#36313}
This commit is contained in:
Evan Shrubsole
2022-03-21 17:16:39 +01:00
committed by WebRTC LUCI CQ
parent ccc9d979a5
commit 195b0a9849
5 changed files with 327 additions and 141 deletions

View File

@ -574,6 +574,7 @@ if (rtc_include_tests && !build_with_chromium) {
"rtc_base/task_utils:pending_task_safety_flag_unittests",
"rtc_base/task_utils:repeating_task_unittests",
"rtc_base/task_utils:to_queued_task_unittests",
"rtc_base/time:timestamp_extrapolator_unittests",
"rtc_base/units:units_unittests",
"sdk:sdk_tests",
"test:rtp_test_utils",

View File

@ -17,6 +17,26 @@ rtc_library("timestamp_extrapolator") {
"timestamp_extrapolator.cc",
"timestamp_extrapolator.h",
]
deps = [ "../../api/units:timestamp" ]
deps = [
"../../api/units:frequency",
"../../api/units:timestamp",
"../../modules:module_api_public",
]
absl_deps = [ "//third_party/abseil-cpp/absl/types:optional" ]
}
if (rtc_include_tests) {
rtc_library("timestamp_extrapolator_unittests") {
testonly = true
sources = [ "timestamp_extrapolator_unittest.cc" ]
deps = [
":timestamp_extrapolator",
"../../api/units:frequency",
"../../api/units:time_delta",
"../../api/units:timestamp",
"../../system_wrappers",
"../../test:test_support",
]
absl_deps = [ "//third_party/abseil-cpp/absl/types:optional" ]
}
}

View File

@ -13,87 +13,83 @@
#include <algorithm>
#include "absl/types/optional.h"
#include "api/units/frequency.h"
#include "modules/include/module_common_types_public.h"
namespace webrtc {
namespace {
constexpr double kLambda = 1;
constexpr uint32_t kStartUpFilterDelayInPackets = 2;
constexpr double kAlarmThreshold = 60e3;
// in timestamp ticks, i.e. 15 ms
constexpr double kAccDrift = 6600;
constexpr double kAccMaxError = 7000;
constexpr double kP11 = 1e10;
} // namespace
TimestampExtrapolator::TimestampExtrapolator(Timestamp start)
: _start(Timestamp::Zero()),
_prev(Timestamp::Zero()),
_firstTimestamp(0),
_wrapArounds(0),
_prevUnwrappedTimestamp(-1),
_prevWrapTimestamp(-1),
_lambda(1),
_firstAfterReset(true),
_packetCount(0),
_startUpFilterDelayInPackets(2),
_detectorAccumulatorPos(0),
_detectorAccumulatorNeg(0),
_alarmThreshold(60e3),
_accDrift(6600), // in timestamp ticks, i.e. 15 ms
_accMaxError(7000),
_pP11(1e10) {
: start_(Timestamp::Zero()),
prev_(Timestamp::Zero()),
packet_count_(0),
detector_accumulator_pos_(0),
detector_accumulator_neg_(0) {
Reset(start);
}
void TimestampExtrapolator::Reset(Timestamp start) {
_start = start;
_prev = _start;
_firstTimestamp = 0;
_w[0] = 90.0;
_w[1] = 0;
_pP[0][0] = 1;
_pP[1][1] = _pP11;
_pP[0][1] = _pP[1][0] = 0;
_firstAfterReset = true;
_prevUnwrappedTimestamp = -1;
_prevWrapTimestamp = -1;
_wrapArounds = 0;
_packetCount = 0;
_detectorAccumulatorPos = 0;
_detectorAccumulatorNeg = 0;
start_ = start;
prev_ = start_;
first_unwrapped_timestamp_ = absl::nullopt;
w_[0] = 90.0;
w_[1] = 0;
p_[0][0] = 1;
p_[1][1] = kP11;
p_[0][1] = p_[1][0] = 0;
unwrapper_ = TimestampUnwrapper();
packet_count_ = 0;
detector_accumulator_pos_ = 0;
detector_accumulator_neg_ = 0;
}
void TimestampExtrapolator::Update(Timestamp now, uint32_t ts90khz) {
if (now - _prev > TimeDelta::Seconds(10)) {
if (now - prev_ > TimeDelta::Seconds(10)) {
// Ten seconds without a complete frame.
// Reset the extrapolator
Reset(now);
} else {
_prev = now;
prev_ = now;
}
// Remove offset to prevent badly scaled matrices
const TimeDelta offset = now - _start;
double tMs = offset.ms();
const TimeDelta offset = now - start_;
double t_ms = offset.ms();
CheckForWrapArounds(ts90khz);
int64_t unwrapped_ts90khz = unwrapper_.Unwrap(ts90khz);
int64_t unwrapped_ts90khz =
static_cast<int64_t>(ts90khz) +
_wrapArounds * ((static_cast<int64_t>(1) << 32) - 1);
if (_firstAfterReset) {
if (!first_unwrapped_timestamp_) {
// Make an initial guess of the offset,
// should be almost correct since tMs - _startMs
// should be almost correct since t_ms - start
// should about zero at this time.
_w[1] = -_w[0] * tMs;
_firstTimestamp = unwrapped_ts90khz;
_firstAfterReset = false;
w_[1] = -w_[0] * t_ms;
first_unwrapped_timestamp_ = unwrapped_ts90khz;
}
double residual = (static_cast<double>(unwrapped_ts90khz) - _firstTimestamp) -
tMs * _w[0] - _w[1];
double residual =
(static_cast<double>(unwrapped_ts90khz) - *first_unwrapped_timestamp_) -
t_ms * w_[0] - w_[1];
if (DelayChangeDetection(residual) &&
_packetCount >= _startUpFilterDelayInPackets) {
packet_count_ >= kStartUpFilterDelayInPackets) {
// A sudden change of average network delay has been detected.
// Force the filter to adjust its offset parameter by changing
// the offset uncertainty. Don't do this during startup.
_pP[1][1] = _pP11;
p_[1][1] = kP11;
}
if (_prevUnwrappedTimestamp >= 0 &&
unwrapped_ts90khz < _prevUnwrappedTimestamp) {
if (prev_unwrapped_timestamp_ &&
unwrapped_ts90khz < prev_unwrapped_timestamp_) {
// Drop reordered frames.
return;
}
@ -102,94 +98,62 @@ void TimestampExtrapolator::Update(Timestamp now, uint32_t ts90khz) {
// that = T'*w;
// K = P*T/(lambda + T'*P*T);
double K[2];
K[0] = _pP[0][0] * tMs + _pP[0][1];
K[1] = _pP[1][0] * tMs + _pP[1][1];
double TPT = _lambda + tMs * K[0] + K[1];
K[0] = p_[0][0] * t_ms + p_[0][1];
K[1] = p_[1][0] * t_ms + p_[1][1];
double TPT = kLambda + t_ms * K[0] + K[1];
K[0] /= TPT;
K[1] /= TPT;
// w = w + K*(ts(k) - that);
_w[0] = _w[0] + K[0] * residual;
_w[1] = _w[1] + K[1] * residual;
w_[0] = w_[0] + K[0] * residual;
w_[1] = w_[1] + K[1] * residual;
// P = 1/lambda*(P - K*T'*P);
double p00 =
1 / _lambda * (_pP[0][0] - (K[0] * tMs * _pP[0][0] + K[0] * _pP[1][0]));
1 / kLambda * (p_[0][0] - (K[0] * t_ms * p_[0][0] + K[0] * p_[1][0]));
double p01 =
1 / _lambda * (_pP[0][1] - (K[0] * tMs * _pP[0][1] + K[0] * _pP[1][1]));
_pP[1][0] =
1 / _lambda * (_pP[1][0] - (K[1] * tMs * _pP[0][0] + K[1] * _pP[1][0]));
_pP[1][1] =
1 / _lambda * (_pP[1][1] - (K[1] * tMs * _pP[0][1] + K[1] * _pP[1][1]));
_pP[0][0] = p00;
_pP[0][1] = p01;
_prevUnwrappedTimestamp = unwrapped_ts90khz;
if (_packetCount < _startUpFilterDelayInPackets) {
_packetCount++;
1 / kLambda * (p_[0][1] - (K[0] * t_ms * p_[0][1] + K[0] * p_[1][1]));
p_[1][0] =
1 / kLambda * (p_[1][0] - (K[1] * t_ms * p_[0][0] + K[1] * p_[1][0]));
p_[1][1] =
1 / kLambda * (p_[1][1] - (K[1] * t_ms * p_[0][1] + K[1] * p_[1][1]));
p_[0][0] = p00;
p_[0][1] = p01;
prev_unwrapped_timestamp_ = unwrapped_ts90khz;
if (packet_count_ < kStartUpFilterDelayInPackets) {
packet_count_++;
}
}
absl::optional<Timestamp> TimestampExtrapolator::ExtrapolateLocalTime(
uint32_t timestamp90khz) {
CheckForWrapArounds(timestamp90khz);
double unwrapped_ts90khz =
static_cast<double>(timestamp90khz) +
_wrapArounds * ((static_cast<int64_t>(1) << 32) - 1);
if (_packetCount == 0) {
return absl::nullopt;
} else if (_packetCount < _startUpFilterDelayInPackets) {
auto diffMs = static_cast<int64_t>(
static_cast<double>(unwrapped_ts90khz - _prevUnwrappedTimestamp) /
90.0 +
0.5);
return _prev + TimeDelta::Millis(diffMs);
} else if (_w[0] < 1e-3) {
return _start;
} else {
double timestampDiff =
unwrapped_ts90khz - static_cast<double>(_firstTimestamp);
auto diffMs = static_cast<int64_t>((timestampDiff - _w[1]) / _w[0] + 0.5);
return _start + TimeDelta::Millis(diffMs);
}
}
uint32_t timestamp90khz) const {
int64_t unwrapped_ts90khz = unwrapper_.UnwrapWithoutUpdate(timestamp90khz);
// Investigates if the timestamp clock has overflowed since the last timestamp
// and keeps track of the number of wrap arounds since reset.
void TimestampExtrapolator::CheckForWrapArounds(uint32_t ts90khz) {
if (_prevWrapTimestamp == -1) {
_prevWrapTimestamp = ts90khz;
return;
}
if (ts90khz < _prevWrapTimestamp) {
// This difference will probably be less than -2^31 if we have had a wrap
// around (e.g. timestamp = 1, _previousTimestamp = 2^32 - 1). Since it is
// casted to a Word32, it should be positive.
if (static_cast<int32_t>(ts90khz - _prevWrapTimestamp) > 0) {
// Forward wrap around
_wrapArounds++;
}
if (!first_unwrapped_timestamp_) {
return absl::nullopt;
} else if (packet_count_ < kStartUpFilterDelayInPackets) {
constexpr Frequency k90KHz = Frequency::KiloHertz(90);
TimeDelta diff = (unwrapped_ts90khz - *prev_unwrapped_timestamp_) / k90KHz;
return prev_ + diff;
} else if (w_[0] < 1e-3) {
return start_;
} else {
// This difference will probably be less than -2^31 if we have had a
// backward wrap around. Since it is casted to a Word32, it should be
// positive.
if (static_cast<int32_t>(_prevWrapTimestamp - ts90khz) > 0) {
// Backward wrap around
_wrapArounds--;
}
double timestampDiff = unwrapped_ts90khz - *first_unwrapped_timestamp_;
auto diff_ms = static_cast<int64_t>((timestampDiff - w_[1]) / w_[0] + 0.5);
return start_ + TimeDelta::Millis(diff_ms);
}
_prevWrapTimestamp = ts90khz;
}
bool TimestampExtrapolator::DelayChangeDetection(double error) {
// CUSUM detection of sudden delay changes
error = (error > 0) ? std::min(error, _accMaxError)
: std::max(error, -_accMaxError);
_detectorAccumulatorPos =
std::max(_detectorAccumulatorPos + error - _accDrift, double{0});
_detectorAccumulatorNeg =
std::min(_detectorAccumulatorNeg + error + _accDrift, double{0});
if (_detectorAccumulatorPos > _alarmThreshold ||
_detectorAccumulatorNeg < -_alarmThreshold) {
error = (error > 0) ? std::min(error, kAccMaxError)
: std::max(error, -kAccMaxError);
detector_accumulator_pos_ =
std::max(detector_accumulator_pos_ + error - kAccDrift, double{0});
detector_accumulator_neg_ =
std::min(detector_accumulator_neg_ + error + kAccDrift, double{0});
if (detector_accumulator_pos_ > kAlarmThreshold ||
detector_accumulator_neg_ < -kAlarmThreshold) {
// Alarm
_detectorAccumulatorPos = _detectorAccumulatorNeg = 0;
detector_accumulator_pos_ = detector_accumulator_neg_ = 0;
return true;
}
return false;

View File

@ -15,6 +15,7 @@
#include "absl/types/optional.h"
#include "api/units/timestamp.h"
#include "modules/include/module_common_types_public.h"
namespace webrtc {
@ -23,31 +24,23 @@ class TimestampExtrapolator {
public:
explicit TimestampExtrapolator(Timestamp start);
void Update(Timestamp now, uint32_t ts90khz);
absl::optional<Timestamp> ExtrapolateLocalTime(uint32_t timestamp90khz);
absl::optional<Timestamp> ExtrapolateLocalTime(uint32_t timestamp90khz) const;
void Reset(Timestamp start);
private:
void CheckForWrapArounds(uint32_t ts90khz);
bool DelayChangeDetection(double error);
double _w[2];
double _pP[2][2];
Timestamp _start;
Timestamp _prev;
uint32_t _firstTimestamp;
int32_t _wrapArounds;
int64_t _prevUnwrappedTimestamp;
int64_t _prevWrapTimestamp;
const double _lambda;
bool _firstAfterReset;
uint32_t _packetCount;
const uint32_t _startUpFilterDelayInPackets;
double _detectorAccumulatorPos;
double _detectorAccumulatorNeg;
const double _alarmThreshold;
const double _accDrift;
const double _accMaxError;
const double _pP11;
double w_[2];
double p_[2][2];
Timestamp start_;
Timestamp prev_;
absl::optional<int64_t> first_unwrapped_timestamp_;
TimestampUnwrapper unwrapper_;
absl::optional<int64_t> prev_unwrapped_timestamp_;
uint32_t packet_count_;
double detector_accumulator_pos_;
double detector_accumulator_neg_;
};
} // namespace webrtc

View File

@ -0,0 +1,208 @@
/*
* Copyright (c) 2022 The WebRTC project authors. All Rights Reserved.
*
* Use of this source code is governed by a BSD-style license
* that can be found in the LICENSE file in the root of the source
* tree. An additional intellectual property rights grant can be found
* in the file PATENTS. All contributing project authors may
* be found in the AUTHORS file in the root of the source tree.
*/
#include "rtc_base/time/timestamp_extrapolator.h"
#include <stdint.h>
#include <limits>
#include "absl/types/optional.h"
#include "api/units/frequency.h"
#include "api/units/time_delta.h"
#include "api/units/timestamp.h"
#include "system_wrappers/include/clock.h"
#include "test/gmock.h"
#include "test/gtest.h"
namespace webrtc {
using ::testing::Eq;
using ::testing::Optional;
namespace {
constexpr Frequency kRtpHz = Frequency::KiloHertz(90);
constexpr Frequency k25Fps = Frequency::Hertz(25);
constexpr TimeDelta k25FpsDelay = 1 / k25Fps;
} // namespace
TEST(TimestampExtrapolatorTest, ExtrapolationOccursAfter2Packets) {
SimulatedClock clock(Timestamp::Millis(1337));
TimestampExtrapolator ts_extrapolator(clock.CurrentTime());
// No packets so no timestamp.
EXPECT_THAT(ts_extrapolator.ExtrapolateLocalTime(90000), Eq(absl::nullopt));
uint32_t rtp = 90000;
clock.AdvanceTime(k25FpsDelay);
// First result is a bit confusing since it is based off the "start" time,
// which is arbitrary.
ts_extrapolator.Update(clock.CurrentTime(), rtp);
EXPECT_THAT(ts_extrapolator.ExtrapolateLocalTime(rtp),
Optional(clock.CurrentTime()));
rtp += kRtpHz / k25Fps;
clock.AdvanceTime(k25FpsDelay);
ts_extrapolator.Update(clock.CurrentTime(), rtp);
EXPECT_THAT(ts_extrapolator.ExtrapolateLocalTime(rtp),
Optional(clock.CurrentTime()));
EXPECT_THAT(ts_extrapolator.ExtrapolateLocalTime(rtp + 90000),
Optional(clock.CurrentTime() + TimeDelta::Seconds(1)));
}
TEST(TimestampExtrapolatorTest, ResetsAfter10SecondPause) {
SimulatedClock clock(Timestamp::Millis(1337));
TimestampExtrapolator ts_extrapolator(clock.CurrentTime());
uint32_t rtp = 90000;
ts_extrapolator.Update(clock.CurrentTime(), rtp);
EXPECT_THAT(ts_extrapolator.ExtrapolateLocalTime(rtp),
Optional(clock.CurrentTime()));
rtp += kRtpHz / k25Fps;
clock.AdvanceTime(k25FpsDelay);
ts_extrapolator.Update(clock.CurrentTime(), rtp);
EXPECT_THAT(ts_extrapolator.ExtrapolateLocalTime(rtp),
Optional(clock.CurrentTime()));
rtp += 10 * kRtpHz.hertz();
clock.AdvanceTime(TimeDelta::Seconds(10) + TimeDelta::Micros(1));
ts_extrapolator.Update(clock.CurrentTime(), rtp);
EXPECT_THAT(ts_extrapolator.ExtrapolateLocalTime(rtp),
Optional(clock.CurrentTime()));
}
TEST(TimestampExtrapolatorTest, TimestampExtrapolatesMultipleRtpWrapArounds) {
SimulatedClock clock(Timestamp::Millis(1337));
TimestampExtrapolator ts_extrapolator(clock.CurrentTime());
uint32_t rtp = std::numeric_limits<uint32_t>::max();
ts_extrapolator.Update(clock.CurrentTime(), rtp);
EXPECT_THAT(ts_extrapolator.ExtrapolateLocalTime(rtp),
Optional(clock.CurrentTime()));
// One overflow. Static cast to avoid undefined behaviour with +=.
rtp += static_cast<uint32_t>(kRtpHz / k25Fps);
clock.AdvanceTime(k25FpsDelay);
ts_extrapolator.Update(clock.CurrentTime(), rtp);
EXPECT_THAT(ts_extrapolator.ExtrapolateLocalTime(rtp),
Optional(clock.CurrentTime()));
// Assert that extrapolation works across the boundary as expected.
EXPECT_THAT(ts_extrapolator.ExtrapolateLocalTime(rtp + 90000),
Optional(clock.CurrentTime() + TimeDelta::Seconds(1)));
// This is not quite 1s since the math always rounds up.
EXPECT_THAT(ts_extrapolator.ExtrapolateLocalTime(rtp - 90000),
Optional(clock.CurrentTime() - TimeDelta::Millis(999)));
// In order to avoid a wrap arounds reset, add a packet every 10s until we
// overflow twice.
constexpr TimeDelta kRtpOverflowDelay =
std::numeric_limits<uint32_t>::max() / kRtpHz;
const Timestamp overflow_time = clock.CurrentTime() + kRtpOverflowDelay * 2;
while (clock.CurrentTime() < overflow_time) {
clock.AdvanceTime(TimeDelta::Seconds(10));
// Static-cast before += to avoid undefined behaviour of overflow.
rtp += static_cast<uint32_t>(kRtpHz * TimeDelta::Seconds(10));
ts_extrapolator.Update(clock.CurrentTime(), rtp);
EXPECT_THAT(ts_extrapolator.ExtrapolateLocalTime(rtp),
Optional(clock.CurrentTime()));
}
}
TEST(TimestampExtrapolatorTest, Slow90KHzClock) {
// This simulates a slow camera, which produces frames at 24Hz instead of
// 25Hz. The extrapolator should be able to resolve this with enough data.
SimulatedClock clock(Timestamp::Millis(1337));
TimestampExtrapolator ts_extrapolator(clock.CurrentTime());
constexpr TimeDelta k24FpsDelay = 1 / Frequency::Hertz(24);
uint32_t rtp = 90000;
ts_extrapolator.Update(clock.CurrentTime(), rtp);
// Slow camera will increment RTP at 25 FPS rate even though its producing at
// 24 FPS. After 25 frames the extrapolator should settle at this rate.
for (int i = 0; i < 25; ++i) {
rtp += kRtpHz / k25Fps;
clock.AdvanceTime(k24FpsDelay);
ts_extrapolator.Update(clock.CurrentTime(), rtp);
}
// The camera would normally produce 25 frames in 90K ticks, but is slow
// so takes 1s + k24FpsDelay for 90K ticks.
constexpr Frequency kSlowRtpHz = 90000 / (25 * k24FpsDelay);
// The extrapolator will be predicting that time at millisecond precision.
auto ts = ts_extrapolator.ExtrapolateLocalTime(rtp + kSlowRtpHz.hertz());
ASSERT_TRUE(ts.has_value());
EXPECT_EQ(ts->ms(), clock.TimeInMilliseconds() + 1000);
}
TEST(TimestampExtrapolatorTest, Fast90KHzClock) {
// This simulates a fast camera, which produces frames at 26Hz instead of
// 25Hz. The extrapolator should be able to resolve this with enough data.
SimulatedClock clock(Timestamp::Millis(1337));
TimestampExtrapolator ts_extrapolator(clock.CurrentTime());
constexpr TimeDelta k26FpsDelay = 1 / Frequency::Hertz(26);
uint32_t rtp = 90000;
ts_extrapolator.Update(clock.CurrentTime(), rtp);
// Fast camera will increment RTP at 25 FPS rate even though its producing at
// 26 FPS. After 25 frames the extrapolator should settle at this rate.
for (int i = 0; i < 25; ++i) {
rtp += kRtpHz / k25Fps;
clock.AdvanceTime(k26FpsDelay);
ts_extrapolator.Update(clock.CurrentTime(), rtp);
}
// The camera would normally produce 25 frames in 90K ticks, but is slow
// so takes 1s + k24FpsDelay for 90K ticks.
constexpr Frequency kSlowRtpHz = 90000 / (25 * k26FpsDelay);
// The extrapolator will be predicting that time at millisecond precision.
auto ts = ts_extrapolator.ExtrapolateLocalTime(rtp + kSlowRtpHz.hertz());
ASSERT_TRUE(ts.has_value());
EXPECT_EQ(ts->ms(), clock.TimeInMilliseconds() + 1000);
}
TEST(TimestampExtrapolatorTest, TimestampJump) {
// This simulates a jump in RTP timestamp, which could occur if a camera was
// swapped for example.
SimulatedClock clock(Timestamp::Millis(1337));
TimestampExtrapolator ts_extrapolator(clock.CurrentTime());
uint32_t rtp = 90000;
clock.AdvanceTime(k25FpsDelay);
ts_extrapolator.Update(clock.CurrentTime(), rtp);
rtp += kRtpHz / k25Fps;
clock.AdvanceTime(k25FpsDelay);
ts_extrapolator.Update(clock.CurrentTime(), rtp);
rtp += kRtpHz / k25Fps;
clock.AdvanceTime(k25FpsDelay);
ts_extrapolator.Update(clock.CurrentTime(), rtp);
EXPECT_THAT(ts_extrapolator.ExtrapolateLocalTime(rtp),
Optional(clock.CurrentTime()));
EXPECT_THAT(ts_extrapolator.ExtrapolateLocalTime(rtp + 90000),
Optional(clock.CurrentTime() + TimeDelta::Seconds(1)));
// Jump RTP.
uint32_t new_rtp = 1337 * 90000;
clock.AdvanceTime(k25FpsDelay);
ts_extrapolator.Update(clock.CurrentTime(), new_rtp);
new_rtp += kRtpHz / k25Fps;
clock.AdvanceTime(k25FpsDelay);
ts_extrapolator.Update(clock.CurrentTime(), new_rtp);
EXPECT_THAT(ts_extrapolator.ExtrapolateLocalTime(new_rtp),
Optional(clock.CurrentTime()));
}
} // namespace webrtc