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