620 lines
23 KiB
C#
620 lines
23 KiB
C#
/*
|
|
* 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;
|
|
}
|
|
}
|