Initial commit

2026-07-03 16:31:37 +07:00
commit 899c7c637d
1939 changed files with 641750 additions and 0 deletions
--- a/srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/IPoseGraph.cs
+++ b/srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/IPoseGraph.cs
@@ -0,0 +1,460 @@
+/*
+ * 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;
+using  RobotNet10.Shared.Numbers;
+
+namespace CartographerSharp.Mapping;
+
+/// <summary>
+/// Interface for pose graph operations.
+/// </summary>
+public interface IPoseGraph
+{
+    /// <summary>
+    /// A "constraint" as in the paper by Konolige, Kurt, et al. "Efficient sparse
+    /// pose adjustment for 2d mapping." Intelligent Robots and Systems (IROS),
+    /// 2010 IEEE/RSJ International Conference on (pp. 22--29). IEEE, 2010.
+    /// </summary>
+    public struct Constraint(SubmapId submapId, NodeId nodeId, Constraint.Pose pose, Constraint.Tag tag, double score = 0.0, Constraint.State state = Constraint.State.Enabled)
+    {
+        /// <summary>
+        /// Constraint pose information.
+        /// </summary>
+        public struct Pose(Rigid3d zbarIj, double translationWeight, double rotationWeight)
+        {
+            public Rigid3d ZbarIj { get; set; } = zbarIj;
+            public double TranslationWeight { get; set; } = translationWeight;
+            public double RotationWeight { get; set; } = rotationWeight;
+        }
+
+        public SubmapId SubmapId { get; set; } = submapId;
+        public NodeId NodeId { get; set; } = nodeId;
+
+        /// <summary>
+        /// Pose of the node 'j' relative to submap 'i'.
+        /// </summary>
+        public Pose ConstraintPose { get; set; } = pose;
+
+        /// <summary>
+        /// Differentiates between intra-submap (where node 'j' was inserted into
+        /// submap 'i') and inter-submap constraints (where node 'j' was not inserted
+        /// into submap 'i').
+        /// </summary>
+        public enum Tag
+        {
+            IntraSubmap,
+            InterSubmap
+        }
+
+        public Tag ConstraintTag { get; set; } = tag;
+
+        /// <summary>
+        /// Match C++: score field for constraint quality.
+        /// </summary>
+        public double Score { get; set; } = score;
+
+        /// <summary>
+        /// Match C++: state enum for enabled/disabled constraints.
+        /// </summary>
+        public enum State
+        {
+            Enabled,
+            Disabled
+        }
+
+        public State ConstraintState { get; set; } = state;
+    }
+
+    /// <summary>
+    /// Landmark node information.
+    /// </summary>
+    public struct LandmarkNode(
+        List<LandmarkNode.LandmarkObservation>? landmarkObservations = null,
+        Rigid3d? globalLandmarkPose = null,
+        bool frozen = false)
+    {
+        /// <summary>
+        /// Landmark observation.
+        /// </summary>
+        public struct LandmarkObservation(
+            int trajectoryId,
+            long time,
+            Rigid3d landmarkToTrackingTransform,
+            double translationWeight,
+            double rotationWeight)
+        {
+            public int TrajectoryId { get; set; } = trajectoryId;
+            public long Time { get; set; } = time;
+            public Rigid3d LandmarkToTrackingTransform { get; set; } = landmarkToTrackingTransform;
+            public double TranslationWeight { get; set; } = translationWeight;
+            public double RotationWeight { get; set; } = rotationWeight;
+        }
+
+        public List<LandmarkObservation> LandmarkObservations { get; set; } = landmarkObservations ?? [];
+        public Rigid3d? GlobalLandmarkPose { get; set; } = globalLandmarkPose;
+        public bool Frozen { get; set; } = frozen;
+    }
+
+    /// <summary>
+    /// Submap pose information.
+    /// </summary>
+    public struct SubmapPose(int version, Rigid3d pose)
+    {
+        public int Version { get; set; } = version;
+        public Rigid3d Pose { get; set; } = pose;
+    }
+
+    /// <summary>
+    /// Submap data with pose.
+    /// </summary>
+    public struct SubmapData(Submap? submap, Rigid3d pose)
+    {
+        public Submap? Submap { get; set; } = submap;
+        public Rigid3d Pose { get; set; } = pose;
+    }
+
+    /// <summary>
+    /// Trajectory data.
+    /// </summary>
+    public struct TrajectoryData(
+        double gravityConstant = 9.8,
+        Quaternion? imuCalibration = null,
+        Rigid3d? fixedFrameOriginInMap = null)
+    {
+        public double GravityConstant { get; set; } = gravityConstant;
+        public Quaternion ImuCalibration { get; set; } = imuCalibration ?? Quaternion.Identity;
+        public Rigid3d? FixedFrameOriginInMap { get; set; } = fixedFrameOriginInMap;
+    }
+
+    /// <summary>
+    /// Trajectory state enumeration.
+    /// </summary>
+    public enum TrajectoryState
+    {
+        Active,
+        Finished,
+        Frozen,
+        Deleted
+    }
+
+    /// <summary>
+    /// Gets the total number of work items added to the work queue.
+    /// </summary>
+    public int WorkItemsAdded { get; }
+
+    /// <summary>
+    /// Gets the total number of work items completed by the work queue.
+    /// </summary>
+    public int WorkItemsCompleted { get; }
+
+    /// <summary>
+    /// Gets the number of work items currently pending in the work queue.
+    /// </summary>
+    public int WorkItemsPending { get; }
+
+    /// <summary>
+    /// Gets the current number of items in the work queue.
+    /// </summary>
+    public int WorkQueueCount { get; }
+
+    /// <summary>
+    /// Gets the number of nodes started in the constraint builder.
+    /// </summary>
+    public int ConstraintBuilderNodesStarted { get; }
+
+    /// <summary>
+    /// Gets the number of nodes finished in the constraint builder.
+    /// </summary>
+    public int ConstraintBuilderNodesFinished { get; }
+
+    /// <summary>
+    /// Gets the total number of trajectory nodes in the pose graph.
+    /// Used for progress tracking during optimization.
+    /// </summary>
+    public int TrajectoryNodesCount { get; }
+
+    /// <summary>
+    /// Gets the total number of constraint tasks dispatched for scan matching.
+    /// </summary>
+    public int ConstraintTasksTotal { get; }
+
+    /// <summary>
+    /// Gets the number of constraint tasks that have finished scan matching.
+    /// </summary>
+    public int ConstraintTasksFinished { get; }
+
+    /// <summary>
+    /// Inserts an IMU measurement.
+    /// </summary>
+    void AddImuData(int trajectoryId, ImuData imuData);
+
+    /// <summary>
+    /// Inserts an odometry measurement.
+    /// </summary>
+    void AddOdometryData(int trajectoryId, OdometryData odometryData);
+
+    /// <summary>
+    /// Inserts a fixed frame pose measurement.
+    /// </summary>
+    void AddFixedFramePoseData(int trajectoryId, FixedFramePoseData fixedFramePoseData);
+
+    /// <summary>
+    /// Inserts landmarks observations.
+    /// </summary>
+    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.
+    /// </summary>
+    void DrainWorkQueue();
+
+    /// <summary>
+    /// Finishes the given trajectory.
+    /// </summary>
+    void FinishTrajectory(int trajectoryId);
+
+    /// <summary>
+    /// Freezes a trajectory. Poses in this trajectory will not be optimized.
+    /// </summary>
+    void FreezeTrajectory(int trajectoryId);
+
+    /// <summary>
+    /// Adds a 'submap' from a proto with the given 'global_pose' to the
+    /// appropriate trajectory.
+    /// </summary>
+    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>
+    void AddNodeFromProto(Rigid3d globalPose, Models.Mapping.Node node);
+
+    /// <summary>
+    /// Sets the trajectory data from a proto.
+    /// </summary>
+    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>
+    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>
+    void AddSerializedConstraints(List<Constraint> constraints);
+
+    /// <summary>
+    /// Adds a 'trimmer'. It will be used after all data added before it has been
+    /// included in the pose graph.
+    /// </summary>
+    void AddTrimmer(PoseGraphTrimmer trimmer);
+
+    /// <summary>
+    /// Returns the current trajectory clusters.
+    /// </summary>
+    List<List<int>> GetConnectedTrajectories();
+
+    /// <summary>
+    /// Returns the IMU data.
+    /// </summary>
+    Dictionary<int, List<ImuData>> GetImuData();
+
+    /// <summary>
+    /// Returns the odometry data.
+    /// </summary>
+    Dictionary<int, List<OdometryData>> GetOdometryData();
+
+    /// <summary>
+    /// Returns the fixed frame pose data.
+    /// </summary>
+    Dictionary<int, List<FixedFramePoseData>> GetFixedFramePoseData();
+
+    /// <summary>
+    /// Returns the landmark data.
+    /// </summary>
+    Dictionary<string, LandmarkNode> GetLandmarkNodes();
+
+    /// <summary>
+    /// Sets a relative initial pose 'relative_pose' for 'from_trajectory_id' with
+    /// respect to 'to_trajectory_id' at time 'time'.
+    /// </summary>
+    void SetInitialTrajectoryPose(
+        int fromTrajectoryId,
+        int toTrajectoryId,
+        Rigid3d pose,
+        long time);
+
+    /// <summary>
+    /// Waits for all computations to finish and computes optimized poses.
+    /// </summary>
+    void RunFinalOptimization();
+
+    /// <summary>
+    /// Returns data for all submaps.
+    /// </summary>
+    MapById<SubmapId, SubmapData> GetAllSubmapData();
+
+    /// <summary>
+    /// Returns the current optimized transform and submap itself for the given
+    /// 'submap_id'. Returns 'null' for the 'submap' member if the submap does
+    /// not exist (anymore).
+    /// </summary>
+    SubmapData GetSubmapData(SubmapId submapId);
+
+    /// <summary>
+    /// Returns the global poses for all submaps.
+    /// </summary>
+    MapById<SubmapId, SubmapPose> GetAllSubmapPoses();
+
+    /// <summary>
+    /// Returns the transform converting data in the local map frame (i.e. the
+    /// continuous, non-loop-closed frame) into the global map frame (i.e. the
+    /// discontinuous, loop-closed frame).
+    /// </summary>
+    Rigid3d GetLocalToGlobalTransform(int trajectoryId);
+
+    /// <summary>
+    /// Returns the current optimized trajectories.
+    /// </summary>
+    MapById<NodeId, TrajectoryNode> GetTrajectoryNodes();
+
+    /// <summary>
+    /// Returns the current optimized trajectory poses.
+    /// </summary>
+    MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses();
+
+    /// <summary>
+    /// Returns the states of trajectories.
+    /// </summary>
+    Dictionary<int, TrajectoryState> GetTrajectoryStates();
+
+    /// <summary>
+    /// Returns the current optimized landmark poses.
+    /// </summary>
+    Dictionary<string, Rigid3d> GetLandmarkPoses();
+
+    /// <summary>
+    /// Sets global pose of landmark 'landmark_id' to given 'global_pose'.
+    /// </summary>
+    void SetLandmarkPose(string landmarkId, Rigid3d globalPose, bool frozen = false);
+
+    /// <summary>
+    /// Deletes a trajectory asynchronously.
+    /// </summary>
+    void DeleteTrajectory(int trajectoryId);
+
+    /// <summary>
+    /// Checks if the given trajectory is finished.
+    /// </summary>
+    bool IsTrajectoryFinished(int trajectoryId);
+
+    /// <summary>
+    /// Checks if the given trajectory is frozen.
+    /// </summary>
+    bool IsTrajectoryFrozen(int trajectoryId);
+
+    /// <summary>
+    /// Returns the trajectory data.
+    /// </summary>
+    Dictionary<int, TrajectoryData> GetTrajectoryData();
+
+    /// <summary>
+    /// Returns the collection of constraints.
+    /// </summary>
+    List<Constraint> Constraints();
+
+    /// <summary>
+    /// Serializes the constraints and trajectories. If
+    /// 'include_unfinished_submaps' is set to 'true', unfinished submaps, i.e.
+    /// submaps that have not yet received all rangefinder data insertions, will
+    /// be included, otherwise not.
+    /// </summary>
+    Models.Mapping.PoseGraph ToProto(bool includeUnfinishedSubmaps);
+
+    /// <summary>
+    /// Sets the callback function that is invoked whenever the global optimization
+    /// problem is solved.
+    /// </summary>
+    void SetGlobalSlamOptimizationCallback(GlobalSlamOptimizationCallback callback);
+
+    /// <summary>
+    /// Sets the transform from local map frame to global map frame (map origin).
+    /// Used when loading state from pbstream; matches C++ SetTransformToMap.
+    /// </summary>
+    void SetTransformToMap(Rigid3d transform);
+
+    /// <summary>
+    /// Returns the transform from local map frame to global map frame.
+    /// Matches C++ GetTransformToMap.
+    /// </summary>
+    Rigid3d GetTransformToMap();
+
+    /// <summary>
+    /// Manually compute a constraint between a node and submap using global scan matching.
+    /// Match C++: manualComputeConstraint (pose_graph_interface.h:181-192)
+    /// </summary>
+    (double Score, Constraint? Constraint) ManualComputeConstraint(NodeId nodeId, SubmapId submapId);
+
+    /// <summary>
+    /// Manually compute constraint score from an initial pose estimate.
+    /// Match C++: manualComputeConstraintScore (pose_graph_interface.h:194-197)
+    /// </summary>
+    double ManualComputeConstraintScore(NodeId nodeId, SubmapId submapId, Rigid3d initialPose);
+
+    /// <summary>
+    /// Manually compute scan matcher score with refined pose output.
+    /// Match C++: manualComputeScanMatcher (pose_graph_interface.h:199-202)
+    /// </summary>
+    double ManualComputeScanMatcher(NodeId nodeId, SubmapId submapId, Rigid3d initialPose, out Rigid3d poseManualEstimate);
+
+    /// <summary>
+    /// Manually relocalize a trajectory against finished submaps.
+    /// Match C++: ManualRelocalization (pose_graph_interface.h:212-216)
+    /// </summary>
+    bool ManualRelocalization(int trajectoryId, out double score, Rigid2d initialPose, LocalizationResultCallback? callback);
+}
+
+/// <summary>
+/// Callback for global SLAM optimization.
+/// </summary>
+public delegate void GlobalSlamOptimizationCallback(
+    Dictionary<int, SubmapId> submapIds,
+    Dictionary<int, NodeId> nodeIds);
+
+/// <summary>
+/// Result of manual relocalization operation.
+/// Match C++: LocalizationResultCallback (pose_graph_interface.h)
+/// </summary>
+public struct LocalizationResult
+{
+    public NodeId NodeId { get; init; }
+    public SubmapId SubmapId { get; init; }
+    public Rigid3d GlobalPose { get; init; }
+    public double Score { get; init; }
+}
+
+/// <summary>
+/// Callback for localization/relocalization results.
+/// Match C++: LocalizationResultCallback (pose_graph_interface.h)
+/// </summary>
+public delegate void LocalizationResultCallback(LocalizationResult result);