271 lines
12 KiB
C#
271 lines
12 KiB
C#
/*
|
|
* Copyright 2018 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 System.Globalization;
|
|
|
|
namespace CartographerSharp.Mapping.Internal;
|
|
|
|
/// <summary>
|
|
/// Synchronizes TimedPointCloudData from different sensors. Input needs only be
|
|
/// monotonous in 'TimedPointCloudData::time', output is monotonous in per-point
|
|
/// timing. Up to one message per sensor is buffered, so a delay of the period of
|
|
/// the slowest sensor may be introduced, which can be alleviated by passing
|
|
/// subdivisions.
|
|
/// </summary>
|
|
public class RangeDataCollator(IEnumerable<string> expectedRangeSensorIds)
|
|
{
|
|
private const double kDefaultIntensityValue = 0.0;
|
|
|
|
private readonly HashSet<string> _expectedSensorIds = [.. expectedRangeSensorIds];
|
|
private readonly Dictionary<string, TimedPointCloudData> _idToPendingData = [];
|
|
private long _currentStart = long.MinValue; // Universal Time Scale ticks
|
|
private long _currentEnd = long.MinValue; // Universal Time Scale ticks
|
|
|
|
// Debug: Track per-sensor timestamps to detect out-of-order data
|
|
private readonly Dictionary<string, long> _lastSensorTimestamp = [];
|
|
private static readonly object _collatorLogLock = new();
|
|
private static readonly string _collatorLogPath = "collator.log";
|
|
private long _lastOutputTime = long.MinValue;
|
|
|
|
private static void LogCollator(string message)
|
|
{
|
|
lock (_collatorLogLock)
|
|
{
|
|
try
|
|
{
|
|
var timestamp = DateTime.Now.ToString("yyyy-MM-dd HH:mm:ss.fff", CultureInfo.InvariantCulture);
|
|
var line = $"{timestamp}|{message}";
|
|
File.AppendAllText(_collatorLogPath, line + Environment.NewLine);
|
|
}
|
|
catch { /* Ignore logging errors */ }
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// If timed_point_cloud_data has incomplete intensity data, we will fill the
|
|
/// missing intensities with kDefaultIntensityValue.
|
|
/// </summary>
|
|
public TimedPointCloudOriginData AddRangeData(string sensorId, TimedPointCloudData timedPointCloudData)
|
|
{
|
|
if (!_expectedSensorIds.Contains(sensorId))
|
|
{
|
|
throw new ArgumentException($"Unexpected sensor ID: {sensorId}", nameof(sensorId));
|
|
}
|
|
|
|
// DEBUG: Timestamp validation - check if input is monotonic per sensor
|
|
var currentTime = timedPointCloudData.Time;
|
|
var tickMs = currentTime / TimeSpan.TicksPerMillisecond;
|
|
if (_lastSensorTimestamp.TryGetValue(sensorId, out var lastTime))
|
|
{
|
|
if (currentTime < lastTime)
|
|
{
|
|
var diffMs = (lastTime - currentTime) / (double)TimeSpan.TicksPerMillisecond;
|
|
LogCollator($"WARNING|sensor={sensorId}|TIME_REVERSAL|prev_tick={lastTime / TimeSpan.TicksPerMillisecond}|curr_tick={tickMs}|diff={diffMs:F3}ms");
|
|
}
|
|
else
|
|
{
|
|
var deltaMs = (currentTime - lastTime) / (double)TimeSpan.TicksPerMillisecond;
|
|
// Log normal data flow (can comment out for less verbose logging)
|
|
// LogCollator($"INFO|sensor={sensorId}|tick={tickMs}|delta={deltaMs:F3}ms|points={timedPointCloudData.Ranges.Count}");
|
|
}
|
|
}
|
|
_lastSensorTimestamp[sensorId] = currentTime;
|
|
|
|
// Fill missing intensities
|
|
// Match C++: timed_point_cloud_data.intensities.resize(
|
|
// timed_point_cloud_data.ranges.size(), kDefaultIntensityValue);
|
|
// This resizes to exactly ranges.size(), filling with kDefaultIntensityValue if needed,
|
|
// or truncating if intensities is larger than ranges
|
|
if (timedPointCloudData.Intensities.Count != timedPointCloudData.Ranges.Count)
|
|
{
|
|
var intensities = new List<double>(timedPointCloudData.Intensities);
|
|
// Resize to match ranges.Count exactly
|
|
if (intensities.Count < timedPointCloudData.Ranges.Count)
|
|
{
|
|
// Fill missing with default value
|
|
while (intensities.Count < timedPointCloudData.Ranges.Count)
|
|
{
|
|
intensities.Add(kDefaultIntensityValue);
|
|
}
|
|
}
|
|
else if (intensities.Count > timedPointCloudData.Ranges.Count)
|
|
{
|
|
// Truncate if larger
|
|
intensities.RemoveRange(timedPointCloudData.Ranges.Count, intensities.Count - timedPointCloudData.Ranges.Count);
|
|
}
|
|
timedPointCloudData.Intensities = intensities;
|
|
}
|
|
|
|
if (_idToPendingData.TryGetValue(sensorId, out TimedPointCloudData value))
|
|
{
|
|
_currentStart = _currentEnd;
|
|
_currentEnd = value.Time;
|
|
var result = CropAndMerge();
|
|
_idToPendingData[sensorId] = timedPointCloudData;
|
|
return result;
|
|
}
|
|
|
|
_idToPendingData[sensorId] = timedPointCloudData;
|
|
|
|
if (_expectedSensorIds.Count != _idToPendingData.Count)
|
|
{
|
|
return new TimedPointCloudOriginData(0, [], []);
|
|
}
|
|
|
|
_currentStart = _currentEnd;
|
|
// We have messages from all sensors, move forward to oldest.
|
|
var oldestTimestamp = _idToPendingData.Values.Min(d => d.Time);
|
|
_currentEnd = oldestTimestamp;
|
|
return CropAndMerge();
|
|
}
|
|
|
|
private TimedPointCloudOriginData CropAndMerge()
|
|
{
|
|
var result = new TimedPointCloudOriginData(_currentEnd, [], []);
|
|
|
|
// DEBUG: Check if output time is monotonic
|
|
var outputTickMs = _currentEnd / TimeSpan.TicksPerMillisecond;
|
|
if (_lastOutputTime != long.MinValue && _currentEnd < _lastOutputTime)
|
|
{
|
|
var diffMs = (_lastOutputTime - _currentEnd) / (double)TimeSpan.TicksPerMillisecond;
|
|
LogCollator($"WARNING|OUTPUT_TIME_REVERSAL|prev_output={_lastOutputTime / TimeSpan.TicksPerMillisecond}|curr_output={outputTickMs}|diff={diffMs:F3}ms|start={_currentStart / TimeSpan.TicksPerMillisecond}");
|
|
}
|
|
_lastOutputTime = _currentEnd;
|
|
|
|
var warnedForDroppedPoints = false;
|
|
|
|
// Use ToList() to create a snapshot for iteration, but we'll modify _idToPendingData during iteration
|
|
var sensorIds = _idToPendingData.Keys.ToList();
|
|
|
|
foreach (var sensorId in sensorIds)
|
|
{
|
|
if (!_idToPendingData.TryGetValue(sensorId, out var data))
|
|
{
|
|
continue; // Already removed
|
|
}
|
|
|
|
var ranges = data.Ranges;
|
|
var intensities = data.Intensities;
|
|
|
|
// Find overlap range (matching C++ line 69-80)
|
|
var overlapBegin = 0;
|
|
while (overlapBegin < ranges.Count)
|
|
{
|
|
// Convert seconds to ticks: use double for precision, then round to long
|
|
// This matches C++: data.time + common::FromSeconds((*overlap_begin).time)
|
|
var pointTime = data.Time + (long)Math.Round(ranges[overlapBegin].Time * TimeSpan.TicksPerSecond);
|
|
if (pointTime >= _currentStart)
|
|
{
|
|
break;
|
|
}
|
|
overlapBegin++;
|
|
}
|
|
|
|
var overlapEnd = overlapBegin;
|
|
while (overlapEnd < ranges.Count)
|
|
{
|
|
// Convert seconds to ticks: use double for precision, then round to long
|
|
// This matches C++: data.time + common::FromSeconds((*overlap_end).time)
|
|
var pointTime = data.Time + (long)Math.Round(ranges[overlapEnd].Time * TimeSpan.TicksPerSecond);
|
|
if (pointTime > _currentEnd)
|
|
{
|
|
break;
|
|
}
|
|
overlapEnd++;
|
|
}
|
|
|
|
if (overlapBegin > 0 && !warnedForDroppedPoints)
|
|
{
|
|
// Log warning about dropped points (matching C++ line 81-84)
|
|
warnedForDroppedPoints = true;
|
|
}
|
|
|
|
// Copy overlapping range (matching C++ line 88-106)
|
|
if (overlapBegin < overlapEnd)
|
|
{
|
|
var originIndex = result.Origins.Count;
|
|
result.Origins.Add(data.Origin);
|
|
|
|
// CRITICAL FIX: Apply time correction to point_time.time (match C++ line 91-103)
|
|
// C++: const double time_correction = static_cast<double>(common::ToSeconds(data.time - current_end_));
|
|
// C++: point.point_time.time += time_correction;
|
|
// Time correction converts the difference between data.Time and currentEnd from ticks to seconds
|
|
var timeCorrection = ((data.Time - _currentEnd) / 10_000_000.0); // Convert ticks to seconds (10 million ticks per second)
|
|
|
|
for (int i = overlapBegin; i < overlapEnd; i++)
|
|
{
|
|
// Apply time correction to point time
|
|
// Create new TimedRangefinderPoint with corrected time
|
|
var correctedPointTime = ranges[i].Time + timeCorrection;
|
|
var correctedPoint = new TimedRangefinderPoint(
|
|
ranges[i].Position,
|
|
correctedPointTime);
|
|
|
|
var rangeMeasurement = new TimedPointCloudOriginData.RangeMeasurement(
|
|
correctedPoint,
|
|
intensities[i],
|
|
originIndex);
|
|
result.Ranges.Add(rangeMeasurement);
|
|
}
|
|
}
|
|
|
|
// CRITICAL FIX: Drop buffered points until overlap_end (matching C++ line 108-121)
|
|
// This prevents reprocessing of already-processed points
|
|
if (overlapEnd == ranges.Count)
|
|
{
|
|
// All points processed, remove entry
|
|
_idToPendingData.Remove(sensorId);
|
|
}
|
|
else if (overlapEnd == 0)
|
|
{
|
|
// No points processed, keep entry as is
|
|
// Continue to next sensor
|
|
}
|
|
else
|
|
{
|
|
// Some points processed, keep only unprocessed points
|
|
var remainingRanges = new TimedPointCloud();
|
|
var remainingIntensities = new List<double>();
|
|
|
|
for (int i = overlapEnd; i < ranges.Count; i++)
|
|
{
|
|
remainingRanges.Add(ranges[i]);
|
|
remainingIntensities.Add(intensities[i]);
|
|
}
|
|
|
|
_idToPendingData[sensorId] = new TimedPointCloudData(
|
|
data.Time,
|
|
data.Origin,
|
|
remainingRanges,
|
|
remainingIntensities
|
|
);
|
|
}
|
|
}
|
|
|
|
// CRITICAL FIX: Sort ranges by time (match C++ line 124-128)
|
|
// C++: std::sort(result.ranges.begin(), result.ranges.end(),
|
|
// [](const auto& a, const auto& b) { return a.point_time.time < b.point_time.time; });
|
|
// This ensures output is monotonous in per-point timing as documented
|
|
if (result.Ranges.Count > 0)
|
|
{
|
|
result.Ranges = [.. result.Ranges.OrderBy(r => r.PointTime.Time)];
|
|
}
|
|
|
|
return result;
|
|
}
|
|
}
|