QUYVN/I150
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
QUYVN
2026-07-03 16:37:12 +07:00
commit 63b8c1ea8b
1931 changed files with 640587 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

184
srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Mapping/2D/Submap2D.cs Normal file
View File

@@ -0,0 +1,184 @@
/*
* 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.Models.Transform;
using CartographerSharp.Sensor;
using CartographerSharp.Transform;
using System;
using RobotNet10.Shared.Numbers;
namespace CartographerSharp.Mapping.D2D;
/// <summary>
/// 2D Submap implementation.
/// </summary>
public class Submap2D : Submap
{
private Grid2D? _grid;
private Grid2D? _highResGrid;
private readonly ValueConversionTables _conversionTables;
public Submap2D(Rigid3d localSubmapPose, Grid2D grid, ValueConversionTables conversionTables, Grid2D? highResGrid = null)
: base(localSubmapPose) // Pass the full Rigid3d pose including rotation
{
_grid = grid;
_highResGrid = highResGrid;
_conversionTables = conversionTables;
}
// Keep old constructor for backward compatibility (if needed)
public Submap2D(Vector2 origin, Grid2D grid, ValueConversionTables conversionTables)
: this(new Rigid3d(new Vector3(origin.X, origin.Y, 0.0), Quaternion.Identity), grid, conversionTables)
{
}
public Submap2D(Models.Mapping.Submap2D proto, ValueConversionTables conversionTables)
: base((Rigid3d)proto.LocalPose)
{
_conversionTables = conversionTables;
NumRangeData = proto.NumRangeData;
InsertionFinished = proto.Finished;
// Match C++: if (proto.has_grid()) - no resolution check
if (proto.Grid.HasValue)
{
var gridProto = proto.Grid.Value;
if (gridProto.ProbabilityGrid2D.HasValue)
{
_grid = new ProbabilityGrid(gridProto, conversionTables);
}
else if (gridProto.Tsdf2D.HasValue)
{
_grid = new TSDF2D(gridProto, conversionTables);
}
else
{
throw new ArgumentException("proto::Submap2D has grid with unknown type.", nameof(proto));
}
}
}
/// <summary>
/// Gets the grid.
/// </summary>
public Grid2D? Grid => _grid;
/// <summary>
/// Gets the optional high resolution grid for fine-grained Ceres scan matching.
/// </summary>
public Grid2D? HighResGrid => _highResGrid;
/// <summary>
/// Insert 'range_data' into this submap using 'range_data_inserter'. The
/// submap must not be finished yet.
/// </summary>
public void InsertRangeData(RangeData rangeData, IRangeDataInserter rangeDataInserter)
{
if (InsertionFinished)
{
throw new InvalidOperationException("Cannot insert range data into finished submap");
}
if (_grid == null)
{
throw new InvalidOperationException("Grid is not initialized");
}
// Insert range data directly (already transformed to submap frame in ActiveSubmaps2D)
if (_highResGrid != null)
{
// Main grid and high-res grid are independent - insert in parallel.
var highResGrid = _highResGrid;
var highResTask = Task.Run(() => rangeDataInserter.Insert(rangeData, (IGrid)highResGrid));
rangeDataInserter.Insert(rangeData, (IGrid)_grid);
highResTask.GetAwaiter().GetResult();
}
else
{
rangeDataInserter.Insert(rangeData, (IGrid)_grid);
}
NumRangeData++;
}
/// <summary>
/// Finishes the submap.
/// Match C++: grid_ = grid_->ComputeCroppedGrid(); to reduce memory usage
/// </summary>
public void Finish()
{
if (_grid == null)
{
throw new InvalidOperationException("Grid is not initialized");
}
if (InsertionFinished)
{
throw new InvalidOperationException("Submap is already finished");
}
// Match C++: Crop grid to reduce memory usage when submap is finished
_grid = _grid.ComputeCroppedGrid();
_highResGrid = _highResGrid?.ComputeCroppedGrid();
InsertionFinished = true;
}
/// <summary>
/// Converts to proto representation.
/// </summary>
public override Models.Mapping.Submap ToProto(bool includeGridData)
{
var submap2D = new Models.Mapping.Submap2D(
(Rigid3dProto)LocalPose,
NumRangeData,
InsertionFinished,
includeGridData && _grid != null ? _grid.ToProto() : null
);
// Note: SubmapId will be set by caller
// Note: SubmapId will be set by caller when creating full Submap
return new Models.Mapping.Submap(new Models.Mapping.PoseGraph.SubmapId(0, 0), submap2D, null);
}
/// <summary>
/// Updates from proto representation.
/// Match C++: CHECK(proto.has_submap_2d())
/// </summary>
public override void UpdateFromProto(Models.Mapping.Submap proto)
{
if (!proto.Submap2D.HasValue)
{
throw new ArgumentException("Proto must contain Submap2D", nameof(proto));
}
var submap2D = proto.Submap2D.Value;
NumRangeData = submap2D.NumRangeData;
InsertionFinished = submap2D.Finished;
// Match C++: if (proto.submap_2d().has_grid()) - no resolution check
if (submap2D.Grid.HasValue)
{
var gridProto = submap2D.Grid.Value;
if (gridProto.ProbabilityGrid2D.HasValue)
{
_grid = new ProbabilityGrid(gridProto, _conversionTables);
}
else if (gridProto.Tsdf2D.HasValue)
{
_grid = new TSDF2D(gridProto, _conversionTables);
}
else
{
throw new ArgumentException("proto::Submap2D has grid with unknown type.", nameof(proto));
}
}
}
}