112 lines
4.8 KiB
C#
112 lines
4.8 KiB
C#
/*
|
||
* Copyright 2016 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.Common.Time;
|
||
using CartographerSharp.Models.GroundTruth;
|
||
using CartographerSharp.Models.Transform;
|
||
using CartographerSharp.Transform;
|
||
using System.Globalization;
|
||
using RobotNet10.Shared.Numbers;
|
||
using GroundTruthProto = CartographerSharp.Models.GroundTruth.GroundTruth;
|
||
using QuaternionUtils = CartographerSharp.Transform.QuaternionUtils;
|
||
|
||
namespace CartographerSharp.GroundTruth
|
||
{
|
||
/// <summary>
|
||
/// Reads a text file and converts it to a GroundTruth proto. Each line contains:
|
||
/// time1 time2 x y z roll pitch yaw
|
||
/// using Unix epoch timestamps.
|
||
///
|
||
/// This is the format used in the relations files provided for:
|
||
/// R. Kuemmerle, B. Steder, C. Dornhege, M. Ruhnke, G. Grisetti, C. Stachniss,
|
||
/// and A. Kleiner, "On measuring the accuracy of SLAM algorithms," Autonomous
|
||
/// Robots, vol. 27, no. 4, pp. 387–407, 2009.
|
||
/// </summary>
|
||
public static class RelationsTextFile
|
||
{
|
||
/// <summary>
|
||
/// Reads relations from a text file.
|
||
/// </summary>
|
||
/// <param name="relationsFilename">Path to the relations text file.</param>
|
||
/// <returns>GroundTruth proto with relations.</returns>
|
||
public static GroundTruthProto ReadRelationsTextFile(string relationsFilename)
|
||
{
|
||
if (string.IsNullOrEmpty(relationsFilename))
|
||
throw new ArgumentException("Relations filename cannot be null or empty", nameof(relationsFilename));
|
||
if (!File.Exists(relationsFilename))
|
||
throw new FileNotFoundException("Relations file not found", relationsFilename);
|
||
|
||
var groundTruth = new GroundTruthProto
|
||
{
|
||
Relations = []
|
||
};
|
||
|
||
var lines = File.ReadAllLines(relationsFilename);
|
||
foreach (var line in lines)
|
||
{
|
||
if (string.IsNullOrWhiteSpace(line))
|
||
continue;
|
||
|
||
var parts = line.Split([' ', '\t'], StringSplitOptions.RemoveEmptyEntries);
|
||
if (parts.Length < 8)
|
||
continue;
|
||
|
||
if (!double.TryParse(parts[0], NumberStyles.Float, CultureInfo.InvariantCulture, out var unixTime1) ||
|
||
!double.TryParse(parts[1], NumberStyles.Float, CultureInfo.InvariantCulture, out var unixTime2) ||
|
||
!double.TryParse(parts[2], NumberStyles.Float, CultureInfo.InvariantCulture, out var x) ||
|
||
!double.TryParse(parts[3], NumberStyles.Float, CultureInfo.InvariantCulture, out var y) ||
|
||
!double.TryParse(parts[4], NumberStyles.Float, CultureInfo.InvariantCulture, out var z) ||
|
||
!double.TryParse(parts[5], NumberStyles.Float, CultureInfo.InvariantCulture, out var roll) ||
|
||
!double.TryParse(parts[6], NumberStyles.Float, CultureInfo.InvariantCulture, out var pitch) ||
|
||
!double.TryParse(parts[7], NumberStyles.Float, CultureInfo.InvariantCulture, out var yaw))
|
||
{
|
||
continue;
|
||
}
|
||
|
||
var commonTime1 = UnixToCommonTime(unixTime1);
|
||
var commonTime2 = UnixToCommonTime(unixTime2);
|
||
|
||
// Create expected transform from translation and Euler angles
|
||
var expected = new Rigid3d( new Vector3(x, y, z), QuaternionUtils.RollPitchYaw(roll, pitch, yaw));
|
||
|
||
// Convert TimeSpan to universal ticks (same as ToUniversal for DateTime)
|
||
var relation = new Relation
|
||
{
|
||
Timestamp1 = commonTime1.Ticks,
|
||
Timestamp2 = commonTime2.Ticks,
|
||
Expected = (Rigid3dProto)expected, // Explicit conversion from Rigid3d to Rigid3dProto
|
||
CoveredDistance = 0.0 // Will be computed later if needed
|
||
};
|
||
|
||
groundTruth.Relations.Add(relation);
|
||
}
|
||
|
||
return groundTruth;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Converts Unix timestamp to Common time (TimeSpan since epoch).
|
||
/// </summary>
|
||
private static TimeSpan UnixToCommonTime(double unixTime)
|
||
{
|
||
const long kUtsTicksPerSecond = 10000000;
|
||
var utsEpochOffsetTicks = TimeUtils.UtsEpochOffsetFromUnixEpochInSeconds * kUtsTicksPerSecond;
|
||
var unixTimeTicks = (long)(unixTime * kUtsTicksPerSecond);
|
||
return TimeSpan.FromTicks(utsEpochOffsetTicks + unixTimeTicks);
|
||
}
|
||
}
|
||
}
|