2017 lines
84 KiB
C#
2017 lines
84 KiB
C#
using CartographerSharp.IO;
|
||
using CartographerSharp.Mapping;
|
||
using CartographerSharp.Mapping.D2D;
|
||
using CartographerSharp.Transform;
|
||
using RobotNet10.RobotApp.SLAM.Cartographer;
|
||
using RobotNet10.Shared.Geometry;
|
||
using RobotNet10.Shared.Localization;
|
||
using RobotNet10.Shared.Numbers;
|
||
|
||
namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
|
||
|
||
/// <summary>
|
||
/// Unified occupancy grid generator from MapBuilder submaps.
|
||
/// UNIFIED CONVENTION: All grids use ROS convention (row 0 = world BOTTOM, Y-axis pointing UP).
|
||
/// </summary>
|
||
public static class OccupancyGridGenerator
|
||
{
|
||
#region Public API
|
||
|
||
/// <summary>
|
||
/// Generate occupancy grid from MapBuilder using ROS convention.
|
||
/// </summary>
|
||
/// <param name="mapBuilder">MapBuilder containing submaps</param>
|
||
/// <param name="resolution">Target grid resolution</param>
|
||
/// <param name="padding">Padding around map bounds</param>
|
||
/// <param name="logger">Optional logger</param>
|
||
/// <returns>OccupancyGrid with ROS convention (row 0 = world bottom), or null if generation fails</returns>
|
||
public static OccupancyGrid? GenerateFromMapBuilder(
|
||
IMapBuilder mapBuilder,
|
||
double resolution,
|
||
double padding,
|
||
ILogger? logger = null)
|
||
{
|
||
try
|
||
{
|
||
var poseGraph = mapBuilder.PoseGraph;
|
||
var allSubmapData = poseGraph.GetAllSubmapData();
|
||
|
||
// Get TransformToMap and compute its inverse for applying to all poses
|
||
// This matches C++ xloc.cc behavior where poses are transformed using:
|
||
// transformedPose = GetTransformToMap().inverse() * pose
|
||
var transformToMap = poseGraph.GetTransformToMap();
|
||
var transformToMapInverse = transformToMap.Inverse();
|
||
|
||
// Collect all 2D submaps from pose graph (finished submaps)
|
||
// IMPORTANT: We store localToMapTransform (NOT globalPose) because:
|
||
// - GetCellCenter() returns coordinates in LOCAL MAP FRAME (trajectory local frame)
|
||
// - NOT in submap frame as previously assumed
|
||
// - localToMapTransform = TransformToMapInverse * LocalToGlobalTransform
|
||
// - This correctly transforms from local map frame to map frame
|
||
var submap2DList = new List<(Submap2D Submap, Rigid3d LocalToMapTransform)>();
|
||
|
||
int submapIndexDebug = 0;
|
||
foreach (var idDataRef in allSubmapData)
|
||
{
|
||
var submapData = idDataRef.Data;
|
||
if (submapData.Submap is Submap2D submap2D)
|
||
{
|
||
var submapId = idDataRef.Id;
|
||
var trajectoryId = submapId.TrajectoryId;
|
||
|
||
// FIX: Use LocalToGlobalTransform instead of SubmapData.Pose
|
||
// Cell coordinates from GetCellCenter() are in LOCAL MAP FRAME (L),
|
||
// so we need T_map_local = TransformToMapInverse * LocalToGlobalTransform
|
||
// Previously used: T_map_submap = TransformToMapInverse * SubmapData.Pose (WRONG)
|
||
var localToGlobal = poseGraph.GetLocalToGlobalTransform(trajectoryId);
|
||
Rigid3d localToMapTransform = transformToMapInverse * localToGlobal;
|
||
|
||
// Validate that the transform is valid (not NaN/Infinity)
|
||
if (!localToMapTransform.IsValid())
|
||
{
|
||
logger?.LogWarning(
|
||
"OccupancyGridGenerator: Submap {TrajectoryId}:{SubmapIndex} has invalid localToMapTransform, skipping",
|
||
submapId.TrajectoryId, submapId.SubmapIndex);
|
||
continue;
|
||
}
|
||
|
||
submap2DList.Add((submap2D, localToMapTransform));
|
||
submapIndexDebug++;
|
||
}
|
||
}
|
||
|
||
if (submap2DList.Count == 0)
|
||
{
|
||
logger?.LogWarning("OccupancyGridGenerator: No 2D submaps found");
|
||
return null;
|
||
}
|
||
|
||
// Calculate bounds from all submaps
|
||
var (minX, minY, maxX, maxY) = CalculateBoundsFromSubmaps(submap2DList);
|
||
|
||
// Validate bounds
|
||
if (minX >= maxX || minY >= maxY || double.IsInfinity(minX) || double.IsInfinity(maxX) ||
|
||
double.IsInfinity(minY) || double.IsInfinity(maxY))
|
||
{
|
||
logger?.LogWarning(
|
||
"OccupancyGridGenerator: Invalid bounds - minX={MinX}, minY={MinY}, maxX={MaxX}, maxY={MaxY}",
|
||
minX, minY, maxX, maxY);
|
||
return null;
|
||
}
|
||
|
||
// Add padding
|
||
var adjustedMinX = minX - padding;
|
||
var adjustedMinY = minY - padding;
|
||
var adjustedMaxX = maxX + padding;
|
||
var adjustedMaxY = maxY + padding;
|
||
|
||
var width = (int)Math.Ceiling((adjustedMaxX - adjustedMinX) / resolution);
|
||
var height = (int)Math.Ceiling((adjustedMaxY - adjustedMinY) / resolution);
|
||
|
||
// Set origin to bottom-left corner (cell (0,0)) according to ROS occupancy grid convention
|
||
var gridOriginPosition = new Vector3(adjustedMinX, adjustedMinY, 0.0);
|
||
var origin = new Pose
|
||
{
|
||
Position = gridOriginPosition,
|
||
Orientation = new Quaternion(0, 0, 0, 1)
|
||
};
|
||
|
||
var occupancyGrid = new OccupancyGrid(resolution, width, height, origin);
|
||
|
||
// Merge all submaps into occupancy grid
|
||
int totalCellsMerged = 0;
|
||
int totalCellsSkipped = 0;
|
||
|
||
foreach (var (submap2D, localToMapTransform) in submap2DList)
|
||
{
|
||
var (merged, skipped) = MergeSubmapIntoOccupancyGrid(submap2D, localToMapTransform, occupancyGrid, resolution);
|
||
totalCellsMerged += merged;
|
||
totalCellsSkipped += skipped;
|
||
}
|
||
|
||
return occupancyGrid;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
logger?.LogError(ex, "OccupancyGridGenerator: Failed to generate occupancy grid");
|
||
return null;
|
||
}
|
||
}
|
||
|
||
#endregion
|
||
|
||
#region Bounds Calculation
|
||
|
||
/// <summary>
|
||
/// Calculate bounds from submaps.
|
||
/// LocalToMapTransform transforms points from LOCAL MAP FRAME to MAP FRAME.
|
||
/// </summary>
|
||
private static (double minX, double minY, double maxX, double maxY) CalculateBoundsFromSubmaps(
|
||
List<(Submap2D Submap, Rigid3d LocalToMapTransform)> submapList,
|
||
ILogger? logger = null)
|
||
{
|
||
// Delegate to the snapshot-aware overload with null snapshots
|
||
var extended = submapList
|
||
.Select(t => (t.Submap, t.LocalToMapTransform, (Grid2D.GridCellSnapshot?)null))
|
||
.ToList();
|
||
return CalculateBoundsFromSubmaps(extended, logger);
|
||
}
|
||
|
||
private static (double minX, double minY, double maxX, double maxY) CalculateBoundsFromSubmaps(
|
||
List<(Submap2D Submap, Rigid3d LocalToMapTransform, Grid2D.GridCellSnapshot? Snapshot)> submapList,
|
||
ILogger? logger = null)
|
||
{
|
||
double minX = double.MaxValue, minY = double.MaxValue;
|
||
double maxX = double.MinValue, maxY = double.MinValue;
|
||
|
||
int submapIdx = 0;
|
||
foreach (var (submap, localToMapTransform, snapshot) in submapList)
|
||
{
|
||
MapLimits limits;
|
||
CartographerSharp.Common.Math.Array2i croppedOffset;
|
||
CellLimits croppedLimits;
|
||
|
||
if (snapshot != null)
|
||
{
|
||
// Use snapshot data (active submap – thread-safe)
|
||
limits = snapshot.Limits;
|
||
snapshot.ComputeCroppedLimits(out croppedOffset, out croppedLimits);
|
||
}
|
||
else
|
||
{
|
||
var grid = submap.Grid;
|
||
if (grid == null) continue;
|
||
limits = grid.Limits;
|
||
grid.ComputeCroppedLimits(out croppedOffset, out croppedLimits);
|
||
}
|
||
|
||
if (croppedLimits.NumXCells <= 0 || croppedLimits.NumYCells <= 0)
|
||
continue;
|
||
|
||
// Get bounds of cropped (known) cells in submap local frame
|
||
var cornerIndices = new[]
|
||
{
|
||
new CartographerSharp.Common.Math.Array2i(croppedOffset.X, croppedOffset.Y),
|
||
new CartographerSharp.Common.Math.Array2i(croppedOffset.X + croppedLimits.NumXCells - 1, croppedOffset.Y),
|
||
new CartographerSharp.Common.Math.Array2i(croppedOffset.X, croppedOffset.Y + croppedLimits.NumYCells - 1),
|
||
new CartographerSharp.Common.Math.Array2i(croppedOffset.X + croppedLimits.NumXCells - 1, croppedOffset.Y + croppedLimits.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 from local map frame to map 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 cornerPoint = new Vector3(corner.X, corner.Y, 0);
|
||
var mapCorner = localToMapTransform.TransformPoint(cornerPoint);
|
||
|
||
submapGlobalMinX = Math.Min(submapGlobalMinX, mapCorner.X);
|
||
submapGlobalMinY = Math.Min(submapGlobalMinY, mapCorner.Y);
|
||
submapGlobalMaxX = Math.Max(submapGlobalMaxX, mapCorner.X);
|
||
submapGlobalMaxY = Math.Max(submapGlobalMaxY, mapCorner.Y);
|
||
|
||
minX = Math.Min(minX, mapCorner.X);
|
||
minY = Math.Min(minY, mapCorner.Y);
|
||
maxX = Math.Max(maxX, mapCorner.X);
|
||
maxY = Math.Max(maxY, mapCorner.Y);
|
||
}
|
||
|
||
submapIdx++;
|
||
}
|
||
|
||
return (minX, minY, maxX, maxY);
|
||
}
|
||
|
||
#endregion
|
||
|
||
#region Submap Processing
|
||
|
||
/// <summary>
|
||
/// Merge submap into occupancy grid using ROS convention.
|
||
/// Supports both ProbabilityGrid and TSDF2D grid types.
|
||
/// </summary>
|
||
/// <param name="submap2D">The submap to merge</param>
|
||
/// <param name="localToMapTransform">Transform from LOCAL MAP FRAME to MAP FRAME</param>
|
||
/// <param name="occupancyGrid">Target occupancy grid</param>
|
||
/// <param name="targetResolution">Target grid resolution</param>
|
||
/// <returns>(mergedCells, skippedCells) for statistics</returns>
|
||
private static (int mergedCells, int skippedCells) MergeSubmapIntoOccupancyGrid(
|
||
Submap2D submap2D,
|
||
Rigid3d localToMapTransform,
|
||
OccupancyGrid occupancyGrid,
|
||
double targetResolution)
|
||
{
|
||
var grid = submap2D.Grid;
|
||
if (grid == null)
|
||
return (0, 0);
|
||
|
||
// Dispatch to appropriate handler based on grid type
|
||
if (grid is ProbabilityGrid probabilityGrid)
|
||
{
|
||
return MergeSubmapIntoOccupancyGridFromProbabilityGrid(
|
||
probabilityGrid, localToMapTransform, occupancyGrid, targetResolution);
|
||
}
|
||
else if (grid is TSDF2D tsdfGrid)
|
||
{
|
||
return MergeSubmapIntoOccupancyGridFromTSDF(
|
||
tsdfGrid, localToMapTransform, occupancyGrid, targetResolution);
|
||
}
|
||
|
||
return (0, 0);
|
||
}
|
||
|
||
/// <summary>
|
||
/// Merge ProbabilityGrid submap into occupancy grid.
|
||
/// </summary>
|
||
/// <param name="probabilityGrid">The probability grid to merge</param>
|
||
/// <param name="localToMapTransform">Transform from LOCAL MAP FRAME to MAP FRAME</param>
|
||
/// <param name="occupancyGrid">Target occupancy grid</param>
|
||
/// <param name="targetResolution">Target grid resolution</param>
|
||
private static (int mergedCells, int skippedCells) MergeSubmapIntoOccupancyGridFromProbabilityGrid(
|
||
ProbabilityGrid probabilityGrid,
|
||
Rigid3d localToMapTransform,
|
||
OccupancyGrid occupancyGrid,
|
||
double targetResolution)
|
||
{
|
||
var limits = probabilityGrid.Limits;
|
||
int mergedCells = 0;
|
||
int skippedCells = 0;
|
||
|
||
// Use ComputeCroppedLimits to get only known cells bounds
|
||
probabilityGrid.ComputeCroppedLimits(out var croppedOffset, out var croppedLimits);
|
||
|
||
if (croppedLimits.NumXCells <= 0 || croppedLimits.NumYCells <= 0)
|
||
return (0, 0);
|
||
|
||
// Iterate through known cells region only
|
||
for (int y = 0; y < croppedLimits.NumYCells; y++)
|
||
{
|
||
for (int x = 0; x < croppedLimits.NumXCells; x++)
|
||
{
|
||
var cellIndex = new CartographerSharp.Common.Math.Array2i(croppedOffset.X + x, croppedOffset.Y + y);
|
||
|
||
// Skip unknown cells
|
||
if (!probabilityGrid.IsKnown(cellIndex))
|
||
{
|
||
skippedCells++;
|
||
continue;
|
||
}
|
||
|
||
// Get cell center in LOCAL MAP FRAME (NOT submap frame!)
|
||
// GetCellCenter returns coordinates relative to grid's Max, which is set
|
||
// based on LocalPose.Translation when the submap is created.
|
||
var cellCenter = limits.GetCellCenter(cellIndex);
|
||
var cellCenterInLocalFrame = new Vector3(cellCenter.X, cellCenter.Y, 0.0);
|
||
|
||
// Transform from LOCAL MAP FRAME to MAP FRAME
|
||
var mapPoint = localToMapTransform.TransformPoint(cellCenterInLocalFrame);
|
||
|
||
// Convert to occupancy grid coordinates
|
||
var gridX = (int)Math.Floor((mapPoint.X - occupancyGrid.Origin.Position.X) / targetResolution);
|
||
var gridY = (int)Math.Floor((mapPoint.Y - occupancyGrid.Origin.Position.Y) / targetResolution);
|
||
|
||
if (gridX >= 0 && gridX < occupancyGrid.Width && gridY >= 0 && gridY < occupancyGrid.Height)
|
||
{
|
||
// Get probability value from submap
|
||
var probability = probabilityGrid.GetProbability(cellIndex);
|
||
|
||
// Convert probability to occupancy value using Cartographer's texture-based conversion
|
||
var logOddsInteger = CartographerSharp.Mapping.SubmapProbabilityUtils.ProbabilityToLogOddsInteger(probability);
|
||
int delta = 128 - logOddsInteger;
|
||
|
||
byte textureValue = (byte)(delta > 0 ? delta : 0);
|
||
byte textureAlpha = (byte)(delta > 0 ? 0 : -delta);
|
||
|
||
// Convert texture value/alpha to occupancy value (0 = free, 100 = occupied, -1 = unknown)
|
||
sbyte occupancyValue;
|
||
if (textureAlpha > 0)
|
||
{
|
||
// Occupied space (high confidence)
|
||
occupancyValue = 100;
|
||
}
|
||
else if (textureValue >= 100)
|
||
{
|
||
// Free space (high confidence)
|
||
occupancyValue = 0;
|
||
}
|
||
else
|
||
{
|
||
// Skip ambiguous cells
|
||
skippedCells++;
|
||
continue;
|
||
}
|
||
|
||
var currentValue = occupancyGrid.GetCell(gridX, gridY);
|
||
|
||
// Merge logic: Protect known cells from being overwritten by unknown/ambiguous cells
|
||
bool shouldMerge = false;
|
||
|
||
if (currentValue == -1)
|
||
{
|
||
if (occupancyValue != -1)
|
||
{
|
||
shouldMerge = true;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if (occupancyValue != -1 && occupancyValue > currentValue)
|
||
{
|
||
shouldMerge = true;
|
||
}
|
||
}
|
||
|
||
if (shouldMerge)
|
||
{
|
||
occupancyGrid.Data[gridY * occupancyGrid.Width + gridX] = occupancyValue;
|
||
mergedCells++;
|
||
}
|
||
else
|
||
{
|
||
skippedCells++;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
skippedCells++;
|
||
}
|
||
}
|
||
}
|
||
|
||
return (mergedCells, skippedCells);
|
||
}
|
||
|
||
/// <summary>
|
||
/// Merge TSDF2D submap into occupancy grid.
|
||
/// TSDF convention:
|
||
/// - tsd > 0: Free space (away from obstacles)
|
||
/// - tsd < 0: Occupied space (inside/near obstacles)
|
||
/// - tsd = 0: On obstacle surface
|
||
/// </summary>
|
||
/// <param name="tsdfGrid">The TSDF grid to merge</param>
|
||
/// <param name="localToMapTransform">Transform from LOCAL MAP FRAME to MAP FRAME</param>
|
||
/// <param name="occupancyGrid">Target occupancy grid</param>
|
||
/// <param name="targetResolution">Target grid resolution</param>
|
||
/// <param name="config">Optional configuration</param>
|
||
private static (int mergedCells, int skippedCells) MergeSubmapIntoOccupancyGridFromTSDF(
|
||
TSDF2D tsdfGrid,
|
||
Rigid3d localToMapTransform,
|
||
OccupancyGrid occupancyGrid,
|
||
double targetResolution,
|
||
OccupancyGridConfiguration? config = null)
|
||
{
|
||
var limits = tsdfGrid.Limits;
|
||
int mergedCells = 0;
|
||
int skippedCells = 0;
|
||
|
||
// Get TSDF-specific thresholds from config
|
||
var freeThreshold = config?.TsdfFreeThreshold ?? 0.05;
|
||
var occupiedThreshold = config?.TsdfOccupiedThreshold ?? -0.02;
|
||
var minWeight = config?.TsdfMinWeight ?? 0.1;
|
||
|
||
// Use ComputeCroppedLimits to get only known cells bounds
|
||
tsdfGrid.ComputeCroppedLimits(out var croppedOffset, out var croppedLimits);
|
||
|
||
if (croppedLimits.NumXCells <= 0 || croppedLimits.NumYCells <= 0)
|
||
return (0, 0);
|
||
|
||
// Iterate through known cells region only
|
||
for (int y = 0; y < croppedLimits.NumYCells; y++)
|
||
{
|
||
for (int x = 0; x < croppedLimits.NumXCells; x++)
|
||
{
|
||
var cellIndex = new CartographerSharp.Common.Math.Array2i(croppedOffset.X + x, croppedOffset.Y + y);
|
||
|
||
// Skip unknown cells
|
||
if (!tsdfGrid.IsKnown(cellIndex))
|
||
{
|
||
skippedCells++;
|
||
continue;
|
||
}
|
||
|
||
// Get cell center in LOCAL MAP FRAME (NOT submap frame!)
|
||
var cellCenter = limits.GetCellCenter(cellIndex);
|
||
var cellCenterInLocalFrame = new Vector3(cellCenter.X, cellCenter.Y, 0.0);
|
||
|
||
// Transform from LOCAL MAP FRAME to MAP FRAME
|
||
var mapPoint = localToMapTransform.TransformPoint(cellCenterInLocalFrame);
|
||
|
||
// Convert to occupancy grid coordinates
|
||
var gridX = (int)Math.Floor((mapPoint.X - occupancyGrid.Origin.Position.X) / targetResolution);
|
||
var gridY = (int)Math.Floor((mapPoint.Y - occupancyGrid.Origin.Position.Y) / targetResolution);
|
||
|
||
if (gridX >= 0 && gridX < occupancyGrid.Width && gridY >= 0 && gridY < occupancyGrid.Height)
|
||
{
|
||
// Get TSD and weight from TSDF grid
|
||
var (tsd, weight) = tsdfGrid.GetTSDAndWeight(cellIndex);
|
||
|
||
// Skip cells with very low weight (not enough observations)
|
||
if (weight < minWeight)
|
||
{
|
||
skippedCells++;
|
||
continue;
|
||
}
|
||
|
||
// Convert TSD to occupancy value
|
||
// tsd > 0: Free space → occupancy = 0
|
||
// tsd < 0: Occupied → occupancy = 100
|
||
// tsd ≈ 0: Surface/ambiguous
|
||
sbyte occupancyValue;
|
||
|
||
if (tsd > freeThreshold)
|
||
{
|
||
// Free space (high confidence)
|
||
occupancyValue = 0;
|
||
}
|
||
else if (tsd < occupiedThreshold)
|
||
{
|
||
// Occupied space (high confidence)
|
||
occupancyValue = 100;
|
||
}
|
||
else
|
||
{
|
||
// Ambiguous (near surface) - skip
|
||
skippedCells++;
|
||
continue;
|
||
}
|
||
|
||
var currentValue = occupancyGrid.GetCell(gridX, gridY);
|
||
|
||
// Merge logic
|
||
bool shouldMerge = false;
|
||
|
||
if (currentValue == -1)
|
||
{
|
||
if (occupancyValue != -1)
|
||
{
|
||
shouldMerge = true;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if (occupancyValue != -1 && occupancyValue > currentValue)
|
||
{
|
||
shouldMerge = true;
|
||
}
|
||
}
|
||
|
||
if (shouldMerge)
|
||
{
|
||
occupancyGrid.Data[gridY * occupancyGrid.Width + gridX] = occupancyValue;
|
||
mergedCells++;
|
||
}
|
||
else
|
||
{
|
||
skippedCells++;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
skippedCells++;
|
||
}
|
||
}
|
||
}
|
||
|
||
return (mergedCells, skippedCells);
|
||
}
|
||
|
||
#endregion
|
||
|
||
#region Helpers
|
||
|
||
/// <summary>
|
||
/// Extract yaw angle in degrees from quaternion (for debugging)
|
||
/// </summary>
|
||
private static double QuaternionToYawDegrees(RobotNet10.Shared.Geometry.Quaternion q)
|
||
{
|
||
// Yaw (Z-axis rotation) = atan2(2*(w*z + x*y), 1 - 2*(y*y + z*z))
|
||
var siny_cosp = 2.0 * (q.W * q.Z + q.X * q.Y);
|
||
var cosy_cosp = 1.0 - 2.0 * (q.Y * q.Y + q.Z * q.Z);
|
||
var yawRad = Math.Atan2(siny_cosp, cosy_cosp);
|
||
return yawRad * 180.0 / Math.PI;
|
||
}
|
||
|
||
#endregion
|
||
|
||
#region Texture-Based Generation
|
||
|
||
/// <summary>
|
||
/// Generate occupancy grid from MapBuilder using texture-based approach.
|
||
/// This method uses DrawToSubmapTexture (from ProbabilityGrid/TSDF2D) and SubmapPainter
|
||
/// to generate the occupancy grid, matching the C++ xloc.cc implementation exactly.
|
||
/// </summary>
|
||
/// <param name="mapBuilder">MapBuilder containing submaps</param>
|
||
/// <param name="resolution">Target grid resolution</param>
|
||
/// <param name="padding">Padding around map bounds</param>
|
||
/// <param name="logger">Optional logger</param>
|
||
/// <param name="config">Optional occupancy grid configuration for threshold and post-processing</param>
|
||
/// <returns>OccupancyGrid with ROS convention (row 0 = world bottom), or null if generation fails</returns>
|
||
public static OccupancyGrid? GenerateFromMapBuilderUsingTextures(
|
||
IMapBuilder mapBuilder,
|
||
double resolution,
|
||
double padding,
|
||
ILogger? logger = null,
|
||
OccupancyGridConfiguration? config = null)
|
||
{
|
||
try
|
||
{
|
||
var poseGraph = mapBuilder.PoseGraph;
|
||
var allSubmapData = poseGraph.GetAllSubmapData();
|
||
|
||
// Get TransformToMap and compute its inverse for applying to all poses
|
||
// This matches C++ xloc.cc behavior where poses are transformed using:
|
||
// transformedPose = GetTransformToMap().inverse() * pose
|
||
var transformToMap = poseGraph.GetTransformToMap();
|
||
var transformToMapInverse = transformToMap.Inverse();
|
||
|
||
// Collect submap slices
|
||
var submapSlices = new Dictionary<SubmapId, SubmapSlice>();
|
||
int submapIndexDebug = 0;
|
||
|
||
foreach (var idDataRef in allSubmapData)
|
||
{
|
||
var submapData = idDataRef.Data;
|
||
if (submapData.Submap is Submap2D submap2D)
|
||
{
|
||
var submapId = idDataRef.Id;
|
||
var trajectoryId = submapId.TrajectoryId;
|
||
|
||
// Apply TransformToMap.Inverse() to transform from internal coordinate to map frame
|
||
// This matches C++ xloc.cc: GetTransformToMap().inverse() * pose
|
||
var localPose = submap2D.LocalPose;
|
||
Rigid3d globalPose = transformToMapInverse * submapData.Pose;
|
||
|
||
// Validate pose
|
||
if (!globalPose.IsValid())
|
||
{
|
||
logger?.LogWarning(
|
||
"OccupancyGridGenerator: Submap {TrajectoryId}:{SubmapIndex} has invalid pose, attempting fallback",
|
||
submapId.TrajectoryId, submapId.SubmapIndex);
|
||
|
||
var localToGlobal = poseGraph.GetLocalToGlobalTransform(trajectoryId);
|
||
var globalPoseComputed = transformToMapInverse * (localToGlobal * localPose);
|
||
|
||
if (globalPoseComputed.IsValid())
|
||
{
|
||
globalPose = globalPoseComputed;
|
||
}
|
||
else
|
||
{
|
||
logger?.LogError(
|
||
"OccupancyGridGenerator: Both poses invalid for submap {TrajectoryId}:{SubmapIndex}, skipping",
|
||
submapId.TrajectoryId, submapId.SubmapIndex);
|
||
continue;
|
||
}
|
||
}
|
||
|
||
// Create submap slice using DrawToSubmapTexture
|
||
try
|
||
{
|
||
var slice = SubmapPainter.CreateSubmapSlice(submap2D, globalPose);
|
||
if (slice.PixelData != null && slice.Width > 0 && slice.Height > 0)
|
||
{
|
||
submapSlices[submapId] = slice;
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
logger?.LogWarning(ex,
|
||
"OccupancyGridGenerator: Failed to create slice for submap {TrajectoryId}:{SubmapIndex}",
|
||
submapId.TrajectoryId, submapId.SubmapIndex);
|
||
}
|
||
|
||
submapIndexDebug++;
|
||
}
|
||
}
|
||
|
||
if (submapSlices.Count == 0)
|
||
{
|
||
logger?.LogWarning("OccupancyGridGenerator: No valid submap slices created");
|
||
return null;
|
||
}
|
||
|
||
// Paint all submap slices into combined image
|
||
var paintResult = SubmapPainter.PaintSubmapSlices(submapSlices, resolution);
|
||
if (paintResult == null)
|
||
{
|
||
logger?.LogWarning("OccupancyGridGenerator: PaintSubmapSlices returned null");
|
||
return null;
|
||
}
|
||
|
||
// Add padding to the result
|
||
var paddingPixels = (int)Math.Ceiling(padding / resolution);
|
||
var paddedWidth = paintResult.Width + 2 * paddingPixels;
|
||
var paddedHeight = paintResult.Height + 2 * paddingPixels;
|
||
|
||
// Create occupancy grid with padding
|
||
// paintResult.Origin.Y is the world Y at the TOP of the paint image
|
||
// OccupancyGrid origin must be at the BOTTOM-LEFT (ROS convention)
|
||
// Match C++: origin.y = (-height + originY_device) * resolution
|
||
// The bottom of the image in world Y = paintResult.Origin.Y - height * resolution
|
||
// Then subtract extra padding below
|
||
var origin = new Pose
|
||
{
|
||
Position = new Vector3(
|
||
paintResult.Origin.X - padding,
|
||
paintResult.Origin.Y - paintResult.Height * resolution - padding,
|
||
0.0),
|
||
Orientation = new Quaternion(0, 0, 0, 1)
|
||
};
|
||
|
||
var occupancyGrid = new OccupancyGrid(resolution, paddedWidth, paddedHeight, origin);
|
||
|
||
// Convert paint result to occupancy values with optional config
|
||
var occupancyValues = ConvertToOccupancyValuesWithConfig(paintResult, config);
|
||
|
||
// Copy occupancy values with padding offset
|
||
// Note: PaintSubmapSlices uses image coordinates (Y increases downward)
|
||
// OccupancyGrid uses ROS convention (row 0 = world bottom, Y increases upward)
|
||
// So we need to flip Y when copying
|
||
for (int py = 0; py < paintResult.Height; py++)
|
||
{
|
||
for (int px = 0; px < paintResult.Width; px++)
|
||
{
|
||
var srcIndex = py * paintResult.Width + px;
|
||
var occupancyValue = occupancyValues[srcIndex];
|
||
|
||
// Flip Y for ROS convention
|
||
var gridX = px + paddingPixels;
|
||
var gridY = (paintResult.Height - 1 - py) + paddingPixels;
|
||
|
||
if (gridX >= 0 && gridX < paddedWidth && gridY >= 0 && gridY < paddedHeight)
|
||
{
|
||
var dstIndex = gridY * paddedWidth + gridX;
|
||
occupancyGrid.Data[dstIndex] = occupancyValue;
|
||
}
|
||
}
|
||
}
|
||
|
||
// Apply post-processing if configured
|
||
if (config != null)
|
||
{
|
||
ApplyPostProcessing(occupancyGrid, config, logger);
|
||
}
|
||
|
||
logger?.LogDebug(
|
||
"OccupancyGridGenerator: Generated grid {W}x{H} from {Count} submaps using DrawToSubmapTexture",
|
||
paddedWidth, paddedHeight, submapSlices.Count);
|
||
|
||
return occupancyGrid;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
logger?.LogError(ex, "OccupancyGridGenerator: Failed to generate occupancy grid using textures");
|
||
return null;
|
||
}
|
||
}
|
||
|
||
#endregion
|
||
|
||
#region Config-Based Conversion
|
||
|
||
/// <summary>
|
||
/// Convert paint result to occupancy values using configuration thresholds.
|
||
/// </summary>
|
||
private static sbyte[] ConvertToOccupancyValuesWithConfig(
|
||
PaintSubmapSlicesResult paintResult,
|
||
OccupancyGridConfiguration? config)
|
||
{
|
||
// Use default conversion if no config provided
|
||
if (config == null)
|
||
{
|
||
return SubmapPainter.ConvertToOccupancyValues(paintResult);
|
||
}
|
||
|
||
var occupancyValues = new sbyte[paintResult.Width * paintResult.Height];
|
||
|
||
for (int i = 0; i < paintResult.PixelData.Length; i++)
|
||
{
|
||
var pixel = paintResult.PixelData[i];
|
||
// Match C++ pixel format: (alpha << 24) | (intensity/color << 16) | (observed << 8) | 0
|
||
var color = (int)((pixel >> 16) & 0xFF); // RED channel = intensity/color
|
||
var observed = (int)((pixel >> 8) & 0xFF); // GREEN channel = observed flag
|
||
var alpha = (int)((pixel >> 24) & 0xFF); // ALPHA channel
|
||
|
||
if (observed == 0)
|
||
{
|
||
// Unknown cell - not observed
|
||
occupancyValues[i] = -1;
|
||
continue;
|
||
}
|
||
|
||
// Calculate texture-like values for threshold comparison
|
||
// logOddsInteger approximation from color: color=0 → high occupied, color=255 → high free
|
||
// delta = 128 - logOddsInteger
|
||
// If color is high (white/free), delta is positive → textureValue = delta, textureAlpha = 0
|
||
// If color is low (black/occupied), delta is negative → textureValue = 0, textureAlpha = -delta
|
||
|
||
// Approximate: textureValue ≈ color (for free space)
|
||
// textureAlpha ≈ 255 - color (for occupied space)
|
||
int textureValue = color;
|
||
int textureAlpha = alpha > 0 ? alpha : (color < 128 ? 128 - color : 0);
|
||
|
||
sbyte occupancyValue;
|
||
|
||
if (config.UseBinaryOutput)
|
||
{
|
||
// Binary output mode with configurable thresholds
|
||
if (textureAlpha > config.OccupiedSpaceThreshold)
|
||
{
|
||
// Occupied space
|
||
occupancyValue = 100;
|
||
}
|
||
else if (textureValue >= config.FreeSpaceThreshold)
|
||
{
|
||
// Free space
|
||
occupancyValue = 0;
|
||
}
|
||
else
|
||
{
|
||
// Ambiguous cell - check probability range
|
||
// Approximate probability from color: p ≈ 1 - color/255
|
||
double approxProbability = 1.0 - color / 255.0;
|
||
|
||
if (approxProbability >= config.AmbiguousRangeLower &&
|
||
approxProbability <= config.AmbiguousRangeUpper)
|
||
{
|
||
// In ambiguous range - use configured value
|
||
occupancyValue = config.AmbiguousCellValue;
|
||
}
|
||
else if (approxProbability < config.AmbiguousRangeLower)
|
||
{
|
||
// Below ambiguous range = more likely free
|
||
occupancyValue = 0;
|
||
}
|
||
else
|
||
{
|
||
// Above ambiguous range = more likely occupied
|
||
occupancyValue = 100;
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// Gradient output mode
|
||
// Match C++ formula: occupancy = round((1 - color/255) * 100)
|
||
var occupancy = (int)Math.Round((1.0 - color / 255.0) * 100.0);
|
||
occupancyValue = (sbyte)Math.Clamp(occupancy, 0, 100);
|
||
}
|
||
|
||
occupancyValues[i] = occupancyValue;
|
||
}
|
||
|
||
return occupancyValues;
|
||
}
|
||
|
||
#endregion
|
||
|
||
#region Post-Processing
|
||
|
||
/// <summary>
|
||
/// Apply post-processing operations based on configuration.
|
||
/// </summary>
|
||
private static void ApplyPostProcessing(
|
||
OccupancyGrid grid,
|
||
OccupancyGridConfiguration config,
|
||
ILogger? logger)
|
||
{
|
||
// Apply median filter first (noise reduction)
|
||
if (config.EnableMedianFilter)
|
||
{
|
||
ApplyMedianFilter(grid, config.MedianFilterKernelSize);
|
||
logger?.LogDebug("OccupancyGridGenerator: Applied median filter with kernel size {Size}",
|
||
config.MedianFilterKernelSize);
|
||
}
|
||
|
||
// Apply wall thinning (erosion)
|
||
if (config.EnableWallThinning && config.WallThinningIterations > 0)
|
||
{
|
||
for (int i = 0; i < config.WallThinningIterations; i++)
|
||
{
|
||
ApplyWallThinning(grid, config.MinWallThicknessPixels);
|
||
}
|
||
logger?.LogDebug("OccupancyGridGenerator: Applied wall thinning with {Iterations} iterations",
|
||
config.WallThinningIterations);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Apply median filter to reduce noise.
|
||
/// </summary>
|
||
private static void ApplyMedianFilter(OccupancyGrid grid, int kernelSize)
|
||
{
|
||
if (kernelSize < 3 || kernelSize % 2 == 0)
|
||
kernelSize = 3; // Ensure odd kernel size
|
||
|
||
var halfKernel = kernelSize / 2;
|
||
var newData = new sbyte[grid.Data.Length];
|
||
Array.Copy(grid.Data, newData, grid.Data.Length);
|
||
|
||
var neighbors = new List<sbyte>(kernelSize * kernelSize);
|
||
|
||
for (int y = halfKernel; y < grid.Height - halfKernel; y++)
|
||
{
|
||
for (int x = halfKernel; x < grid.Width - halfKernel; x++)
|
||
{
|
||
neighbors.Clear();
|
||
|
||
// Collect neighbors
|
||
for (int ky = -halfKernel; ky <= halfKernel; ky++)
|
||
{
|
||
for (int kx = -halfKernel; kx <= halfKernel; kx++)
|
||
{
|
||
var idx = (y + ky) * grid.Width + (x + kx);
|
||
var value = grid.Data[idx];
|
||
if (value != -1) // Only consider known cells
|
||
{
|
||
neighbors.Add(value);
|
||
}
|
||
}
|
||
}
|
||
|
||
// Apply median if enough neighbors
|
||
if (neighbors.Count >= kernelSize)
|
||
{
|
||
neighbors.Sort();
|
||
var median = neighbors[neighbors.Count / 2];
|
||
newData[y * grid.Width + x] = median;
|
||
}
|
||
}
|
||
}
|
||
|
||
Array.Copy(newData, grid.Data, grid.Data.Length);
|
||
}
|
||
|
||
/// <summary>
|
||
/// Apply morphological erosion to thin walls.
|
||
/// Only erodes occupied cells (value = 100) that have free neighbors.
|
||
/// </summary>
|
||
private static void ApplyWallThinning(OccupancyGrid grid, int minThickness)
|
||
{
|
||
var newData = new sbyte[grid.Data.Length];
|
||
Array.Copy(grid.Data, newData, grid.Data.Length);
|
||
|
||
// 4-connectivity erosion kernel (up, down, left, right)
|
||
int[] dx = { 0, 0, -1, 1 };
|
||
int[] dy = { -1, 1, 0, 0 };
|
||
|
||
for (int y = 1; y < grid.Height - 1; y++)
|
||
{
|
||
for (int x = 1; x < grid.Width - 1; x++)
|
||
{
|
||
var idx = y * grid.Width + x;
|
||
var value = grid.Data[idx];
|
||
|
||
// Only process occupied cells
|
||
if (value != 100)
|
||
continue;
|
||
|
||
// Check if this cell should be eroded
|
||
// Count free neighbors
|
||
int freeNeighbors = 0;
|
||
int occupiedNeighbors = 0;
|
||
|
||
for (int d = 0; d < 4; d++)
|
||
{
|
||
var nx = x + dx[d];
|
||
var ny = y + dy[d];
|
||
var neighborIdx = ny * grid.Width + nx;
|
||
var neighborValue = grid.Data[neighborIdx];
|
||
|
||
if (neighborValue == 0)
|
||
freeNeighbors++;
|
||
else if (neighborValue == 100)
|
||
occupiedNeighbors++;
|
||
}
|
||
|
||
// Erode if:
|
||
// 1. Has at least one free neighbor (is on wall boundary)
|
||
// 2. Has enough occupied neighbors to maintain minimum thickness
|
||
if (freeNeighbors > 0 && occupiedNeighbors >= minThickness)
|
||
{
|
||
// Check perpendicular thickness to ensure we don't break thin walls
|
||
bool canErode = true;
|
||
|
||
// Check horizontal thickness
|
||
if (freeNeighbors == 1 || freeNeighbors == 2)
|
||
{
|
||
int hThickness = 1;
|
||
for (int tx = x - 1; tx >= 0 && grid.Data[y * grid.Width + tx] == 100; tx--)
|
||
hThickness++;
|
||
for (int tx = x + 1; tx < grid.Width && grid.Data[y * grid.Width + tx] == 100; tx++)
|
||
hThickness++;
|
||
|
||
int vThickness = 1;
|
||
for (int ty = y - 1; ty >= 0 && grid.Data[ty * grid.Width + x] == 100; ty--)
|
||
vThickness++;
|
||
for (int ty = y + 1; ty < grid.Height && grid.Data[ty * grid.Width + x] == 100; ty++)
|
||
vThickness++;
|
||
|
||
// Don't erode if it would make wall too thin
|
||
if (Math.Min(hThickness, vThickness) <= minThickness)
|
||
canErode = false;
|
||
}
|
||
|
||
if (canErode)
|
||
{
|
||
newData[idx] = 0; // Erode to free space
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
Array.Copy(newData, grid.Data, grid.Data.Length);
|
||
}
|
||
|
||
#endregion
|
||
|
||
#region Log-Odds Based Generation
|
||
|
||
/// <summary>
|
||
/// Generate occupancy grid from MapBuilder using log-odds summation (Bayesian approach).
|
||
/// This method directly reads probability values from each submap's ProbabilityGrid,
|
||
/// sums log-odds for overlapping cells, and converts back to occupancy values.
|
||
/// Provides clearer free/occupied distinction compared to texture-based blending.
|
||
/// </summary>
|
||
/// <param name="mapBuilder">MapBuilder containing submaps</param>
|
||
/// <param name="resolution">Target grid resolution</param>
|
||
/// <param name="padding">Padding around map bounds</param>
|
||
/// <param name="logger">Optional logger</param>
|
||
/// <param name="config">Configuration for thresholds and log-odds clamp</param>
|
||
/// <returns>OccupancyGrid with ROS convention (row 0 = world bottom), or null if generation fails</returns>
|
||
public static OccupancyGrid? GenerateFromMapBuilderUsingLogOdds(
|
||
IMapBuilder mapBuilder,
|
||
double resolution,
|
||
double padding,
|
||
ILogger? logger = null,
|
||
OccupancyGridConfiguration? config = null,
|
||
MapById<SubmapId, IPoseGraph.SubmapData>? prefetchedSubmapData = null,
|
||
Rigid3d? prefetchedTransformToMap = null)
|
||
{
|
||
try
|
||
{
|
||
var poseGraph = mapBuilder.PoseGraph;
|
||
var allSubmapData = prefetchedSubmapData ?? poseGraph.GetAllSubmapData();
|
||
|
||
// Get TransformToMap inverse for coordinate transformation
|
||
// NOTE: During ScanMapping, TransformToMap should be Identity.
|
||
// The laser scan and robot pose from GlobalTrajectoryBuilder use localToGlobal only
|
||
// (optimization frame), so TransformToMap.Inverse() must equal Identity for alignment.
|
||
// If non-identity, the occupancy grid will be in a different frame from pose/laser.
|
||
var transformToMap = prefetchedTransformToMap ?? poseGraph.GetTransformToMap();
|
||
var transformToMapInverse = transformToMap.Inverse();
|
||
|
||
// Warn if TransformToMap is non-identity (could cause frame mismatch with pose/laser)
|
||
if (Math.Abs(transformToMap.Translation.X) > 0.001 ||
|
||
Math.Abs(transformToMap.Translation.Y) > 0.001 ||
|
||
Math.Abs(transformToMap.Translation.Z) > 0.001 ||
|
||
Math.Abs(transformToMap.Rotation.X) > 0.001 ||
|
||
Math.Abs(transformToMap.Rotation.Y) > 0.001 ||
|
||
Math.Abs(transformToMap.Rotation.Z) > 0.001)
|
||
{
|
||
logger?.LogWarning(
|
||
"OccupancyGridGenerator: TransformToMap is NON-IDENTITY! " +
|
||
"Grid frame may not match pose/laser frame. " +
|
||
"T=[{TX:F4},{TY:F4},{TZ:F4}], R=[{RW:F4},{RX:F4},{RY:F4},{RZ:F4}]",
|
||
transformToMap.Translation.X, transformToMap.Translation.Y, transformToMap.Translation.Z,
|
||
transformToMap.Rotation.W, transformToMap.Rotation.X, transformToMap.Rotation.Y, transformToMap.Rotation.Z);
|
||
}
|
||
|
||
// Collect all 2D submaps with their local-to-map transforms.
|
||
// IMPORTANT: We use LocalToGlobalTransform (NOT SubmapData.Pose) because:
|
||
// - GetCellCenter() returns coordinates in LOCAL MAP FRAME (trajectory local frame)
|
||
// - NOT in submap frame as previously assumed
|
||
// - localToMapTransform = TransformToMapInverse * LocalToGlobalTransform
|
||
// For active (non-finished) submaps, snapshot the grid cell data to avoid
|
||
// race conditions with concurrent InsertRangeData on the sensor thread.
|
||
// Finished submaps are immutable and can be read directly.
|
||
var submap2DList = new List<(Submap2D Submap, Rigid3d LocalToMapTransform, Grid2D.GridCellSnapshot? Snapshot)>();
|
||
|
||
foreach (var idDataRef in allSubmapData)
|
||
{
|
||
var submapData = idDataRef.Data;
|
||
if (submapData.Submap is Submap2D submap2D)
|
||
{
|
||
var submapId = idDataRef.Id;
|
||
var trajectoryId = submapId.TrajectoryId;
|
||
|
||
// FIX: Use LocalToGlobalTransform instead of SubmapData.Pose
|
||
// Cell coordinates from GetCellCenter() are in LOCAL MAP FRAME (L),
|
||
// so we need T_map_local = TransformToMapInverse * LocalToGlobalTransform
|
||
var localToGlobal = poseGraph.GetLocalToGlobalTransform(trajectoryId);
|
||
Rigid3d localToMapTransform = transformToMapInverse * localToGlobal;
|
||
|
||
// Validate transform
|
||
if (!localToMapTransform.IsValid())
|
||
{
|
||
logger?.LogWarning(
|
||
"OccupancyGridGenerator: Skipping submap {TrajectoryId}:{SubmapIndex} with invalid localToMapTransform",
|
||
submapId.TrajectoryId, submapId.SubmapIndex);
|
||
continue;
|
||
}
|
||
|
||
// Snapshot active submaps to decouple from sensor thread writes
|
||
Grid2D.GridCellSnapshot? snapshot = null;
|
||
if (!submap2D.InsertionFinished && submap2D.Grid != null)
|
||
{
|
||
snapshot = submap2D.Grid.SnapshotCellData();
|
||
}
|
||
|
||
submap2DList.Add((submap2D, localToMapTransform, snapshot));
|
||
}
|
||
}
|
||
|
||
if (submap2DList.Count == 0)
|
||
{
|
||
logger?.LogWarning("OccupancyGridGenerator: No 2D submaps found for log-odds generation");
|
||
return null;
|
||
}
|
||
|
||
// Log per-submap details for debugging
|
||
for (int si = 0; si < submap2DList.Count; si++)
|
||
{
|
||
var (sm, localToMap, snap) = submap2DList[si];
|
||
var smGrid = sm.Grid;
|
||
var gridType = smGrid?.GetType().Name ?? "null";
|
||
var numCellsX = smGrid?.Limits.CellLimits.NumXCells ?? 0;
|
||
var numCellsY = smGrid?.Limits.CellLimits.NumYCells ?? 0;
|
||
|
||
// Get local pose and grid max point for debugging
|
||
var localPose = sm.LocalPose;
|
||
var gridMaxX = smGrid?.Limits.Max.X ?? 0;
|
||
var gridMaxY = smGrid?.Limits.Max.Y ?? 0;
|
||
}
|
||
|
||
// Calculate bounds from all submaps
|
||
var (minX, minY, maxX, maxY) = CalculateBoundsFromSubmaps(submap2DList, logger);
|
||
|
||
// Validate bounds
|
||
if (minX >= maxX || minY >= maxY || double.IsInfinity(minX) || double.IsInfinity(maxX) ||
|
||
double.IsInfinity(minY) || double.IsInfinity(maxY))
|
||
{
|
||
logger?.LogWarning(
|
||
"OccupancyGridGenerator: Invalid bounds for log-odds generation: " +
|
||
"min=[{MinX:F3},{MinY:F3}], max=[{MaxX:F3},{MaxY:F3}]",
|
||
minX, minY, maxX, maxY);
|
||
return null;
|
||
}
|
||
|
||
// Add padding
|
||
var adjustedMinX = minX - padding;
|
||
var adjustedMinY = minY - padding;
|
||
var adjustedMaxX = maxX + padding;
|
||
var adjustedMaxY = maxY + padding;
|
||
|
||
var width = (int)Math.Ceiling((adjustedMaxX - adjustedMinX) / resolution);
|
||
var height = (int)Math.Ceiling((adjustedMaxY - adjustedMinY) / resolution);
|
||
|
||
// Set origin to bottom-left corner (ROS convention)
|
||
var gridOriginPosition = new Vector3(adjustedMinX, adjustedMinY, 0.0);
|
||
var origin = new Pose
|
||
{
|
||
Position = gridOriginPosition,
|
||
Orientation = new Quaternion(0, 0, 0, 1)
|
||
};
|
||
|
||
// Create accumulation buffers
|
||
// logOddsSum: sum of log-odds for each cell
|
||
// observationCount: number of observations per cell
|
||
var logOddsSum = new double[width * height];
|
||
var observationCount = new int[width * height];
|
||
|
||
// Initialize to zero (prior = 0.5, logOdds = 0)
|
||
Array.Fill(logOddsSum, 0.0);
|
||
Array.Fill(observationCount, 0);
|
||
|
||
// Get log-odds clamp value
|
||
var logOddsClamp = config?.LogOddsClamp ?? 10.0;
|
||
|
||
// Merge all submaps using log-odds summation.
|
||
// Active submaps use their pre-captured snapshot to avoid racing with
|
||
// concurrent InsertRangeData; finished submaps read the grid directly.
|
||
foreach (var (submap2D, localToMapTransform, snapshot) in submap2DList)
|
||
{
|
||
if (snapshot != null)
|
||
{
|
||
// Active submap → use thread-safe snapshot
|
||
MergeSnapshotUsingLogOdds(
|
||
snapshot, localToMapTransform,
|
||
logOddsSum, observationCount,
|
||
width, height,
|
||
adjustedMinX, adjustedMinY,
|
||
resolution, logOddsClamp);
|
||
}
|
||
else
|
||
{
|
||
// Finished submap → grid is immutable, safe to read directly
|
||
MergeSubmapUsingLogOdds(
|
||
submap2D, localToMapTransform,
|
||
logOddsSum, observationCount,
|
||
width, height,
|
||
adjustedMinX, adjustedMinY,
|
||
resolution, logOddsClamp, config);
|
||
}
|
||
}
|
||
|
||
// Debug: count total observations
|
||
int totalObservations = 0;
|
||
int observedCells = 0;
|
||
double minLogOdds = double.MaxValue;
|
||
double maxLogOdds = double.MinValue;
|
||
for (int i = 0; i < observationCount.Length; i++)
|
||
{
|
||
if (observationCount[i] > 0)
|
||
{
|
||
observedCells++;
|
||
totalObservations += observationCount[i];
|
||
minLogOdds = Math.Min(minLogOdds, logOddsSum[i]);
|
||
maxLogOdds = Math.Max(maxLogOdds, logOddsSum[i]);
|
||
}
|
||
}
|
||
|
||
// Convert log-odds to occupancy values
|
||
var occupancyGrid = new OccupancyGrid(resolution, width, height, origin);
|
||
var stats = ConvertLogOddsToOccupancy(
|
||
logOddsSum, observationCount,
|
||
occupancyGrid, config, logger);
|
||
|
||
// Apply post-processing if configured
|
||
if (config != null)
|
||
{
|
||
ApplyPostProcessing(occupancyGrid, config, logger);
|
||
}
|
||
|
||
return occupancyGrid;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
logger?.LogError(ex, "OccupancyGridGenerator: Failed to generate occupancy grid using log-odds");
|
||
return null;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Merge a single submap into log-odds accumulation buffers.
|
||
/// Supports both ProbabilityGrid and TSDF2D grid types.
|
||
/// </summary>
|
||
/// <param name="submap2D">The submap to merge</param>
|
||
/// <param name="localToMapTransform">Transform from LOCAL MAP FRAME to MAP FRAME</param>
|
||
private static void MergeSubmapUsingLogOdds(
|
||
Submap2D submap2D,
|
||
Rigid3d localToMapTransform,
|
||
double[] logOddsSum,
|
||
int[] observationCount,
|
||
int gridWidth,
|
||
int gridHeight,
|
||
double originX,
|
||
double originY,
|
||
double resolution,
|
||
double logOddsClamp,
|
||
OccupancyGridConfiguration? config = null)
|
||
{
|
||
var grid = submap2D.Grid;
|
||
if (grid == null)
|
||
return;
|
||
|
||
// Dispatch to appropriate handler based on grid type
|
||
if (grid is ProbabilityGrid probabilityGrid)
|
||
{
|
||
MergeSubmapUsingLogOddsFromProbabilityGrid(
|
||
probabilityGrid, localToMapTransform, logOddsSum, observationCount,
|
||
gridWidth, gridHeight, originX, originY, resolution, logOddsClamp);
|
||
}
|
||
else if (grid is TSDF2D tsdfGrid)
|
||
{
|
||
MergeSubmapUsingLogOddsFromTSDF(
|
||
tsdfGrid, localToMapTransform, logOddsSum, observationCount,
|
||
gridWidth, gridHeight, originX, originY, resolution, logOddsClamp, config);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Merge ProbabilityGrid submap into log-odds accumulation buffers.
|
||
/// </summary>
|
||
/// <param name="probabilityGrid">The probability grid to merge</param>
|
||
/// <param name="localToMapTransform">Transform from LOCAL MAP FRAME to MAP FRAME</param>
|
||
private static void MergeSubmapUsingLogOddsFromProbabilityGrid(
|
||
ProbabilityGrid probabilityGrid,
|
||
Rigid3d localToMapTransform,
|
||
double[] logOddsSum,
|
||
int[] observationCount,
|
||
int gridWidth,
|
||
int gridHeight,
|
||
double originX,
|
||
double originY,
|
||
double resolution,
|
||
double logOddsClamp)
|
||
{
|
||
var limits = probabilityGrid.Limits;
|
||
|
||
// Use ComputeCroppedLimits to get only known cells bounds
|
||
probabilityGrid.ComputeCroppedLimits(out var croppedOffset, out var croppedLimits);
|
||
|
||
if (croppedLimits.NumXCells <= 0 || croppedLimits.NumYCells <= 0)
|
||
return;
|
||
|
||
// Iterate through known cells region only
|
||
for (int y = 0; y < croppedLimits.NumYCells; y++)
|
||
{
|
||
for (int x = 0; x < croppedLimits.NumXCells; x++)
|
||
{
|
||
var cellIndex = new CartographerSharp.Common.Math.Array2i(croppedOffset.X + x, croppedOffset.Y + y);
|
||
|
||
// Skip unknown cells
|
||
if (!probabilityGrid.IsKnown(cellIndex))
|
||
continue;
|
||
|
||
// Get cell center in LOCAL MAP FRAME (NOT submap frame!)
|
||
var cellCenter = limits.GetCellCenter(cellIndex);
|
||
var cellCenterInLocalFrame = new Vector3(cellCenter.X, cellCenter.Y, 0.0);
|
||
|
||
// Transform from LOCAL MAP FRAME to MAP FRAME
|
||
var mapPoint = localToMapTransform.TransformPoint(cellCenterInLocalFrame);
|
||
|
||
// Convert to grid coordinates (ROS convention)
|
||
var gridX = (int)Math.Floor((mapPoint.X - originX) / resolution);
|
||
var gridY = (int)Math.Floor((mapPoint.Y - originY) / resolution);
|
||
|
||
if (gridX >= 0 && gridX < gridWidth && gridY >= 0 && gridY < gridHeight)
|
||
{
|
||
// Get probability value from submap
|
||
var probability = probabilityGrid.GetProbability(cellIndex);
|
||
|
||
// Clamp probability to avoid log(0) or log(infinity)
|
||
probability = Math.Clamp(probability, 0.001, 0.999);
|
||
|
||
// Convert probability to log-odds: log(p / (1 - p))
|
||
var logOdds = Math.Log(probability / (1.0 - probability));
|
||
|
||
// Clamp log-odds to prevent extreme values
|
||
logOdds = Math.Clamp(logOdds, -logOddsClamp, logOddsClamp);
|
||
|
||
// Accumulate
|
||
var idx = gridY * gridWidth + gridX;
|
||
logOddsSum[idx] += logOdds;
|
||
observationCount[idx]++;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Merge a GridCellSnapshot (from an active submap) into log-odds accumulation buffers.
|
||
/// Works identically to MergeSubmapUsingLogOddsFromProbabilityGrid but reads from the
|
||
/// snapshot arrays rather than the live grid, eliminating race conditions.
|
||
/// </summary>
|
||
/// <param name="snapshot">The grid cell snapshot to merge</param>
|
||
/// <param name="localToMapTransform">Transform from LOCAL MAP FRAME to MAP FRAME</param>
|
||
private static void MergeSnapshotUsingLogOdds(
|
||
Grid2D.GridCellSnapshot snapshot,
|
||
Rigid3d localToMapTransform,
|
||
double[] logOddsSum,
|
||
int[] observationCount,
|
||
int gridWidth,
|
||
int gridHeight,
|
||
double originX,
|
||
double originY,
|
||
double resolution,
|
||
double logOddsClamp)
|
||
{
|
||
var limits = snapshot.Limits;
|
||
snapshot.ComputeCroppedLimits(out var croppedOffset, out var croppedLimits);
|
||
|
||
if (croppedLimits.NumXCells <= 0 || croppedLimits.NumYCells <= 0)
|
||
return;
|
||
|
||
var cells = snapshot.Cells;
|
||
var numXCells = limits.CellLimits.NumXCells;
|
||
|
||
for (int y = 0; y < croppedLimits.NumYCells; y++)
|
||
{
|
||
for (int x = 0; x < croppedLimits.NumXCells; x++)
|
||
{
|
||
var cellIndex = new CartographerSharp.Common.Math.Array2i(croppedOffset.X + x, croppedOffset.Y + y);
|
||
|
||
if (!snapshot.IsKnown(cellIndex))
|
||
continue;
|
||
|
||
// Read cell value from snapshot and convert to probability
|
||
// (matches ProbabilityGrid.GetProbability: ValueToCorrespondenceCost → CorrespondenceCostToProbability)
|
||
var flatIndex = numXCells * cellIndex.Y + cellIndex.X;
|
||
if (flatIndex < 0 || flatIndex >= cells.Length)
|
||
continue;
|
||
|
||
var value = cells[flatIndex];
|
||
const ushort kUpdateMarker = (ushort)(1u << 15);
|
||
if (value >= kUpdateMarker)
|
||
value -= kUpdateMarker;
|
||
if (value == 0) continue; // unknown
|
||
|
||
var probability = ProbabilityValues.CorrespondenceCostToProbability(
|
||
ProbabilityValues.ValueToCorrespondenceCost(value));
|
||
|
||
// Get cell center in LOCAL MAP FRAME (NOT submap frame!)
|
||
var cellCenter = limits.GetCellCenter(cellIndex);
|
||
var mapPoint = localToMapTransform.TransformPoint(new Vector3(cellCenter.X, cellCenter.Y, 0.0));
|
||
|
||
var gridX = (int)Math.Floor((mapPoint.X - originX) / resolution);
|
||
var gridY = (int)Math.Floor((mapPoint.Y - originY) / resolution);
|
||
|
||
if (gridX >= 0 && gridX < gridWidth && gridY >= 0 && gridY < gridHeight)
|
||
{
|
||
probability = Math.Clamp(probability, 0.001, 0.999);
|
||
var logOdds = Math.Log(probability / (1.0 - probability));
|
||
logOdds = Math.Clamp(logOdds, -logOddsClamp, logOddsClamp);
|
||
|
||
var idx = gridY * gridWidth + gridX;
|
||
logOddsSum[idx] += logOdds;
|
||
observationCount[idx]++;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Merge TSDF2D submap into log-odds accumulation buffers.
|
||
/// Uses TSDF-specific thresholds for direct classification:
|
||
/// tsd > TsdfFreeThreshold → FREE (strong negative logOdds)
|
||
/// tsd < TsdfOccupiedThreshold → OCCUPIED (strong positive logOdds)
|
||
/// else → near surface, use linear mapping
|
||
/// Weight is used only as a minimum confidence filter.
|
||
/// </summary>
|
||
/// <param name="tsdfGrid">The TSDF grid to merge</param>
|
||
/// <param name="localToMapTransform">Transform from LOCAL MAP FRAME to MAP FRAME</param>
|
||
private static void MergeSubmapUsingLogOddsFromTSDF(
|
||
TSDF2D tsdfGrid,
|
||
Rigid3d localToMapTransform,
|
||
double[] logOddsSum,
|
||
int[] observationCount,
|
||
int gridWidth,
|
||
int gridHeight,
|
||
double originX,
|
||
double originY,
|
||
double resolution,
|
||
double logOddsClamp,
|
||
OccupancyGridConfiguration? config = null)
|
||
{
|
||
var limits = tsdfGrid.Limits;
|
||
|
||
// Get TSDF-specific thresholds from config
|
||
var tsdfMaxTsd = config?.TsdfMaxTsd ?? 0.3;
|
||
var tsdfMinWeight = config?.TsdfMinWeight ?? 0.1;
|
||
var tsdfFreeThreshold = config?.TsdfFreeThreshold ?? 0.05;
|
||
var tsdfOccupiedThreshold = config?.TsdfOccupiedThreshold ?? -0.02;
|
||
|
||
// Use ComputeCroppedLimits to get only known cells bounds
|
||
tsdfGrid.ComputeCroppedLimits(out var croppedOffset, out var croppedLimits);
|
||
|
||
if (croppedLimits.NumXCells <= 0 || croppedLimits.NumYCells <= 0)
|
||
return;
|
||
|
||
// Iterate through known cells region only
|
||
for (int y = 0; y < croppedLimits.NumYCells; y++)
|
||
{
|
||
for (int x = 0; x < croppedLimits.NumXCells; x++)
|
||
{
|
||
var cellIndex = new CartographerSharp.Common.Math.Array2i(croppedOffset.X + x, croppedOffset.Y + y);
|
||
|
||
// Skip unknown cells
|
||
if (!tsdfGrid.IsKnown(cellIndex))
|
||
continue;
|
||
|
||
// Get cell center in LOCAL MAP FRAME (NOT submap frame!)
|
||
var cellCenter = limits.GetCellCenter(cellIndex);
|
||
var cellCenterInLocalFrame = new Vector3(cellCenter.X, cellCenter.Y, 0.0);
|
||
|
||
// Transform from LOCAL MAP FRAME to MAP FRAME
|
||
var mapPoint = localToMapTransform.TransformPoint(cellCenterInLocalFrame);
|
||
|
||
// Convert to grid coordinates (ROS convention)
|
||
var gridX = (int)Math.Floor((mapPoint.X - originX) / resolution);
|
||
var gridY = (int)Math.Floor((mapPoint.Y - originY) / resolution);
|
||
|
||
if (gridX >= 0 && gridX < gridWidth && gridY >= 0 && gridY < gridHeight)
|
||
{
|
||
// Get TSD and weight from TSDF grid
|
||
var (tsd, weight) = tsdfGrid.GetTSDAndWeight(cellIndex);
|
||
|
||
// Skip cells with very low weight (not enough observations to be reliable)
|
||
if (weight < tsdfMinWeight)
|
||
continue;
|
||
|
||
// Convert TSD directly to probability using thresholds
|
||
// TSDF convention:
|
||
// tsd > 0: Free space (away from obstacles)
|
||
// tsd < 0: Occupied space (inside/near obstacles)
|
||
// tsd = 0: On obstacle surface
|
||
double probability;
|
||
if (tsd > tsdfFreeThreshold)
|
||
{
|
||
// Clearly free: map tsd [freeThreshold, maxTsd] → probability [0.2, 0.01]
|
||
var t = Math.Clamp((tsd - tsdfFreeThreshold) / (tsdfMaxTsd - tsdfFreeThreshold), 0.0, 1.0);
|
||
probability = 0.2 - t * 0.19; // 0.2 → 0.01
|
||
}
|
||
else if (tsd < tsdfOccupiedThreshold)
|
||
{
|
||
// Clearly occupied: map tsd [occupiedThreshold, -maxTsd] → probability [0.8, 0.99]
|
||
var t = Math.Clamp((tsdfOccupiedThreshold - tsd) / (tsdfMaxTsd + tsdfOccupiedThreshold), 0.0, 1.0);
|
||
probability = 0.8 + t * 0.19; // 0.8 → 0.99
|
||
}
|
||
else
|
||
{
|
||
// Near surface: steep linear mapping between thresholds
|
||
// tsd = occupiedThreshold → probability = 0.95 (deep into occupied territory)
|
||
// tsd = freeThreshold → probability = 0.05 (deep into free territory)
|
||
// This steep mapping minimizes cells in ambiguous range [0.35, 0.65]
|
||
var range = tsdfFreeThreshold - tsdfOccupiedThreshold;
|
||
if (range > 0)
|
||
{
|
||
var t = (tsd - tsdfOccupiedThreshold) / range; // 0 at occupied, 1 at free
|
||
probability = 0.95 - t * 0.9; // 0.95 → 0.05
|
||
}
|
||
else
|
||
{
|
||
probability = 0.5;
|
||
}
|
||
}
|
||
|
||
// Clamp probability to valid range
|
||
probability = Math.Clamp(probability, 0.001, 0.999);
|
||
|
||
// Convert probability to log-odds
|
||
var logOdds = Math.Log(probability / (1.0 - probability));
|
||
|
||
// Clamp log-odds
|
||
logOdds = Math.Clamp(logOdds, -logOddsClamp, logOddsClamp);
|
||
|
||
// Accumulate
|
||
var idx = gridY * gridWidth + gridX;
|
||
logOddsSum[idx] += logOdds;
|
||
observationCount[idx]++;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Convert accumulated log-odds to occupancy values.
|
||
/// Returns statistics about the conversion.
|
||
/// </summary>
|
||
private static (int freeCells, int occupiedCells, int unknownCells) ConvertLogOddsToOccupancy(
|
||
double[] logOddsSum,
|
||
int[] observationCount,
|
||
OccupancyGrid occupancyGrid,
|
||
OccupancyGridConfiguration? config,
|
||
ILogger? logger = null)
|
||
{
|
||
var useBinary = config?.UseBinaryOutput ?? true;
|
||
var ambiguousValue = config?.AmbiguousCellValue ?? (sbyte)-1;
|
||
var ambiguousLower = config?.AmbiguousRangeLower ?? 0.35;
|
||
var ambiguousUpper = config?.AmbiguousRangeUpper ?? 0.65;
|
||
var useAverage = config?.UseLogOddsAverage ?? true;
|
||
|
||
int freeCells = 0, occupiedCells = 0, unknownCells = 0;
|
||
double minProb = 1.0, maxProb = 0.0;
|
||
|
||
for (int i = 0; i < logOddsSum.Length; i++)
|
||
{
|
||
if (observationCount[i] == 0)
|
||
{
|
||
// No observations - unknown
|
||
occupancyGrid.Data[i] = -1;
|
||
unknownCells++;
|
||
continue;
|
||
}
|
||
|
||
// Convert log-odds to probability
|
||
// If useAverage=true, divide by observation count to get average log-odds
|
||
// This prevents amplification when a cell is observed by many submaps
|
||
var logOdds = useAverage ? logOddsSum[i] / observationCount[i] : logOddsSum[i];
|
||
// P = 1 / (1 + exp(-logOdds))
|
||
var probability = 1.0 / (1.0 + Math.Exp(-logOdds));
|
||
|
||
minProb = Math.Min(minProb, probability);
|
||
maxProb = Math.Max(maxProb, probability);
|
||
|
||
// Convert probability to occupancy value [0, 100]
|
||
// probability = 0 → occupancy = 0 (free)
|
||
// probability = 1 → occupancy = 100 (occupied)
|
||
var occupancy = (int)Math.Round(probability * 100.0);
|
||
|
||
if (useBinary)
|
||
{
|
||
// Binary output mode using ambiguous range thresholds directly
|
||
// probability < ambiguousLower → FREE (high confidence free)
|
||
// probability > ambiguousUpper → OCCUPIED (high confidence occupied)
|
||
// else → AMBIGUOUS
|
||
if (probability < ambiguousLower)
|
||
{
|
||
occupancyGrid.Data[i] = 0;
|
||
freeCells++;
|
||
}
|
||
else if (probability > ambiguousUpper)
|
||
{
|
||
occupancyGrid.Data[i] = 100;
|
||
occupiedCells++;
|
||
}
|
||
else
|
||
{
|
||
// Ambiguous range
|
||
occupancyGrid.Data[i] = ambiguousValue;
|
||
unknownCells++;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// Gradient output mode
|
||
occupancyGrid.Data[i] = (sbyte)Math.Clamp(occupancy, 0, 100);
|
||
if (occupancy < 35)
|
||
freeCells++;
|
||
else if (occupancy > 65)
|
||
occupiedCells++;
|
||
else
|
||
unknownCells++;
|
||
}
|
||
}
|
||
|
||
return (freeCells, occupiedCells, unknownCells);
|
||
}
|
||
|
||
#endregion
|
||
|
||
#region Max Probability Generation
|
||
|
||
/// <summary>
|
||
/// Generate occupancy grid using max probability strategy.
|
||
/// Takes the maximum (most occupied) probability for overlapping cells.
|
||
/// Conservative approach good for navigation safety.
|
||
/// </summary>
|
||
public static OccupancyGrid? GenerateFromMapBuilderUsingMaxProbability(
|
||
IMapBuilder mapBuilder,
|
||
double resolution,
|
||
double padding,
|
||
ILogger? logger = null,
|
||
OccupancyGridConfiguration? config = null)
|
||
{
|
||
try
|
||
{
|
||
var poseGraph = mapBuilder.PoseGraph;
|
||
var allSubmapData = poseGraph.GetAllSubmapData();
|
||
|
||
var transformToMap = poseGraph.GetTransformToMap();
|
||
var transformToMapInverse = transformToMap.Inverse();
|
||
|
||
// IMPORTANT: We store localToMapTransform (NOT globalPose) because:
|
||
// - GetCellCenter() returns coordinates in LOCAL MAP FRAME (trajectory local frame)
|
||
// - NOT in submap frame as previously assumed
|
||
// - localToMapTransform = TransformToMapInverse * LocalToGlobalTransform
|
||
// - This correctly transforms from local map frame to map frame
|
||
var submap2DList = new List<(Submap2D Submap, Rigid3d LocalToMapTransform)>();
|
||
|
||
foreach (var idDataRef in allSubmapData)
|
||
{
|
||
var submapData = idDataRef.Data;
|
||
if (submapData.Submap is Submap2D submap2D)
|
||
{
|
||
var submapId = idDataRef.Id;
|
||
var trajectoryId = submapId.TrajectoryId;
|
||
|
||
// FIX: Use LocalToGlobalTransform instead of SubmapData.Pose
|
||
// Cell coordinates from GetCellCenter() are in LOCAL MAP FRAME (L),
|
||
// so we need T_map_local = TransformToMapInverse * LocalToGlobalTransform
|
||
// Previously used: T_map_submap = TransformToMapInverse * SubmapData.Pose (WRONG)
|
||
var localToGlobal = poseGraph.GetLocalToGlobalTransform(trajectoryId);
|
||
Rigid3d localToMapTransform = transformToMapInverse * localToGlobal;
|
||
|
||
if (!localToMapTransform.IsValid())
|
||
{
|
||
continue;
|
||
}
|
||
|
||
submap2DList.Add((submap2D, localToMapTransform));
|
||
}
|
||
}
|
||
|
||
if (submap2DList.Count == 0)
|
||
return null;
|
||
|
||
var (minX, minY, maxX, maxY) = CalculateBoundsFromSubmaps(submap2DList);
|
||
|
||
if (minX >= maxX || minY >= maxY || double.IsInfinity(minX) || double.IsInfinity(maxX) ||
|
||
double.IsInfinity(minY) || double.IsInfinity(maxY))
|
||
return null;
|
||
|
||
var adjustedMinX = minX - padding;
|
||
var adjustedMinY = minY - padding;
|
||
var adjustedMaxX = maxX + padding;
|
||
var adjustedMaxY = maxY + padding;
|
||
|
||
var width = (int)Math.Ceiling((adjustedMaxX - adjustedMinX) / resolution);
|
||
var height = (int)Math.Ceiling((adjustedMaxY - adjustedMinY) / resolution);
|
||
|
||
var origin = new Pose
|
||
{
|
||
Position = new Vector3(adjustedMinX, adjustedMinY, 0.0),
|
||
Orientation = new Quaternion(0, 0, 0, 1)
|
||
};
|
||
|
||
// Max probability buffer (initialized to -1 meaning no observation)
|
||
var maxProbability = new double[width * height];
|
||
Array.Fill(maxProbability, -1.0);
|
||
|
||
foreach (var (submap2D, localToMapTransform) in submap2DList)
|
||
{
|
||
MergeSubmapUsingMaxProbability(
|
||
submap2D, localToMapTransform,
|
||
maxProbability,
|
||
width, height,
|
||
adjustedMinX, adjustedMinY,
|
||
resolution);
|
||
}
|
||
|
||
var occupancyGrid = new OccupancyGrid(resolution, width, height, origin);
|
||
ConvertMaxProbabilityToOccupancy(maxProbability, occupancyGrid, config);
|
||
|
||
if (config != null)
|
||
{
|
||
ApplyPostProcessing(occupancyGrid, config, logger);
|
||
}
|
||
|
||
logger?.LogDebug(
|
||
"OccupancyGridGenerator: Generated grid {W}x{H} from {Count} submaps using max probability",
|
||
width, height, submap2DList.Count);
|
||
|
||
return occupancyGrid;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
logger?.LogError(ex, "OccupancyGridGenerator: Failed to generate occupancy grid using max probability");
|
||
return null;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Merge a single submap into max probability buffer.
|
||
/// Supports both ProbabilityGrid and TSDF2D grid types.
|
||
/// IMPORTANT: localToMapTransform transforms from LOCAL MAP FRAME to map frame,
|
||
/// NOT from submap frame. GetCellCenter() returns local map frame coordinates.
|
||
/// </summary>
|
||
private static void MergeSubmapUsingMaxProbability(
|
||
Submap2D submap2D,
|
||
Rigid3d localToMapTransform,
|
||
double[] maxProbability,
|
||
int gridWidth,
|
||
int gridHeight,
|
||
double originX,
|
||
double originY,
|
||
double resolution)
|
||
{
|
||
var grid = submap2D.Grid;
|
||
if (grid == null)
|
||
return;
|
||
|
||
// Dispatch to appropriate handler based on grid type
|
||
if (grid is ProbabilityGrid probabilityGrid)
|
||
{
|
||
MergeSubmapUsingMaxProbabilityFromProbabilityGrid(
|
||
probabilityGrid, localToMapTransform, maxProbability,
|
||
gridWidth, gridHeight, originX, originY, resolution);
|
||
}
|
||
else if (grid is TSDF2D tsdfGrid)
|
||
{
|
||
MergeSubmapUsingMaxProbabilityFromTSDF(
|
||
tsdfGrid, localToMapTransform, maxProbability,
|
||
gridWidth, gridHeight, originX, originY, resolution);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Merge ProbabilityGrid submap into max probability buffer.
|
||
/// IMPORTANT: localToMapTransform transforms from LOCAL MAP FRAME to map frame.
|
||
/// GetCellCenter() returns coordinates in local map frame (NOT submap frame).
|
||
/// </summary>
|
||
private static void MergeSubmapUsingMaxProbabilityFromProbabilityGrid(
|
||
ProbabilityGrid probabilityGrid,
|
||
Rigid3d localToMapTransform,
|
||
double[] maxProbability,
|
||
int gridWidth,
|
||
int gridHeight,
|
||
double originX,
|
||
double originY,
|
||
double resolution)
|
||
{
|
||
var limits = probabilityGrid.Limits;
|
||
probabilityGrid.ComputeCroppedLimits(out var croppedOffset, out var croppedLimits);
|
||
|
||
if (croppedLimits.NumXCells <= 0 || croppedLimits.NumYCells <= 0)
|
||
return;
|
||
|
||
for (int y = 0; y < croppedLimits.NumYCells; y++)
|
||
{
|
||
for (int x = 0; x < croppedLimits.NumXCells; x++)
|
||
{
|
||
var cellIndex = new CartographerSharp.Common.Math.Array2i(croppedOffset.X + x, croppedOffset.Y + y);
|
||
|
||
if (!probabilityGrid.IsKnown(cellIndex))
|
||
continue;
|
||
|
||
// cellCenter is in LOCAL MAP FRAME (because limits.Max is set from LocalPose.Translation)
|
||
var cellCenter = limits.GetCellCenter(cellIndex);
|
||
// Transform from local map frame to map frame
|
||
var mapPoint = localToMapTransform.TransformPoint(new Vector3(cellCenter.X, cellCenter.Y, 0.0));
|
||
|
||
var gridX = (int)Math.Floor((mapPoint.X - originX) / resolution);
|
||
var gridY = (int)Math.Floor((mapPoint.Y - originY) / resolution);
|
||
|
||
if (gridX >= 0 && gridX < gridWidth && gridY >= 0 && gridY < gridHeight)
|
||
{
|
||
var probability = probabilityGrid.GetProbability(cellIndex);
|
||
var idx = gridY * gridWidth + gridX;
|
||
|
||
// Take maximum probability (most pessimistic)
|
||
if (maxProbability[idx] < 0 || probability > maxProbability[idx])
|
||
{
|
||
maxProbability[idx] = probability;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Merge TSDF2D submap into max probability buffer.
|
||
/// Converts TSD to probability: tsd > 0 (free) → low probability, tsd < 0 (occupied) → high probability
|
||
/// IMPORTANT: localToMapTransform transforms from LOCAL MAP FRAME to map frame.
|
||
/// GetCellCenter() returns coordinates in local map frame (NOT submap frame).
|
||
/// </summary>
|
||
private static void MergeSubmapUsingMaxProbabilityFromTSDF(
|
||
TSDF2D tsdfGrid,
|
||
Rigid3d localToMapTransform,
|
||
double[] maxProbability,
|
||
int gridWidth,
|
||
int gridHeight,
|
||
double originX,
|
||
double originY,
|
||
double resolution)
|
||
{
|
||
var limits = tsdfGrid.Limits;
|
||
tsdfGrid.ComputeCroppedLimits(out var croppedOffset, out var croppedLimits);
|
||
|
||
if (croppedLimits.NumXCells <= 0 || croppedLimits.NumYCells <= 0)
|
||
return;
|
||
|
||
for (int y = 0; y < croppedLimits.NumYCells; y++)
|
||
{
|
||
for (int x = 0; x < croppedLimits.NumXCells; x++)
|
||
{
|
||
var cellIndex = new CartographerSharp.Common.Math.Array2i(croppedOffset.X + x, croppedOffset.Y + y);
|
||
|
||
if (!tsdfGrid.IsKnown(cellIndex))
|
||
continue;
|
||
|
||
// cellCenter is in LOCAL MAP FRAME (because limits.Max is set from LocalPose.Translation)
|
||
var cellCenter = limits.GetCellCenter(cellIndex);
|
||
// Transform from local map frame to map frame
|
||
var mapPoint = localToMapTransform.TransformPoint(new Vector3(cellCenter.X, cellCenter.Y, 0.0));
|
||
|
||
var gridX = (int)Math.Floor((mapPoint.X - originX) / resolution);
|
||
var gridY = (int)Math.Floor((mapPoint.Y - originY) / resolution);
|
||
|
||
if (gridX >= 0 && gridX < gridWidth && gridY >= 0 && gridY < gridHeight)
|
||
{
|
||
// Get TSD and weight from TSDF grid
|
||
var (tsd, weight) = tsdfGrid.GetTSDAndWeight(cellIndex);
|
||
|
||
// Skip cells with very low weight
|
||
if (weight < 0.1)
|
||
continue;
|
||
|
||
// Convert TSD to probability
|
||
// tsd > 0: Free space → probability LOW
|
||
// tsd < 0: Occupied → probability HIGH
|
||
const double maxTsd = 0.3; // Typical truncation distance
|
||
var normalizedTsd = Math.Clamp(tsd / maxTsd, -1.0, 1.0);
|
||
|
||
// Convert to probability: tsd=1 → prob=0, tsd=-1 → prob=1
|
||
var probability = 0.5 * (1.0 - normalizedTsd);
|
||
|
||
// Scale by weight for confidence
|
||
var normalizedWeight = Math.Min(weight / 10.0, 1.0);
|
||
probability = 0.5 + (probability - 0.5) * normalizedWeight;
|
||
|
||
probability = Math.Clamp(probability, 0.0, 1.0);
|
||
|
||
var idx = gridY * gridWidth + gridX;
|
||
|
||
// Take maximum probability (most pessimistic)
|
||
if (maxProbability[idx] < 0 || probability > maxProbability[idx])
|
||
{
|
||
maxProbability[idx] = probability;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
private static void ConvertMaxProbabilityToOccupancy(
|
||
double[] maxProbability,
|
||
OccupancyGrid occupancyGrid,
|
||
OccupancyGridConfiguration? config)
|
||
{
|
||
var useBinary = config?.UseBinaryOutput ?? true;
|
||
var ambiguousLower = config?.AmbiguousRangeLower ?? 0.35;
|
||
var ambiguousUpper = config?.AmbiguousRangeUpper ?? 0.65;
|
||
var ambiguousValue = config?.AmbiguousCellValue ?? (sbyte)-1;
|
||
|
||
for (int i = 0; i < maxProbability.Length; i++)
|
||
{
|
||
if (maxProbability[i] < 0)
|
||
{
|
||
occupancyGrid.Data[i] = -1;
|
||
continue;
|
||
}
|
||
|
||
var probability = maxProbability[i];
|
||
var occupancy = (int)Math.Round(probability * 100.0);
|
||
|
||
if (useBinary)
|
||
{
|
||
if (probability < ambiguousLower)
|
||
occupancyGrid.Data[i] = 0;
|
||
else if (probability > ambiguousUpper)
|
||
occupancyGrid.Data[i] = 100;
|
||
else
|
||
occupancyGrid.Data[i] = ambiguousValue;
|
||
}
|
||
else
|
||
{
|
||
occupancyGrid.Data[i] = (sbyte)Math.Clamp(occupancy, 0, 100);
|
||
}
|
||
}
|
||
}
|
||
|
||
#endregion
|
||
|
||
#region Strategy Dispatcher
|
||
|
||
/// <summary>
|
||
/// Generate occupancy grid using the configured merge strategy.
|
||
/// This is the main entry point that dispatches to the appropriate implementation.
|
||
/// </summary>
|
||
public static OccupancyGrid? Generate(
|
||
IMapBuilder mapBuilder,
|
||
double resolution,
|
||
double padding,
|
||
ILogger? logger = null,
|
||
OccupancyGridConfiguration? config = null)
|
||
=> Generate(mapBuilder, resolution, padding, -1, out _, logger, config);
|
||
|
||
/// <summary>
|
||
/// Generate occupancy grid with version-based change detection.
|
||
/// If <paramref name="lastGeneratedVersion"/> >= 0 and no new nodes have been
|
||
/// inserted since that version, returns null immediately (skips regeneration).
|
||
/// <paramref name="snapshotVersion"/> receives the PoseGraph version at the time
|
||
/// of the snapshot so the caller can cache it for the next call.
|
||
/// </summary>
|
||
public static OccupancyGrid? Generate(
|
||
IMapBuilder mapBuilder,
|
||
double resolution,
|
||
double padding,
|
||
int lastGeneratedVersion,
|
||
out int snapshotVersion,
|
||
ILogger? logger = null,
|
||
OccupancyGridConfiguration? config = null)
|
||
{
|
||
snapshotVersion = 0;
|
||
var strategy = config?.MergeStrategy ?? SubmapMergeStrategy.LogOddsSum;
|
||
|
||
// Non-blocking snapshot: try to get submap data without blocking AddSensorData.
|
||
// TryGetSubmapSnapshot uses Monitor.TryEnter(0) on PoseGraph._dataLock.
|
||
// If the lock is busy (e.g., during optimization or AddNode), we skip this generation
|
||
// and the next 2-second cycle will retry.
|
||
var poseGraph = mapBuilder.PoseGraph;
|
||
MapById<SubmapId, IPoseGraph.SubmapData>? allSubmapData;
|
||
Rigid3d transformToMap;
|
||
|
||
if (poseGraph is CartographerSharp.Mapping.Internal.D2D.PoseGraph2D pg2d)
|
||
{
|
||
if (!pg2d.TryGetSubmapSnapshot(out allSubmapData, out transformToMap, out snapshotVersion))
|
||
{
|
||
logger?.LogDebug("OccupancyGridGenerator.Generate: Skipped - PoseGraph lock busy");
|
||
return null;
|
||
}
|
||
|
||
// Skip regeneration if no new nodes have been inserted since the last generation.
|
||
if (lastGeneratedVersion >= 0 && snapshotVersion == lastGeneratedVersion)
|
||
{
|
||
logger?.LogDebug(
|
||
"OccupancyGridGenerator.Generate: Skipped - no new data (version={Version})",
|
||
snapshotVersion);
|
||
return null;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
allSubmapData = poseGraph.GetAllSubmapData();
|
||
transformToMap = poseGraph.GetTransformToMap();
|
||
}
|
||
|
||
if (allSubmapData == null)
|
||
return null;
|
||
|
||
int totalSubmaps = 0;
|
||
int submap2DCount = 0;
|
||
foreach (var idDataRef in allSubmapData)
|
||
{
|
||
totalSubmaps++;
|
||
if (idDataRef.Data.Submap is CartographerSharp.Mapping.D2D.Submap2D)
|
||
submap2DCount++;
|
||
}
|
||
|
||
if (submap2DCount == 0)
|
||
{
|
||
logger?.LogWarning("OccupancyGridGenerator.Generate: No Submap2D found, returning null");
|
||
return null;
|
||
}
|
||
|
||
// Pass pre-fetched submap data to strategy methods to avoid double GetAllSubmapData() call
|
||
return strategy switch
|
||
{
|
||
SubmapMergeStrategy.LogOddsSum => GenerateFromMapBuilderUsingLogOdds(
|
||
mapBuilder, resolution, padding, logger, config, allSubmapData, transformToMap),
|
||
|
||
SubmapMergeStrategy.MaxProbability => GenerateFromMapBuilderUsingMaxProbability(
|
||
mapBuilder, resolution, padding, logger, config),
|
||
|
||
SubmapMergeStrategy.PorterDuff => GenerateFromMapBuilderUsingTextures(
|
||
mapBuilder, resolution, padding, logger, config),
|
||
|
||
_ => GenerateFromMapBuilderUsingLogOdds(
|
||
mapBuilder, resolution, padding, logger, config, allSubmapData, transformToMap)
|
||
};
|
||
}
|
||
|
||
#endregion
|
||
}
|