344 lines
14 KiB
C#
344 lines
14 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.Models.Transform;
|
|
using CartographerSharp.Sensor;
|
|
using CartographerSharp.Transform;
|
|
|
|
namespace CartographerSharp.Mapping;
|
|
|
|
/// <summary>
|
|
/// Base class for pose graph implementations.
|
|
/// </summary>
|
|
public abstract class PoseGraph : IPoseGraph
|
|
{
|
|
/// <summary>
|
|
/// Initial trajectory pose information.
|
|
/// </summary>
|
|
public struct InitialTrajectoryPose(int toTrajectoryId, Rigid3d relativePose, long time)
|
|
{
|
|
public int ToTrajectoryId { get; set; } = toTrajectoryId;
|
|
public Rigid3d RelativePose { get; set; } = relativePose;
|
|
public long Time { get; set; } = time;
|
|
}
|
|
|
|
protected PoseGraph()
|
|
{
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets the total number of work items added to the work queue.
|
|
/// </summary>
|
|
public abstract int WorkItemsAdded { get; }
|
|
|
|
/// <summary>
|
|
/// Gets the total number of work items completed by the work queue.
|
|
/// </summary>
|
|
public abstract int WorkItemsCompleted { get; }
|
|
|
|
/// <summary>
|
|
/// Gets the number of work items currently pending in the work queue.
|
|
/// </summary>
|
|
public abstract int WorkItemsPending { get; }
|
|
|
|
/// <summary>
|
|
/// Gets the current number of items in the work queue.
|
|
/// </summary>
|
|
public abstract int WorkQueueCount { get; }
|
|
|
|
/// <summary>
|
|
/// Gets the number of nodes started in the constraint builder.
|
|
/// </summary>
|
|
public abstract int ConstraintBuilderNodesStarted { get; }
|
|
|
|
/// <summary>
|
|
/// Gets the number of nodes finished in the constraint builder.
|
|
/// </summary>
|
|
public abstract int ConstraintBuilderNodesFinished { get; }
|
|
|
|
/// <summary>
|
|
/// Gets the total number of trajectory nodes in the pose graph.
|
|
/// Used for progress tracking during optimization.
|
|
/// </summary>
|
|
public abstract int TrajectoryNodesCount { get; }
|
|
|
|
/// <summary>
|
|
/// Gets the total number of constraint tasks dispatched for scan matching.
|
|
/// </summary>
|
|
public abstract int ConstraintTasksTotal { get; }
|
|
|
|
/// <summary>
|
|
/// Gets the number of constraint tasks that have finished scan matching.
|
|
/// </summary>
|
|
public abstract int ConstraintTasksFinished { get; }
|
|
|
|
/// <summary>
|
|
/// Inserts an IMU measurement.
|
|
/// </summary>
|
|
public abstract void AddImuData(int trajectoryId, ImuData imuData);
|
|
|
|
/// <summary>
|
|
/// Inserts an odometry measurement.
|
|
/// </summary>
|
|
public abstract void AddOdometryData(int trajectoryId, OdometryData odometryData);
|
|
|
|
/// <summary>
|
|
/// Inserts a fixed frame pose measurement.
|
|
/// </summary>
|
|
public abstract void AddFixedFramePoseData(int trajectoryId, FixedFramePoseData fixedFramePoseData);
|
|
|
|
/// <summary>
|
|
/// Inserts landmarks observations.
|
|
/// </summary>
|
|
public abstract void AddLandmarkData(int trajectoryId, LandmarkData landmarkData);
|
|
|
|
/// <summary>
|
|
/// Drains the work queue to ensure all pending operations are completed.
|
|
/// This should be called before finishing a trajectory to avoid race conditions.
|
|
/// Default implementation does nothing (for pose graphs without work queues).
|
|
/// </summary>
|
|
public virtual void DrainWorkQueue()
|
|
{
|
|
// Default implementation: do nothing (for pose graphs without work queues)
|
|
}
|
|
|
|
/// <summary>
|
|
/// Finishes the given trajectory.
|
|
/// </summary>
|
|
public abstract void FinishTrajectory(int trajectoryId);
|
|
|
|
/// <summary>
|
|
/// Freezes a trajectory. Poses in this trajectory will not be optimized.
|
|
/// </summary>
|
|
public abstract void FreezeTrajectory(int trajectoryId);
|
|
|
|
/// <summary>
|
|
/// Adds a 'submap' from a proto with the given 'global_pose' to the
|
|
/// appropriate trajectory.
|
|
/// </summary>
|
|
public abstract void AddSubmapFromProto(Rigid3d globalPose, Models.Mapping.Submap submap);
|
|
|
|
/// <summary>
|
|
/// Adds a 'node' from a proto with the given 'global_pose' to the
|
|
/// appropriate trajectory.
|
|
/// </summary>
|
|
public abstract void AddNodeFromProto(Rigid3d globalPose, Models.Mapping.Node node);
|
|
|
|
/// <summary>
|
|
/// Sets the trajectory data from a proto.
|
|
/// </summary>
|
|
public abstract void SetTrajectoryDataFromProto(Models.Mapping.TrajectoryData data);
|
|
|
|
/// <summary>
|
|
/// Adds information that 'node_id' was inserted into 'submap_id'. The submap
|
|
/// has to be deserialized first.
|
|
/// </summary>
|
|
public abstract void AddNodeToSubmap(NodeId nodeId, SubmapId submapId);
|
|
|
|
/// <summary>
|
|
/// Adds serialized constraints. The corresponding trajectory nodes and submaps
|
|
/// have to be deserialized before calling this function.
|
|
/// </summary>
|
|
public abstract void AddSerializedConstraints(List<IPoseGraph.Constraint> constraints);
|
|
|
|
/// <summary>
|
|
/// Adds a 'trimmer'. It will be used after all data added before it has been
|
|
/// included in the pose graph.
|
|
/// </summary>
|
|
public abstract void AddTrimmer(PoseGraphTrimmer trimmer);
|
|
|
|
/// <summary>
|
|
/// Gets the current trajectory clusters.
|
|
/// </summary>
|
|
public abstract List<List<int>> GetConnectedTrajectories();
|
|
|
|
/// <summary>
|
|
/// Returns the IMU data.
|
|
/// </summary>
|
|
public abstract Dictionary<int, List<ImuData>> GetImuData();
|
|
|
|
/// <summary>
|
|
/// Returns the odometry data.
|
|
/// </summary>
|
|
public abstract Dictionary<int, List<OdometryData>> GetOdometryData();
|
|
|
|
/// <summary>
|
|
/// Returns the fixed frame pose data.
|
|
/// </summary>
|
|
public abstract Dictionary<int, List<FixedFramePoseData>> GetFixedFramePoseData();
|
|
|
|
/// <summary>
|
|
/// Returns the landmark data.
|
|
/// </summary>
|
|
public abstract Dictionary<string, IPoseGraph.LandmarkNode> GetLandmarkNodes();
|
|
|
|
/// <summary>
|
|
/// Sets a relative initial pose 'relative_pose' for 'from_trajectory_id' with
|
|
/// respect to 'to_trajectory_id' at time 'time'.
|
|
/// </summary>
|
|
public abstract void SetInitialTrajectoryPose(
|
|
int fromTrajectoryId,
|
|
int toTrajectoryId,
|
|
Rigid3d pose,
|
|
long time);
|
|
|
|
/// <summary>
|
|
/// Sets localization initial poses for relocalizing against the map.
|
|
/// Match C++: SetLocalizationInitialPoses (pose_graph.h:139)
|
|
/// C++ signature: const std::vector<transform::Rigid3d> & (const reference)
|
|
/// C# equivalent: IReadOnlyList<Rigid3d> (read-only collection)
|
|
/// </summary>
|
|
public abstract void SetLocalizationInitialPoses(IReadOnlyList<Rigid3d> localizationInitialPoses);
|
|
|
|
public abstract void RunFinalOptimization();
|
|
public abstract MapById<SubmapId, IPoseGraph.SubmapData> GetAllSubmapData();
|
|
public abstract IPoseGraph.SubmapData GetSubmapData(SubmapId submapId);
|
|
public abstract MapById<SubmapId, IPoseGraph.SubmapPose> GetAllSubmapPoses();
|
|
public abstract Rigid3d GetLocalToGlobalTransform(int trajectoryId);
|
|
public abstract MapById<NodeId, TrajectoryNode> GetTrajectoryNodes();
|
|
public abstract MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses();
|
|
public abstract Dictionary<int, IPoseGraph.TrajectoryState> GetTrajectoryStates();
|
|
public abstract Dictionary<string, Rigid3d> GetLandmarkPoses();
|
|
public abstract void SetLandmarkPose(string landmarkId, Rigid3d globalPose, bool frozen = false);
|
|
public abstract void DeleteTrajectory(int trajectoryId);
|
|
public abstract bool IsTrajectoryFinished(int trajectoryId);
|
|
public abstract bool IsTrajectoryFrozen(int trajectoryId);
|
|
public abstract Dictionary<int, IPoseGraph.TrajectoryData> GetTrajectoryData();
|
|
public abstract List<IPoseGraph.Constraint> Constraints();
|
|
public abstract Models.Mapping.PoseGraph ToProto(bool includeUnfinishedSubmaps);
|
|
public abstract void SetGlobalSlamOptimizationCallback(GlobalSlamOptimizationCallback callback);
|
|
public abstract void SetTransformToMap(Rigid3d transform);
|
|
public abstract Rigid3d GetTransformToMap();
|
|
|
|
// Manual compute methods (match C++ PoseGraphInterface)
|
|
public abstract (double Score, IPoseGraph.Constraint? Constraint) ManualComputeConstraint(NodeId nodeId, SubmapId submapId);
|
|
public abstract double ManualComputeConstraintScore(NodeId nodeId, SubmapId submapId, Rigid3d initialPose);
|
|
public abstract double ManualComputeScanMatcher(NodeId nodeId, SubmapId submapId, Rigid3d initialPose, out Rigid3d poseManualEstimate);
|
|
public abstract bool ManualRelocalization(int trajectoryId, out double score, Rigid2d initialPose, LocalizationResultCallback? callback);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Pose graph trimmer interface.
|
|
/// </summary>
|
|
public abstract class PoseGraphTrimmer
|
|
{
|
|
/// <summary>
|
|
/// Trims the pose graph.
|
|
/// </summary>
|
|
public abstract void Trim(ITrimmable trimmable);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Interface for trimmable pose graph operations.
|
|
/// </summary>
|
|
public interface ITrimmable
|
|
{
|
|
int NumSubmaps(int trajectoryId);
|
|
List<SubmapId> GetSubmapIds(int trajectoryId);
|
|
MapById<SubmapId, IPoseGraph.SubmapData> GetOptimizedSubmapData();
|
|
MapById<NodeId, TrajectoryNode> GetTrajectoryNodes();
|
|
List<IPoseGraph.Constraint> GetConstraints();
|
|
void TrimSubmap(SubmapId submapId);
|
|
bool IsFinished(int trajectoryId);
|
|
void SetTrajectoryState(int trajectoryId, IPoseGraph.TrajectoryState state);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Conversion utilities for constraints.
|
|
/// </summary>
|
|
public static class ConstraintOperations
|
|
{
|
|
/// <summary>
|
|
/// Converts constraint to proto.
|
|
/// Match C++: ToProto in pose_graph.cc:147-169
|
|
/// </summary>
|
|
public static Models.Mapping.PoseGraph.Constraint ToProto(IPoseGraph.Constraint constraint)
|
|
{
|
|
// Match C++: convert tag (line 161 in pose_graph.cc)
|
|
var tag = constraint.ConstraintTag == IPoseGraph.Constraint.Tag.IntraSubmap
|
|
? Models.Mapping.PoseGraph.Constraint.Tag.IntraSubmap
|
|
: Models.Mapping.PoseGraph.Constraint.Tag.InterSubmap;
|
|
|
|
// Match C++: convert state (line 166 in pose_graph.cc)
|
|
var state = constraint.ConstraintState == IPoseGraph.Constraint.State.Enabled
|
|
? Models.Mapping.PoseGraph.Constraint.State.Enabled
|
|
: Models.Mapping.PoseGraph.Constraint.State.Disabled;
|
|
|
|
// Match C++: create constraint proto with all fields (lines 147-168 in pose_graph.cc)
|
|
return new Models.Mapping.PoseGraph.Constraint(
|
|
new Models.Mapping.PoseGraph.SubmapId(constraint.SubmapId.TrajectoryId, constraint.SubmapId.SubmapIndex),
|
|
new Models.Mapping.PoseGraph.NodeId(constraint.NodeId.TrajectoryId, constraint.NodeId.NodeIndex),
|
|
(Rigid3dProto)constraint.ConstraintPose.ZbarIj,
|
|
constraint.ConstraintPose.TranslationWeight,
|
|
constraint.ConstraintPose.RotationWeight,
|
|
tag,
|
|
constraint.Score, // CRITICAL FIX: include score field (line 162 in pose_graph.cc)
|
|
state // CRITICAL FIX: include state field (line 166 in pose_graph.cc)
|
|
);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Creates constraint from proto.
|
|
/// Match C++: FromProto in pose_graph.cc:77-97
|
|
/// </summary>
|
|
public static List<IPoseGraph.Constraint> FromProto(List<Models.Mapping.PoseGraph.Constraint> constraintProtos)
|
|
{
|
|
var constraints = new List<IPoseGraph.Constraint>();
|
|
foreach (var constraintProto in constraintProtos)
|
|
{
|
|
// Match C++: convert tag
|
|
IPoseGraph.Constraint.Tag tag;
|
|
if (constraintProto.ConstraintTag == Models.Mapping.PoseGraph.Constraint.Tag.IntraSubmap)
|
|
{
|
|
tag = IPoseGraph.Constraint.Tag.IntraSubmap;
|
|
}
|
|
else
|
|
{
|
|
tag = IPoseGraph.Constraint.Tag.InterSubmap;
|
|
}
|
|
|
|
// Match C++: convert state (lines 93 in pose_graph.cc)
|
|
IPoseGraph.Constraint.State state;
|
|
if (constraintProto.ConstraintState == Models.Mapping.PoseGraph.Constraint.State.Enabled)
|
|
{
|
|
state = IPoseGraph.Constraint.State.Enabled;
|
|
}
|
|
else
|
|
{
|
|
state = IPoseGraph.Constraint.State.Disabled;
|
|
}
|
|
|
|
// Match C++: extract score (line 92 in pose_graph.cc)
|
|
var score = constraintProto.Score;
|
|
|
|
// Match C++: create constraint with all fields (line 94 in pose_graph.cc)
|
|
var constraint = new IPoseGraph.Constraint(
|
|
new SubmapId(constraintProto.SubmapId.TrajectoryId, constraintProto.SubmapId.SubmapIndex),
|
|
new NodeId(constraintProto.NodeId.TrajectoryId, constraintProto.NodeId.NodeIndex),
|
|
new IPoseGraph.Constraint.Pose(
|
|
(Rigid3d)constraintProto.RelativePose,
|
|
constraintProto.TranslationWeight,
|
|
constraintProto.RotationWeight),
|
|
tag,
|
|
score, // CRITICAL FIX: include score field
|
|
state // CRITICAL FIX: include state field
|
|
);
|
|
constraints.Add(constraint);
|
|
}
|
|
return constraints;
|
|
}
|
|
}
|