Initial commit
This commit is contained in:
@@ -0,0 +1,297 @@
|
||||
using CartographerSharp.Mapping;
|
||||
using CartographerSharp.Mapping.D2D;
|
||||
using CartographerSharp.Transform;
|
||||
using RobotNet10.Shared.Geometry;
|
||||
using RobotNet10.Shared.Localization;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
//using Pose = RobotNet10.Shared.Geometry.Pose;
|
||||
|
||||
namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
|
||||
|
||||
/// <summary>
|
||||
/// Helper class for occupancy grid generation and manipulation.
|
||||
/// Contains pure functions extracted from OccupancyGridProvider.
|
||||
/// </summary>
|
||||
public static class OccupancyGridHelper
|
||||
{
|
||||
#region Constants
|
||||
|
||||
/// <summary>
|
||||
/// Initial size for empty occupancy grid (cells)
|
||||
/// </summary>
|
||||
public const int InitialEmptyGridSize = 100;
|
||||
|
||||
/// <summary>
|
||||
/// Probability threshold for free space
|
||||
/// </summary>
|
||||
public const double FreeSpaceProbabilityThreshold = 0.01;
|
||||
|
||||
/// <summary>
|
||||
/// Probability threshold for occupied space
|
||||
/// </summary>
|
||||
public const double OccupiedSpaceProbabilityThreshold = 0.99;
|
||||
|
||||
/// <summary>
|
||||
/// Lower bound for unknown space probability range
|
||||
/// </summary>
|
||||
public const double UnknownSpaceLowerBound = 0.4;
|
||||
|
||||
/// <summary>
|
||||
/// Upper bound for unknown space probability range
|
||||
/// </summary>
|
||||
public const double UnknownSpaceUpperBound = 0.6;
|
||||
|
||||
#endregion
|
||||
|
||||
#region Bounds Calculation
|
||||
|
||||
/// <summary>
|
||||
/// Calculate global bounds of a submap by transforming its corners to global frame
|
||||
/// </summary>
|
||||
public static (double minX, double minY, double maxX, double maxY) CalculateSubmapGlobalBounds(
|
||||
Submap2D submap2D,
|
||||
Rigid3d globalPose)
|
||||
{
|
||||
var grid = submap2D.Grid;
|
||||
if (grid == null)
|
||||
{
|
||||
return (double.MaxValue, double.MaxValue, double.MinValue, double.MinValue);
|
||||
}
|
||||
|
||||
var limits = grid.Limits;
|
||||
|
||||
// Use cropped limits if available (only known cells) for more accurate bounds
|
||||
grid.ComputeCroppedLimits(out var croppedOffset, out var croppedLimits);
|
||||
|
||||
// If cropped limits are valid (has known cells), use them; otherwise use full limits
|
||||
var hasCroppedLimits = croppedLimits.NumXCells > 0 && croppedLimits.NumYCells > 0;
|
||||
|
||||
// Get bounds of cropped (known) cells in submap local frame using GetCellCenter
|
||||
var cornerIndices = new[]
|
||||
{
|
||||
new CartographerSharp.Common.Math.Array2i(hasCroppedLimits ? croppedOffset.X : 0,
|
||||
hasCroppedLimits ? croppedOffset.Y : 0),
|
||||
new CartographerSharp.Common.Math.Array2i(hasCroppedLimits ? croppedOffset.X + croppedLimits.NumXCells - 1 : limits.CellLimits.NumXCells - 1,
|
||||
hasCroppedLimits ? croppedOffset.Y : 0),
|
||||
new CartographerSharp.Common.Math.Array2i(hasCroppedLimits ? croppedOffset.X : 0,
|
||||
hasCroppedLimits ? croppedOffset.Y + croppedLimits.NumYCells - 1 : limits.CellLimits.NumYCells - 1),
|
||||
new CartographerSharp.Common.Math.Array2i(hasCroppedLimits ? croppedOffset.X + croppedLimits.NumXCells - 1 : limits.CellLimits.NumXCells - 1,
|
||||
hasCroppedLimits ? croppedOffset.Y + croppedLimits.NumYCells - 1 : limits.CellLimits.NumYCells - 1)
|
||||
};
|
||||
|
||||
double croppedMinX = double.MaxValue, croppedMinY = double.MaxValue;
|
||||
double croppedMaxX = double.MinValue, croppedMaxY = double.MinValue;
|
||||
|
||||
foreach (var cornerIndex in cornerIndices)
|
||||
{
|
||||
var cellCenter = limits.GetCellCenter(cornerIndex);
|
||||
croppedMinX = Math.Min(croppedMinX, cellCenter.X);
|
||||
croppedMinY = Math.Min(croppedMinY, cellCenter.Y);
|
||||
croppedMaxX = Math.Max(croppedMaxX, cellCenter.X);
|
||||
croppedMaxY = Math.Max(croppedMaxY, cellCenter.Y);
|
||||
}
|
||||
|
||||
// Transform corners to global frame
|
||||
var corners = new[]
|
||||
{
|
||||
new Vector2(croppedMinX, croppedMinY),
|
||||
new Vector2(croppedMaxX, croppedMinY),
|
||||
new Vector2(croppedMaxX, croppedMaxY),
|
||||
new Vector2(croppedMinX, croppedMaxY)
|
||||
};
|
||||
|
||||
double submapGlobalMinX = double.MaxValue, submapGlobalMinY = double.MaxValue;
|
||||
double submapGlobalMaxX = double.MinValue, submapGlobalMaxY = double.MinValue;
|
||||
|
||||
foreach (var corner in corners)
|
||||
{
|
||||
var corner3D = new Vector3(corner.X, corner.Y, 0.0);
|
||||
var globalCorner = globalPose.TransformPoint(corner3D);
|
||||
|
||||
submapGlobalMinX = Math.Min((float)submapGlobalMinX, (float)globalCorner.X);
|
||||
submapGlobalMinY = Math.Min((float)submapGlobalMinY, (float)globalCorner.Y);
|
||||
submapGlobalMaxX = Math.Max((float)submapGlobalMaxX, (float)globalCorner.X);
|
||||
submapGlobalMaxY = Math.Max((float)submapGlobalMaxY, (float)globalCorner.Y);
|
||||
}
|
||||
|
||||
return (submapGlobalMinX, submapGlobalMinY, submapGlobalMaxX, submapGlobalMaxY);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get current grid bounds from origin and dimensions
|
||||
/// </summary>
|
||||
public static (double minX, double minY, double maxX, double maxY) GetGridBounds(OccupancyGrid grid)
|
||||
{
|
||||
var gridMinX = grid.Origin.Position.X;
|
||||
var gridMinY = grid.Origin.Position.Y;
|
||||
var gridMaxX = gridMinX + (grid.Width * grid.Resolution);
|
||||
var gridMaxY = gridMinY + (grid.Height * grid.Resolution);
|
||||
return (gridMinX, gridMinY, gridMaxX, gridMaxY);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Calculate bounds from a list of submaps with padding
|
||||
/// </summary>
|
||||
public static (double minX, double minY, double maxX, double maxY) CalculateBounds(
|
||||
List<(Submap2D Submap, Rigid3d GlobalPose)> submapList,
|
||||
double mapPadding)
|
||||
{
|
||||
double minX = double.MaxValue, minY = double.MaxValue;
|
||||
double maxX = double.MinValue, maxY = double.MinValue;
|
||||
|
||||
foreach (var (submap, globalPose) in submapList)
|
||||
{
|
||||
var (submapMinX, submapMinY, submapMaxX, submapMaxY) =
|
||||
CalculateSubmapGlobalBounds(submap, globalPose);
|
||||
|
||||
minX = Math.Min(minX, submapMinX);
|
||||
minY = Math.Min(minY, submapMinY);
|
||||
maxX = Math.Max(maxX, submapMaxX);
|
||||
maxY = Math.Max(maxY, submapMaxY);
|
||||
}
|
||||
|
||||
return (minX - mapPadding, minY - mapPadding, maxX + mapPadding, maxY + mapPadding);
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Probability Conversion
|
||||
|
||||
/// <summary>
|
||||
/// Convert probability value to occupancy grid value
|
||||
/// </summary>
|
||||
public static sbyte ConvertProbabilityToOccupancyValue(double probability)
|
||||
{
|
||||
if (probability < FreeSpaceProbabilityThreshold)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
if (probability > OccupiedSpaceProbabilityThreshold)
|
||||
{
|
||||
return 100;
|
||||
}
|
||||
if (probability < UnknownSpaceLowerBound || probability > UnknownSpaceUpperBound)
|
||||
{
|
||||
var occupancyValue = (sbyte)Math.Round(probability * 100.0);
|
||||
return (sbyte)Math.Clamp(occupancyValue, (sbyte)0, (sbyte)100);
|
||||
}
|
||||
return -1; // Unknown space
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Grid Creation
|
||||
|
||||
/// <summary>
|
||||
/// Create an empty occupancy grid with default size
|
||||
/// </summary>
|
||||
public static OccupancyGrid CreateEmptyOccupancyGrid(double resolution)
|
||||
{
|
||||
var origin = new Pose
|
||||
{
|
||||
Position = new Vector3(0, 0, 0),
|
||||
Orientation = new Quaternion(0, 0, 0, 1)
|
||||
};
|
||||
|
||||
return new OccupancyGrid(resolution, InitialEmptyGridSize, InitialEmptyGridSize, origin);
|
||||
}
|
||||
|
||||
// NOTE: GenerateOccupancyGridFromSubmapList and MergeSubmapIntoOccupancyGrid
|
||||
// have been moved to OccupancyGridGenerator for unified grid generation.
|
||||
// Use OccupancyGridGenerator.GenerateFromMapBuilder instead.
|
||||
|
||||
#endregion
|
||||
|
||||
#region Grid Expansion
|
||||
|
||||
/// <summary>
|
||||
/// Expand grid if needed to fit multiple submaps (optimized version)
|
||||
/// </summary>
|
||||
public static OccupancyGrid? ExpandGridToFitSubmaps(
|
||||
OccupancyGrid currentGrid,
|
||||
List<(Submap2D Submap, Rigid3d GlobalPose, SubmapId SubmapId)> submaps,
|
||||
double resolution,
|
||||
double mapPadding)
|
||||
{
|
||||
if (currentGrid == null || submaps == null || submaps.Count == 0)
|
||||
return null;
|
||||
|
||||
// Calculate combined bounds of all submaps
|
||||
double combinedMinX = double.MaxValue, combinedMinY = double.MaxValue;
|
||||
double combinedMaxX = double.MinValue, combinedMaxY = double.MinValue;
|
||||
bool hasValidBounds = false;
|
||||
|
||||
foreach (var (submap2D, globalPose, _) in submaps)
|
||||
{
|
||||
var grid = submap2D.Grid;
|
||||
if (grid == null)
|
||||
continue;
|
||||
|
||||
var (submapGlobalMinX, submapGlobalMinY, submapGlobalMaxX, submapGlobalMaxY) =
|
||||
CalculateSubmapGlobalBounds(submap2D, globalPose);
|
||||
|
||||
if (submapGlobalMinX < submapGlobalMaxX && submapGlobalMinY < submapGlobalMaxY &&
|
||||
submapGlobalMinX != double.MaxValue && submapGlobalMinY != double.MaxValue &&
|
||||
submapGlobalMaxX != double.MinValue && submapGlobalMaxY != double.MinValue)
|
||||
{
|
||||
combinedMinX = Math.Min(combinedMinX, submapGlobalMinX);
|
||||
combinedMinY = Math.Min(combinedMinY, submapGlobalMinY);
|
||||
combinedMaxX = Math.Max(combinedMaxX, submapGlobalMaxX);
|
||||
combinedMaxY = Math.Max(combinedMaxY, submapGlobalMaxY);
|
||||
hasValidBounds = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (!hasValidBounds)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
var (gridMinX, gridMinY, gridMaxX, gridMaxY) = GetGridBounds(currentGrid);
|
||||
|
||||
var needsExpansion = combinedMinX < gridMinX || combinedMinY < gridMinY ||
|
||||
combinedMaxX > gridMaxX || combinedMaxY > gridMaxY;
|
||||
|
||||
if (!needsExpansion)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
var newMinX = Math.Min(gridMinX, combinedMinX) - mapPadding;
|
||||
var newMinY = Math.Min(gridMinY, combinedMinY) - mapPadding;
|
||||
var newMaxX = Math.Max(gridMaxX, combinedMaxX) + mapPadding;
|
||||
var newMaxY = Math.Max(gridMaxY, combinedMaxY) + mapPadding;
|
||||
|
||||
var newWidth = (int)Math.Ceiling((newMaxX - newMinX) / resolution);
|
||||
var newHeight = (int)Math.Ceiling((newMaxY - newMinY) / resolution);
|
||||
|
||||
var newOrigin = new Pose
|
||||
{
|
||||
Position = new Vector3(newMinX, newMinY, 0),
|
||||
Orientation = new Quaternion(0, 0, 0, 1)
|
||||
};
|
||||
|
||||
var expandedGrid = new OccupancyGrid(resolution, newWidth, newHeight, newOrigin);
|
||||
|
||||
// Copy existing grid data to expanded grid
|
||||
for (int y = 0; y < currentGrid.Height; y++)
|
||||
{
|
||||
for (int x = 0; x < currentGrid.Width; x++)
|
||||
{
|
||||
var (worldX, worldY) = currentGrid.GridToWorld(x, y);
|
||||
var (newGridX, newGridY) = expandedGrid.WorldToGrid(worldX, worldY);
|
||||
|
||||
if (newGridX >= 0 && newGridX < expandedGrid.Width &&
|
||||
newGridY >= 0 && newGridY < expandedGrid.Height)
|
||||
{
|
||||
var cellValue = currentGrid.GetCell(x, y);
|
||||
expandedGrid.SetCell(newGridX, newGridY, cellValue);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return expandedGrid;
|
||||
}
|
||||
|
||||
#endregion
|
||||
}
|
||||
Reference in New Issue
Block a user