BQP/srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/3D/ActiveSubmaps3D.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 CartographerSharp.Sensor;
using CartographerSharp.Transform;
using  RobotNet10.Shared.Numbers;

namespace CartographerSharp.Mapping.D3D;

/// <summary>
/// Options for 3D submaps.
/// </summary>
public struct SubmapsOptions3D(
    double highResolution,
    double highResolutionMaxRange,
    double lowResolution,
    int numRangeData,
    RangeDataInserterOptions3D rangeDataInserterOptions)
{
    public double HighResolution { get; set; } = highResolution;
    public double HighResolutionMaxRange { get; set; } = highResolutionMaxRange;
    public double LowResolution { get; set; } = lowResolution;
    public int NumRangeData { get; set; } = numRangeData;
    public RangeDataInserterOptions3D RangeDataInserterOptions { get; set; } = rangeDataInserterOptions;
}

/// <summary>
/// The first active submap will be created on the insertion of the first range
/// data. Except during this initialization when no or only one single submap
/// exists, there are always two submaps into which range data is inserted: an
/// old submap that is used for matching, and a new one, which will be used for
/// matching next, that is being initialized.
///
/// Once a certain number of range data have been inserted, the new submap is
/// considered initialized: the old submap is no longer changed, the "new" submap
/// is now the "old" submap and is used for scan-to-map matching. Moreover, a
/// "new" submap gets created. The "old" submap is forgotten by this object.
/// </summary>
public class ActiveSubmaps3D
{
    private readonly SubmapsOptions3D _options;
    private readonly List<Submap3D> _submaps = [];
    private readonly RangeDataInserter3D _rangeDataInserter;

    public ActiveSubmaps3D(SubmapsOptions3D options)
    {
        if (options.NumRangeData <= 0)
        {
            throw new ArgumentException("num_range_data must be greater than 0", nameof(options));
        }

        _options = options;
        _rangeDataInserter = new RangeDataInserter3D(options.RangeDataInserterOptions);
    }

    /// <summary>
    /// Inserts 'range_data_in_local' into the Submap collection.
    /// 'local_from_gravity_aligned' is used for the orientation of new submaps so
    /// that the z axis approximately aligns with gravity.
    /// 'rotational_scan_matcher_histogram_in_gravity' will be accumulated in all
    /// submaps of the Submap collection.
    /// </summary>
    public List<Submap3D> InsertData(
        RangeData rangeDataInLocal,
        Quaternion localFromGravityAligned,
        List<double> rotationalScanMatcherHistogramInGravity)
    {
        // Create new submap if needed
        if (_submaps.Count == 0 ||
            _submaps[^1].NumRangeData == _options.NumRangeData)
        {
            var localSubmapPose = new Rigid3d(
                (Vector3)rangeDataInLocal.Origin,
                localFromGravityAligned);
            AddSubmap(localSubmapPose, rotationalScanMatcherHistogramInGravity.Count);
        }

        // Insert into all active submaps
        foreach (var submap in _submaps)
        {
            submap.InsertData(
                rangeDataInLocal,
                _rangeDataInserter,
                _options.HighResolutionMaxRange,
                localFromGravityAligned,
                rotationalScanMatcherHistogramInGravity);
        }

        // Finish the first submap if it has reached 2 * num_range_data
        if (_submaps.Count > 0 && _submaps[0].NumRangeData == 2 * _options.NumRangeData)
        {
            _submaps[0].Finish();
        }

        return [.. _submaps];
    }

    /// <summary>
    /// Gets the current active submaps.
    /// </summary>
    public List<Submap3D> Submaps()
    {
        return [.. _submaps];
    }

    /// <summary>
    /// Adds a new submap to the collection.
    /// </summary>
    private void AddSubmap(Rigid3d localSubmapPose, int rotationalScanMatcherHistogramSize)
    {
        if (_submaps.Count >= 2)
        {
            // This will crop the finished Submap before inserting a new Submap to
            // reduce peak memory usage a bit.
            if (!_submaps[0].InsertionFinished)
            {
                throw new InvalidOperationException(
                    "First submap must be finished before adding a new one");
            }

            // We use `ForgetIntensityHybridGrid` to reduce memory usage. Since we use
            // active submaps and their associated intensity hybrid grids for scan
            // matching, we call `ForgetIntensityHybridGrid` once we remove the submap
            // from active submaps and no longer need the intensity hybrid grid.
            _submaps[0].ForgetIntensityHybridGrid();
            _submaps.RemoveAt(0);
        }

        var initialRotationalScanMatcherHistogram = new List<double>(rotationalScanMatcherHistogramSize);
        for (int i = 0; i < rotationalScanMatcherHistogramSize; i++)
        {
            initialRotationalScanMatcherHistogram.Add(0.0);
        }

        var submap = new Submap3D(
            _options.HighResolution,
            _options.LowResolution,
            localSubmapPose,
            initialRotationalScanMatcherHistogram);

        _submaps.Add(submap);
    }
}