// TBD: this shouldn't be static, I believe
static void calculationsForBoundingBox()
{
//Debug.Log("started bounding boxes processing thread");
//while (true)
{
try
{
//Thread.Sleep (2);
// BoxData temp;
// Debug.Log ("box before dequeue");
// bool success = boxBufferToCalc.TryDequeue (out temp);
// Debug.Log ("box dequeue: " + success);
// if (success) {
Debug.LogFormat("checking bounding boxes for calculations... {0}", boundingBoxBufferToCalc_.Count);
while (boundingBoxBufferToCalc_.Count > 0)
{
BoxData frameBoxData = boundingBoxBufferToCalc_.Dequeue();
int boxCount = frameBoxData.count;
//Vector3[] min = new Vector3[boxCount];
float[] averageZ = new float[boxCount];
int[] numWithinBox = new int[boxCount];
List <float>[] pointsInBounds = new List <float> [boxCount];
for (int i = 0; i < boxCount; i++)
{
//min [i] = new Vector3 (100, 100, 100);
pointsInBounds[i] = new List <float>();
averageZ[i] = 0;
numWithinBox[i] = 0;
}
List <Vector4> frameCloudPoints = frameBoxData.points;
Debug.LogFormat("pointcloud points count {0}. applying transform", frameCloudPoints.Count);
frameBoxData.cam.transform.position = frameBoxData.camPos;
frameBoxData.cam.transform.rotation = frameBoxData.camRot;
Vector2[] centerPosXY = new Vector2[boxCount];
Vector2[] worldCenter = new Vector2[boxCount];
Vector3[] position = new Vector3[boxCount];
Vector2[] viewportTopLeft = new Vector2[boxCount];
Vector2[] viewportTopRight = new Vector2[boxCount];
Vector2[] viewportBottomLeft = new Vector2[boxCount];
Vector2[] viewportBottomRight = new Vector2[boxCount];
Vector3[] worldTopLeft = new Vector3[boxCount];
Vector3[] worldTopRight = new Vector3[boxCount];
Vector3[] worldBottomLeft = new Vector3[boxCount];
Vector3[] worldBottomRight = new Vector3[boxCount];
float[] x = new float[boxCount];
float[] y = new float[boxCount];
float[] z = new float[boxCount];
for (int i = 0; i < boxCount; i++)
{
//calucate 4 viewport corners
viewportTopLeft[i] = new Vector2(frameBoxData.xleft[i], frameBoxData.ytop[i]);
viewportTopRight[i] = new Vector2(frameBoxData.xright[i], frameBoxData.ytop[i]);
viewportBottomLeft[i] = new Vector2(frameBoxData.xleft[i], frameBoxData.ybottom[i]);
viewportBottomRight[i] = new Vector2(frameBoxData.xright[i], frameBoxData.ybottom[i]);
//calculate center of box in viewport coords
centerPosXY[i] = new Vector2(frameBoxData.xleft[i] + Mathf.Abs(viewportTopLeft[i].x - viewportTopRight[i].x) / 2,
frameBoxData.ybottom[i] + Mathf.Abs(viewportTopLeft[i].y - viewportBottomLeft[i].y) / 2);
Debug.LogFormat("bbox {0}: topleft: {1} topright {2} botleft {3} botright {4} center {5}",
frameBoxData.label[i],
viewportTopLeft[i].ToString(), viewportTopRight[i].ToString(),
viewportBottomLeft[i].ToString(), viewportBottomRight[i].ToString(),
centerPosXY[i].ToString());
}
try
{
// iterating all cloud points to place them within each bounding box
for (int i = 0; i < frameCloudPoints.Count; i++)
{
for (int j = 0; j < boxCount; j++)
{
// //calculate center of box in world coords
// worldCenter [j] = temp.cam.ViewportToWorldPoint (new Vector2 (centerPosXY [j].x, centerPosXY [j].y));
// //find point in points[] that most nearly matches center position
// if (Vector2.Distance (new Vector2 (points [i].x, points [i].y), worldCenter [j]) < Vector2.Distance (new Vector2 (min [j].x, min [j].y), worldCenter [j])) {
// min [j] = points [i];
// }
//find if points[i] is outside of the bounding box
Vector3 viewportPoint = frameBoxData.cam.WorldToViewportPoint(frameCloudPoints[i]);
if (viewportPoint.x < frameBoxData.xleft[j] ||
viewportPoint.x > frameBoxData.xright[j] ||
viewportPoint.y <frameBoxData.ybottom[j] ||
viewportPoint.y> frameBoxData.ytop[j])
{
//points[i] is out of the limits of the bounding box
}
else
{
//points[i] is in the bounding box
pointsInBounds[j].Add(frameCloudPoints[i].z);
averageZ[j] += frameCloudPoints[i].z;
numWithinBox[j]++;
}
}
}
}
catch (System.Exception e)
{
Debug.LogExceptionFormat(e, "while counting sorting points");
}
for (int i = 0; i < boxCount; i++)
{
float median;
float depth;
// every list has just Z coordinate of a point, sort them
pointsInBounds[i].Sort();
//median = pointsInBounds [i][pointsInBounds[i].Count / 2];
//Debug.Log("median = " + median);
//averageZ [i] /= numWithinBox [i];
// if there are no points for particular box - it won't be rendered, apparently
if (!(pointsInBounds[i].Count == 0))
{
//float depth = Mathf.Abs(min [i].z);
Debug.LogFormat("median: float array len {0}, idx {1}", pointsInBounds[i].Count, pointsInBounds[i].Count / 2);
median = pointsInBounds[i][pointsInBounds[i].Count / 2];
Debug.LogFormat("median {0}", median);
depth = Mathf.Abs(median);
//float depth = Mathf.Abs (averageZ [i]);
if (depth < 0.5f)
{
depth = 0.5f;
}
//calculate center of box in world coords
// i believe this is not correct -- depth is a cloud point's Z location, i.e. in world coordiantes
// here, it's assigned as a Z component of a vector, which X and Y are in Viewport coordinate system
// it then gets transformed to the world coordinates again
// Z value shall be assigned after this transformation is done
position[i] = frameBoxData.cam.ViewportToWorldPoint(new Vector3(centerPosXY[i].x, centerPosXY[i].y, depth));
Debug.LogFormat("box {0}: position {1}", i, position[i].ToString());
//Debug.Log ("box: found min " + min.ToString ());
//calculate Z value for world corners
worldTopLeft[i] = frameBoxData.cam.ViewportToWorldPoint(new Vector3(viewportTopLeft[i].x, viewportTopLeft[i].y, depth));
worldTopRight[i] = frameBoxData.cam.ViewportToWorldPoint(new Vector3(viewportTopRight[i].x, viewportTopRight[i].y, depth));
worldBottomLeft[i] = frameBoxData.cam.ViewportToWorldPoint(new Vector3(viewportBottomLeft[i].x, viewportBottomLeft[i].y, depth));
worldBottomRight[i] = frameBoxData.cam.ViewportToWorldPoint(new Vector3(viewportBottomRight[i].x, viewportBottomRight[i].y, depth));
//calculate x, y, z size values
x[i] = Mathf.Abs(Vector3.Distance(worldTopLeft[i], worldTopRight[i]));
y[i] = Mathf.Abs(Vector3.Distance(worldTopLeft[i], worldBottomLeft[i]));
z[i] = 0; // why Z is zero here?
{
CreateBoxData boxData = new CreateBoxData();
boxData.label = frameBoxData.label[i];
boxData.position = position[i];
boxData.x = x[i];
boxData.y = y[i];
boxData.z = z[i];
boxData.cam = frameBoxData.cam;
boxData.frameNum = frameBoxData.frameNumber;
boxData.timestamp = frameBoxData.timestamp;
//boxBufferToUpdate.Enqueue (boxData);
boundingBoxBufferToUpdate_.Enqueue(boxData);
}
}
}
}
}
catch (System.Exception e)
{
Debug.LogExceptionFormat(e, "while making bb calculations");
}
}
}
static void calculationsForBoundingBox()
{
while (true)
{
try
{
//Thread.Sleep (2);
// BoxData temp;
// Debug.Log ("box before dequeue");
// bool success = boxBufferToCalc.TryDequeue (out temp);
// Debug.Log ("box dequeue: " + success);
// if (success) {
if (boundingBoxBufferToCalc.Count > 0)
{
BoxData temp = boundingBoxBufferToCalc.Dequeue();
int boxCount = temp.count;
//Vector3[] min = new Vector3[boxCount];
float[] averageZ = new float[boxCount];
int[] numWithinBox = new int[boxCount];
List <float>[] pointsInBounds = new List <float> [boxCount];
for (int i = 0; i < boxCount; i++)
{
//min [i] = new Vector3 (100, 100, 100);
pointsInBounds[i] = new List <float>();
averageZ [i] = 0;
numWithinBox [i] = 0;
}
List <Vector4> points = temp.points;
//int count = temp.numPoints;
int count = points.Count;
Debug.Log("Pointcloud count points: " + points.Count);
Debug.Log("Pointcloud count count: " + count);
temp.cam.transform.position = temp.camPos;
temp.cam.transform.rotation = temp.camRot;
Debug.Log("Camera log: cam position" + temp.cam.transform.position.ToString());
Debug.Log("Camera log: frame cam position" + temp.camPos.ToString());
Debug.Log("Camera log: cam rotation" + temp.cam.transform.rotation.ToString());
Debug.Log("Camera log: frame cam rotation" + temp.camRot.ToString());
Vector2[] centerPosXY = new Vector2[boxCount];
Vector2[] worldCenter = new Vector2[boxCount];
Vector3[] position = new Vector3[boxCount];
Vector2[] viewportTopLeft = new Vector2[boxCount];
Vector2[] viewportTopRight = new Vector2[boxCount];
Vector2[] viewportBottomLeft = new Vector2[boxCount];
Vector2[] viewportBottomRight = new Vector2[boxCount];
Vector3[] worldTopLeft = new Vector3[boxCount];
Vector3[] worldTopRight = new Vector3[boxCount];
Vector3[] worldBottomLeft = new Vector3[boxCount];
Vector3[] worldBottomRight = new Vector3[boxCount];
float[] x = new float[boxCount];
float[] y = new float[boxCount];
float[] z = new float[boxCount];
for (int i = 0; i < boxCount; i++)
{
//calucate 4 viewport corners
viewportTopLeft [i] = new Vector2(temp.xleft [i], temp.ytop [i]);
viewportTopRight [i] = new Vector2(temp.xright [i], temp.ytop [i]);
viewportBottomLeft [i] = new Vector2(temp.xleft [i], temp.ybottom [i]);
viewportBottomRight [i] = new Vector2(temp.xright [i], temp.ybottom [i]);
//calculate center of box in viewport coords
centerPosXY [i] = new Vector2(temp.xleft [i] + Mathf.Abs(viewportTopLeft [i].x - viewportTopRight [i].x) / 2,
temp.ybottom [i] + Mathf.Abs(viewportTopLeft [i].y - viewportBottomLeft [i].y) / 2);
}
try{
//search points[]
for (int i = 0; i < count; i++)
{
for (int j = 0; j < boxCount; j++)
{
// //calculate center of box in world coords
// worldCenter [j] = temp.cam.ViewportToWorldPoint (new Vector2 (centerPosXY [j].x, centerPosXY [j].y));
// //find point in points[] that most nearly matches center position
// if (Vector2.Distance (new Vector2 (points [i].x, points [i].y), worldCenter [j]) < Vector2.Distance (new Vector2 (min [j].x, min [j].y), worldCenter [j])) {
// min [j] = points [i];
// }
//find if points[i] is outside of the bounding box
Vector3 viewportPoint = temp.cam.WorldToViewportPoint(points[i]);
if (viewportPoint.x < temp.xleft[j] || viewportPoint.x > temp.xright[j] || viewportPoint.y <temp.ybottom[j] || viewportPoint.y> temp.ytop[j])
{
//points[i] is out of the limits of the bounding box
}
else
{
//points[i] is in the bounding box
pointsInBounds[j].Add(points[i].z);
averageZ[j] += points[i].z;
numWithinBox[j]++;
}
}
}
}
catch (System.Exception e)
{
Debug.Log("exception caught here" + e.ToString());
}
for (int i = 0; i < boxCount; i++)
{
float median;
float depth;
pointsInBounds [i].Sort();
//median = pointsInBounds [i][pointsInBounds[i].Count / 2];
//Debug.Log("median = " + median);
//averageZ [i] /= numWithinBox [i];
if (!(pointsInBounds[i].Count == 0))
{
//float depth = Mathf.Abs(min [i].z);
Debug.Log("Median: Length of float array: " + pointsInBounds[i].Count);
Debug.Log("Median: Index of median: " + pointsInBounds[i].Count / 2);
median = pointsInBounds [i][pointsInBounds[i].Count / 2];
Debug.Log("Median: " + median);
depth = Mathf.Abs(median);
//float depth = Mathf.Abs (averageZ [i]);
if (depth < 0.5f)
{
depth = 0.5f;
}
//calculate center of box in world coords
position [i] = temp.cam.ViewportToWorldPoint(new Vector3(centerPosXY [i].x, centerPosXY [i].y, depth));
Debug.Log("box position: " + position.ToString());
//Debug.Log ("box: found min " + min.ToString ());
//calculate Z value for world corners
worldTopLeft [i] = temp.cam.ViewportToWorldPoint(new Vector3(viewportTopLeft [i].x, viewportTopLeft [i].y, depth));
worldTopRight [i] = temp.cam.ViewportToWorldPoint(new Vector3(viewportTopRight [i].x, viewportTopRight [i].y, depth));
worldBottomLeft [i] = temp.cam.ViewportToWorldPoint(new Vector3(viewportBottomLeft [i].x, viewportBottomLeft [i].y, depth));
worldBottomRight [i] = temp.cam.ViewportToWorldPoint(new Vector3(viewportBottomRight [i].x, viewportBottomRight [i].y, depth));
//calculate x, y, z size values
x [i] = Mathf.Abs(Vector3.Distance(worldTopLeft [i], worldTopRight [i]));
y [i] = Mathf.Abs(Vector3.Distance(worldTopLeft [i], worldBottomLeft [i]));
z [i] = 0;
if (temp.prob[i] >= 0.6f)
{
CreateBoxData boxData = new CreateBoxData();
boxData.label = temp.label [i];
boxData.position = position [i];
boxData.x = x [i];
boxData.y = y [i];
boxData.z = z [i];
boxData.cam = temp.cam;
boxData.frameNum = temp.frameNumber;
boxData.timestamp = temp.timestamp;
//boxBufferToUpdate.Enqueue (boxData);
boundingBoxBufferToUpdate.Enqueue(boxData);
}
}
}
}
}
catch (System.Exception e)
{
Debug.Log("exception caught" + e.ToString());
}
}
}
void Update()
{
calculationsForBoundingBox();
Debug.LogFormat("running update for {0} bounding boxes", boundingBoxBufferToUpdate_.Count);
try
{
for (int i = 0; i < boundingBoxBufferToUpdate_.Count; i++)
{
CreateBoxData temp = boundingBoxBufferToUpdate_.Dequeue();
Debug.Log("frame number for box: " + temp.frameNum);
textbox.text = "Yolo " + temp.frameNum;
Debug.Log("queue size: " + i);
Color c = colors_[UnityEngine.Random.Range(0, colors_.Count)];
List <BoundingBox> boundingBoxes;
CreateBoxData box = new CreateBoxData();
bool updatedBox = false;
//found color for this label
//boxMgr.CreateBoundingBoxObject (temp.position, temp.x, temp.y, temp.z, temp.label, c);
if (boxMgr_.boundingBoxObjects.TryGetValue(temp.label, out boundingBoxes))
{
//Debug.Log ("Update found label");
for (int j = 0; j < boundingBoxes.Count; j++)
{
//find bounding box and label that matches
//Debug.Log ("Update searching list for box");
//float distance = Vector3.Distance (temp.position, boundingBoxes [j].box.transform.position);
float distance = Vector2.Distance(Camera.main.WorldToViewportPoint(temp.position), Camera.main.WorldToViewportPoint(boundingBoxes[j].box.transform.position));
//Vector3 direction = new Vector3 (temp.position.x - boundingBoxes [j].box.transform.position.x, temp.position.y - boundingBoxes [j].box.transform.position.y, temp.position.z - boundingBoxes [j].box.transform.position.z);
//float speed = distance / (Mathf.Abs ((float)(temp.timestamp - offset.m_screenUpdateTime)));
//Debug.Log("Distance and speed: " + distance + ", " + speed);
if (distance < 0.2f)
{
//Debug.Log ("Update found box");
Vector3 filteredPos = kalman_[boundingBoxes[j].guid].Update(temp.position);
boxMgr_.UpdateBoundingBoxObject(boundingBoxes[j], temp.position, temp.x, temp.y, temp.z, temp.label, temp.position);
Debug.Log("Update bounding box: " + temp.label);
updatedBox = true;
}
}
//none of the labels looked like the wanted box, must be a new instance of this label
if (!updatedBox)
{
box.position = temp.position;
box.x = temp.x;
box.y = temp.y;
box.z = temp.z;
box.label = temp.label;
boxData_.Add(box);
}
}
else
{
//boxMgr.CreateBoundingBoxObject (temp.position, temp.x, temp.y, temp.z, temp.label, c);
box.position = temp.position;
box.x = temp.x;
box.y = temp.y;
box.z = temp.z;
box.label = temp.label;
boxData_.Add(box);
}
}
}
catch (System.Exception e)
{
Debug.LogExceptionFormat(e, "while updating bounding boxes");
}
try
{
Debug.LogFormat("will create {0} new boxes", boxData_.Count);
if (boxData_.Count > 0)
{
CreateBoxes(boxData_);
}
}
catch (System.Exception e)
{
Debug.LogExceptionFormat(e, "while creating new bounding boxes");
}
}
void Update()
{
if (Input.touchCount == 1)
{
//spawnBox();
}
// foreach( KeyValuePair<string, List<BoundingBox>> kvp in boxMgr.boundingBoxObjects )
// {
// for (int i = 0; i < kvp.Value.Count; i++) {
// //update box position and size?
// Vector3 position = Vector3.Lerp (kvp.Value [i].box.transform.position, kvp.Value [i].target, kvp.Value [i].speed);
// boxMgr.UpdateBoundingBoxObject (kvp.Value [i], position, kvp.Value [i].x, kvp.Value [i].y, kvp.Value [i].z, kvp.Value [i].direction, kvp.Value [i].speed, kvp.Value [i].target);
// }
// }
// string output = "";
// foreach( KeyValuePair<string, List<BoundingBox>> kvp in boxMgr.boundingBoxObjects )
// {
// output = output + kvp.Key.ToString () + ", " + kvp.Value.ToString() + ": ";
// }
// Debug.Log ("Bounding box label list: " + output);
//int max = boxBufferToUpdate.Count;
// for(int i = 0; i < max; i++) {
// CreateBoxData temp;
// bool success = boxBufferToUpdate.TryDequeue (out temp);
// if (success) {
// boxMgr.CreateBoundingBoxObject (temp.position, temp.x, temp.y, temp.z, temp.label, colors [Random.Range (0, colors.Count)]);
// }
// }
int max = boundingBoxBufferToUpdate.Count;
for (int i = 0; i < max; i++)
{
CreateBoxData temp = boundingBoxBufferToUpdate.Dequeue();
Debug.Log("frame number for box: " + temp.frameNum);
textbox.text = "Yolo " + temp.frameNum;
Debug.Log("queue size: " + i);
Color c = colors [UnityEngine.Random.Range(0, colors.Count)];
List <BoundingBox> boundingBoxes;
CreateBoxData box = new CreateBoxData();
bool updatedBox = false;
//found color for this label
//boxMgr.CreateBoundingBoxObject (temp.position, temp.x, temp.y, temp.z, temp.label, c);
if (boxMgr.boundingBoxObjects.TryGetValue(temp.label, out boundingBoxes))
{
try {
//Debug.Log ("Update found label");
for (int j = 0; j < boundingBoxes.Count; j++)
{
//find bounding box and label that matches
//Debug.Log ("Update searching list for box");
//float distance = Vector3.Distance (temp.position, boundingBoxes [j].box.transform.position);
float distance = Vector2.Distance(Camera.main.WorldToViewportPoint(temp.position), Camera.main.WorldToViewportPoint(boundingBoxes [j].box.transform.position));
//Vector3 direction = new Vector3 (temp.position.x - boundingBoxes [j].box.transform.position.x, temp.position.y - boundingBoxes [j].box.transform.position.y, temp.position.z - boundingBoxes [j].box.transform.position.z);
//float speed = distance / (Mathf.Abs ((float)(temp.timestamp - offset.m_screenUpdateTime)));
//Debug.Log("Distance and speed: " + distance + ", " + speed);
if (distance < 0.2f)
{
//Debug.Log ("Update found box");
//Vector3 position = Vector3.Lerp (boundingBoxes [j].box.transform.position, temp.position);
//Debug.Log("update info: " + boundingBoxes [j] + "; " + temp.position + "; " + temp.x + "; " + temp.y + "; " + temp.z + "; " + temp.position);
Vector3 filteredPos = kalman[boundingBoxes[j].guid].Update(temp.position);
boxMgr.UpdateBoundingBoxObject(boundingBoxes [j], temp.position, temp.x, temp.y, temp.z, temp.label, temp.position);
Debug.Log("Update bounding box: " + temp.label);
updatedBox = true;
}
}
//none of the labels looked like the wanted box, must be a new instance of this label
if (!updatedBox)
{
box.position = temp.position;
box.x = temp.x;
box.y = temp.y;
box.z = temp.z;
box.label = temp.label;
boxData.Add(box);
}
} catch (System.Exception e) {
Debug.Log("exception caught box update: " + e);
}
}
else
{
//boxMgr.CreateBoundingBoxObject (temp.position, temp.x, temp.y, temp.z, temp.label, c);
box.position = temp.position;
box.x = temp.x;
box.y = temp.y;
box.z = temp.z;
box.label = temp.label;
boxData.Add(box);
}
// if(labelColors.TryGetValue(temp.label, out c))
// {
// //found color for this label
// boxMgr.CreateBoundingBoxObject (temp.position, temp.x, temp.y, temp.z, temp.label, c);
//
// if(boxMgr.boundingBoxObjects.TryGetValue(temp.label, out boundingBoxes))
// {
// try{
// Debug.Log ("Update found label");
// for(int j = 0; j < boundingBoxes.Count; j++)
// {
// //find bounding box and label that matches
// Debug.Log ("Update searching list for box");
// float distance = Vector3.Distance (temp.position, boundingBoxes [j].box.transform.position);
// Vector3 direction = new Vector3 (temp.position.x - boundingBoxes [j].box.transform.position.x, temp.position.y - boundingBoxes [j].box.transform.position.y, temp.position.z - boundingBoxes [j].box.transform.position.z);
// float speed = distance * (Mathf.Abs ((float)(temp.timestamp - offset.m_screenUpdateTime)));
// if ( distance < 0.1f) {
// Debug.Log ("Update found box");
// Vector3 position = Vector3.Lerp (boundingBoxes [j].box.transform.position, temp.position, speed);
// boxMgr.UpdateBoundingBoxObject (boundingBoxes [j], temp.position, temp.x, temp.y, temp.z, direction, speed, temp.position);
// Debug.Log ("Update bounding box: " + temp.label);
// }
// }
// }
// catch(System.Exception e) {
// Debug.Log("exception caught box update: " + e);
// }
// }
// }
// else
// {
// //label is not in the dictionary, add it and assign color
// labelColors.Add(temp.label, colors [Random.Range (0, colors.Count)]);
// boxMgr.CreateBoundingBoxObject (temp.position, temp.x, temp.y, temp.z, temp.label, labelColors [temp.label]);
//
// }
}
// string output = "label output = ";
// foreach( KeyValuePair<string, Color> kvp in labelColors )
// {
// output = output + "; " + kvp.Key + ", " + kvp.Value;
// }
// Debug.Log (output);
if (boxData.Count > 0)
{
CreateBoxes(boxData);
}
}
