Initial commit

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Models/Mapping/PoseGraphOptionsProto.cs

@@ -0,0 +1,164 @@
+/*
+ * 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.Text.Json.Serialization;
+
+namespace CartographerSharp.Models.Mapping;
+
+/// <summary>
+/// Protocol buffer representation of pose graph options.
+/// </summary>
+public struct PoseGraphOptions
+{
+    /// <summary>
+    /// Overlapping submaps trimmer options for 2D.
+    /// </summary>
+    public struct OverlappingSubmapsTrimmerOptions2D
+    {
+        [JsonPropertyName("fresh_submaps_count")]
+        public int FreshSubmapsCount { get; set; }
+
+        [JsonPropertyName("min_covered_area")]
+        public double MinCoveredArea { get; set; }
+
+        [JsonPropertyName("min_added_submaps_count")]
+        public int MinAddedSubmapsCount { get; set; }
+
+        public OverlappingSubmapsTrimmerOptions2D(int freshSubmapsCount, double minCoveredArea, int minAddedSubmapsCount)
+        {
+            FreshSubmapsCount = freshSubmapsCount;
+            MinCoveredArea = minCoveredArea;
+            MinAddedSubmapsCount = minAddedSubmapsCount;
+        }
+    }
+
+    /// <summary>
+    /// Online loop closure: If positive, will run the loop closure while the map is built.
+    /// </summary>
+    [JsonPropertyName("optimize_every_n_nodes")]
+    public int OptimizeEveryNNodes { get; set; }
+
+    /// <summary>
+    /// Weight used in the optimization problem for the translational component of
+    /// non-loop-closure scan matcher constraints.
+    /// </summary>
+    [JsonPropertyName("matcher_translation_weight")]
+    public double MatcherTranslationWeight { get; set; }
+
+    /// <summary>
+    /// Weight used in the optimization problem for the rotational component of
+    /// non-loop-closure scan matcher constraints.
+    /// </summary>
+    [JsonPropertyName("matcher_rotation_weight")]
+    public double MatcherRotationWeight { get; set; }
+
+    /// <summary>
+    /// Number of iterations to use in 'optimization_problem_options' for the final optimization.
+    /// </summary>
+    [JsonPropertyName("max_num_final_iterations")]
+    public int MaxNumFinalIterations { get; set; }
+
+    /// <summary>
+    /// Rate at which we sample a single trajectory's nodes for global localization.
+    /// </summary>
+    [JsonPropertyName("global_sampling_ratio")]
+    public double GlobalSamplingRatio { get; set; }
+
+    /// <summary>
+    /// Whether to output histograms for the pose residuals.
+    /// </summary>
+    [JsonPropertyName("log_residual_histograms")]
+    public bool LogResidualHistograms { get; set; }
+
+    /// <summary>
+    /// If for the duration specified by this option no global constraint has been
+    /// added between two trajectories, loop closure searches will be performed
+    /// globally rather than in a smaller search window.
+    /// </summary>
+    [JsonPropertyName("global_constraint_search_after_n_seconds")]
+    public double GlobalConstraintSearchAfterNSeconds { get; set; }
+
+    /// <summary>
+    /// Instantiates the 'OverlappingSubmapsTrimmer2d' which trims submaps from the
+    /// pose graph based on the area of overlap.
+    /// </summary>
+    [JsonPropertyName("overlapping_submaps_trimmer_2d")]
+    [JsonIgnore(Condition = JsonIgnoreCondition.WhenWritingNull)]
+    public OverlappingSubmapsTrimmerOptions2D? OverlappingSubmapsTrimmer2D { get; set; }
+
+    /// <summary>
+    /// Options for the constraint builder used to find loop closures.
+    /// </summary>
+    [JsonPropertyName("constraint_builder_options")]
+    [JsonIgnore(Condition = JsonIgnoreCondition.WhenWritingNull)]
+    public ConstraintBuilderOptions? ConstraintBuilderOptions { get; set; }
+
+    /// <summary>
+    /// Options for the optimization problem.
+    /// </summary>
+    [JsonPropertyName("optimization_problem_options")]
+    [JsonIgnore(Condition = JsonIgnoreCondition.WhenWritingNull)]
+    public OptimizationProblemOptions? OptimizationProblemOptions { get; set; }
+
+    /// <summary>
+    /// Max number of nodes to search for relocalization constraint (match C++ max_number_relocalization_nodes).
+    /// </summary>
+    [JsonPropertyName("max_number_relocalization_nodes")]
+    public int MaxNumberRelocalizationNodes { get; set; } = 12;
+
+    /// <summary>
+    /// Enable loop closure detection within single trajectory.
+    /// When enabled, uses MatchFullSubmap instead of Match for nodes that are
+    /// far from their insertion submap but close to older finished submaps.
+    /// This is an extension to C++ Cartographer which only supports multi-trajectory loop closure.
+    /// </summary>
+    [JsonPropertyName("enable_single_trajectory_loop_closure")]
+    public bool EnableSingleTrajectoryLoopClosure { get; set; } = true;
+
+    /// <summary>
+    /// Distance threshold (in meters) for triggering single-trajectory loop closure.
+    /// If a node's initial_relative_pose distance to a finished submap exceeds this threshold,
+    /// but the node is within search range, use MatchFullSubmap for global search.
+    /// Only used when EnableSingleTrajectoryLoopClosure is true.
+    /// </summary>
+    [JsonPropertyName("single_trajectory_loop_closure_distance_threshold")]
+    public double SingleTrajectoryLoopClosureDistanceThreshold { get; set; } = 3.0;
+
+    public PoseGraphOptions(
+        int optimizeEveryNNodes = 90,
+        double matcherTranslationWeight = 5e2,
+        double matcherRotationWeight = 1.6e3,
+        int maxNumFinalIterations = 200,
+        double globalSamplingRatio = 0.003,
+        bool logResidualHistograms = false,
+        double globalConstraintSearchAfterNSeconds = 0.0,
+        OverlappingSubmapsTrimmerOptions2D? overlappingSubmapsTrimmer2D = null,
+        ConstraintBuilderOptions? constraintBuilderOptions = null,
+        OptimizationProblemOptions? optimizationProblemOptions = null)
+    {
+        OptimizeEveryNNodes = optimizeEveryNNodes;
+        MatcherTranslationWeight = matcherTranslationWeight;
+        MatcherRotationWeight = matcherRotationWeight;
+        MaxNumFinalIterations = maxNumFinalIterations;
+        GlobalSamplingRatio = globalSamplingRatio;
+        LogResidualHistograms = logResidualHistograms;
+        GlobalConstraintSearchAfterNSeconds = globalConstraintSearchAfterNSeconds;
+        OverlappingSubmapsTrimmer2D = overlappingSubmapsTrimmer2D;
+        ConstraintBuilderOptions = constraintBuilderOptions;
+        OptimizationProblemOptions = optimizationProblemOptions;
+        MaxNumberRelocalizationNodes = 12;
+    }
+}