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); }