///|
/// Versioned motion state after the last committed frame. Int64 fields must use
/// exact decimal strings in wire formats. Arrays are detached from the engine.
pub(all) struct MotionCheckpoint {
version : Int32
session_id : String
config : MotionConfig
last_timestamp_us : Int64?
next_segment_id : Int64
tracks : Array[TrackCheckpoint]
} derive(Debug)
///|
pub extend MotionCheckpoint with @debug.Debug::{to_repr}
///|
/// Accepted state of one active segment. Covariance uses row-major state order
/// [x, y, vx, vy]. Times refer to accepted observations, not missing frames.
pub(all) struct TrackCheckpoint {
track_id : Int32
segment_id : Int64
first_timestamp_us : Int64
last_timestamp_us : Int64
accepted_count : Int32
state : Array[Double]
covariance : Array[Double]
first_filtered : @types.Point2
last_filtered : @types.Point2
observed_path_length_m : Double
} derive(Debug)
///|
pub extend TrackCheckpoint with @debug.Debug::{to_repr}
///|
/// Copy bounded accepted state without changing frame time or segment allocation.
pub fn MotionEstimator::checkpoint(self : MotionEstimator) -> MotionCheckpoint {
let tracks = self.tracks
.values()
.map(track => TrackCheckpoint::{
track_id: track.track_id,
segment_id: track.segment_id,
first_timestamp_us: track.first_timestamp_us,
last_timestamp_us: track.last_timestamp_us,
accepted_count: track.accepted_count,
state: track.state.copy(),
covariance: flatten_covariance(track.covariance),
first_filtered: track.first_filtered,
last_filtered: track.last_filtered,
observed_path_length_m: track.observed_path_length_m,
})
.collect()
tracks.sort_by((a, b) => a.track_id.compare(b.track_id))
{
version: 1,
session_id: self.session_id,
config: self.config,
last_timestamp_us: self.last_timestamp,
next_segment_id: self.next_segment,
tracks,
}
}
///|
fn restored_track(
checkpoint : TrackCheckpoint,
frame_timestamp_us : Int64,
next_segment_id : Int64,
config : MotionConfig,
) -> TrackState raise @types.GroundError {
if checkpoint.track_id <= 0 ||
checkpoint.segment_id <= 0L ||
checkpoint.segment_id >= next_segment_id {
raise @types.InvalidInput("invalid checkpoint track or segment identity")
}
let first = checkpoint.first_timestamp_us
let last = checkpoint.last_timestamp_us
if first < 0L || last < first || last > frame_timestamp_us {
raise @types.InvalidInput("invalid checkpoint accepted timestamps")
}
if frame_timestamp_us - last >= config.max_gap_us {
raise @types.InvalidInput("checkpoint contains an expired track")
}
let count = checkpoint.accepted_count
if count < 1 ||
(count == 1) != (first == last) ||
Int64::from_int(count - 1) > last - first {
raise @types.InvalidInput("inconsistent checkpoint observation count")
}
if checkpoint.state.length() != 4 || checkpoint.covariance.length() != 16 {
raise @types.InvalidInput("checkpoint requires state4 and covariance16")
}
let state = checkpoint.state.copy()
let covariance = @kalman.Matrix::from_rows(
Array::makei(4, row => {
Array::makei(4, col => checkpoint.covariance[row * 4 + col])
}),
)
if !healthy_state(state, covariance) ||
covariance_block(covariance, 0) is None ||
covariance_block(covariance, 2) is None ||
!@types.is_finite(norm2(state[2], state[3])) {
raise @types.InvalidInput("invalid checkpoint state or covariance")
}
checkpoint.first_filtered.validate()
checkpoint.last_filtered.validate()
if checkpoint.last_filtered.x != state[0] ||
checkpoint.last_filtered.y != state[1] {
raise @types.InvalidInput(
"checkpoint last filtered point differs from state",
)
}
let path = checkpoint.observed_path_length_m
let displacement = norm2(
checkpoint.last_filtered.x - checkpoint.first_filtered.x,
checkpoint.last_filtered.y - checkpoint.first_filtered.y,
)
if !@types.is_finite(path) || path < 0.0 || !@types.is_finite(displacement) {
raise @types.InvalidInput("invalid checkpoint filtered path")
}
let scale = if path > displacement { path } else { displacement }
if scale > 0.0 && displacement / scale > path / scale + 0.000000001 {
raise @types.InvalidInput("checkpoint displacement exceeds filtered path")
}
if count == 1 {
let prior = config.initial_velocity_std * config.initial_velocity_std
if path != 0.0 ||
checkpoint.first_filtered.x != state[0] ||
checkpoint.first_filtered.y != state[1] ||
state[2] != 0.0 ||
state[3] != 0.0 ||
covariance.get(0, 1) != covariance.get(1, 0) ||
covariance.get(2, 2) != prior ||
covariance.get(3, 3) != prior {
raise @types.InvalidInput("inconsistent initial checkpoint state")
}
for row in 0..<4 {
for col in 0..<4 {
if (
(row < 2 && col >= 2) ||
(row >= 2 && col < 2) ||
(row >= 2 && col >= 2 && row != col)
) &&
covariance.get(row, col) != 0.0 {
raise @types.InvalidInput(
"inconsistent initial checkpoint covariance",
)
}
}
}
}
{
track_id: checkpoint.track_id,
segment_id: checkpoint.segment_id,
first_timestamp_us: first,
last_timestamp_us: last,
accepted_count: count,
state,
covariance,
first_filtered: checkpoint.first_filtered,
last_filtered: checkpoint.last_filtered,
observed_path_length_m: path,
}
}
///|
/// Restore a new engine only after the complete checkpoint validates. Original
/// covariance entries are preserved, including roundoff asymmetry; no filter
/// prediction, covariance repair or wall-clock expiration happens here.
pub fn MotionEstimator::from_checkpoint(
checkpoint : MotionCheckpoint,
) -> MotionEstimator raise @types.GroundError {
if checkpoint.version != 1 {
raise @types.InvalidInput("unsupported motion checkpoint version")
}
let source = checkpoint.config
let config = MotionConfig::new(
max_gap_us=source.max_gap_us,
min_observations=source.min_observations,
min_span_us=source.min_span_us,
q=source.acceleration_spectral_density,
initial_velocity_std=source.initial_velocity_std,
gate=source.innovation_gate,
max_tracks=source.max_tracks,
max_position_std_m?=source.max_position_std_m,
max_velocity_std_mps?=source.max_velocity_std_mps,
)
let estimator = MotionEstimator::new(checkpoint.session_id, config)
if checkpoint.next_segment_id < 1L ||
checkpoint.tracks.length() > config.max_tracks {
raise @types.InvalidInput("invalid checkpoint segment counter or capacity")
}
let tracks : Map[Int32, TrackState] = Map([])
let segments : Map[Int64, Bool] = Map([])
match checkpoint.last_timestamp_us {
None =>
if checkpoint.tracks.length() != 0 || checkpoint.next_segment_id != 1L {
raise @types.InvalidInput(
"unstarted checkpoint contains allocated state",
)
}
Some(timestamp) => {
if timestamp < 0L {
raise @types.InvalidInput(
"checkpoint frame timestamp must be nonnegative",
)
}
for track in checkpoint.tracks {
if tracks.contains(track.track_id) ||
segments.contains(track.segment_id) {
raise @types.InvalidInput(
"duplicate checkpoint track or segment identity",
)
}
let restored = restored_track(
track,
timestamp,
checkpoint.next_segment_id,
config,
)
tracks[restored.track_id] = restored
segments[restored.segment_id] = true
}
}
}
estimator.tracks = tracks
estimator.last_timestamp = checkpoint.last_timestamp_us
estimator.next_segment = checkpoint.next_segment_id
estimator
}