Initial commit
This commit is contained in:
@@ -0,0 +1,343 @@
|
||||
/*
|
||||
* 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;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user