public static Detected3DObjectArray ConvertTo(apollo.perception.PerceptionObstacles data)
{
var detections = new List <Detected3DObject>();
foreach (var obstacle in data.perception_obstacle)
{
String label;
switch (obstacle.type)
{
case apollo.perception.PerceptionObstacle.Type.Vehicle:
label = "Car";
break;
case apollo.perception.PerceptionObstacle.Type.Pedestrian:
label = "Pedestrian";
break;
case apollo.perception.PerceptionObstacle.Type.Bicycle:
label = "Bicycle";
break;
default:
label = "Unknown";
break;
}
GpsData gps = new GpsData()
{
Easting = obstacle.position.x,
Northing = obstacle.position.y,
Altitude = obstacle.position.z,
};
var det = new Detected3DObject()
{
Id = (uint)obstacle.id,
Label = label,
Gps = gps,
Heading = 90 - obstacle.theta * Mathf.Rad2Deg,
Scale = new Vector3
(
(float)obstacle.width,
(float)obstacle.height,
(float)obstacle.length
),
};
detections.Add(det);
}
return(new Detected3DObjectArray()
{
Data = detections.ToArray(),
});
}
public void RecordJSONLog(Detected3DObject detected3DObject, string labelId, GameObject parent, float cycleTime)
{
var sensorRotation = transform.rotation;
var sensorPosition = transform.position;
var globalPos = parent.transform.position;
var globalRot = parent.transform.rotation;
if (!prevTimes.ContainsKey(labelId))
{
prevTimes.Add(labelId, SimulatorManager.Instance.CurrentTime);
}
// Get GPS coordinates.
bool IgnoreMapOrigin = false;
GpsLocation location = new GpsLocation();
Vector3 position = transform.position;
location = MapOrigin.GetGpsLocation(position, IgnoreMapOrigin);
if (SimulatorManager.Instance.CurrentTime - prevTimes[labelId] > cycleTime)
{
// string labelId = null;
var groundTruth3D = new JSONGroundTruth3D()
{
Id = detected3DObject.Id,
Label = detected3DObject.Label,
Position = (float3)detected3DObject.Position,
Rotation = detected3DObject.Rotation,
Velocity = detected3DObject.LinearVelocity,
Dimension = detected3DObject.Scale,
Time = SimulatorManager.Instance.CurrentTime,
GpsPosition = new Vector3((float)(location.Easting + (IgnoreMapOrigin ? -500000 : 0)),
(float)location.Northing, (float)location.Altitude),
GpsRotation = sensorRotation,
GlobalPosition = globalPos,
GlobalRotation = globalRot
};
if (!LogJsonDetected.ContainsKey(labelId))
{
var jsonGroundTruth3Ds = new List <JSONGroundTruth3D>();
jsonGroundTruth3Ds.Add(groundTruth3D);
LogJsonDetected.Add(labelId, jsonGroundTruth3Ds);
}
else
{
LogJsonDetected[labelId].Add(groundTruth3D);
}
prevTimes[labelId] = SimulatorManager.Instance.CurrentTime;
}
}
void WhileInRange(Collider other)
{
GameObject egoGO = transform.parent.gameObject;
GameObject parent = other.transform.parent.gameObject;
if (parent == egoGO)
{
return;
}
if (!(other.gameObject.layer == LayerMask.NameToLayer("GroundTruth")) || !parent.activeInHierarchy)
{
return;
}
uint id;
string label;
Vector3 velocity;
float angular_speed; // Angular speed around up axis of objects, in radians/sec
if (parent.layer == LayerMask.NameToLayer("Agent"))
{
var egoC = parent.GetComponent <VehicleController>();
var rb = parent.GetComponent <Rigidbody>();
id = egoC.GTID;
label = "Sedan";
velocity = rb.velocity;
angular_speed = rb.angularVelocity.y;
}
else if (parent.layer == LayerMask.NameToLayer("NPC"))
{
var npcC = parent.GetComponent <NPCController>();
id = npcC.GTID;
label = npcC.NPCLabel;
velocity = npcC.GetVelocity();
angular_speed = npcC.GetAngularVelocity().y;
}
else if (parent.layer == LayerMask.NameToLayer("Pedestrian"))
{
var pedC = parent.GetComponent <PedestrianController>();
id = pedC.GTID;
label = "Pedestrian";
velocity = pedC.CurrentVelocity;
angular_speed = pedC.CurrentAngularVelocity.y;
}
else
{
return;
}
Vector3 size = ((BoxCollider)other).size;
if (size.magnitude == 0)
{
return;
}
// Linear speed in forward direction of objects, in meters/sec
float speed = Vector3.Dot(velocity, parent.transform.forward);
// Local position of object in ego local space
Vector3 relPos = transform.InverseTransformPoint(parent.transform.position);
// Relative rotation of objects wrt ego frame
Quaternion relRot = Quaternion.Inverse(transform.rotation) * parent.transform.rotation;
var mapRotation = MapOrigin.transform.localRotation;
velocity = Quaternion.Inverse(mapRotation) * velocity;
var heading = parent.transform.localEulerAngles.y - mapRotation.eulerAngles.y;
// Center of bounding box
GpsLocation location = MapOrigin.GetGpsLocation(((BoxCollider)other).bounds.center);
GpsData gps = new GpsData()
{
Easting = location.Easting,
Northing = location.Northing,
Altitude = location.Altitude,
};
if (!Detected.ContainsKey(id))
{
var det = new Detected3DObject()
{
Id = id,
Label = label,
Score = 1.0f,
Position = relPos,
Rotation = relRot,
Scale = size,
LinearVelocity = new Vector3(speed, 0, 0),
AngularVelocity = new Vector3(0, 0, angular_speed),
Velocity = velocity,
Gps = gps,
Heading = heading,
TrackingTime = 0f,
};
Detected.Add(id, new Tuple <Detected3DObject, Collider>(det, other));
}
else
{
var det = Detected[id].Item1;
det.Position = relPos;
det.Rotation = relRot;
det.LinearVelocity = new Vector3(speed, 0, 0);
det.AngularVelocity = new Vector3(0, 0, angular_speed);
det.Acceleration = (velocity - det.Velocity) / Time.fixedDeltaTime;
det.Velocity = velocity;
det.Gps = gps;
det.Heading = heading;
det.TrackingTime += Time.fixedDeltaTime;
}
CurrentIDs.Add(id);
}
