public static Lgsvl.Detection3DArray ConvertFrom(Detected3DObjectData data) { var arr = new Lgsvl.Detection3DArray() { header = new Ros.Header() { seq = data.Sequence, stamp = Conversions.ConvertTime(data.Time), frame_id = data.Frame, }, detections = new List <Lgsvl.Detection3D>(), }; foreach (var d in data.Data) { // Transform from (Right/Up/Forward) to (Forward/Left/Up) var position = d.Position; position.Set(position.z, -position.x, position.y); var orientation = d.Rotation; orientation.Set(-orientation.z, orientation.x, -orientation.y, orientation.w); var size = d.Scale; size.Set(size.z, size.x, size.y); d.AngularVelocity.z = -d.AngularVelocity.z; var det = new Lgsvl.Detection3D() { id = d.Id, label = d.Label, score = d.Score, bbox = new Lgsvl.BoundingBox3D() { position = new Ros.Pose() { position = ConvertToPoint(position), orientation = Convert(orientation), }, size = ConvertToVector(size), }, velocity = new Ros.Twist() { linear = ConvertToVector(d.LinearVelocity), angular = ConvertToVector(d.AngularVelocity), }, }; arr.detections.Add(det); } return(arr); }
public static Detected3DObjectArray ConvertTo(Lgsvl.Detection3DArray data) { return(new Detected3DObjectArray() { Data = data.detections.Select(obj => new Detected3DObject() { Id = obj.id, Label = obj.label, Score = obj.score, Position = Convert(obj.bbox.position.position), Rotation = Convert(obj.bbox.position.orientation), Scale = Convert(obj.bbox.size), LinearVelocity = Convert(obj.velocity.linear), AngularVelocity = Convert(obj.velocity.angular), }).ToArray(), }); }