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