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