Initial commit
This commit is contained in:
@@ -0,0 +1,33 @@
|
||||
/*
|
||||
* Copyright 2018 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.
|
||||
*/
|
||||
|
||||
namespace CartographerSharp.IO;
|
||||
|
||||
/// <summary>
|
||||
/// Interface for reading proto messages from a pbstream.
|
||||
/// </summary>
|
||||
public interface IProtoStreamReader
|
||||
{
|
||||
/// <summary>
|
||||
/// Deserializes compressed proto from the pb stream.
|
||||
/// </summary>
|
||||
bool ReadProto<T>(out T? proto);
|
||||
|
||||
/// <summary>
|
||||
/// 'End-of-file' marker for the pb stream.
|
||||
/// </summary>
|
||||
bool Eof { get; }
|
||||
}
|
||||
@@ -0,0 +1,33 @@
|
||||
/*
|
||||
* Copyright 2018 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.
|
||||
*/
|
||||
|
||||
namespace CartographerSharp.IO;
|
||||
|
||||
/// <summary>
|
||||
/// Interface for writing proto messages to a pbstream.
|
||||
/// </summary>
|
||||
public interface IProtoStreamWriter
|
||||
{
|
||||
/// <summary>
|
||||
/// Serializes, compresses and writes the proto to the stream.
|
||||
/// </summary>
|
||||
void WriteProto<T>(T proto);
|
||||
|
||||
/// <summary>
|
||||
/// This should be called to check whether writing was successful.
|
||||
/// </summary>
|
||||
bool Close();
|
||||
}
|
||||
@@ -0,0 +1,294 @@
|
||||
/*
|
||||
* Copyright 2018 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.Models.Mapping;
|
||||
using CartographerSharp.Models.Transform;
|
||||
using CartographerSharp.Sensor;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
using PoseGraphProto = CartographerSharp.Models.Mapping.PoseGraph;
|
||||
|
||||
namespace CartographerSharp.IO;
|
||||
|
||||
/// <summary>
|
||||
/// The current serialization format version.
|
||||
/// </summary>
|
||||
internal static class MappingStateSerialization
|
||||
{
|
||||
private const uint FormatVersion = 2;
|
||||
private const uint FormatVersionWithoutSubmapHistograms = 1;
|
||||
|
||||
/// <summary>
|
||||
/// Serializes mapping state to a pbstream.
|
||||
/// </summary>
|
||||
public static void WritePbStream(
|
||||
IPoseGraph poseGraph,
|
||||
List<TrajectoryBuilderOptionsWithSensorIds> trajectoryBuilderOptions,
|
||||
IProtoStreamWriter writer,
|
||||
bool includeUnfinishedSubmaps)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(poseGraph);
|
||||
ArgumentNullException.ThrowIfNull(writer);
|
||||
|
||||
// Write header
|
||||
var header = new SerializationHeader(FormatVersion);
|
||||
var headerData = new SerializedData { SerializationHeader = header };
|
||||
writer.WriteProto(headerData);
|
||||
|
||||
// Serialize pose graph
|
||||
var poseGraphProto = poseGraph.ToProto(includeUnfinishedSubmaps);
|
||||
var poseGraphData = new SerializedData { PoseGraph = poseGraphProto };
|
||||
writer.WriteProto(poseGraphData);
|
||||
|
||||
// Get valid trajectory IDs (not deleted)
|
||||
var trajectoryStates = poseGraph.GetTrajectoryStates();
|
||||
var validTrajectoryIds = GetValidTrajectoryIds(trajectoryStates);
|
||||
|
||||
// Serialize trajectory builder options
|
||||
var allOptions = CreateAllTrajectoryBuilderOptionsProto(trajectoryBuilderOptions, validTrajectoryIds);
|
||||
var optionsData = new SerializedData { AllTrajectoryBuilderOptions = allOptions };
|
||||
writer.WriteProto(optionsData);
|
||||
|
||||
// Serialize submaps
|
||||
var submapData = poseGraph.GetAllSubmapData();
|
||||
SerializeSubmaps(submapData, includeUnfinishedSubmaps, writer);
|
||||
|
||||
// Serialize trajectory nodes
|
||||
var trajectoryNodes = poseGraph.GetTrajectoryNodes();
|
||||
SerializeTrajectoryNodes(trajectoryNodes, writer);
|
||||
|
||||
// Serialize trajectory data
|
||||
var allTrajectoryData = poseGraph.GetTrajectoryData();
|
||||
SerializeTrajectoryData(allTrajectoryData, writer);
|
||||
|
||||
// Serialize IMU data
|
||||
var imuData = poseGraph.GetImuData();
|
||||
SerializeImuData(imuData, writer);
|
||||
|
||||
// Serialize odometry data
|
||||
var odometryData = poseGraph.GetOdometryData();
|
||||
SerializeOdometryData(odometryData, writer);
|
||||
|
||||
// Serialize fixed frame pose data
|
||||
var fixedFramePoseData = poseGraph.GetFixedFramePoseData();
|
||||
SerializeFixedFramePoseData(fixedFramePoseData, writer);
|
||||
|
||||
// Serialize landmark data
|
||||
var landmarkNodes = poseGraph.GetLandmarkNodes();
|
||||
SerializeLandmarkData(landmarkNodes, writer);
|
||||
}
|
||||
|
||||
private static List<int> GetValidTrajectoryIds(Dictionary<int, IPoseGraph.TrajectoryState> trajectoryStates)
|
||||
{
|
||||
var validTrajectories = new List<int>();
|
||||
foreach (var kvp in trajectoryStates)
|
||||
{
|
||||
if (kvp.Value != IPoseGraph.TrajectoryState.Deleted)
|
||||
{
|
||||
validTrajectories.Add(kvp.Key);
|
||||
}
|
||||
}
|
||||
return validTrajectories;
|
||||
}
|
||||
|
||||
private static AllTrajectoryBuilderOptions CreateAllTrajectoryBuilderOptionsProto(
|
||||
List<TrajectoryBuilderOptionsWithSensorIds> allOptionsWithSensorIds,
|
||||
List<int> trajectoryIdsToSerialize)
|
||||
{
|
||||
var optionsList = new List<TrajectoryBuilderOptionsWithSensorIds>();
|
||||
foreach (var id in trajectoryIdsToSerialize)
|
||||
{
|
||||
if (id >= 0 && id < allOptionsWithSensorIds.Count)
|
||||
{
|
||||
optionsList.Add(allOptionsWithSensorIds[id]);
|
||||
}
|
||||
}
|
||||
return new AllTrajectoryBuilderOptions(optionsList);
|
||||
}
|
||||
|
||||
private static void SerializeSubmaps(
|
||||
MapById<SubmapId, IPoseGraph.SubmapData> submapData,
|
||||
bool includeUnfinishedSubmaps,
|
||||
IProtoStreamWriter writer)
|
||||
{
|
||||
foreach (var kvp in submapData)
|
||||
{
|
||||
if (!includeUnfinishedSubmaps)
|
||||
{
|
||||
if (kvp.Data.Submap != null && !kvp.Data.Submap.InsertionFinished)
|
||||
{
|
||||
continue; // Skip unfinished submaps
|
||||
}
|
||||
}
|
||||
|
||||
if (kvp.Data.Submap == null)
|
||||
continue;
|
||||
|
||||
var submapProto = kvp.Data.Submap.ToProto(includeGridData: true);
|
||||
|
||||
var submapWithId = new Models.Mapping.Submap(
|
||||
new PoseGraphProto.SubmapId(kvp.Id.TrajectoryId, kvp.Id.SubmapIndex),
|
||||
submapProto.Submap2D,
|
||||
submapProto.Submap3D
|
||||
);
|
||||
|
||||
var submapDataProto = new SerializedData { Submap = submapWithId };
|
||||
writer.WriteProto(submapDataProto);
|
||||
}
|
||||
}
|
||||
|
||||
private static void SerializeTrajectoryNodes(
|
||||
MapById<NodeId, TrajectoryNode> trajectoryNodes,
|
||||
IProtoStreamWriter writer)
|
||||
{
|
||||
foreach (var kvp in trajectoryNodes)
|
||||
{
|
||||
if (kvp.Data.ConstantData == null)
|
||||
continue;
|
||||
|
||||
var nodeData = TrajectoryNodeOperations.ToProto(kvp.Data.ConstantData);
|
||||
var nodeWithId = new Models.Mapping.Node(
|
||||
new PoseGraphProto.NodeId(kvp.Id.TrajectoryId, kvp.Id.NodeIndex),
|
||||
nodeData
|
||||
);
|
||||
|
||||
var nodeDataProto = new SerializedData { Node = nodeWithId };
|
||||
writer.WriteProto(nodeDataProto);
|
||||
}
|
||||
}
|
||||
|
||||
private static void SerializeTrajectoryData(
|
||||
Dictionary<int, IPoseGraph.TrajectoryData> allTrajectoryData,
|
||||
IProtoStreamWriter writer)
|
||||
{
|
||||
foreach (var kvp in allTrajectoryData)
|
||||
{
|
||||
var trajectoryData = kvp.Value;
|
||||
var imuCalibration = trajectoryData.ImuCalibration != Quaternion.Identity
|
||||
? (Quaterniond?)new Quaterniond(
|
||||
trajectoryData.ImuCalibration.X,
|
||||
trajectoryData.ImuCalibration.Y,
|
||||
trajectoryData.ImuCalibration.Z,
|
||||
trajectoryData.ImuCalibration.W
|
||||
)
|
||||
: null;
|
||||
|
||||
var fixedFrameOriginInMap = trajectoryData.FixedFrameOriginInMap.HasValue
|
||||
? (Rigid3dProto?)(Rigid3dProto)trajectoryData.FixedFrameOriginInMap.Value
|
||||
: null;
|
||||
|
||||
var serializedTrajectoryData = new SerializedTrajectoryData(
|
||||
kvp.Key,
|
||||
trajectoryData.GravityConstant,
|
||||
imuCalibration,
|
||||
fixedFrameOriginInMap
|
||||
);
|
||||
|
||||
var trajectoryDataProto = new SerializedData { SerializedTrajectoryData = serializedTrajectoryData };
|
||||
writer.WriteProto(trajectoryDataProto);
|
||||
}
|
||||
}
|
||||
|
||||
private static void SerializeImuData(
|
||||
Dictionary<int, List<ImuData>> imuData,
|
||||
IProtoStreamWriter writer)
|
||||
{
|
||||
foreach (var kvp in imuData)
|
||||
{
|
||||
var trajectoryId = kvp.Key;
|
||||
foreach (var imu in kvp.Value)
|
||||
{
|
||||
var imuProto = Sensor.ImuDataOperations.ToProto(imu);
|
||||
var serializedImuData = new SerializedImuData(trajectoryId, imuProto);
|
||||
var serializedData = new SerializedData { ImuData = serializedImuData };
|
||||
writer.WriteProto(serializedData);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static void SerializeOdometryData(
|
||||
Dictionary<int, List<OdometryData>> odometryData,
|
||||
IProtoStreamWriter writer)
|
||||
{
|
||||
foreach (var kvp in odometryData)
|
||||
{
|
||||
var trajectoryId = kvp.Key;
|
||||
foreach (var odometry in kvp.Value)
|
||||
{
|
||||
var odometryProto = Sensor.OdometryDataOperations.ToProto(odometry);
|
||||
var serializedOdometryData = new SerializedOdometryData(trajectoryId, odometryProto);
|
||||
var serializedData = new SerializedData { OdometryData = serializedOdometryData };
|
||||
writer.WriteProto(serializedData);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static void SerializeFixedFramePoseData(
|
||||
Dictionary<int, List<FixedFramePoseData>> fixedFramePoseData,
|
||||
IProtoStreamWriter writer)
|
||||
{
|
||||
foreach (var kvp in fixedFramePoseData)
|
||||
{
|
||||
var trajectoryId = kvp.Key;
|
||||
foreach (var fixedFramePose in kvp.Value)
|
||||
{
|
||||
var fixedFramePoseProto = Sensor.FixedFramePoseDataOperations.ToProto(fixedFramePose);
|
||||
var serializedFixedFramePoseData = new SerializedFixedFramePoseData(trajectoryId, fixedFramePoseProto);
|
||||
var serializedData = new SerializedData { FixedFramePoseData = serializedFixedFramePoseData };
|
||||
writer.WriteProto(serializedData);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static void SerializeLandmarkData(
|
||||
Dictionary<string, IPoseGraph.LandmarkNode> landmarkNodes,
|
||||
IProtoStreamWriter writer)
|
||||
{
|
||||
foreach (var kvp in landmarkNodes)
|
||||
{
|
||||
var landmarkId = kvp.Key;
|
||||
var landmarkNode = kvp.Value;
|
||||
|
||||
// Serialize each landmark observation
|
||||
foreach (var observation in landmarkNode.LandmarkObservations)
|
||||
{
|
||||
// Create landmark data from observation
|
||||
var landmarkData = new Sensor.LandmarkData(
|
||||
observation.Time,
|
||||
[
|
||||
new(
|
||||
landmarkId,
|
||||
observation.LandmarkToTrackingTransform,
|
||||
observation.TranslationWeight,
|
||||
observation.RotationWeight
|
||||
)
|
||||
]
|
||||
);
|
||||
|
||||
// Convert to proto
|
||||
var landmarkDataProto = Sensor.LandmarkDataOperations.ToProto(landmarkData);
|
||||
|
||||
var serializedLandmarkData = new SerializedLandmarkData(
|
||||
observation.TrajectoryId,
|
||||
landmarkDataProto
|
||||
);
|
||||
|
||||
var serializedData = new SerializedData { LandmarkData = serializedLandmarkData };
|
||||
writer.WriteProto(serializedData);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,173 @@
|
||||
/*
|
||||
* Copyright 2018 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
using CartographerSharp.Models.Mapping;
|
||||
|
||||
namespace CartographerSharp.IO;
|
||||
|
||||
/// <summary>
|
||||
/// Helper for deserializing a previously serialized mapping state from a proto stream,
|
||||
/// abstracting away the format parsing logic.
|
||||
/// </summary>
|
||||
public class ProtoStreamDeserializer
|
||||
{
|
||||
private const uint FormatVersion = 2;
|
||||
private const uint FormatVersionWithoutSubmapHistograms = 1;
|
||||
|
||||
private readonly IProtoStreamReader _reader;
|
||||
private readonly SerializationHeader _header;
|
||||
private readonly SerializedData _poseGraph;
|
||||
private readonly SerializedData _allTrajectoryBuilderOptions;
|
||||
|
||||
public ProtoStreamDeserializer(IProtoStreamReader reader)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(reader);
|
||||
|
||||
_reader = reader;
|
||||
|
||||
// Read header - use same pattern as ReadNextSerializedData
|
||||
// ReadProto returns T? which for structs becomes Nullable<T>
|
||||
// Using out var should work, but compiler may not infer nullable correctly
|
||||
// So we use a helper method pattern
|
||||
if (!ReadNextSerializedData(out var headerDataNullable) || !headerDataNullable.HasValue)
|
||||
{
|
||||
throw new InvalidOperationException("Failed to read SerializationHeader.");
|
||||
}
|
||||
|
||||
var headerData = headerDataNullable.Value;
|
||||
if (!headerData.SerializationHeader.HasValue)
|
||||
{
|
||||
throw new InvalidOperationException("SerializedData does not contain SerializationHeader.");
|
||||
}
|
||||
|
||||
_header = headerData.SerializationHeader.Value;
|
||||
|
||||
// Validate format version
|
||||
if (!IsVersionSupported(_header))
|
||||
{
|
||||
throw new NotSupportedException(
|
||||
$"Unsupported serialization format version: {_header.FormatVersion}. " +
|
||||
$"Supported versions: {FormatVersionWithoutSubmapHistograms}, {FormatVersion}");
|
||||
}
|
||||
|
||||
// Read pose graph
|
||||
if (!ReadNextSerializedData(out SerializedData? poseGraphDataNullable) || !poseGraphDataNullable.HasValue)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"Serialized stream misses PoseGraph. Expecting `PoseGraph` after `SerializationHeader`.");
|
||||
}
|
||||
|
||||
var poseGraphData = poseGraphDataNullable.Value;
|
||||
if (!poseGraphData.PoseGraph.HasValue)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"SerializedData does not contain PoseGraph. Expecting `PoseGraph` after `SerializationHeader`.");
|
||||
}
|
||||
|
||||
_poseGraph = poseGraphData;
|
||||
|
||||
// Read trajectory builder options
|
||||
if (!ReadNextSerializedData(out var optionsDataNullable) || !optionsDataNullable.HasValue)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"Serialized stream misses `AllTrajectoryBuilderOptions`. " +
|
||||
"Expecting `AllTrajectoryBuilderOptions` after PoseGraph.");
|
||||
}
|
||||
|
||||
var optionsData = optionsDataNullable.Value;
|
||||
if (!optionsData.AllTrajectoryBuilderOptions.HasValue)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"SerializedData does not contain AllTrajectoryBuilderOptions. " +
|
||||
"Expecting `AllTrajectoryBuilderOptions` after PoseGraph.");
|
||||
}
|
||||
|
||||
_allTrajectoryBuilderOptions = optionsData;
|
||||
|
||||
// Validate that trajectory count matches
|
||||
if (_poseGraph.PoseGraph.HasValue && _allTrajectoryBuilderOptions.AllTrajectoryBuilderOptions.HasValue)
|
||||
{
|
||||
var poseGraphProto = _poseGraph.PoseGraph.Value;
|
||||
var optionsProto = _allTrajectoryBuilderOptions.AllTrajectoryBuilderOptions.Value;
|
||||
|
||||
if (poseGraphProto.Trajectories.Count != optionsProto.OptionsWithSensorIds.Count)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
$"Trajectory count mismatch: PoseGraph has {poseGraphProto.Trajectories.Count} trajectories, " +
|
||||
$"but AllTrajectoryBuilderOptions has {optionsProto.OptionsWithSensorIds.Count} options.");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the serialization header.
|
||||
/// </summary>
|
||||
public SerializationHeader Header => _header;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the pose graph proto.
|
||||
/// </summary>
|
||||
public PoseGraph PoseGraph
|
||||
{
|
||||
get
|
||||
{
|
||||
if (!_poseGraph.PoseGraph.HasValue)
|
||||
throw new InvalidOperationException("PoseGraph is not available.");
|
||||
return _poseGraph.PoseGraph.Value;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets all trajectory builder options.
|
||||
/// </summary>
|
||||
public AllTrajectoryBuilderOptions AllTrajectoryBuilderOptions
|
||||
{
|
||||
get
|
||||
{
|
||||
if (!_allTrajectoryBuilderOptions.AllTrajectoryBuilderOptions.HasValue)
|
||||
throw new InvalidOperationException("AllTrajectoryBuilderOptions is not available.");
|
||||
return _allTrajectoryBuilderOptions.AllTrajectoryBuilderOptions.Value;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Reads the next SerializedData message from the ProtoStream.
|
||||
/// Returns true if the message was successfully read, or false if there are no more messages or an error occurred.
|
||||
/// </summary>
|
||||
public bool ReadNextSerializedData(out SerializedData? data)
|
||||
{
|
||||
if (_reader.Eof)
|
||||
{
|
||||
data = null;
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!_reader.ReadProto<SerializedData>(out var proto))
|
||||
{
|
||||
data = null;
|
||||
return false;
|
||||
}
|
||||
|
||||
data = proto;
|
||||
return data.HasValue;
|
||||
}
|
||||
|
||||
private static bool IsVersionSupported(SerializationHeader header)
|
||||
{
|
||||
return header.FormatVersion == FormatVersion ||
|
||||
header.FormatVersion == FormatVersionWithoutSubmapHistograms;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,158 @@
|
||||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
using System.IO.Compression;
|
||||
using System.Text;
|
||||
using System.Text.Json;
|
||||
|
||||
namespace CartographerSharp.IO;
|
||||
|
||||
/// <summary>
|
||||
/// A reader of the format produced by ProtoStreamWriter.
|
||||
/// </summary>
|
||||
public class ProtoStreamReader : IProtoStreamReader, IDisposable
|
||||
{
|
||||
// First eight bytes to identify our proto stream format.
|
||||
private const ulong Magic = 0x7b1d1f7b5bf501db;
|
||||
|
||||
|
||||
private static readonly JsonSerializerOptions JsonOptions = new()
|
||||
{
|
||||
PropertyNameCaseInsensitive = true
|
||||
};
|
||||
|
||||
private readonly FileStream _fileStream;
|
||||
private bool _disposed;
|
||||
private bool _magicRead;
|
||||
|
||||
public ProtoStreamReader(string filename)
|
||||
{
|
||||
if (string.IsNullOrEmpty(filename))
|
||||
throw new ArgumentException("Filename cannot be null or empty", nameof(filename));
|
||||
|
||||
if (!File.Exists(filename))
|
||||
throw new FileNotFoundException($"File not found: {filename}", filename);
|
||||
|
||||
_fileStream = new FileStream(filename, FileMode.Open, FileAccess.Read, FileShare.Read);
|
||||
ReadMagic();
|
||||
}
|
||||
|
||||
private void ReadMagic()
|
||||
{
|
||||
ulong magic = ReadSizeAsLittleEndian();
|
||||
if (magic != Magic)
|
||||
{
|
||||
throw new InvalidDataException($"Invalid proto stream format. Expected magic: 0x{Magic:X16}, got: 0x{magic:X16}");
|
||||
}
|
||||
_magicRead = true;
|
||||
}
|
||||
|
||||
private ulong ReadSizeAsLittleEndian()
|
||||
{
|
||||
ulong size = 0;
|
||||
for (int i = 0; i < 8; i++)
|
||||
{
|
||||
int byteValue = _fileStream.ReadByte();
|
||||
if (byteValue == -1)
|
||||
throw new EndOfStreamException("Unexpected end of stream while reading size");
|
||||
|
||||
size >>= 8;
|
||||
size += (ulong)byteValue << 56;
|
||||
}
|
||||
return size;
|
||||
}
|
||||
|
||||
private bool Read(out string decompressedData)
|
||||
{
|
||||
decompressedData = string.Empty;
|
||||
|
||||
if (!_magicRead)
|
||||
{
|
||||
ReadMagic();
|
||||
}
|
||||
|
||||
if (_fileStream.Position >= _fileStream.Length)
|
||||
{
|
||||
return false; // EOF
|
||||
}
|
||||
|
||||
// Read compressed size
|
||||
ulong compressedSize = ReadSizeAsLittleEndian();
|
||||
|
||||
if (compressedSize == 0 || compressedSize > int.MaxValue)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
// Read compressed data
|
||||
byte[] compressedBytes = new byte[compressedSize];
|
||||
int bytesRead = _fileStream.Read(compressedBytes, 0, (int)compressedSize);
|
||||
|
||||
if (bytesRead != (int)compressedSize)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
// Decompress using GZip
|
||||
using var memoryStream = new MemoryStream(compressedBytes);
|
||||
using var gzipStream = new GZipStream(memoryStream, CompressionMode.Decompress);
|
||||
using var reader = new StreamReader(gzipStream, Encoding.UTF8);
|
||||
|
||||
decompressedData = reader.ReadToEnd();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Deserializes compressed proto from the pb stream.
|
||||
/// </summary>
|
||||
public bool ReadProto<T>(out T? proto)
|
||||
{
|
||||
proto = default;
|
||||
|
||||
if (!Read(out string decompressedData))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
proto = JsonSerializer.Deserialize<T>(decompressedData, JsonOptions);
|
||||
return proto != null && !EqualityComparer<T>.Default.Equals(proto, default);
|
||||
}
|
||||
catch (JsonException)
|
||||
{
|
||||
proto = default;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 'End-of-file' marker for the pb stream.
|
||||
/// </summary>
|
||||
public bool Eof => _fileStream.Position >= _fileStream.Length;
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
if (!_disposed)
|
||||
{
|
||||
_fileStream?.Dispose();
|
||||
_disposed = true;
|
||||
}
|
||||
|
||||
GC.SuppressFinalize(this);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,131 @@
|
||||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
using System.IO.Compression;
|
||||
using System.Text;
|
||||
using System.Text.Json;
|
||||
|
||||
namespace CartographerSharp.IO;
|
||||
|
||||
/// <summary>
|
||||
/// A simple writer of a compressed sequence of protocol buffer messages to a file.
|
||||
/// The format is not intended to be compatible with any other format used outside of Cartographer.
|
||||
/// </summary>
|
||||
public class ProtoStreamWriter : IProtoStreamWriter, IDisposable
|
||||
{
|
||||
// First eight bytes to identify our proto stream format.
|
||||
private const ulong Magic = 0x7b1d1f7b5bf501db;
|
||||
|
||||
private static readonly JsonSerializerOptions JsonOptions = new()
|
||||
{
|
||||
WriteIndented = false,
|
||||
DefaultIgnoreCondition = System.Text.Json.Serialization.JsonIgnoreCondition.Never
|
||||
};
|
||||
|
||||
private readonly FileStream _fileStream;
|
||||
private bool _disposed;
|
||||
private bool _magicWritten;
|
||||
|
||||
public ProtoStreamWriter(string filename)
|
||||
{
|
||||
if (string.IsNullOrEmpty(filename))
|
||||
throw new ArgumentException("Filename cannot be null or empty", nameof(filename));
|
||||
|
||||
_fileStream = new FileStream(filename, FileMode.Create, FileAccess.Write, FileShare.Read);
|
||||
WriteMagic();
|
||||
}
|
||||
|
||||
private void WriteMagic()
|
||||
{
|
||||
WriteSizeAsLittleEndian(Magic);
|
||||
_magicWritten = true;
|
||||
}
|
||||
|
||||
private void WriteSizeAsLittleEndian(ulong size)
|
||||
{
|
||||
for (int i = 0; i < 8; i++)
|
||||
{
|
||||
_fileStream.WriteByte((byte)(size & 0xff));
|
||||
size >>= 8;
|
||||
}
|
||||
}
|
||||
|
||||
private void Write(string uncompressedData)
|
||||
{
|
||||
if (!_magicWritten)
|
||||
{
|
||||
WriteMagic();
|
||||
}
|
||||
|
||||
// Compress using GZip
|
||||
byte[] uncompressedBytes = Encoding.UTF8.GetBytes(uncompressedData);
|
||||
byte[] compressedBytes;
|
||||
|
||||
using (var memoryStream = new MemoryStream())
|
||||
{
|
||||
using (var gzipStream = new GZipStream(memoryStream, CompressionMode.Compress, leaveOpen: true))
|
||||
{
|
||||
gzipStream.Write(uncompressedBytes, 0, uncompressedBytes.Length);
|
||||
}
|
||||
compressedBytes = memoryStream.ToArray();
|
||||
}
|
||||
|
||||
// Write compressed size
|
||||
WriteSizeAsLittleEndian((ulong)compressedBytes.Length);
|
||||
|
||||
// Write compressed data
|
||||
_fileStream.Write(compressedBytes, 0, compressedBytes.Length);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Serializes, compresses and writes the proto to the file.
|
||||
/// </summary>
|
||||
public void WriteProto<T>(T proto)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(proto, nameof(proto));
|
||||
|
||||
// Serialize to JSON (we use System.Text.Json instead of Google.Protobuf)
|
||||
string json = JsonSerializer.Serialize(proto, JsonOptions);
|
||||
|
||||
Write(json);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// This should be called to check whether writing was successful.
|
||||
/// </summary>
|
||||
public bool Close()
|
||||
{
|
||||
if (_disposed)
|
||||
return false;
|
||||
|
||||
_fileStream.Flush();
|
||||
_fileStream.Close();
|
||||
_disposed = true; // Prevent Dispose() from calling Close() again (double-close would throw on Flush)
|
||||
return true;
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
if (!_disposed)
|
||||
{
|
||||
Close();
|
||||
_fileStream?.Dispose();
|
||||
_disposed = true;
|
||||
}
|
||||
|
||||
GC.SuppressFinalize(this);
|
||||
}
|
||||
}
|
||||
@@ -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