Initial commit
This commit is contained in:
@@ -0,0 +1,325 @@
|
||||
/*
|
||||
* 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.Sensor;
|
||||
using CartographerSharp.Transform;
|
||||
|
||||
namespace CartographerSharp.Mapping;
|
||||
|
||||
/// <summary>
|
||||
/// This interface is used for both 2D and 3D SLAM. Implementations wire up a
|
||||
/// global SLAM stack, i.e. local SLAM for initial pose estimates, scan matching
|
||||
/// to detect loop closure, and a sparse pose graph optimization to compute
|
||||
/// optimized pose estimates.
|
||||
/// </summary>
|
||||
public interface ITrajectoryBuilder
|
||||
{
|
||||
/// <summary>
|
||||
/// Result of inserting data into submaps.
|
||||
/// </summary>
|
||||
public struct InsertionResult(NodeId nodeId, TrajectoryNode.Data? constantData, List<Submap> insertionSubmaps)
|
||||
{
|
||||
public NodeId NodeId { get; set; } = nodeId;
|
||||
public TrajectoryNode.Data? ConstantData { get; set; } = constantData;
|
||||
public List<Submap> InsertionSubmaps { get; set; } = insertionSubmaps ?? [];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Result of matching sensor data (scan matching, localization, etc.)
|
||||
/// Returned when range data accumulation is completed.
|
||||
/// Match C++: MatchingResult{time, pose_estimate, range_data_in_local, insertion_result, pose_confidence, ceres_score}
|
||||
/// </summary>
|
||||
public struct MatchingResult(
|
||||
int trajectoryId,
|
||||
long time,
|
||||
Rigid3d localPose,
|
||||
RangeData rangeDataInLocal,
|
||||
InsertionResult? insertionResult,
|
||||
double poseConfidence = -1.0,
|
||||
double ceresScore = -1.0,
|
||||
PointCloud? samplePointCloudGlobal = null)
|
||||
{
|
||||
public int TrajectoryId { get; set; } = trajectoryId;
|
||||
public long Time { get; set; } = time;
|
||||
public Rigid3d LocalPose { get; set; } = localPose;
|
||||
public RangeData RangeDataInLocal { get; set; } = rangeDataInLocal;
|
||||
public InsertionResult? InsertionResult { get; set; } = insertionResult;
|
||||
/// <summary>
|
||||
/// Pose confidence score from real-time correlative scan matcher.
|
||||
/// Match C++: pose_confidence field in MatchingResult.
|
||||
/// Value is -1.0 if confidence score is not provided.
|
||||
/// </summary>
|
||||
public double PoseConfidence { get; set; } = poseConfidence;
|
||||
/// <summary>
|
||||
/// Ceres solver final cost from scan matching.
|
||||
/// Match C++: ceres_score tracked in ScanMatch function.
|
||||
/// Value is -1.0 if ceres score is not available.
|
||||
/// </summary>
|
||||
public double CeresScore { get; set; } = ceresScore;
|
||||
/// <summary>
|
||||
/// Sample point cloud for visualization/debugging.
|
||||
/// IMPORTANT: When returned from LocalTrajectoryBuilder2D, this is in LOCAL trajectory frame.
|
||||
/// GlobalTrajectoryBuilder2D transforms it to GLOBAL map frame before returning to the caller.
|
||||
/// After GlobalTrajectoryBuilder2D processing, this IS in global frame as the name suggests.
|
||||
/// </summary>
|
||||
public PointCloud? SamplePointCloudGlobal { get; set; } = samplePointCloudGlobal;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Sensor identifier.
|
||||
/// </summary>
|
||||
public struct SensorId(SensorId.SensorType type, string id) : IEquatable<SensorId>, IComparable<SensorId>
|
||||
{
|
||||
public enum SensorType
|
||||
{
|
||||
Range = 0,
|
||||
Imu,
|
||||
Odometry,
|
||||
FixedFramePose,
|
||||
Landmark,
|
||||
LocalSlamResult
|
||||
}
|
||||
|
||||
public SensorType Type { get; set; } = type;
|
||||
public string Id { get; set; } = id;
|
||||
|
||||
public readonly bool Equals(SensorId other)
|
||||
{
|
||||
return Type == other.Type && Id == other.Id;
|
||||
}
|
||||
|
||||
public readonly override bool Equals(object? obj)
|
||||
{
|
||||
return obj is SensorId other && Equals(other);
|
||||
}
|
||||
|
||||
public readonly override int GetHashCode()
|
||||
{
|
||||
return HashCode.Combine(Type, Id);
|
||||
}
|
||||
|
||||
public readonly int CompareTo(SensorId other)
|
||||
{
|
||||
var typeComparison = Type.CompareTo(other.Type);
|
||||
if (typeComparison != 0) return typeComparison;
|
||||
return string.Compare(Id, other.Id, StringComparison.Ordinal);
|
||||
}
|
||||
|
||||
public static bool operator ==(SensorId left, SensorId right)
|
||||
{
|
||||
return left.Equals(right);
|
||||
}
|
||||
|
||||
public static bool operator !=(SensorId left, SensorId right)
|
||||
{
|
||||
return !left.Equals(right);
|
||||
}
|
||||
|
||||
public static bool operator <(SensorId left, SensorId right)
|
||||
{
|
||||
return left.CompareTo(right) < 0;
|
||||
}
|
||||
|
||||
public static bool operator <=(SensorId left, SensorId right)
|
||||
{
|
||||
return left.CompareTo(right) <= 0;
|
||||
}
|
||||
|
||||
public static bool operator >(SensorId left, SensorId right)
|
||||
{
|
||||
return left.CompareTo(right) > 0;
|
||||
}
|
||||
|
||||
public static bool operator >=(SensorId left, SensorId right)
|
||||
{
|
||||
return left.CompareTo(right) >= 0;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Adds timed point cloud data from a sensor.
|
||||
/// Returns MatchingResult when range data accumulation is completed, otherwise null.
|
||||
/// </summary>
|
||||
MatchingResult? AddSensorData(string sensorId, TimedPointCloudData timedPointCloudData);
|
||||
|
||||
/// <summary>
|
||||
/// Adds IMU data from a sensor.
|
||||
/// </summary>
|
||||
void AddSensorData(string sensorId, ImuData imuData);
|
||||
|
||||
/// <summary>
|
||||
/// Adds odometry data from a sensor.
|
||||
/// </summary>
|
||||
void AddSensorData(string sensorId, OdometryData odometryData);
|
||||
|
||||
/// <summary>
|
||||
/// Adds fixed frame pose data from a sensor.
|
||||
/// </summary>
|
||||
void AddSensorData(string sensorId, FixedFramePoseData fixedFramePoseData);
|
||||
|
||||
/// <summary>
|
||||
/// Adds landmark data from a sensor.
|
||||
/// </summary>
|
||||
void AddSensorData(string sensorId, LandmarkData landmarkData);
|
||||
|
||||
/// <summary>
|
||||
/// Allows to directly add local SLAM results to the 'PoseGraph'. Note that it
|
||||
/// is invalid to add local SLAM results for a trajectory that has a
|
||||
/// 'LocalTrajectoryBuilder2D/3D'.
|
||||
/// </summary>
|
||||
void AddLocalSlamResultData(LocalSlamResultData localSlamResultData);
|
||||
|
||||
/// <summary>
|
||||
/// Tries to get the current pose from the internal extrapolator at the given time.
|
||||
/// Returns null when there is no local trajectory builder / extrapolator, or when
|
||||
/// the requested time is before the last pose time. Allows callers (e.g. CartographerService)
|
||||
/// to always read a live extrapolated pose instead of only the last scan-matched pose.
|
||||
/// </summary>
|
||||
/// <param name="time">Time in ticks (e.g. DateTime.UtcNow.Ticks) to extrapolate to.</param>
|
||||
/// <returns>Extrapolated pose in trajectory local frame, or null if not available.</returns>
|
||||
Rigid3d? TryGetExtrapolatedPose(long time);
|
||||
|
||||
/// <summary>
|
||||
/// Tries to get the current pose with low-pass filter to reduce jitter during direction changes.
|
||||
/// Match C++: ExtrapolatePose_filter - should be used for publishing pose to external systems.
|
||||
/// </summary>
|
||||
/// <param name="time">Time in ticks (e.g. DateTime.UtcNow.Ticks) to extrapolate to.</param>
|
||||
/// <returns>Filtered extrapolated pose in trajectory local frame, or null if not available.</returns>
|
||||
Rigid3d? TryGetExtrapolatedPoseFilter(long time);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Local SLAM result data.
|
||||
/// </summary>
|
||||
public class LocalSlamResultData(
|
||||
int trajectoryId,
|
||||
long time,
|
||||
Rigid3d localPose,
|
||||
RangeData rangeData,
|
||||
ITrajectoryBuilder.InsertionResult? insertionResult = null)
|
||||
{
|
||||
public int TrajectoryId { get; set; } = trajectoryId;
|
||||
public long Time { get; set; } = time;
|
||||
public Rigid3d LocalPose { get; set; } = localPose;
|
||||
public RangeData RangeData { get; set; } = rangeData;
|
||||
public ITrajectoryBuilder.InsertionResult? InsertionResult { get; set; } = insertionResult;
|
||||
|
||||
/// <summary>
|
||||
/// Adds this local SLAM result to the pose graph.
|
||||
/// This is used when running in pure localization mode or when replaying data.
|
||||
/// </summary>
|
||||
/// <param name="trajectoryId">The trajectory ID to add the result to</param>
|
||||
/// <param name="poseGraph">The pose graph to add the result to</param>
|
||||
public void AddToPoseGraph(int trajectoryId, IPoseGraph poseGraph)
|
||||
{
|
||||
if (InsertionResult == null)
|
||||
{
|
||||
// No insertion result, nothing to add to pose graph
|
||||
return;
|
||||
}
|
||||
|
||||
var insertionResult = InsertionResult.Value;
|
||||
|
||||
// Add node to pose graph
|
||||
// Note: The actual AddNode implementation depends on whether we're using 2D or 3D
|
||||
// For now, we'll need to cast to the specific pose graph type
|
||||
if (poseGraph is Mapping.Internal.D2D.PoseGraph2D poseGraph2D)
|
||||
{
|
||||
// For 2D, we need to convert Submap to Submap2D
|
||||
var submaps2D = insertionResult.InsertionSubmaps
|
||||
.OfType<Mapping.D2D.Submap2D>()
|
||||
.ToList();
|
||||
|
||||
if (submaps2D.Count != insertionResult.InsertionSubmaps.Count)
|
||||
{
|
||||
throw new InvalidOperationException("Cannot add 3D submaps to 2D pose graph");
|
||||
}
|
||||
|
||||
if(insertionResult.ConstantData is null)
|
||||
throw new NullReferenceException(nameof(insertionResult.ConstantData));
|
||||
|
||||
// Add node to pose graph
|
||||
poseGraph2D.AddNode(insertionResult.ConstantData, trajectoryId, submaps2D);
|
||||
}
|
||||
else if (poseGraph is Mapping.Internal.D3D.PoseGraph3D poseGraph3D)
|
||||
{
|
||||
// For 3D, we need to convert Submap to Submap3D
|
||||
var submaps3D = insertionResult.InsertionSubmaps
|
||||
.OfType<Mapping.D3D.Submap3D>()
|
||||
.ToList();
|
||||
|
||||
if (submaps3D.Count != insertionResult.InsertionSubmaps.Count)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"Cannot add 2D submaps to 3D pose graph");
|
||||
}
|
||||
|
||||
if (insertionResult.ConstantData is null)
|
||||
throw new NullReferenceException(nameof(insertionResult.ConstantData));
|
||||
|
||||
poseGraph3D.AddNode(insertionResult.ConstantData, trajectoryId, submaps3D);
|
||||
}
|
||||
else
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
$"Unknown pose graph type: {poseGraph.GetType().Name}");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Conversion utilities for SensorId.
|
||||
/// </summary>
|
||||
public static class SensorIdOperations
|
||||
{
|
||||
/// <summary>
|
||||
/// Converts to proto representation.
|
||||
/// </summary>
|
||||
public static Models.Mapping.SensorId ToProto(ITrajectoryBuilder.SensorId sensorId)
|
||||
{
|
||||
var type = sensorId.Type switch
|
||||
{
|
||||
ITrajectoryBuilder.SensorId.SensorType.Range => Models.Mapping.SensorId.SensorType.Range,
|
||||
ITrajectoryBuilder.SensorId.SensorType.Imu => Models.Mapping.SensorId.SensorType.Imu,
|
||||
ITrajectoryBuilder.SensorId.SensorType.Odometry => Models.Mapping.SensorId.SensorType.Odometry,
|
||||
ITrajectoryBuilder.SensorId.SensorType.FixedFramePose => Models.Mapping.SensorId.SensorType.FixedFramePose,
|
||||
ITrajectoryBuilder.SensorId.SensorType.Landmark => Models.Mapping.SensorId.SensorType.Landmark,
|
||||
ITrajectoryBuilder.SensorId.SensorType.LocalSlamResult => Models.Mapping.SensorId.SensorType.LocalSlamResult,
|
||||
_ => throw new ArgumentException($"Unknown sensor type: {sensorId.Type}")
|
||||
};
|
||||
|
||||
return new Models.Mapping.SensorId(type, sensorId.Id);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Creates from proto representation.
|
||||
/// </summary>
|
||||
public static ITrajectoryBuilder.SensorId FromProto(Models.Mapping.SensorId sensorIdProto)
|
||||
{
|
||||
var type = sensorIdProto.Type switch
|
||||
{
|
||||
Models.Mapping.SensorId.SensorType.Range => ITrajectoryBuilder.SensorId.SensorType.Range,
|
||||
Models.Mapping.SensorId.SensorType.Imu => ITrajectoryBuilder.SensorId.SensorType.Imu,
|
||||
Models.Mapping.SensorId.SensorType.Odometry => ITrajectoryBuilder.SensorId.SensorType.Odometry,
|
||||
Models.Mapping.SensorId.SensorType.FixedFramePose => ITrajectoryBuilder.SensorId.SensorType.FixedFramePose,
|
||||
Models.Mapping.SensorId.SensorType.Landmark => ITrajectoryBuilder.SensorId.SensorType.Landmark,
|
||||
Models.Mapping.SensorId.SensorType.LocalSlamResult => ITrajectoryBuilder.SensorId.SensorType.LocalSlamResult,
|
||||
_ => throw new ArgumentException($"Unknown sensor type: {sensorIdProto.Type}")
|
||||
};
|
||||
|
||||
return new ITrajectoryBuilder.SensorId(type, sensorIdProto.Id);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user