Initial commit

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/PureLocalizationTrimmer.cs

@@ -0,0 +1,70 @@
+/*
+ * 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.
+ */
+
+namespace CartographerSharp.Mapping;
+
+/// <summary>
+/// Match C++ PureLocalizationTrimmer: keeps only the last 'num_submaps_to_keep'
+/// submaps of a localization trajectory, trimming older ones.
+/// Used when trajectory_options has pure_localization_trimmer (map_builder.cc MaybeAddPureLocalizationTrimmer).
+/// </summary>
+public class PureLocalizationTrimmer : PoseGraphTrimmer
+{
+    private readonly int _trajectoryId;
+    private int _numSubmapsToKeep;
+    private bool _finished;
+
+    /// <summary>
+    /// Match C++ PureLocalizationTrimmer(trajectory_id, num_submaps_to_keep).
+    /// CHECK_GE(num_submaps_to_keep, 2) in C++.
+    /// </summary>
+    public PureLocalizationTrimmer(int trajectoryId, int numSubmapsToKeep)
+    {
+        if (numSubmapsToKeep < 2)
+            throw new ArgumentException("Cannot trim with less than 2 submaps", nameof(numSubmapsToKeep));
+        _trajectoryId = trajectoryId;
+        _numSubmapsToKeep = numSubmapsToKeep;
+    }
+
+    /// <summary>
+    /// Match C++ Trim(Trimmable*).
+    /// </summary>
+    public override void Trim(ITrimmable trimmable)
+    {
+        if (trimmable.IsFinished(_trajectoryId))
+        {
+            _numSubmapsToKeep = 0;
+        }
+
+        var submapIds = trimmable.GetSubmapIds(_trajectoryId);
+        // C++: for (i = 0; i + num_submaps_to_keep_ < submap_ids.size(); ++i) TrimSubmap(submap_ids.at(i));
+        for (int i = 0; i + _numSubmapsToKeep < submapIds.Count; ++i)
+        {
+            trimmable.TrimSubmap(submapIds[i]);
+        }
+
+        if (_numSubmapsToKeep == 0)
+        {
+            _finished = true;
+            trimmable.SetTrajectoryState(_trajectoryId, IPoseGraph.TrajectoryState.Deleted);
+        }
+    }
+
+    /// <summary>
+    /// Match C++ IsFinished().
+    /// </summary>
+    public bool IsFinished() => _finished;
+}