Denso/srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Models/Mapping/TrajectoryBuilderOptions.cs

/*
 * 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 initial trajectory pose.
/// </summary>
public struct InitialTrajectoryPose
{
    [JsonPropertyName("relative_pose")]
    public Rigid3dProto RelativePose { get; set; }

    [JsonPropertyName("to_trajectory_id")]
    public int ToTrajectoryId { get; set; }

    [JsonPropertyName("timestamp")]
    public long Timestamp { get; set; }

    public InitialTrajectoryPose(Rigid3dProto relativePose, int toTrajectoryId, long timestamp)
    {
        RelativePose = relativePose;
        ToTrajectoryId = toTrajectoryId;
        Timestamp = timestamp;
    }
}

/// <summary>
/// Protocol buffer representation of trajectory builder options.
/// Note: LocalTrajectoryBuilderOptions2D and LocalTrajectoryBuilderOptions3D
/// are complex types that will be implemented in later phases.
/// </summary>
public struct TrajectoryBuilderOptions(
    System.Text.Json.JsonElement? trajectoryBuilder2DOptions = null,
    System.Text.Json.JsonElement? trajectoryBuilder3DOptions = null,
    InitialTrajectoryPose? initialTrajectoryPose = null,
    TrajectoryBuilderOptions.PureLocalizationTrimmerOptions? pureLocalizationTrimmer = null,
    bool collateFixedFrame = false,
    bool collateLandmarks = false,
    MotionFilterOptions? poseGraphOdometryMotionFilter = null)
{
    // Note: These will be implemented as separate proto files in later phases
    // For now, we use JsonElement to handle them as raw JSON
    [JsonPropertyName("trajectory_builder_2d_options")]
    public System.Text.Json.JsonElement? TrajectoryBuilder2DOptions { get; set; } = trajectoryBuilder2DOptions;

    [JsonPropertyName("trajectory_builder_3d_options")]
    public System.Text.Json.JsonElement? TrajectoryBuilder3DOptions { get; set; } = trajectoryBuilder3DOptions;

    [JsonPropertyName("initial_trajectory_pose")]
    public InitialTrajectoryPose? InitialTrajectoryPose { get; set; } = initialTrajectoryPose;

    [JsonPropertyName("pure_localization")]
    [System.Obsolete("Deprecated")]
    public bool PureLocalization { get; set; }

    /// <summary>
    /// Pure localization trimmer options.
    /// </summary>
    public struct PureLocalizationTrimmerOptions
    {
        [JsonPropertyName("max_submaps_to_keep")]
        public int MaxSubmapsToKeep { get; set; }

        public PureLocalizationTrimmerOptions(int maxSubmapsToKeep)
        {
            MaxSubmapsToKeep = maxSubmapsToKeep;
        }
    }

    [JsonPropertyName("pure_localization_trimmer")]
    public PureLocalizationTrimmerOptions? PureLocalizationTrimmer { get; set; } = pureLocalizationTrimmer;

    [JsonPropertyName("collate_fixed_frame")]
    public bool CollateFixedFrame { get; set; } = collateFixedFrame;

    [JsonPropertyName("collate_landmarks")]
    public bool CollateLandmarks { get; set; } = collateLandmarks;

    [JsonPropertyName("pose_graph_odometry_motion_filter")]
    [JsonIgnore(Condition = JsonIgnoreCondition.WhenWritingNull)]
    public MotionFilterOptions? PoseGraphOdometryMotionFilter { get; set; } = poseGraphOdometryMotionFilter;
}

/// <summary>
/// Protocol buffer representation of sensor ID.
/// </summary>
public struct SensorId
{
    /// <summary>
    /// Sensor type enumeration.
    /// </summary>
    public enum SensorType
    {
        Range = 0,
        Imu = 1,
        Odometry = 2,
        FixedFramePose = 3,
        Landmark = 4,
        LocalSlamResult = 5
    }

    [JsonPropertyName("type")]
    public SensorType Type { get; set; }

    [JsonPropertyName("id")]
    public string Id { get; set; }

    public SensorId(SensorType type, string id)
    {
        Type = type;
        Id = id;
    }
}

/// <summary>
/// Protocol buffer representation of trajectory builder options with sensor IDs.
/// </summary>
public struct TrajectoryBuilderOptionsWithSensorIds
{
    [JsonPropertyName("sensor_id")]
    public List<SensorId> SensorIds { get; set; }

    [JsonPropertyName("trajectory_builder_options")]
    public TrajectoryBuilderOptions TrajectoryBuilderOptions { get; set; }

    public TrajectoryBuilderOptionsWithSensorIds(
        List<SensorId>? sensorIds = null,
        TrajectoryBuilderOptions trajectoryBuilderOptions = default)
    {
        SensorIds = sensorIds ?? new List<SensorId>();
        TrajectoryBuilderOptions = trajectoryBuilderOptions;
    }
}

/// <summary>
/// Protocol buffer representation of all trajectory builder options.
/// </summary>
public struct AllTrajectoryBuilderOptions
{
    [JsonPropertyName("options_with_sensor_ids")]
    public List<TrajectoryBuilderOptionsWithSensorIds> OptionsWithSensorIds { get; set; }

    public AllTrajectoryBuilderOptions(List<TrajectoryBuilderOptionsWithSensorIds>? optionsWithSensorIds = null)
    {
        OptionsWithSensorIds = optionsWithSensorIds ?? new List<TrajectoryBuilderOptionsWithSensorIds>();
    }
}