71 lines
2.4 KiB
C#
71 lines
2.4 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.
|
|
*/
|
|
|
|
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;
|
|
}
|