278 lines
8.3 KiB
C#
278 lines
8.3 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 System.Collections.Generic;
|
|
using System.Text.Json.Serialization;
|
|
using CartographerSharp.Models.Transform;
|
|
|
|
namespace CartographerSharp.Models.Mapping;
|
|
|
|
/// <summary>
|
|
/// Protocol buffer representation of a pose graph.
|
|
/// </summary>
|
|
public struct PoseGraph
|
|
{
|
|
/// <summary>
|
|
/// Submap ID.
|
|
/// </summary>
|
|
public struct SubmapId
|
|
{
|
|
[JsonPropertyName("trajectory_id")]
|
|
public int TrajectoryId { get; set; }
|
|
|
|
/// <summary>
|
|
/// Submap index in the given trajectory.
|
|
/// </summary>
|
|
[JsonPropertyName("submap_index")]
|
|
public int SubmapIndex { get; set; }
|
|
|
|
public SubmapId(int trajectoryId, int submapIndex)
|
|
{
|
|
TrajectoryId = trajectoryId;
|
|
SubmapIndex = submapIndex;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Node ID.
|
|
/// </summary>
|
|
public struct NodeId
|
|
{
|
|
[JsonPropertyName("trajectory_id")]
|
|
public int TrajectoryId { get; set; }
|
|
|
|
/// <summary>
|
|
/// Node index in the given trajectory.
|
|
/// </summary>
|
|
[JsonPropertyName("node_index")]
|
|
public int NodeIndex { get; set; }
|
|
|
|
public NodeId(int trajectoryId, int nodeIndex)
|
|
{
|
|
TrajectoryId = trajectoryId;
|
|
NodeIndex = nodeIndex;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Constraint in the pose graph.
|
|
/// </summary>
|
|
public struct Constraint
|
|
{
|
|
/// <summary>
|
|
/// Differentiates between intra-submap (where the range data was inserted
|
|
/// into the submap) and inter-submap constraints (where the range data was
|
|
/// not inserted into the submap).
|
|
/// </summary>
|
|
public enum Tag
|
|
{
|
|
IntraSubmap = 0,
|
|
InterSubmap = 1
|
|
}
|
|
|
|
/// <summary>
|
|
/// Differentiates between enable state (condition to add to optimization)
|
|
/// and disable state (do not use to optimize).
|
|
/// </summary>
|
|
public enum State
|
|
{
|
|
Enabled = 0,
|
|
Disabled = 1
|
|
}
|
|
|
|
/// <summary>
|
|
/// Submap ID.
|
|
/// </summary>
|
|
[JsonPropertyName("submap_id")]
|
|
public SubmapId SubmapId { get; set; }
|
|
|
|
/// <summary>
|
|
/// Node ID.
|
|
/// </summary>
|
|
[JsonPropertyName("node_id")]
|
|
public NodeId NodeId { get; set; }
|
|
|
|
/// <summary>
|
|
/// Pose of the node relative to submap, i.e. taking data from the node frame
|
|
/// into the submap frame.
|
|
/// </summary>
|
|
[JsonPropertyName("relative_pose")]
|
|
public Rigid3dProto RelativePose { get; set; }
|
|
|
|
/// <summary>
|
|
/// Weight of the translational part of the constraint.
|
|
/// </summary>
|
|
[JsonPropertyName("translation_weight")]
|
|
public double TranslationWeight { get; set; }
|
|
|
|
/// <summary>
|
|
/// Weight of the rotational part of the constraint.
|
|
/// </summary>
|
|
[JsonPropertyName("rotation_weight")]
|
|
public double RotationWeight { get; set; }
|
|
|
|
/// <summary>
|
|
/// Tag indicating constraint type.
|
|
/// </summary>
|
|
[JsonPropertyName("tag")]
|
|
public Tag ConstraintTag { get; set; }
|
|
|
|
/// <summary>
|
|
/// Score of the constraint.
|
|
/// </summary>
|
|
[JsonPropertyName("score")]
|
|
public double Score { get; set; }
|
|
|
|
/// <summary>
|
|
/// State of the constraint (enabled or disabled).
|
|
/// </summary>
|
|
[JsonPropertyName("state")]
|
|
public State ConstraintState { get; set; }
|
|
|
|
public Constraint(
|
|
SubmapId submapId,
|
|
NodeId nodeId,
|
|
Rigid3dProto relativePose,
|
|
double translationWeight,
|
|
double rotationWeight,
|
|
Tag constraintTag,
|
|
double score = 0.0,
|
|
State constraintState = State.Enabled)
|
|
{
|
|
SubmapId = submapId;
|
|
NodeId = nodeId;
|
|
RelativePose = relativePose;
|
|
TranslationWeight = translationWeight;
|
|
RotationWeight = rotationWeight;
|
|
ConstraintTag = constraintTag;
|
|
Score = score;
|
|
ConstraintState = constraintState;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Landmark pose in the pose graph.
|
|
/// </summary>
|
|
public struct LandmarkPose
|
|
{
|
|
[JsonPropertyName("landmark_id")]
|
|
public string LandmarkId { get; set; }
|
|
|
|
[JsonPropertyName("global_pose")]
|
|
public Rigid3dProto GlobalPose { get; set; }
|
|
|
|
public LandmarkPose(string landmarkId, Rigid3dProto globalPose)
|
|
{
|
|
LandmarkId = landmarkId;
|
|
GlobalPose = globalPose;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Accurate submap in the pose graph.
|
|
/// </summary>
|
|
public struct AccurateSubmap
|
|
{
|
|
[JsonPropertyName("submap_id")]
|
|
public SubmapId SubmapId { get; set; }
|
|
|
|
[JsonPropertyName("parent_id")]
|
|
public SubmapId ParentId { get; set; }
|
|
|
|
[JsonPropertyName("pose")]
|
|
public Rigid2dProto Pose { get; set; }
|
|
|
|
[JsonPropertyName("parent_relative_pose")]
|
|
public Rigid2dProto ParentRelativePose { get; set; }
|
|
|
|
/// <summary>
|
|
/// Stop station in the accurate submap.
|
|
/// </summary>
|
|
public struct StopStation
|
|
{
|
|
public enum StopStationType
|
|
{
|
|
StartPoint = 0,
|
|
StopPoint = 1
|
|
}
|
|
|
|
[JsonPropertyName("pose")]
|
|
public Rigid2dProto Pose { get; set; }
|
|
|
|
[JsonPropertyName("type")]
|
|
public StopStationType Type { get; set; }
|
|
|
|
public StopStation(Rigid2dProto pose, StopStationType type)
|
|
{
|
|
Pose = pose;
|
|
Type = type;
|
|
}
|
|
}
|
|
|
|
[JsonPropertyName("stop_stations")]
|
|
public List<StopStation> StopStations { get; set; }
|
|
|
|
[JsonPropertyName("is_shelf")]
|
|
public bool IsShelf { get; set; }
|
|
|
|
public AccurateSubmap(
|
|
SubmapId submapId,
|
|
SubmapId parentId,
|
|
Rigid2dProto pose,
|
|
Rigid2dProto parentRelativePose,
|
|
List<StopStation>? stopStations = null,
|
|
bool isShelf = false)
|
|
{
|
|
SubmapId = submapId;
|
|
ParentId = parentId;
|
|
Pose = pose;
|
|
ParentRelativePose = parentRelativePose;
|
|
StopStations = stopStations ?? new List<StopStation>();
|
|
IsShelf = isShelf;
|
|
}
|
|
}
|
|
|
|
[JsonPropertyName("constraint")]
|
|
public List<Constraint> Constraints { get; set; }
|
|
|
|
[JsonPropertyName("trajectory")]
|
|
public List<Trajectory> Trajectories { get; set; }
|
|
|
|
[JsonPropertyName("landmark_poses")]
|
|
public List<LandmarkPose> LandmarkPoses { get; set; }
|
|
|
|
[JsonPropertyName("accurate_submaps")]
|
|
public List<AccurateSubmap> AccurateSubmaps { get; set; }
|
|
|
|
[JsonPropertyName("transform_to_map")]
|
|
[JsonIgnore(Condition = JsonIgnoreCondition.WhenWritingNull)]
|
|
public Rigid3dProto? TransformToMap { get; set; }
|
|
|
|
public PoseGraph(
|
|
List<Constraint>? constraints = null,
|
|
List<Trajectory>? trajectories = null,
|
|
List<LandmarkPose>? landmarkPoses = null,
|
|
List<AccurateSubmap>? accurateSubmaps = null,
|
|
Rigid3dProto? transformToMap = null)
|
|
{
|
|
Constraints = constraints ?? new List<Constraint>();
|
|
Trajectories = trajectories ?? new List<Trajectory>();
|
|
LandmarkPoses = landmarkPoses ?? new List<LandmarkPose>();
|
|
AccurateSubmaps = accurateSubmaps ?? new List<AccurateSubmap>();
|
|
TransformToMap = transformToMap;
|
|
}
|
|
}
|