public void Setup()
{
_image = new Mock <ISourceTree>();
_image.SetupGet(ld => ld.DisplayName).Returns("Camera");
_tree = new ProtobufContainerTree("Protobuf", _image.Object, new SlamDataInfoPresenter("Special info"));
var p1 = new SlamPoint(1, Vector3.zero, Color.black);
var p2 = new SlamPoint(2, Vector3.zero, Color.black);
_tree.Points.Add(p1);
_tree.Points.Add(p2);
_tree.Points.AddConnections(new[] { (p1.Id, p2.Id) });
public static SlamPoint[] ToSlamPoints(this PointCloud2Message cloud)
{
var data = cloud.data.ToArray();
var res = new SlamPoint[data.Length / cloud.point_step];
var xOffset = cloud.fields.First(f => f.Name == "x").Offset;
var yOffset = cloud.fields.First(f => f.Name == "y").Offset;
var zOffset = cloud.fields.First(f => f.Name == "z").Offset;
var intensityOffset = cloud.fields.FirstOrDefault(f => f.Name == "intensity")?.Offset ??
cloud.point_step + 1;
var rgbOffset = cloud.fields.FirstOrDefault(f => f.Name == "rgb")?.Offset ?? cloud.point_step + 1;
var rgbaOffset = cloud.fields.FirstOrDefault(f => f.Name == "rgba")?.Offset ??
cloud.point_step + 1;
for (int i = 0; i < data.Length / cloud.point_step; i++)
{
var x = BitConverter.ToSingle(data, (int)(i * cloud.point_step + xOffset));
var y = BitConverter.ToSingle(data, (int)(i * cloud.point_step + yOffset));
var z = BitConverter.ToSingle(data, (int)(i * cloud.point_step + zOffset));
Color color = Color.black;
if (intensityOffset < cloud.point_step)
{
var intensity = BitConverter.ToSingle(data, (int)(i * cloud.point_step + intensityOffset));
color = RosMessageConvertExtender.ColorFromIntensity(intensity);
}
else if (rgbOffset < cloud.point_step)
{
byte r = data[i * cloud.point_step + rgbOffset + 0];
byte g = data[i * cloud.point_step + rgbOffset + 1];
byte b = data[i * cloud.point_step + rgbOffset + 2];
color = new Color(r / (float)byte.MaxValue, g / (float)byte.MaxValue, b / (float)byte.MaxValue);
}
else if (rgbaOffset < cloud.point_step)
{
byte a = data[i * cloud.point_step + rgbaOffset + 0];
byte r = data[i * cloud.point_step + rgbaOffset + 1];
byte g = data[i * cloud.point_step + rgbaOffset + 2];
byte b = data[i * cloud.point_step + rgbaOffset + 3];
color = new Color(r / (float)byte.MaxValue, g / (float)byte.MaxValue, b / (float)byte.MaxValue,
a / (float)byte.MaxValue);
}
var pos = new Vector3(x, y, z);
RosMessageConvertExtender.Converter?.Convert(ref pos);
res[i] = new SlamPoint(i, pos, color);
}
return(res);
}
/// <summary>
/// Just change color field of SlamObservation point now
/// </summary>
/// <param name="obj"></param>
public void ChangeColor(SlamObservation obj)
{
if (obj == null)
{
Debug.LogWarning("[SlamObservationsContainer.ChangeColor] Null observation");
}
//Debug.Assert(
// Exists(obj),
// $"[SlamObservationsContainer.ChangeColor] Graph doesn't contain observation with id {obj.Point.id}");
if (!Exists(obj))
{
throw new InvalidSlamContainerOperationException($"[SlamObservationsContainer.ChangeColor] Graph doesn't contain observation with id {obj.Point.id}");
}
SlamObservation node = m_nodes.First(n => obj.Point.id == n.Point.id);
SlamPoint point = node.Point;
point.color = obj.Point.color;
node.Point = point;
}
private void Convert(ref SlamPackage pkg)
{
List <SlamObservation> observations = pkg.Observations;
Parallel.For(0, pkg.Observations.Count, (i) =>
{
SlamPoint point = observations[i];
Quaternion rot = observations[i].Orientation;
m_converter.Convert(ref point.position, ref rot);
observations[i].Point = point;
observations[i].Orientation = rot;
});
List <SlamPoint> points = pkg.Points;
Parallel.For(0, pkg.Points.Count, (i) =>
{
SlamPoint pt = points[i];
Quaternion stub = Quaternion.identity;
m_converter.Convert(ref pt.position, ref stub);
points[i] = pt;
});
}
public static PointPb ToProtobuf(this SlamPoint p, ICSConverter converter)
=> new PointPb
{
Id = p.Id, Position = p.Position.ToProtobuf(converter), Color = p.Color, Message = p.Message ?? ""
};
private static Vector3d ToNative(SlamPoint point) => new Vector3d(point.Position.x, point.Position.y, point.Position.z);
public GPUItem(SlamPoint cp)
{
Position = cp.Position;
Color = EncodeColor(cp.Color);
}
