Skip to content
Snippets Groups Projects
Commit 91c0eca4 authored by David Uhm's avatar David Uhm
Browse files

Fix ground truth sensors to correctly handle despawn and unique IDs

parent 973d0a6b
No related branches found
No related tags found
No related merge requests found
......@@ -61,6 +61,10 @@ public class PedestrianController : MonoBehaviour
private Coroutine[] Coroutines = new Coroutine[System.Enum.GetNames(typeof(CoroutineID)).Length];
private Vector3 CurrentTurn;
private float CurrentSpeed;
public Vector3 CurrentVelocity;
public Vector3 CurrentAngularVelocity;
private Vector3 LastRBPosition;
private Quaternion LastRBRotation;
private enum CoroutineID
{
......@@ -318,6 +322,15 @@ public class PedestrianController : MonoBehaviour
{
rb.angularVelocity = Vector3.zero;
}
var euler1 = LastRBRotation.eulerAngles;
var euler2 = rb.rotation.eulerAngles;
var diff = euler2 - euler1;
for (int i = 0; i < 3; i++)
{
diff[i] = (diff[i] + 180) % 360 - 180;
}
CurrentAngularVelocity = diff / Time.fixedDeltaTime * Mathf.Deg2Rad;
}
private void PEDMove()
......@@ -330,6 +343,9 @@ public class PedestrianController : MonoBehaviour
{
rb.velocity = Vector3.zero;
}
CurrentVelocity = (rb.position - LastRBPosition) / Time.fixedDeltaTime;
LastRBPosition = rb.position;
}
private IEnumerator ChangePedState()
......
......@@ -5,6 +5,7 @@
*
*/
using System;
using System.Linq;
using System.Collections.Generic;
using UnityEngine;
......@@ -55,6 +56,8 @@ namespace Simulator.Sensors
RenderTexture activeRT;
private Dictionary<Collider, Detected2DObject> Detected = new Dictionary<Collider, Detected2DObject>();
private Dictionary<int, uint> IDByInstanceID = new Dictionary<int, uint>();
private Detected2DObject[] Visualized = Array.Empty<Detected2DObject>();
AAWireBox AAWireBoxes;
......@@ -102,7 +105,7 @@ namespace Simulator.Sensors
camBoxCollider.center = new Vector3(0, 0, DetectionRange / 2f);
camBoxCollider.size = new Vector3(2 * Mathf.Tan(radHFOV / 2) * DetectionRange, 3f, DetectionRange);
cameraRangeTrigger.SetCallbacks(OnCollider, OnCollider, OnColliderExit);
cameraRangeTrigger.SetCallbacks(OnCollider);
}
public override void OnBridgeSetup(IBridge bridge)
......@@ -127,13 +130,16 @@ namespace Simulator.Sensors
Writer.Write(new Detected2DObjectData()
{
Sequence = seqId++,
Frame = Frame,
Sequence = seqId++,
Data = Detected.Values.ToArray(),
});
Visualized = Detected.Values.ToArray();
Detected.Clear();
}
}
Vector4 CalculateDetectedRect(Vector3 cen, Vector3 ext, Quaternion rotation)
{
ext.Set(ext.y, ext.z, ext.x);
......@@ -202,168 +208,157 @@ namespace Simulator.Sensors
void OnCollider(Collider other)
{
if (Detected.ContainsKey(other))
{
Detected.Remove(other);
}
if (other.isTrigger)
if (other.isTrigger || !other.gameObject.activeInHierarchy)
{
return;
}
if (!other.gameObject.activeInHierarchy)
if (!Detected.ContainsKey(other))
{
return;
}
// Vector from camera to collider
Vector3 vectorFromCamToCol = other.transform.position - Camera.transform.position;
// Vector projected onto camera plane
Vector3 vectorProjToCamPlane = Vector3.ProjectOnPlane(vectorFromCamToCol, Camera.transform.up);
// Angle in degree between collider and camera forward direction
var angleHorizon = Vector3.Angle(vectorProjToCamPlane, Camera.transform.forward);
// Check if collider is out of field of view
if (angleHorizon > degHFOV / 2)
{
return;
}
// Vector from camera to collider
Vector3 vectorFromCamToCol = other.transform.position - Camera.transform.position;
// Vector projected onto camera plane
Vector3 vectorProjToCamPlane = Vector3.ProjectOnPlane(vectorFromCamToCol, Camera.transform.up);
// Angle in degree between collider and camera forward direction
var angleHorizon = Vector3.Angle(vectorProjToCamPlane, Camera.transform.forward);
// Check if collider is out of field of view
if (angleHorizon > degHFOV / 2)
{
return;
}
Vector3 size = Vector3.zero;
if (other is MeshCollider)
{
var mesh = other as MeshCollider;
var npcC = mesh.gameObject.GetComponentInParent<NPCController>();
if (npcC != null)
Vector3 size;
float linear_vel; // Linear velocity in forward direction of objects, in meters/sec
float angular_vel; // Angular velocity around up axis of objects, in radians/sec
if (other is MeshCollider)
{
size.x = npcC.bounds.size.z;
size.y = npcC.bounds.size.x;
size.z = npcC.bounds.size.y;
var mesh = other as MeshCollider;
var npcC = mesh.gameObject.GetComponentInParent<NPCController>();
if (npcC != null)
{
size.x = npcC.bounds.size.z;
size.y = npcC.bounds.size.x;
size.z = npcC.bounds.size.y;
linear_vel = Vector3.Dot(npcC.GetVelocity(), other.transform.forward);
angular_vel = -npcC.GetAngularVelocity().y;
}
else
{
var egoA = mesh.GetComponent<VehicleActions>();
size.x = egoA.bounds.size.z;
size.y = egoA.bounds.size.x;
size.z = egoA.bounds.size.y;
linear_vel = Vector3.Dot(other.attachedRigidbody == null ? Vector3.zero : other.attachedRigidbody.velocity, other.transform.forward);
angular_vel = -(other.attachedRigidbody == null ? Vector3.zero : other.attachedRigidbody.angularVelocity).y;
}
}
else if (other is CapsuleCollider)
{
var capsule = other as CapsuleCollider;
var pedC = other.GetComponent<PedestrianController>();
size.x = capsule.radius * 2;
size.y = capsule.radius * 2;
size.z = capsule.height;
linear_vel = Vector3.Dot(pedC.CurrentVelocity, other.transform.forward);
angular_vel = -pedC.CurrentAngularVelocity.y;
}
else
{
var egoA = mesh.GetComponent<VehicleActions>();
size.x = egoA.bounds.size.z;
size.y = egoA.bounds.size.x;
size.z = egoA.bounds.size.y;
return;
}
}
else if (other is BoxCollider)
{
var box = other as BoxCollider;
size.x = box.size.z;
size.y = box.size.x;
size.z = box.size.y;
}
else if (other is CapsuleCollider)
{
var capsule = other as CapsuleCollider;
size.x = capsule.radius * 2;
size.y = capsule.radius * 2;
size.z = capsule.height;
}
else
{
return;
}
if (size.magnitude == 0)
{
return;
}
string label;
if (size.magnitude == 0)
{
return;
}
if (other.gameObject.layer == LayerMask.NameToLayer("NPC"))
{
label = "Car";
}
else if (other.gameObject.layer == LayerMask.NameToLayer("Pedestrian"))
{
label = "Pedestrian";
}
else if (other.gameObject.layer == LayerMask.NameToLayer("Bicycle"))
{
label = "bicycle";
}
else
{
return;
}
string label;
if (other.gameObject.layer == LayerMask.NameToLayer("NPC"))
{
label = "Car";
}
else if (other.gameObject.layer == LayerMask.NameToLayer("Pedestrian"))
{
label = "Pedestrian";
}
else if (other.gameObject.layer == LayerMask.NameToLayer("Bicycle"))
{
label = "bicycle";
}
else
{
return;
}
RaycastHit hit;
var start = Camera.transform.position;
var end = other.bounds.center;
var direction = (end - start).normalized;
var distance = (end - start).magnitude;
Ray cameraRay = new Ray(start, direction);
RaycastHit hit;
var start = Camera.transform.position;
var end = other.bounds.center;
var direction = (end - start).normalized;
var distance = (end - start).magnitude;
Ray cameraRay = new Ray(start, direction);
if (Physics.Raycast(cameraRay, out hit, distance, ~LayerMask.GetMask("Agent"), QueryTriggerInteraction.Ignore))
{
if (hit.collider == other)
if (Physics.Raycast(cameraRay, out hit, distance, ~LayerMask.GetMask("Agent"), QueryTriggerInteraction.Ignore))
{
Vector4 detectedRect = CalculateDetectedRect(other.bounds.center, size * 0.5f, other.transform.rotation);
if (detectedRect.z < 0 || detectedRect.w < 0)
if (hit.collider == other)
{
return;
Vector4 detectedRect = CalculateDetectedRect(other.bounds.center, size * 0.5f, other.transform.rotation);
if (detectedRect.z < 0 || detectedRect.w < 0)
{
return;
}
Detected.Add(other, new Detected2DObject()
{
Id = GetNextID(other),
Label = label,
Score = 1.0f,
Position = new Vector2(detectedRect.x, detectedRect.y),
Scale = new Vector2(detectedRect.z, detectedRect.w),
LinearVelocity = new Vector3(linear_vel, 0, 0),
AngularVelocity = new Vector3(0, 0, angular_vel),
});
}
// Linear velocity in forward direction of objects, in meters/sec
float linear_vel = Vector3.Dot(other.attachedRigidbody == null ? Vector3.zero : other.attachedRigidbody.velocity, other.transform.forward);
// Angular velocity around up axis of objects, in radians/sec
float angular_vel = -(other.attachedRigidbody == null ? Vector3.zero : other.attachedRigidbody.angularVelocity).y;
Detected.Add(other, new Detected2DObject()
{
Id = objId++,
Label = label,
Score = 1.0f,
Position = new Vector2(detectedRect.x, detectedRect.y),
Scale = new Vector2(detectedRect.z, detectedRect.w),
LinearVelocity = new Vector3(linear_vel, 0, 0),
AngularVelocity = new Vector3(0, 0, angular_vel),
});
}
}
}
void OnColliderExit(Collider other)
private uint GetNextID(Collider other)
{
if (Detected.ContainsKey(other))
int instanceID = other.gameObject.GetInstanceID();
if (!IDByInstanceID.ContainsKey(instanceID))
{
Detected.Remove(other);
IDByInstanceID.Add(instanceID, (uint)IDByInstanceID.Count);
}
return IDByInstanceID[instanceID];
}
public override void OnVisualize(Visualizer visualizer)
{
foreach (var v in Detected)
foreach (var box in Visualized)
{
var collider = v.Key;
if (!collider.gameObject.activeInHierarchy)
{
return;
}
var box = v.Value;
var min = box.Position - box.Scale / 2;
var max = box.Position + box.Scale / 2;
Color color = Color.magenta;
if (v.Value.Label == "Car")
Color color;
switch (box.Label)
{
color = Color.green;
}
else if (v.Value.Label == "Pedestrian")
{
color = Color.yellow;
}
else if (v.Value.Label == "bicycle")
{
color = Color.cyan;
case "Car":
color = Color.green;
break;
case "Pedestrian":
color = Color.yellow;
break;
case "bicycle":
color = Color.cyan;
break;
default:
color = Color.magenta;
break;
}
AAWireBoxes.Draw(min, max, color);
}
visualizer.UpdateRenderTexture(Camera.activeTexture, Camera.aspect);
......@@ -371,7 +366,7 @@ namespace Simulator.Sensors
public override void OnVisualizeToggle(bool state)
{
// TODO clear Detected on toggle or detect respawn
//
}
}
}
......@@ -5,6 +5,7 @@
*
*/
using System;
using System.Linq;
using System.Collections.Generic;
using UnityEngine;
......@@ -19,7 +20,7 @@ namespace Simulator.Sensors
public class GroundTruth3DSensor : SensorBase
{
[SensorParameter]
[Range(1f, 100f)]
[Range(1f, 100f)]
public float Frequency = 10.0f;
[SensorParameter]
......@@ -38,43 +39,38 @@ namespace Simulator.Sensors
private IWriter<Detected3DObjectData> Writer;
private Dictionary<Collider, Detected3DObject> Detected = new Dictionary<Collider, Detected3DObject>();
private Dictionary<Collider, Box> Visualized = new Dictionary<Collider, Box>();
struct Box
{
public Vector3 Size;
public Color Color;
}
private Dictionary<int, uint> IDByInstanceID = new Dictionary<int, uint>();
private Collider[] Visualized = Array.Empty<Collider>();
void Start()
{
WireframeBoxes = SimulatorManager.Instance.WireframeBoxes;
rangeTrigger.SetCallbacks(OnEnterRange, WhileInRange, OnExitRange);
rangeTrigger.SetCallbacks(WhileInRange);
rangeTrigger.transform.localScale = MaxDistance * Vector3.one;
nextSend = Time.time + 1.0f / Frequency;
}
void Update()
{
if (Bridge == null || Bridge.Status != Status.Connected)
{
return;
}
if (Time.time < nextSend)
if (Bridge != null && Bridge.Status == Status.Connected)
{
return;
}
if (Time.time < nextSend)
{
return;
}
Writer.Write(new Detected3DObjectData()
{
Name = Name,
Frame = Frame,
Time = SimulatorManager.Instance.CurrentTime,
Sequence = seqId++,
Writer.Write(new Detected3DObjectData()
{
Name = Name,
Frame = Frame,
Time = SimulatorManager.Instance.CurrentTime,
Sequence = seqId++,
Data = Detected.Values.ToArray(),
});
Data = Detected.Values.ToArray(),
});
Visualized = Detected.Keys.ToArray();
Detected.Clear();
}
}
public override void OnBridgeSetup(IBridge bridge)
......@@ -83,94 +79,78 @@ namespace Simulator.Sensors
Writer = Bridge.AddWriter<Detected3DObjectData>(Topic);
}
void OnEnterRange(Collider other)
void WhileInRange(Collider other)
{
if (other.isTrigger)
{
return;
}
if (!other.gameObject.activeInHierarchy)
if (other.isTrigger || !other.gameObject.activeInHierarchy)
{
return;
}
if (!Detected.ContainsKey(other))
{
var bbox = new Box();
string label = null;
Vector3 size;
float y_offset = 0.0f;
float y_offset;
float linear_vel; // Linear velocity in forward direction of objects, in meters/sec
float angular_vel; // Angular velocity around up axis of objects, in radians/sec
if (other is MeshCollider)
{
var mesh = other as MeshCollider;
var npcC = mesh.gameObject.GetComponentInParent<NPCController>();
if (npcC != null)
{
bbox.Size = npcC.bounds.size;
size.x = npcC.bounds.size.x;
size.y = npcC.bounds.size.y;
size.z = npcC.bounds.size.z;
y_offset = 0f;
linear_vel = Vector3.Dot(npcC.GetVelocity(), other.transform.forward);
angular_vel = -npcC.GetAngularVelocity().y;
}
else
{
var egoA = mesh.GetComponent<VehicleActions>();
bbox.Size = egoA.bounds.size;
size.x = egoA.bounds.size.z;
size.y = egoA.bounds.size.x;
size.z = egoA.bounds.size.y;
y_offset = 0f;
linear_vel = Vector3.Dot(other.attachedRigidbody == null ? Vector3.zero : other.attachedRigidbody.velocity, other.transform.forward);
angular_vel = -(other.attachedRigidbody == null ? Vector3.zero : other.attachedRigidbody.angularVelocity).y;
}
}
else if (other is BoxCollider)
{
var box = other as BoxCollider;
bbox.Size = box.size;
size.x = box.size.z;
size.y = box.size.x;
size.z = box.size.y;
y_offset = box.center.y;
}
else if (other is CapsuleCollider)
{
var capsule = other as CapsuleCollider;
bbox.Size = new Vector3(capsule.radius * 2, capsule.height, capsule.radius * 2);
var pedC = other.GetComponent<PedestrianController>();
size.x = capsule.radius * 2;
size.y = capsule.radius * 2;
size.z = capsule.height;
y_offset = capsule.center.y;
linear_vel = Vector3.Dot(pedC.CurrentVelocity, other.transform.forward);
angular_vel = -pedC.CurrentAngularVelocity.y;
}
else
{
return;
}
if (size.magnitude == 0)
{
return;
}
string label;
if (other.gameObject.layer == LayerMask.NameToLayer("NPC"))
{
label = "Car";
bbox.Color = Color.green;
}
else if (other.gameObject.layer == LayerMask.NameToLayer("Pedestrian"))
{
label = "Pedestrian";
bbox.Color = Color.yellow;
}
else if (other.gameObject.layer == LayerMask.NameToLayer("Bicycle"))
{
label = "bicycle";
bbox.Color = Color.cyan;
}
else
{
bbox.Color = Color.magenta;
}
Visualized.Add(other, bbox);
if (string.IsNullOrEmpty(label))
{
return;
}
......@@ -187,14 +167,9 @@ namespace Simulator.Sensors
// Convert from (Right/Up/Forward) to (Forward/Left/Up)
relRot.Set(relRot.z, -relRot.x, relRot.y, relRot.w);
// Linear velocity in forward direction of objects, in meters/sec
float linear_vel = Vector3.Dot(other.GetComponent<NPCController>().GetVelocity(), other.transform.forward);
// Angular velocity around up axis of objects, in radians/sec
float angular_vel = -other.GetComponent<NPCController>().GetAngularVelocity().y;
Detected.Add(other, new Detected3DObject()
{
Id = objId++,
Id = GetNextID(other),
Label = label,
Score = 1.0f,
Position = relPos,
......@@ -206,56 +181,62 @@ namespace Simulator.Sensors
}
}
void WhileInRange(Collider other)
{
if (Detected.ContainsKey(other))
{
// Local position of object in Lidar local space
Vector3 relPos = transform.InverseTransformPoint(other.transform.position);
// Lift up position to the ground
//if (other is MeshCollider) relPos.y += ((MeshCollider)other).bounds.center.y;
if (other is BoxCollider) relPos.y += ((BoxCollider)other).center.y;
else if (other is CapsuleCollider) relPos.y += ((CapsuleCollider)other).center.y;
// Convert from (Right/Up/Forward) to (Forward/Left/Up)
relPos.Set(relPos.z, -relPos.x, relPos.y);
// Relative rotation of objects wrt Lidar frame
Quaternion relRot = Quaternion.Inverse(transform.rotation) * other.transform.rotation;
// Convert from (Right/Up/Forward) to (Forward/Left/Up)
relRot.Set(relRot.z, -relRot.x, relRot.y, relRot.w);
Detected[other].Position = relPos;
Detected[other].Rotation = relRot;
Detected[other].LinearVelocity = Vector3.right * Vector3.Dot(other.GetComponent<NPCController>().GetVelocity(), other.transform.forward);
Detected[other].AngularVelocity = Vector3.left * other.GetComponent<NPCController>().GetAngularVelocity().y;
}
}
void OnExitRange(Collider other)
private uint GetNextID(Collider other)
{
if (Detected.ContainsKey(other))
int instanceID = other.gameObject.GetInstanceID();
if (!IDByInstanceID.ContainsKey(instanceID))
{
Detected.Remove(other);
IDByInstanceID.Add(instanceID, (uint)IDByInstanceID.Count);
}
if (Visualized.ContainsKey(other))
{
Visualized.Remove(other);
}
return IDByInstanceID[instanceID];
}
public override void OnVisualize(Visualizer visualizer)
{
foreach (var v in Visualized)
foreach (var other in Visualized)
{
var collider = v.Key;
if (!collider.gameObject.activeInHierarchy)
if (!other.gameObject.activeInHierarchy)
{
return;
}
var box = v.Value;
WireframeBoxes.Draw(collider.gameObject.transform.localToWorldMatrix, collider is MeshCollider ? Vector3.zero : new Vector3(0f, collider.bounds.extents.y, 0f), box.Size, box.Color);
Vector3 size = Vector3.zero;
if (other is MeshCollider)
{
var mesh = other as MeshCollider;
var npcC = mesh.gameObject.GetComponentInParent<NPCController>();
if (npcC != null)
{
size = npcC.bounds.size;
}
else
{
var egoA = mesh.GetComponent<VehicleActions>();
size = egoA.bounds.size;
}
}
else if (other is CapsuleCollider)
{
var capsule = other as CapsuleCollider;
size = new Vector3(capsule.radius * 2, capsule.height, capsule.radius * 2);
}
Color color = Color.magenta;
if (other.gameObject.layer == LayerMask.NameToLayer("NPC"))
{
color = Color.green;
}
else if (other.gameObject.layer == LayerMask.NameToLayer("Pedestrian"))
{
color = Color.yellow;
}
else if (other.gameObject.layer == LayerMask.NameToLayer("Bicycle"))
{
color = Color.cyan;
}
WireframeBoxes.Draw(other.gameObject.transform.localToWorldMatrix, other is MeshCollider ? Vector3.zero : new Vector3(0f, other.bounds.extents.y, 0f), size, color);
}
}
......
......@@ -10,29 +10,15 @@ using UnityEngine;
public class RangeTrigger : MonoBehaviour
{
Action<Collider> triggerEnter;
Action<Collider> triggerStay;
Action<Collider> triggerExit;
LayerMask mask;
public void SetCallbacks(Action<Collider> enter, Action<Collider> stay, Action<Collider> exit)
public void SetCallbacks(Action<Collider> stay)
{
triggerEnter = enter;
triggerStay = stay;
triggerExit = exit;
mask = LayerMask.GetMask("NPC", "Pedestrian", "Bicycle");
}
private void OnTriggerEnter(Collider other)
{
if (triggerEnter == null || ((mask.value >> other.gameObject.layer) & 1) == 0)
{
return;
}
triggerEnter(other);
}
void OnTriggerStay(Collider other)
{
if (triggerStay == null || ((mask.value >> other.gameObject.layer) & 1) == 0)
......@@ -42,14 +28,4 @@ public class RangeTrigger : MonoBehaviour
triggerStay(other);
}
private void OnTriggerExit(Collider other)
{
if (triggerExit == null || ((mask.value >> other.gameObject.layer) & 1) == 0)
{
return;
}
triggerExit(other);
}
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment