857 lines
34 KiB
C#
857 lines
34 KiB
C#
/*
|
||
* Copyright 2017 The Cartographer Authors
|
||
*
|
||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||
* you may not use this file except in compliance with the License.
|
||
* You may obtain a copy of the License at
|
||
*
|
||
* http://www.apache.org/licenses/LICENSE-2.0
|
||
*
|
||
* Unless required by applicable law or agreed to in writing, software
|
||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||
* See the License for the specific language governing permissions and
|
||
* limitations under the License.
|
||
*/
|
||
|
||
using CartographerSharp.Sensor;
|
||
using CartographerSharp.Transform;
|
||
using RobotNet10.Shared.Numbers;
|
||
|
||
namespace CartographerSharp.Mapping;
|
||
|
||
/// <summary>
|
||
/// Keep poses for a certain duration to estimate linear and angular velocity.
|
||
/// Uses the velocities to extrapolate motion. Uses IMU and/or odometry data if
|
||
/// available to improve the extrapolation.
|
||
/// Match C++: cartographer/mapping/pose_extrapolator.h/cc
|
||
/// </summary>
|
||
public class PoseExtrapolator(long poseQueueDuration, double imuGravityTimeConstant) : IPoseExtrapolator
|
||
{
|
||
private struct TimedPose(long time, Rigid3d pose)
|
||
{
|
||
public long Time { get; set; } = time;
|
||
public Rigid3d Pose { get; set; } = pose;
|
||
}
|
||
|
||
// Use List instead of Queue for random access (similar to std::deque)
|
||
private readonly List<TimedPose> _timedPoseQueue = [];
|
||
private Vector3 _linearVelocityFromPoses = Vector3.Zero;
|
||
private Vector3 _angularVelocityFromPoses = Vector3.Zero;
|
||
|
||
private readonly List<ImuData> _imuData = [];
|
||
private readonly List<OdometryData> _odometryData = [];
|
||
private Vector3 _linearVelocityFromOdometry = Vector3.Zero;
|
||
private Vector3 _angularVelocityFromOdometry = Vector3.Zero;
|
||
private Vector3 _instantAngularVelocityFromOdometry = Vector3.Zero;
|
||
|
||
// Lock for thread-safe access to _imuData and _odometryData
|
||
private readonly Lock _dataLock = new();
|
||
|
||
private readonly double _gravityTimeConstant = imuGravityTimeConstant;
|
||
private ImuTracker? _imuTracker;
|
||
private ImuTracker? _odometryImuTracker;
|
||
private ImuTracker? _extrapolationImuTracker;
|
||
|
||
// Match C++: cached_extrapolated_pose_ and cached_extrapolated_pose_filter
|
||
private TimedPose? _cachedExtrapolatedPose;
|
||
private TimedPose? _cachedExtrapolatedPoseFilter;
|
||
|
||
// Odometry trajectory-based extrapolation: reference odometry pose at last AddPose time
|
||
// and cached rotation from odom frame to global frame
|
||
private OdometryData? _odometryAtLastPose;
|
||
private Quaternion _odomToGlobalRotation = Quaternion.Identity;
|
||
|
||
|
||
/// <summary>
|
||
/// Match C++: InitializeWithImu factory method
|
||
/// </summary>
|
||
public static PoseExtrapolator InitializeWithImu(
|
||
long poseQueueDuration,
|
||
double imuGravityTimeConstant,
|
||
ImuData imuData)
|
||
{
|
||
var extrapolator = new PoseExtrapolator(poseQueueDuration, imuGravityTimeConstant);
|
||
extrapolator.AddImuData(imuData);
|
||
|
||
// Initialize ImuTracker with first IMU data
|
||
extrapolator._imuTracker = new ImuTracker(imuGravityTimeConstant, imuData.Time);
|
||
extrapolator._imuTracker.AddImuLinearAccelerationObservation(imuData.LinearAcceleration);
|
||
extrapolator._imuTracker.AddImuAngularVelocityObservation(imuData.AngularVelocity);
|
||
extrapolator._imuTracker.Advance(imuData.Time);
|
||
|
||
// Add initial pose with rotation from IMU tracker
|
||
extrapolator.AddPose(
|
||
imuData.Time,
|
||
Rigid3d.FromRotation(extrapolator._imuTracker.Orientation));
|
||
|
||
return extrapolator;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Returns diagnostic info about current odom/IMU data status for debugging.
|
||
/// </summary>
|
||
public (int OdomCount, int ImuCount, long LastOdomTime, long LastPoseTime, Vector3 LinVelOdom, Vector3 LinVelPose) GetDiagnostics()
|
||
{
|
||
lock (_dataLock)
|
||
{
|
||
return (
|
||
OdomCount: _odometryData.Count,
|
||
ImuCount: _imuData.Count,
|
||
LastOdomTime: _odometryData.Count > 0 ? _odometryData[^1].Time : 0,
|
||
LastPoseTime: _timedPoseQueue.Count > 0 ? _timedPoseQueue[^1].Time : 0,
|
||
LinVelOdom: _linearVelocityFromOdometry,
|
||
LinVelPose: _linearVelocityFromPoses
|
||
);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Returns detailed extrapolation breakdown for diagnostics.
|
||
/// Match C++: velocity × dt extrapolation.
|
||
/// </summary>
|
||
public string GetExtrapolationBreakdown(long time)
|
||
{
|
||
if (_timedPoseQueue.Count == 0) return "NO_POSES";
|
||
|
||
var newestTimedPose = _timedPoseQueue[^1];
|
||
var extrapolationDelta = (time - newestTimedPose.Time) / 10_000_000.0;
|
||
|
||
lock (_dataLock)
|
||
{
|
||
var angVelOdom = _angularVelocityFromOdometry;
|
||
var basePose = newestTimedPose.Pose.Translation;
|
||
|
||
if (_odometryData.Count >= 2 && _odometryAtLastPose.HasValue)
|
||
{
|
||
var odomRef = _odometryAtLastPose.Value;
|
||
var idx = FindOdometryIndexBeforeTime(time);
|
||
var odomWindow = (_odometryData[^1].Time - _odometryData[0].Time) / 10_000_000.0;
|
||
|
||
if (idx >= 0 && _odometryData[idx].Time > odomRef.Time)
|
||
{
|
||
var odomAtTime = _odometryData[idx];
|
||
var displacementOdom = odomAtTime.Pose.Translation - odomRef.Pose.Translation;
|
||
var displacementGlobal = Vector3.Transform(displacementOdom, _odomToGlobalRotation);
|
||
var tailDt = (time - odomAtTime.Time) / 10_000_000.0;
|
||
return $"PATH=odom_trajectory, basePose=({basePose.X:F4},{basePose.Y:F4}), dt={extrapolationDelta:F3}s, " +
|
||
$"displ=({displacementGlobal.X:F4},{displacementGlobal.Y:F4}), tailDt={tailDt:F4}s, " +
|
||
$"odomWindow={odomWindow:F3}s, odomCount={_odometryData.Count}, " +
|
||
$"angVelOdom=({angVelOdom.X:F4},{angVelOdom.Y:F4},{angVelOdom.Z:F4})";
|
||
}
|
||
else
|
||
{
|
||
var vel = _linearVelocityFromOdometry;
|
||
return $"PATH=odom_vel_fallback, basePose=({basePose.X:F4},{basePose.Y:F4}), dt={extrapolationDelta:F3}s, " +
|
||
$"vel=({vel.X:F4},{vel.Y:F4}), odomWindow={odomWindow:F3}s, odomCount={_odometryData.Count}, " +
|
||
$"angVelOdom=({angVelOdom.X:F4},{angVelOdom.Y:F4},{angVelOdom.Z:F4})";
|
||
}
|
||
}
|
||
else
|
||
{
|
||
var vel = _linearVelocityFromPoses;
|
||
return $"PATH=pose_vel, basePose=({basePose.X:F4},{basePose.Y:F4}), dt={extrapolationDelta:F3}s, " +
|
||
$"vel=({vel.X:F4},{vel.Y:F4}), odomCount={_odometryData.Count}, " +
|
||
$"angVelOdom=({angVelOdom.X:F4},{angVelOdom.Y:F4},{angVelOdom.Z:F4})";
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Returns the time span (ms) covered by the current odometry data window [oldest, newest].
|
||
/// Large values relative to scan period indicate odom accumulated during a processing
|
||
/// delay (e.g., grid resize blocking the SLAM thread in InsertIntoSubmap).
|
||
/// </summary>
|
||
public double GetOdometryWindowMs()
|
||
{
|
||
lock (_dataLock)
|
||
{
|
||
if (_odometryData.Count < 2) return 0;
|
||
return (_odometryData[^1].Time - _odometryData[0].Time) / 10_000.0;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Match C++: GetLastPoseTime
|
||
/// </summary>
|
||
public long GetLastPoseTime()
|
||
{
|
||
return _timedPoseQueue.Count > 0 ? _timedPoseQueue[^1].Time : long.MinValue;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Match C++: GetLastExtrapolatedTime
|
||
/// </summary>
|
||
public long GetLastExtrapolatedTime()
|
||
{
|
||
return _extrapolationImuTracker?.Time ?? long.MinValue;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Match C++: AddPose (pose_extrapolator.cc:70-91)
|
||
/// </summary>
|
||
public void AddPose(long time, Rigid3d pose)
|
||
{
|
||
// Match C++ lines 72-79: Initialize ImuTracker if needed
|
||
if (_imuTracker == null)
|
||
{
|
||
long trackerStart = time;
|
||
lock (_dataLock)
|
||
{
|
||
if (_imuData.Count > 0)
|
||
{
|
||
trackerStart = Math.Min(trackerStart, _imuData[0].Time);
|
||
}
|
||
}
|
||
_imuTracker = new ImuTracker(_gravityTimeConstant, trackerStart);
|
||
}
|
||
|
||
// Match C++ line 80: Add pose to queue
|
||
_timedPoseQueue.Add(new TimedPose(time, pose));
|
||
|
||
// Match C++ lines 81-84: Trim queue to keep at least 2 poses
|
||
while (_timedPoseQueue.Count > 2 &&
|
||
_timedPoseQueue[1].Time <= time - poseQueueDuration)
|
||
{
|
||
_timedPoseQueue.RemoveAt(0);
|
||
}
|
||
|
||
// Match C++ line 85: Update velocities from poses
|
||
UpdateVelocitiesFromPoses();
|
||
|
||
// Match C++ lines 86-88: Advance IMU tracker and trim data
|
||
// FIX: Extended lock scope to include ImuTracker copies to prevent race condition with AddOdometryData
|
||
lock (_dataLock)
|
||
{
|
||
AdvanceImuTracker(time, _imuTracker);
|
||
TrimImuData();
|
||
TrimOdometryData();
|
||
|
||
// Save reference odometry pose for trajectory-based extrapolation
|
||
if (_odometryData.Count > 0)
|
||
{
|
||
_odometryAtLastPose = _odometryData[0];
|
||
_odomToGlobalRotation = pose.Rotation * Quaternion.Inverse(_odometryData[0].Pose.Rotation);
|
||
}
|
||
else
|
||
{
|
||
_odometryAtLastPose = null;
|
||
}
|
||
|
||
// Match C++ lines 89-90: Create copies of ImuTracker for odometry and extrapolation
|
||
// These must be created inside lock to prevent race condition with AddOdometryData
|
||
_odometryImuTracker = new ImuTracker(_imuTracker);
|
||
_extrapolationImuTracker = new ImuTracker(_imuTracker);
|
||
}
|
||
|
||
// Invalidate cache when new pose is added
|
||
_cachedExtrapolatedPose = null;
|
||
|
||
}
|
||
|
||
/// <summary>
|
||
/// Match C++: AddImuData (pose_extrapolator.cc:93-98)
|
||
/// </summary>
|
||
public void AddImuData(ImuData imuData)
|
||
{
|
||
// Match C++ lines 94-95: CHECK that IMU time >= last pose time
|
||
if (_timedPoseQueue.Count > 0 && imuData.Time < _timedPoseQueue[^1].Time)
|
||
{
|
||
var timeDiffMs = (_timedPoseQueue[^1].Time - imuData.Time) / TimeSpan.TicksPerMillisecond;
|
||
if(timeDiffMs > 5)
|
||
{
|
||
throw new ArgumentException($"IMU data time ({imuData.Time}) must be >= last pose time ({_timedPoseQueue[^1].Time}), diff={timeDiffMs}ms");
|
||
}
|
||
else
|
||
{
|
||
return; // Ignore slightly out-of-order IMU data
|
||
}
|
||
}
|
||
|
||
lock (_dataLock)
|
||
{
|
||
_imuData.Add(imuData);
|
||
TrimImuData();
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Match C++: AddOdometryData (pose_extrapolator.cc:100-144)
|
||
/// CRITICAL FIX: Do NOT reset odometry_imu_tracker_ - let it accumulate state
|
||
/// </summary>
|
||
public void AddOdometryData(OdometryData odometryData)
|
||
{
|
||
// Match C++ lines 105-106: CHECK that odometry time >= last pose time
|
||
if (_timedPoseQueue.Count > 0 && odometryData.Time < _timedPoseQueue[^1].Time)
|
||
{
|
||
var timeDiffMs = (_timedPoseQueue[^1].Time - odometryData.Time) / TimeSpan.TicksPerMillisecond;
|
||
if(timeDiffMs > 5)
|
||
{
|
||
throw new ArgumentException($"Odometry data time ({odometryData.Time}) must be >= last pose time ({_timedPoseQueue[^1].Time}), diff={timeDiffMs}ms");
|
||
}
|
||
else
|
||
{
|
||
return; // Ignore slightly out-of-order odometry data
|
||
}
|
||
}
|
||
|
||
// FIX: Use single lock scope to prevent race condition with AddPose modifying _odometryImuTracker
|
||
// Between two separate lock blocks, _odometryImuTracker could be replaced by AddPose from SLAM thread
|
||
lock (_dataLock)
|
||
{
|
||
// Match C++ line 107-108: Add to queue and trim
|
||
_odometryData.Add(odometryData);
|
||
TrimOdometryData();
|
||
|
||
// Match C++ lines 109-111: Need at least 2 odometry data points
|
||
if (_odometryData.Count < 2)
|
||
{
|
||
return;
|
||
}
|
||
|
||
// Match C++: Use front() and back() of the queue (full window since last AddPose)
|
||
var odometryDataOldest = _odometryData[0];
|
||
var odometryDataNewest = _odometryData[^1];
|
||
|
||
// Match C++ lines 116-117: Compute time delta
|
||
var odometryTimeDelta = (odometryDataOldest.Time - odometryDataNewest.Time) / 10_000_000.0;
|
||
|
||
// Safeguard: Skip if time delta is too small (< 1ms) to avoid noisy velocity estimates
|
||
if (Math.Abs(odometryTimeDelta) < 0.001)
|
||
{
|
||
return;
|
||
}
|
||
|
||
// Match C++ lines 118-119: Compute pose delta
|
||
var odometryPoseDelta = odometryDataNewest.Pose.Inverse() * odometryDataOldest.Pose;
|
||
|
||
// Match C++ lines 120-123: Compute angular velocity from odometry
|
||
var angleAxis = TransformOperations.RotationQuaternionToAngleAxisVector(odometryPoseDelta.Rotation);
|
||
_angularVelocityFromOdometry = new Vector3(
|
||
angleAxis.X / odometryTimeDelta,
|
||
angleAxis.Y / odometryTimeDelta,
|
||
angleAxis.Z / odometryTimeDelta);
|
||
|
||
// Instantaneous angular velocity from last 2 samples for tail-gap extrapolation
|
||
if (_odometryData.Count >= 2)
|
||
{
|
||
var prevOdom = _odometryData[^2];
|
||
var currOdom = _odometryData[^1];
|
||
var dtInstant = (currOdom.Time - prevOdom.Time) / 10_000_000.0;
|
||
if (dtInstant > 0.001)
|
||
{
|
||
var instantDelta = Quaternion.Inverse(prevOdom.Pose.Rotation) * currOdom.Pose.Rotation;
|
||
var instantAxis = TransformOperations.RotationQuaternionToAngleAxisVector(instantDelta);
|
||
_instantAngularVelocityFromOdometry = instantAxis / dtInstant;
|
||
}
|
||
}
|
||
|
||
// Match C++ lines 124-126: Return if no poses yet
|
||
if (_timedPoseQueue.Count == 0)
|
||
{
|
||
return;
|
||
}
|
||
|
||
// Match C++ lines 127-129: Compute linear velocity in tracking frame
|
||
var linearVelocityInTrackingFrameAtNewestOdometryTime = new Vector3(
|
||
odometryPoseDelta.Translation.X / odometryTimeDelta,
|
||
odometryPoseDelta.Translation.Y / odometryTimeDelta,
|
||
odometryPoseDelta.Translation.Z / odometryTimeDelta);
|
||
|
||
// Match C++ lines 130-133: Compute orientation at newest odometry time
|
||
// FIX: _odometryImuTracker is now accessed within the same lock scope
|
||
if (_odometryImuTracker != null)
|
||
{
|
||
var newestTimedPose = _timedPoseQueue[^1];
|
||
var orientationAtNewestOdometryTime =
|
||
newestTimedPose.Pose.Rotation *
|
||
ExtrapolateRotation(odometryDataNewest.Time, _odometryImuTracker);
|
||
|
||
// Match C++ lines 134-136: Transform to global frame
|
||
_linearVelocityFromOdometry = Vector3.Transform(
|
||
linearVelocityInTrackingFrameAtNewestOdometryTime,
|
||
orientationAtNewestOdometryTime);
|
||
}
|
||
else
|
||
{
|
||
// Fallback: use pose rotation directly without IMU extrapolation
|
||
var newestTimedPose = _timedPoseQueue[^1];
|
||
_linearVelocityFromOdometry = Vector3.Transform(
|
||
linearVelocityInTrackingFrameAtNewestOdometryTime,
|
||
newestTimedPose.Pose.Rotation);
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Match C++: ExtrapolatePose (pose_extrapolator.cc:195-208)
|
||
/// </summary>
|
||
public Rigid3d ExtrapolatePose(long time)
|
||
{
|
||
if (_timedPoseQueue.Count == 0)
|
||
{
|
||
return Rigid3d.Identity;
|
||
}
|
||
|
||
// Match C++ line 196-197
|
||
var newestTimedPose = _timedPoseQueue[^1];
|
||
if (time < newestTimedPose.Time)
|
||
{
|
||
var timeDiffMs = (newestTimedPose.Time - time) / TimeSpan.TicksPerMillisecond;
|
||
throw new ArgumentException($"time ({time}) must be >= newest pose time ({newestTimedPose.Time}), diff={timeDiffMs}ms", nameof(time));
|
||
}
|
||
|
||
// Match C++ line 198: if (cached_extrapolated_pose_.time != time)
|
||
if (_cachedExtrapolatedPose.HasValue && _cachedExtrapolatedPose.Value.Time == time)
|
||
{
|
||
return _cachedExtrapolatedPose.Value.Pose;
|
||
}
|
||
|
||
// Match C++ lines 199-200: Compute translation
|
||
var translation = ExtrapolateTranslation(time) + newestTimedPose.Pose.Translation;
|
||
|
||
// Match C++ lines 201-203: Compute rotation
|
||
Quaternion rotation;
|
||
lock (_dataLock)
|
||
{
|
||
if (_extrapolationImuTracker == null)
|
||
{
|
||
rotation = newestTimedPose.Pose.Rotation;
|
||
}
|
||
else if (_imuData.Count == 0)
|
||
{
|
||
// No IMU: prefer trajectory-based rotation from odometry
|
||
var odomRotation = ExtrapolateRotationFromOdometry(time);
|
||
rotation = odomRotation.HasValue
|
||
? newestTimedPose.Pose.Rotation * odomRotation.Value
|
||
: newestTimedPose.Pose.Rotation * ExtrapolateRotation(time, _extrapolationImuTracker);
|
||
}
|
||
else
|
||
{
|
||
rotation = newestTimedPose.Pose.Rotation *
|
||
ExtrapolateRotation(time, _extrapolationImuTracker);
|
||
}
|
||
}
|
||
|
||
// Match C++ lines 204-205: Cache result
|
||
_cachedExtrapolatedPose = new TimedPose(time, new Rigid3d(translation, rotation));
|
||
return _cachedExtrapolatedPose.Value.Pose;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Match C++: ExtrapolatePose_filter (pose_extrapolator.cc:146-193)
|
||
/// Returns filtered pose with low-pass filter to reduce jitter during direction changes.
|
||
/// </summary>
|
||
public Rigid3d ExtrapolatePoseFilter(long time)
|
||
{
|
||
if (_timedPoseQueue.Count == 0)
|
||
{
|
||
return Rigid3d.Identity;
|
||
}
|
||
|
||
var newestTimedPose = _timedPoseQueue[^1];
|
||
if (time < newestTimedPose.Time)
|
||
{
|
||
var timeDiffMs = (newestTimedPose.Time - time) / TimeSpan.TicksPerMillisecond;
|
||
throw new ArgumentException($"time ({time}) must be >= newest pose time ({newestTimedPose.Time}), diff={timeDiffMs}ms", nameof(time));
|
||
}
|
||
|
||
// Match C++ line 149: if (cached_extrapolated_pose_.time != time)
|
||
if (!_cachedExtrapolatedPose.HasValue || _cachedExtrapolatedPose.Value.Time != time)
|
||
{
|
||
// Match C++ lines 150-156: Compute translation and rotation, update cached_extrapolated_pose_
|
||
var translation = ExtrapolateTranslation(time) + newestTimedPose.Pose.Translation;
|
||
|
||
Quaternion rotation;
|
||
lock (_dataLock)
|
||
{
|
||
if (_extrapolationImuTracker == null || _imuTracker == null)
|
||
{
|
||
rotation = newestTimedPose.Pose.Rotation;
|
||
}
|
||
else if (_imuData.Count == 0)
|
||
{
|
||
// No IMU: prefer trajectory-based rotation from odometry
|
||
var odomRotation = ExtrapolateRotationFromOdometry(time);
|
||
rotation = odomRotation.HasValue
|
||
? newestTimedPose.Pose.Rotation * odomRotation.Value
|
||
: newestTimedPose.Pose.Rotation * ExtrapolateRotation(time, _extrapolationImuTracker);
|
||
}
|
||
else
|
||
{
|
||
rotation = newestTimedPose.Pose.Rotation *
|
||
ExtrapolateRotation(time, _extrapolationImuTracker);
|
||
}
|
||
}
|
||
|
||
_cachedExtrapolatedPose = new TimedPose(time, new Rigid3d(translation, rotation));
|
||
|
||
// Match C++ lines 159-190: Update cached_extrapolated_pose_filter
|
||
var extrapolationDeltaFilterTime = _cachedExtrapolatedPoseFilter.HasValue
|
||
? (time - _cachedExtrapolatedPoseFilter.Value.Time) / 10_000_000.0
|
||
: double.PositiveInfinity;
|
||
|
||
Vector3 translationFilter;
|
||
|
||
if (extrapolationDeltaFilterTime < 0.1 && _cachedExtrapolatedPoseFilter.HasValue)
|
||
{
|
||
// Match C++ lines 163-171: Apply low-pass filter
|
||
Vector3 linearVelocity;
|
||
lock (_dataLock)
|
||
{
|
||
linearVelocity = _odometryData.Count < 2
|
||
? _linearVelocityFromPoses
|
||
: _linearVelocityFromOdometry;
|
||
}
|
||
|
||
translationFilter = _cachedExtrapolatedPoseFilter.Value.Pose.Translation +
|
||
new Vector3(
|
||
extrapolationDeltaFilterTime * linearVelocity.X,
|
||
extrapolationDeltaFilterTime * linearVelocity.Y,
|
||
extrapolationDeltaFilterTime * linearVelocity.Z);
|
||
|
||
// Match C++ lines 173-180: Check delta and blend
|
||
var deltaTrans = translationFilter - _cachedExtrapolatedPose.Value.Pose.Translation;
|
||
|
||
if (deltaTrans.Length() < 0.03)
|
||
{
|
||
translationFilter = 0.7 * translationFilter + 0.3 * _cachedExtrapolatedPose.Value.Pose.Translation;
|
||
}
|
||
else
|
||
{
|
||
translationFilter = _cachedExtrapolatedPose.Value.Pose.Translation;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// Match C++ lines 187-189: No filter, use extrapolated pose directly
|
||
translationFilter = _cachedExtrapolatedPose.Value.Pose.Translation;
|
||
}
|
||
|
||
// Match C++ lines 182-183, 188-189: Cache filtered pose
|
||
_cachedExtrapolatedPoseFilter = new TimedPose(time,
|
||
new Rigid3d(translationFilter, _cachedExtrapolatedPose.Value.Pose.Rotation));
|
||
}
|
||
|
||
// Match C++ line 192: return cached_extrapolated_pose_filter.pose
|
||
return _cachedExtrapolatedPoseFilter!.Value.Pose;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Match C++: ExtrapolatePosesWithGravity (pose_extrapolator.cc:306-320)
|
||
/// </summary>
|
||
public ExtrapolationResult ExtrapolatePosesWithGravity(List<long> times)
|
||
{
|
||
var previousPoses = new List<Rigid3f>();
|
||
|
||
for (int i = 0; i < times.Count - 1; i++)
|
||
{
|
||
var pose = ExtrapolatePose(times[i]);
|
||
previousPoses.Add(new Rigid3f(
|
||
new Vector3(pose.Translation.X, pose.Translation.Y, pose.Translation.Z),
|
||
pose.Rotation));
|
||
}
|
||
|
||
var currentPose = ExtrapolatePose(times[^1]);
|
||
|
||
Vector3 currentVelocity;
|
||
lock (_dataLock)
|
||
{
|
||
currentVelocity = _odometryData.Count < 2
|
||
? _linearVelocityFromPoses
|
||
: _linearVelocityFromOdometry;
|
||
}
|
||
|
||
return new ExtrapolationResult
|
||
{
|
||
PreviousPoses = previousPoses,
|
||
CurrentPose = currentPose,
|
||
CurrentVelocity = currentVelocity,
|
||
GravityFromTracking = EstimateGravityOrientation(times[^1])
|
||
};
|
||
}
|
||
|
||
/// <summary>
|
||
/// Match C++: EstimateGravityOrientation (pose_extrapolator.cc:210-215)
|
||
/// </summary>
|
||
public Quaternion EstimateGravityOrientation(long time)
|
||
{
|
||
// Match C++ line 212: ImuTracker imu_tracker = *imu_tracker_;
|
||
// C++ assumes imu_tracker_ is initialized (via AddPose or InitializeWithImu)
|
||
if (_imuTracker == null)
|
||
{
|
||
throw new InvalidOperationException("ImuTracker not initialized. Call AddPose or InitializeWithImu first.");
|
||
}
|
||
|
||
var imuTracker = new ImuTracker(_imuTracker);
|
||
|
||
// Match C++ line 213: AdvanceImuTracker(time, &imu_tracker);
|
||
lock (_dataLock)
|
||
{
|
||
AdvanceImuTracker(time, imuTracker);
|
||
}
|
||
|
||
// Match C++ line 214: return imu_tracker.orientation();
|
||
return imuTracker.Orientation;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Match C++: TrimImuData (pose_extrapolator.cc:243-248)
|
||
/// </summary>
|
||
private void TrimImuData()
|
||
{
|
||
while (_imuData.Count > 1 && _timedPoseQueue.Count > 0 &&
|
||
_imuData[1].Time <= _timedPoseQueue[^1].Time)
|
||
{
|
||
_imuData.RemoveAt(0);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Match C++: TrimOdometryData (pose_extrapolator.cc:250-255)
|
||
/// </summary>
|
||
private void TrimOdometryData()
|
||
{
|
||
while (_odometryData.Count > 2 && _timedPoseQueue.Count > 0 &&
|
||
_odometryData[1].Time <= _timedPoseQueue[^1].Time)
|
||
{
|
||
_odometryData.RemoveAt(0);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Match C++: UpdateVelocitiesFromPoses (pose_extrapolator.cc:217-241)
|
||
/// </summary>
|
||
private void UpdateVelocitiesFromPoses()
|
||
{
|
||
if (_timedPoseQueue.Count < 2)
|
||
{
|
||
return;
|
||
}
|
||
|
||
var newestTimedPose = _timedPoseQueue[^1];
|
||
var newestTime = newestTimedPose.Time;
|
||
var oldestTimedPose = _timedPoseQueue[0];
|
||
var oldestTime = oldestTimedPose.Time;
|
||
|
||
var queueDelta = (newestTime - oldestTime) / 10_000_000.0;
|
||
var requiredQueueDelta = poseQueueDuration / 10_000_000.0;
|
||
|
||
if (queueDelta < requiredQueueDelta)
|
||
{
|
||
return;
|
||
}
|
||
|
||
var newestPose = newestTimedPose.Pose;
|
||
var oldestPose = oldestTimedPose.Pose;
|
||
var translationDelta = newestPose.Translation - oldestPose.Translation;
|
||
|
||
_linearVelocityFromPoses = new Vector3(
|
||
translationDelta.X / queueDelta,
|
||
translationDelta.Y / queueDelta,
|
||
translationDelta.Z / queueDelta);
|
||
|
||
var rotationDelta = Quaternion.Inverse(oldestPose.Rotation) * newestPose.Rotation;
|
||
var angleAxis = TransformOperations.RotationQuaternionToAngleAxisVector(rotationDelta);
|
||
|
||
_angularVelocityFromPoses = new Vector3(
|
||
angleAxis.X / queueDelta,
|
||
angleAxis.Y / queueDelta,
|
||
angleAxis.Z / queueDelta);
|
||
}
|
||
|
||
/// <summary>
|
||
/// Match C++: AdvanceImuTracker (pose_extrapolator.cc:257-286)
|
||
/// </summary>
|
||
private void AdvanceImuTracker(long time, ImuTracker imuTracker)
|
||
{
|
||
if (time < imuTracker.Time)
|
||
{
|
||
var timeDiffMs = (imuTracker.Time - time) / TimeSpan.TicksPerMillisecond;
|
||
throw new ArgumentException($"time ({time}) must be >= imuTracker time ({imuTracker.Time}), diff={timeDiffMs}ms", nameof(time));
|
||
}
|
||
|
||
if (_imuData.Count == 0 || time < _imuData[0].Time)
|
||
{
|
||
imuTracker.Advance(time);
|
||
imuTracker.AddImuLinearAccelerationObservation(Vector3.UnitZ);
|
||
var angularVel = _odometryData.Count < 2
|
||
? _angularVelocityFromPoses
|
||
: _angularVelocityFromOdometry;
|
||
imuTracker.AddImuAngularVelocityObservation(angularVel);
|
||
return;
|
||
}
|
||
|
||
if (imuTracker.Time < _imuData[0].Time)
|
||
{
|
||
imuTracker.Advance(_imuData[0].Time);
|
||
}
|
||
|
||
int startIndex = 0;
|
||
for (int i = 0; i < _imuData.Count; i++)
|
||
{
|
||
if (_imuData[i].Time >= imuTracker.Time)
|
||
{
|
||
startIndex = i;
|
||
break;
|
||
}
|
||
}
|
||
|
||
for (int i = startIndex; i < _imuData.Count && _imuData[i].Time < time; i++)
|
||
{
|
||
if (_imuData[i].Time >= imuTracker.Time)
|
||
{
|
||
imuTracker.Advance(_imuData[i].Time);
|
||
imuTracker.AddImuLinearAccelerationObservation(_imuData[i].LinearAcceleration);
|
||
imuTracker.AddImuAngularVelocityObservation(_imuData[i].AngularVelocity);
|
||
}
|
||
}
|
||
|
||
if (time >= imuTracker.Time)
|
||
{
|
||
imuTracker.Advance(time);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Match C++: ExtrapolateRotation (pose_extrapolator.cc:288-294)
|
||
/// </summary>
|
||
private Quaternion ExtrapolateRotation(long time, ImuTracker imuTracker)
|
||
{
|
||
// Match C++ line 290: CHECK_GE(time, imu_tracker->time());
|
||
if (time < imuTracker.Time)
|
||
{
|
||
var timeDiffMs = (imuTracker.Time - time) / TimeSpan.TicksPerMillisecond;
|
||
throw new ArgumentException($"time ({time}) must be >= imuTracker time ({imuTracker.Time}), diff={timeDiffMs}ms", nameof(time));
|
||
}
|
||
|
||
// Match C++ line 291: AdvanceImuTracker(time, imu_tracker);
|
||
AdvanceImuTracker(time, imuTracker);
|
||
|
||
// Match C++ lines 292-293: return last_orientation.inverse() * imu_tracker->orientation();
|
||
var lastOrientation = _imuTracker!.Orientation;
|
||
return Quaternion.Inverse(lastOrientation) * imuTracker.Orientation;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Find the index of the latest odometry entry with Time <= requested time.
|
||
/// Returns -1 if no suitable entry found.
|
||
/// Must be called under _dataLock.
|
||
/// </summary>
|
||
private int FindOdometryIndexBeforeTime(long time)
|
||
{
|
||
for (int i = _odometryData.Count - 1; i >= 0; i--)
|
||
{
|
||
if (_odometryData[i].Time <= time)
|
||
return i;
|
||
}
|
||
return -1;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Extrapolate rotation using the actual odometry trajectory, mirroring ExtrapolateTranslation.
|
||
/// Returns the relative rotation delta (compatible with ExtrapolateRotation return value).
|
||
/// Returns null when no odometry data is available, signaling fallback to ImuTracker path.
|
||
/// Must be called under _dataLock.
|
||
/// </summary>
|
||
private Quaternion? ExtrapolateRotationFromOdometry(long time)
|
||
{
|
||
if (_odometryData.Count < 2 || !_odometryAtLastPose.HasValue)
|
||
{
|
||
return null;
|
||
}
|
||
|
||
var odomRef = _odometryAtLastPose.Value;
|
||
|
||
// Find latest odometry entry at or before requested time
|
||
var idx = FindOdometryIndexBeforeTime(time);
|
||
|
||
Quaternion deltaRotation;
|
||
long lastCoveredTime;
|
||
|
||
if (idx >= 0 && _odometryData[idx].Time > odomRef.Time)
|
||
{
|
||
// Exact rotation delta from odometry trajectory
|
||
var odomAtTime = _odometryData[idx];
|
||
deltaRotation = Quaternion.Inverse(odomRef.Pose.Rotation) * odomAtTime.Pose.Rotation;
|
||
lastCoveredTime = odomAtTime.Time;
|
||
}
|
||
else
|
||
{
|
||
deltaRotation = Quaternion.Identity;
|
||
lastCoveredTime = _timedPoseQueue[^1].Time;
|
||
}
|
||
|
||
// Instantaneous angular velocity extrapolation for the small gap after last odometry sample
|
||
if (time > lastCoveredTime)
|
||
{
|
||
var dtRemaining = (time - lastCoveredTime) / 10_000_000.0;
|
||
var tailAngleAxis = _instantAngularVelocityFromOdometry * dtRemaining;
|
||
var tailRotation = TransformOperations.AngleAxisVectorToRotationQuaternion(tailAngleAxis);
|
||
deltaRotation = Quaternion.Normalize(deltaRotation * tailRotation);
|
||
}
|
||
|
||
return deltaRotation;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Extrapolate translation using the actual odometry trajectory for exact displacement,
|
||
/// with constant-velocity extrapolation only for the small gap after the last odometry sample.
|
||
/// Falls back to pose-based constant velocity when no odometry data is available.
|
||
/// </summary>
|
||
private Vector3 ExtrapolateTranslation(long time)
|
||
{
|
||
var newestTimedPose = _timedPoseQueue[^1];
|
||
|
||
lock (_dataLock)
|
||
{
|
||
if (_odometryData.Count < 2 || !_odometryAtLastPose.HasValue)
|
||
{
|
||
// No odometry: fall back to constant-velocity from poses
|
||
var dtFallback = (time - newestTimedPose.Time) / 10_000_000.0;
|
||
return new Vector3(
|
||
dtFallback * _linearVelocityFromPoses.X,
|
||
dtFallback * _linearVelocityFromPoses.Y,
|
||
dtFallback * _linearVelocityFromPoses.Z);
|
||
}
|
||
|
||
var odomRef = _odometryAtLastPose.Value;
|
||
|
||
// Find latest odometry entry at or before requested time
|
||
var idx = FindOdometryIndexBeforeTime(time);
|
||
|
||
Vector3 displacementGlobal;
|
||
long lastCoveredTime;
|
||
|
||
if (idx >= 0 && _odometryData[idx].Time > odomRef.Time)
|
||
{
|
||
// Compute exact displacement from odometry trajectory (in odom frame)
|
||
var odomAtTime = _odometryData[idx];
|
||
var displacementOdom = odomAtTime.Pose.Translation - odomRef.Pose.Translation;
|
||
|
||
// Transform odom-frame displacement to global frame
|
||
displacementGlobal = Vector3.Transform(displacementOdom, _odomToGlobalRotation);
|
||
lastCoveredTime = odomAtTime.Time;
|
||
}
|
||
else
|
||
{
|
||
// No odometry data between reference and requested time
|
||
displacementGlobal = Vector3.Zero;
|
||
lastCoveredTime = newestTimedPose.Time;
|
||
}
|
||
|
||
// Constant-velocity extrapolation for the small gap after last odometry sample
|
||
if (time > lastCoveredTime)
|
||
{
|
||
var dtRemaining = (time - lastCoveredTime) / 10_000_000.0;
|
||
displacementGlobal += new Vector3(
|
||
dtRemaining * _linearVelocityFromOdometry.X,
|
||
dtRemaining * _linearVelocityFromOdometry.Y,
|
||
dtRemaining * _linearVelocityFromOdometry.Z);
|
||
}
|
||
|
||
return displacementGlobal;
|
||
}
|
||
}
|
||
}
|