461 lines
16 KiB
C#
461 lines
16 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.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);
|