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