Initial commit
This commit is contained in:
@@ -0,0 +1,619 @@
|
||||
/*
|
||||
* Copyright 2017 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.Mapping;
|
||||
using CartographerSharp.Mapping.D2D;
|
||||
using CartographerSharp.Transform;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
using System.IO.Compression;
|
||||
using System.Runtime.CompilerServices;
|
||||
|
||||
// Use aliases to avoid ambiguity between Mapping.D2D.Submap2D and Models.Mapping.Submap2D
|
||||
using Submap2DClass = CartographerSharp.Mapping.D2D.Submap2D;
|
||||
using SubmapQueryModel = CartographerSharp.Models.Mapping.SubmapQuery;
|
||||
|
||||
namespace CartographerSharp.IO;
|
||||
|
||||
/// <summary>
|
||||
/// Represents unpacked texture pixel data.
|
||||
/// Match C++: SubmapTexture::Pixels
|
||||
/// </summary>
|
||||
public readonly struct SubmapTexturePixels
|
||||
{
|
||||
public readonly byte[] Intensity;
|
||||
public readonly byte[] Alpha;
|
||||
|
||||
public SubmapTexturePixels(byte[] intensity, byte[] alpha)
|
||||
{
|
||||
Intensity = intensity;
|
||||
Alpha = alpha;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Represents a submap slice ready for painting.
|
||||
/// Match C++: SubmapSlice
|
||||
/// </summary>
|
||||
public class SubmapSlice
|
||||
{
|
||||
// Texture data
|
||||
public int Width { get; set; }
|
||||
public int Height { get; set; }
|
||||
public int Version { get; set; }
|
||||
public double Resolution { get; set; }
|
||||
public Rigid3d SlicePose { get; set; }
|
||||
|
||||
// Pixel data (ARGB format, uint32 per pixel)
|
||||
public uint[]? PixelData { get; set; }
|
||||
|
||||
// Metadata
|
||||
public Rigid3d Pose { get; set; }
|
||||
public int MetadataVersion { get; set; } = -1;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Result of painting submap slices.
|
||||
/// Match C++: PaintSubmapSlicesResult
|
||||
/// </summary>
|
||||
public class PaintSubmapSlicesResult
|
||||
{
|
||||
/// <summary>
|
||||
/// Pixel data in ARGB format (row-major, top-to-bottom).
|
||||
/// </summary>
|
||||
public uint[] PixelData { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Width of the result image in pixels.
|
||||
/// </summary>
|
||||
public int Width { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Height of the result image in pixels.
|
||||
/// </summary>
|
||||
public int Height { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Top-left pixel of 'surface' in map frame (world coordinates).
|
||||
/// </summary>
|
||||
public Vector2 Origin { get; }
|
||||
|
||||
public PaintSubmapSlicesResult(uint[] pixelData, int width, int height, Vector2 origin)
|
||||
{
|
||||
PixelData = pixelData;
|
||||
Width = width;
|
||||
Height = height;
|
||||
Origin = origin;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Submap painting utilities for generating occupancy grids from submap textures.
|
||||
/// Enhanced implementation matching Cairo Graphics Library quality:
|
||||
/// - Bilinear interpolation for smooth sampling
|
||||
/// - Porter-Duff Source-Over compositing for proper alpha blending
|
||||
/// - Inverse mapping for sub-pixel accuracy (no holes)
|
||||
/// - Affine transformation matrix support
|
||||
/// </summary>
|
||||
public static class SubmapPainter
|
||||
{
|
||||
private const int kPaddingPixel = 5;
|
||||
|
||||
/// <summary>
|
||||
/// Unpacks cell data as provided by DrawToSubmapTexture into intensity and alpha arrays.
|
||||
/// Match C++: UnpackTextureData
|
||||
/// </summary>
|
||||
/// <param name="compressedCells">GZip compressed cells data (value + alpha pairs)</param>
|
||||
/// <param name="width">Texture width</param>
|
||||
/// <param name="height">Texture height</param>
|
||||
/// <returns>Unpacked intensity and alpha arrays</returns>
|
||||
public static SubmapTexturePixels UnpackTextureData(IList<byte> compressedCells, int width, int height)
|
||||
{
|
||||
// Decompress GZip data
|
||||
byte[] cells;
|
||||
using (var compressedStream = new MemoryStream(compressedCells.ToArray()))
|
||||
using (var gzipStream = new GZipStream(compressedStream, CompressionMode.Decompress))
|
||||
using (var resultStream = new MemoryStream())
|
||||
{
|
||||
gzipStream.CopyTo(resultStream);
|
||||
cells = resultStream.ToArray();
|
||||
}
|
||||
|
||||
var numPixels = width * height;
|
||||
if (cells.Length != 2 * numPixels)
|
||||
{
|
||||
throw new ArgumentException(
|
||||
$"Decompressed cells size mismatch: expected {2 * numPixels}, got {cells.Length}");
|
||||
}
|
||||
|
||||
var intensity = new byte[numPixels];
|
||||
var alpha = new byte[numPixels];
|
||||
|
||||
// Match C++: cells[(i * width + j) * 2] for intensity, +1 for alpha
|
||||
for (int i = 0; i < height; i++)
|
||||
{
|
||||
for (int j = 0; j < width; j++)
|
||||
{
|
||||
var index = i * width + j;
|
||||
intensity[index] = cells[index * 2];
|
||||
alpha[index] = cells[index * 2 + 1];
|
||||
}
|
||||
}
|
||||
|
||||
return new SubmapTexturePixels(intensity, alpha);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Creates pixel data from intensity and alpha arrays.
|
||||
/// Match C++: DrawTexture (without Cairo, using raw pixel arrays)
|
||||
/// </summary>
|
||||
/// <param name="intensity">Intensity values</param>
|
||||
/// <param name="alpha">Alpha values</param>
|
||||
/// <param name="width">Texture width</param>
|
||||
/// <param name="height">Texture height</param>
|
||||
/// <returns>ARGB pixel data (uint32 per pixel)</returns>
|
||||
public static uint[] DrawTexture(byte[] intensity, byte[] alpha, int width, int height)
|
||||
{
|
||||
var pixelData = new uint[width * height];
|
||||
|
||||
for (int i = 0; i < intensity.Length; i++)
|
||||
{
|
||||
var intensityValue = intensity[i];
|
||||
var alphaValue = alpha[i];
|
||||
|
||||
// Match C++: We use the red channel to track intensity information.
|
||||
// The green channel we use to track if a cell was ever observed.
|
||||
byte observed = (intensityValue == 0 && alphaValue == 0) ? (byte)0 : (byte)255;
|
||||
|
||||
// ARGB format: (alpha << 24) | (red << 16) | (green << 8) | blue
|
||||
// Match C++: (alpha_value << 24) | (intensity_value << 16) | (observed << 8) | 0
|
||||
pixelData[i] = ((uint)alphaValue << 24) | ((uint)intensityValue << 16) | ((uint)observed << 8) | 0;
|
||||
}
|
||||
|
||||
return pixelData;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Fills a SubmapSlice from a Submap2D.
|
||||
/// Match C++: Part of FillSubmapSlice functionality
|
||||
/// </summary>
|
||||
public static SubmapSlice CreateSubmapSlice(Submap2DClass submap, Rigid3d globalPose)
|
||||
{
|
||||
var slice = new SubmapSlice
|
||||
{
|
||||
Pose = globalPose,
|
||||
MetadataVersion = submap.NumRangeData
|
||||
};
|
||||
|
||||
var grid = submap.Grid;
|
||||
if (grid == null)
|
||||
{
|
||||
return slice;
|
||||
}
|
||||
|
||||
// Get texture from grid
|
||||
SubmapQueryModel.Texture texture;
|
||||
if (grid is ProbabilityGrid probabilityGrid)
|
||||
{
|
||||
texture = probabilityGrid.DrawToSubmapTexture(submap.LocalPose);
|
||||
}
|
||||
else if (grid is TSDF2D tsdf2D)
|
||||
{
|
||||
texture = tsdf2D.DrawToSubmapTexture(submap.LocalPose);
|
||||
}
|
||||
else
|
||||
{
|
||||
throw new NotSupportedException($"Unsupported grid type: {grid.GetType().Name}");
|
||||
}
|
||||
|
||||
// Unpack texture data
|
||||
var pixels = UnpackTextureData(texture.Cells, texture.Width, texture.Height);
|
||||
|
||||
slice.Width = texture.Width;
|
||||
slice.Height = texture.Height;
|
||||
slice.Resolution = texture.Resolution;
|
||||
slice.SlicePose = texture.SlicePose;
|
||||
slice.Version = submap.NumRangeData;
|
||||
|
||||
// Draw texture to pixel data
|
||||
slice.PixelData = DrawTexture(pixels.Intensity, pixels.Alpha, texture.Width, texture.Height);
|
||||
|
||||
return slice;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Paints all submap slices into a single image using Cairo-style rendering:
|
||||
/// - Inverse mapping for sub-pixel accuracy
|
||||
/// - Bilinear interpolation for smooth sampling
|
||||
/// - Porter-Duff Source-Over compositing
|
||||
/// Match C++: PaintSubmapSlices
|
||||
/// </summary>
|
||||
/// <param name="submapSlices">Dictionary of submap slices keyed by SubmapId</param>
|
||||
/// <param name="resolution">Output resolution in meters per pixel</param>
|
||||
/// <returns>Combined image result with pixel data and origin</returns>
|
||||
public static PaintSubmapSlicesResult? PaintSubmapSlices(
|
||||
Dictionary<SubmapId, SubmapSlice> submapSlices,
|
||||
double resolution)
|
||||
{
|
||||
if (submapSlices.Count == 0)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
// First pass: compute bounding box using all corner transforms
|
||||
double minX = double.MaxValue, minY = double.MaxValue;
|
||||
double maxX = double.MinValue, maxY = double.MinValue;
|
||||
|
||||
foreach (var (_, slice) in submapSlices)
|
||||
{
|
||||
if (slice.PixelData == null || slice.Width <= 0 || slice.Height <= 0)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
// Transform the four corners of the submap texture to global coordinates
|
||||
var corners = new Vector2[]
|
||||
{
|
||||
new(0, 0),
|
||||
new(slice.Width, 0),
|
||||
new(0, slice.Height),
|
||||
new(slice.Width, slice.Height)
|
||||
};
|
||||
|
||||
// Combined transform: globalPose * slicePose
|
||||
var submapTransform = slice.Pose * slice.SlicePose;
|
||||
|
||||
foreach (var corner in corners)
|
||||
{
|
||||
// Convert pixel coordinates to submap local coordinates
|
||||
// Match C++ Cairo matrix: cairo_matrix_init(&matrix, homo(1,0), homo(0,0),
|
||||
// -homo(1,1), -homo(0,1), homo(0,3), -homo(1,3))
|
||||
// In Cartographer's grid convention:
|
||||
// x-index (column) corresponds to world Y axis (decreasing)
|
||||
// y-index (row) corresponds to world X axis (decreasing)
|
||||
// So pixel (col, row) maps to local (-row * res, -col * res) + slice_pose translation
|
||||
var localPoint = new Vector3(
|
||||
-corner.Y * slice.Resolution,
|
||||
-corner.X * slice.Resolution,
|
||||
0);
|
||||
|
||||
// Transform to global coordinates
|
||||
var globalPoint = submapTransform.TransformPoint(localPoint);
|
||||
|
||||
// Update bounding box
|
||||
// Match C++: cairo uses (x, -y) convention for map coordinates
|
||||
var mapX = globalPoint.X / resolution;
|
||||
var mapY = -globalPoint.Y / resolution;
|
||||
|
||||
minX = Math.Min(minX, mapX);
|
||||
minY = Math.Min(minY, mapY);
|
||||
maxX = Math.Max(maxX, mapX);
|
||||
maxY = Math.Max(maxY, mapY);
|
||||
}
|
||||
}
|
||||
|
||||
if (minX >= maxX || minY >= maxY)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
// Calculate output size with padding
|
||||
var width = (int)Math.Ceiling(maxX - minX) + 2 * kPaddingPixel;
|
||||
var height = (int)Math.Ceiling(maxY - minY) + 2 * kPaddingPixel;
|
||||
|
||||
// Origin offset (translation to apply to bring min corner to (padding, padding))
|
||||
var originX = -minX + kPaddingPixel;
|
||||
var originY = -minY + kPaddingPixel;
|
||||
|
||||
// Create output pixel buffer
|
||||
// Match C++: cairo_set_source_rgba(cr.get(), 0.5, 0.0, 0.0, 1.); - dark red background
|
||||
// For occupancy grid: observed=0 indicates unknown
|
||||
var outputPixels = new uint[width * height];
|
||||
// Initialize to unknown (gray color, observed=0)
|
||||
for (int i = 0; i < outputPixels.Length; i++)
|
||||
{
|
||||
outputPixels[i] = 0xFF800000; // Alpha=255, Red=128 (gray), Green=0 (not observed), Blue=0
|
||||
}
|
||||
|
||||
// Second pass: paint each submap slice using inverse mapping + bilinear interpolation
|
||||
foreach (var (_, slice) in submapSlices)
|
||||
{
|
||||
if (slice.PixelData == null || slice.Width <= 0 || slice.Height <= 0)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
PaintSubmapSliceCairoStyle(slice, resolution, originX, originY, outputPixels, width, height);
|
||||
}
|
||||
|
||||
// Calculate the origin in world coordinates
|
||||
var worldOrigin = new Vector2(
|
||||
(minX - kPaddingPixel) * resolution,
|
||||
-(minY - kPaddingPixel) * resolution);
|
||||
|
||||
return new PaintSubmapSlicesResult(outputPixels, width, height, worldOrigin);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Paints a single submap slice using Cairo-style rendering:
|
||||
/// - Inverse mapping: for each output pixel, compute source position
|
||||
/// - Bilinear interpolation: sample from 4 neighboring pixels
|
||||
/// - Porter-Duff Source-Over: proper alpha compositing
|
||||
/// </summary>
|
||||
private static void PaintSubmapSliceCairoStyle(
|
||||
SubmapSlice slice,
|
||||
double resolution,
|
||||
double originX,
|
||||
double originY,
|
||||
uint[] outputPixels,
|
||||
int outputWidth,
|
||||
int outputHeight)
|
||||
{
|
||||
if (slice.PixelData == null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
// Build affine transformation matrix (Cairo style)
|
||||
// Transform chain: output_pixel -> world -> submap_local -> submap_pixel
|
||||
var submapTransform = slice.Pose * slice.SlicePose;
|
||||
var inverseTransform = submapTransform.Inverse();
|
||||
|
||||
// Pre-compute scale factors
|
||||
var outputToWorld = resolution;
|
||||
var worldToSubmap = 1.0 / slice.Resolution;
|
||||
|
||||
// Compute the bounding box of this slice in output coordinates
|
||||
// to avoid iterating over the entire output
|
||||
var sliceCorners = new Vector2[]
|
||||
{
|
||||
new(0, 0),
|
||||
new(slice.Width, 0),
|
||||
new(0, slice.Height),
|
||||
new(slice.Width, slice.Height)
|
||||
};
|
||||
|
||||
int outMinX = outputWidth, outMinY = outputHeight;
|
||||
int outMaxX = 0, outMaxY = 0;
|
||||
|
||||
foreach (var corner in sliceCorners)
|
||||
{
|
||||
// Match C++ Cairo matrix pixel-to-local mapping
|
||||
var localPoint = new Vector3(-corner.Y * slice.Resolution, -corner.X * slice.Resolution, 0);
|
||||
var globalPoint = submapTransform.TransformPoint(localPoint);
|
||||
var outX = (int)Math.Floor(globalPoint.X / resolution + originX);
|
||||
var outY = (int)Math.Floor(-globalPoint.Y / resolution + originY);
|
||||
|
||||
outMinX = Math.Min(outMinX, outX - 2);
|
||||
outMinY = Math.Min(outMinY, outY - 2);
|
||||
outMaxX = Math.Max(outMaxX, outX + 2);
|
||||
outMaxY = Math.Max(outMaxY, outY + 2);
|
||||
}
|
||||
|
||||
// Clamp to output bounds
|
||||
outMinX = Math.Max(0, outMinX);
|
||||
outMinY = Math.Max(0, outMinY);
|
||||
outMaxX = Math.Min(outputWidth - 1, outMaxX);
|
||||
outMaxY = Math.Min(outputHeight - 1, outMaxY);
|
||||
|
||||
// Inverse mapping: for each output pixel in the bounding box
|
||||
for (int outY = outMinY; outY <= outMaxY; outY++)
|
||||
{
|
||||
for (int outX = outMinX; outX <= outMaxX; outX++)
|
||||
{
|
||||
// Convert output pixel to world coordinates
|
||||
// Match C++ cairo convention: output uses (x, -y)
|
||||
var worldX = (outX - originX) * outputToWorld;
|
||||
var worldY = -(outY - originY) * outputToWorld;
|
||||
|
||||
// Transform world to submap local coordinates
|
||||
var worldPoint = new Vector3(worldX, worldY, 0);
|
||||
var submapLocalPoint = inverseTransform.TransformPoint(worldPoint);
|
||||
|
||||
// Convert submap local to pixel coordinates
|
||||
// Inverse of the forward mapping: local = (-row * res, -col * res)
|
||||
// So: col = -local.Y / res, row = -local.X / res
|
||||
var srcX = -submapLocalPoint.Y * worldToSubmap;
|
||||
var srcY = -submapLocalPoint.X * worldToSubmap;
|
||||
|
||||
// Check if within source bounds (with margin for bilinear)
|
||||
if (srcX < 0 || srcX >= slice.Width - 1 || srcY < 0 || srcY >= slice.Height - 1)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
// Bilinear interpolation
|
||||
var sampledPixel = SampleBilinear(slice.PixelData, slice.Width, slice.Height, srcX, srcY);
|
||||
|
||||
// Skip if not observed
|
||||
var srcObserved = (sampledPixel >> 8) & 0xFF;
|
||||
if (srcObserved == 0)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
// Porter-Duff Source-Over compositing
|
||||
var dstIndex = outY * outputWidth + outX;
|
||||
var dstPixel = outputPixels[dstIndex];
|
||||
|
||||
outputPixels[dstIndex] = BlendSourceOver(sampledPixel, dstPixel);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Bilinear interpolation sampling from a pixel array.
|
||||
/// Returns interpolated ARGB pixel value.
|
||||
/// </summary>
|
||||
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
||||
private static uint SampleBilinear(uint[] pixels, int width, int height, double x, double y)
|
||||
{
|
||||
// Get integer and fractional parts
|
||||
int x0 = (int)Math.Floor(x);
|
||||
int y0 = (int)Math.Floor(y);
|
||||
int x1 = Math.Min(x0 + 1, width - 1);
|
||||
int y1 = Math.Min(y0 + 1, height - 1);
|
||||
|
||||
double fx = x - x0;
|
||||
double fy = y - y0;
|
||||
|
||||
// Get four neighboring pixels
|
||||
var p00 = pixels[y0 * width + x0];
|
||||
var p10 = pixels[y0 * width + x1];
|
||||
var p01 = pixels[y1 * width + x0];
|
||||
var p11 = pixels[y1 * width + x1];
|
||||
|
||||
// Check if all neighbors are observed (optimization: skip interpolation if any is unknown)
|
||||
var obs00 = (p00 >> 8) & 0xFF;
|
||||
var obs10 = (p10 >> 8) & 0xFF;
|
||||
var obs01 = (p01 >> 8) & 0xFF;
|
||||
var obs11 = (p11 >> 8) & 0xFF;
|
||||
|
||||
// If any corner is unobserved, use nearest neighbor with observed pixel
|
||||
if (obs00 == 0 || obs10 == 0 || obs01 == 0 || obs11 == 0)
|
||||
{
|
||||
// Find the nearest observed pixel
|
||||
var nearestX = fx < 0.5 ? x0 : x1;
|
||||
var nearestY = fy < 0.5 ? y0 : y1;
|
||||
var nearest = pixels[nearestY * width + nearestX];
|
||||
if (((nearest >> 8) & 0xFF) != 0)
|
||||
{
|
||||
return nearest;
|
||||
}
|
||||
|
||||
// Try other corners
|
||||
if (obs00 != 0) return p00;
|
||||
if (obs10 != 0) return p10;
|
||||
if (obs01 != 0) return p01;
|
||||
if (obs11 != 0) return p11;
|
||||
|
||||
return 0; // All unobserved
|
||||
}
|
||||
|
||||
// Bilinear interpolation weights
|
||||
double w00 = (1 - fx) * (1 - fy);
|
||||
double w10 = fx * (1 - fy);
|
||||
double w01 = (1 - fx) * fy;
|
||||
double w11 = fx * fy;
|
||||
|
||||
// Interpolate each channel
|
||||
var a = (uint)Math.Round(
|
||||
((p00 >> 24) & 0xFF) * w00 +
|
||||
((p10 >> 24) & 0xFF) * w10 +
|
||||
((p01 >> 24) & 0xFF) * w01 +
|
||||
((p11 >> 24) & 0xFF) * w11);
|
||||
|
||||
var r = (uint)Math.Round(
|
||||
((p00 >> 16) & 0xFF) * w00 +
|
||||
((p10 >> 16) & 0xFF) * w10 +
|
||||
((p01 >> 16) & 0xFF) * w01 +
|
||||
((p11 >> 16) & 0xFF) * w11);
|
||||
|
||||
var g = (uint)Math.Round(
|
||||
((p00 >> 8) & 0xFF) * w00 +
|
||||
((p10 >> 8) & 0xFF) * w10 +
|
||||
((p01 >> 8) & 0xFF) * w01 +
|
||||
((p11 >> 8) & 0xFF) * w11);
|
||||
|
||||
var b = (uint)Math.Round(
|
||||
(p00 & 0xFF) * w00 +
|
||||
(p10 & 0xFF) * w10 +
|
||||
(p01 & 0xFF) * w01 +
|
||||
(p11 & 0xFF) * w11);
|
||||
|
||||
return (Math.Min(255u, a) << 24) |
|
||||
(Math.Min(255u, r) << 16) |
|
||||
(Math.Min(255u, g) << 8) |
|
||||
Math.Min(255u, b);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Porter-Duff Source-Over compositing matching Cairo's OVER operator.
|
||||
///
|
||||
/// Cairo uses premultiplied alpha OVER: result = src + dst * (1 - srcA/255)
|
||||
///
|
||||
/// This naturally produces the correct behavior for occupancy grids:
|
||||
/// - Free cells (srcA=0): additive blending (factor=1.0) → R gets brighter with more observations
|
||||
/// - Occupied cells (srcA>0): standard OVER → R gets darker with more observations
|
||||
/// - Multiple free observations → brighter (lower occupancy = more confident free space)
|
||||
/// - Multiple occupied observations → darker (higher occupancy = more confident wall)
|
||||
/// </summary>
|
||||
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
||||
private static uint BlendSourceOver(uint src, uint dst)
|
||||
{
|
||||
// Extract channels
|
||||
// Format: (alpha << 24) | (intensity/red << 16) | (observed/green << 8) | blue
|
||||
var srcA = (src >> 24) & 0xFF;
|
||||
var srcR = (src >> 16) & 0xFF;
|
||||
var srcG = (src >> 8) & 0xFF;
|
||||
var srcB = src & 0xFF;
|
||||
|
||||
var dstA = (dst >> 24) & 0xFF;
|
||||
var dstR = (dst >> 16) & 0xFF;
|
||||
var dstG = (dst >> 8) & 0xFF;
|
||||
var dstB = dst & 0xFF;
|
||||
|
||||
// Cairo OVER operator (premultiplied alpha):
|
||||
// result = src + dst * (1 - srcA / 255)
|
||||
var factor = 1.0 - srcA / 255.0;
|
||||
|
||||
var outA = (uint)Math.Min(255, (int)Math.Round(srcA + dstA * factor));
|
||||
var outR = (uint)Math.Min(255, (int)Math.Round(srcR + dstR * factor));
|
||||
var outG = (uint)Math.Min(255, (int)Math.Round(srcG + dstG * factor));
|
||||
var outB = (uint)Math.Min(255, (int)Math.Round(srcB + dstB * factor));
|
||||
|
||||
return (outA << 24) | (outR << 16) | (outG << 8) | outB;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Converts painted result to occupancy grid values.
|
||||
/// Returns array of occupancy values: 0 = free, 100 = occupied, -1 = unknown.
|
||||
/// Match C++: CreateOccupancyGrid in data_conversion.cc
|
||||
/// </summary>
|
||||
/// <param name="result">Paint result from PaintSubmapSlices</param>
|
||||
/// <returns>Array of sbyte occupancy values</returns>
|
||||
public static sbyte[] ConvertToOccupancyValues(PaintSubmapSlicesResult result)
|
||||
{
|
||||
var occupancyValues = new sbyte[result.Width * result.Height];
|
||||
|
||||
for (int i = 0; i < result.PixelData.Length; i++)
|
||||
{
|
||||
var pixel = result.PixelData[i];
|
||||
// Match C++ pixel format: (alpha << 24) | (intensity/color << 16) | (observed << 8) | 0
|
||||
var color = (pixel >> 16) & 0xFF; // RED channel = intensity/color
|
||||
var observed = (pixel >> 8) & 0xFF; // GREEN channel = observed flag
|
||||
|
||||
if (observed == 0)
|
||||
{
|
||||
// Unknown cell - not observed
|
||||
occupancyValues[i] = -1;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Match C++ formula from data_conversion.cc line 386-389:
|
||||
// const int value = observed == 0
|
||||
// ? -1
|
||||
// : ::cartographer::common::RoundToInt((1. - color / 255.) * 100.);
|
||||
//
|
||||
// color = 0 (black) → occupancy = 100 (occupied)
|
||||
// color = 255 (white) → occupancy = 0 (free)
|
||||
var occupancy = (int)Math.Round((1.0 - color / 255.0) * 100.0);
|
||||
occupancyValues[i] = (sbyte)Math.Clamp(occupancy, 0, 100);
|
||||
}
|
||||
}
|
||||
|
||||
return occupancyValues;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user