QUYVN/BQP
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
QUYVN
2026-07-13 09:25:40 +07:00
parent c08ff54676
commit bccfb156d7
1938 changed files with 641646 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

325
srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/ITrajectoryBuilder.cs Normal file
View File

@@ -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);
}
}