private void Clumpify()
{
for (int y = 0; y < colorImage.height; y++)
{
for (int x = 0; x < colorImage.width; x++)
{
//Checks if the difference in color is within the threshold
Color thisColor = this.contrastImage.GetPixel(x, y);
if (thisColor == Color.black && depthImage.GetPixel(x, y) != Color.black)
{
//Perform flood fill
Clump clump = new Clump(FloodFill(x, y, Color.white));
//Calculate average color of clump
clump.Color = ImageUtils.CalculateAverageColor(clump.GetClumpPixels(colorImage));
//Save clump
clumpQueue.Enqueue(clump);
}
}
}
}
public IEnumerable <GameObject> GetShapes()
{
float angle = Vector3.Angle(normalVector, new Vector3(0, 0, 1));
int i = 0;
while (clumpQueue.Count > 0)
{
//Calculate average position
Clump clump = clumpQueue.Dequeue();
//check for little clumps to ignore
if (clump.Points.Count >= CLUMPTHRESH)
{
IEnumerable <Vector3> points3D = clump.Points.Select(pix => camera.Get3DPointFromPixel((int)pix.x, (int)pix.y));
Vector3 averagePoint = ConvertCoordinates(AveragePoint(points3D), angle);
//set all poses to lie on the ground (y = 0.5) times the scale
averagePoint.y = 0;
logText += "Object: " + i + ": Position = " + averagePoint.ToString() + "\n";
//logText = "HELLO";
i++;
//check for object types
//blue
if (clump.Color.b > clump.Color.r && clump.Color.b > clump.Color.g)
{
if (enableBlueObj)
{
GameObject blueMesh = (GameObject)UnityEngine.Object.Instantiate(Resources.Load(blueFilename));
blueMesh.transform.position = averagePoint;
blueMesh.transform.rotation = new Quaternion(0, 0, 0, 0);
blueMesh.transform.localScale *= BLUESCALE * OBJSCALE;
blueMesh.AddComponent <MeshFilter>();
Bounds b = getBounds(blueMesh);
Vector3 lowerCenter = new Vector3(0, b.center.y - b.extents.y, 0);
blueMesh.transform.position = averagePoint - lowerCenter;
yield return(blueMesh);
}
else
{
GameObject cyl = GameObject.CreatePrimitive(PrimitiveType.Cylinder);
cyl.transform.position = averagePoint;
cyl.transform.rotation = new Quaternion(0, 0, 0, 0);
cyl.transform.localScale = new Vector3(OBJSCALE, OBJSCALE / 2, OBJSCALE);
//cyl.AddComponent<MeshFilter>();
Bounds b = getBounds(cyl);
Vector3 lowerCenter = new Vector3(0, b.center.y - b.extents.y, 0);
cyl.transform.position = averagePoint - lowerCenter;
yield return(cyl);
}
}
//red
else if (clump.Color.r > clump.Color.b && clump.Color.r > clump.Color.g)
{
if (enableRedObj)
{
GameObject redMesh = (GameObject)UnityEngine.Object.Instantiate(Resources.Load(redFilename));
redMesh.transform.position = averagePoint;
redMesh.transform.rotation = new Quaternion(0, 0, 0, 0);
redMesh.transform.localScale *= REDSCALE * OBJSCALE;
redMesh.AddComponent <MeshFilter>();
Bounds b = getBounds(redMesh);
Vector3 lowerCenter = new Vector3(0, b.center.y - b.extents.y, 0);
redMesh.transform.position = averagePoint - lowerCenter;
yield return(redMesh);
}
else
{
GameObject cube = GameObject.CreatePrimitive(PrimitiveType.Cube);
cube.transform.position = averagePoint;
cube.transform.rotation = new Quaternion(0, 0, 0, 0);
cube.transform.localScale *= OBJSCALE;
//cube.AddComponent<MeshFilter>();
Bounds b = getBounds(cube);
Vector3 lowerCenter = new Vector3(0, b.center.y - b.extents.y, 0);
cube.transform.position = averagePoint - lowerCenter;
yield return(cube);
}
}
//green
else if (clump.Color.g > clump.Color.b && clump.Color.g > clump.Color.r)
{
if (enableGreenObj)
{
GameObject greenMesh = (GameObject)UnityEngine.Object.Instantiate(Resources.Load(greenFilename));
greenMesh.transform.position = averagePoint;
greenMesh.transform.rotation = new Quaternion(0, 0, 0, 0);
greenMesh.transform.localScale *= GREENSCALE * OBJSCALE;
greenMesh.AddComponent <MeshFilter>();
Bounds b = getBounds(greenMesh);
Vector3 lowerCenter = new Vector3(0, b.center.y - b.extents.y, 0);
greenMesh.transform.position = averagePoint - lowerCenter;
yield return(greenMesh);
}
else
{
GameObject obj1 = GameObject.CreatePrimitive(PrimitiveType.Cube);
obj1.transform.position = averagePoint;
obj1.transform.rotation = new Quaternion(0, 0, 0, 0);
obj1.transform.localScale *= OBJSCALE;
//obj1.AddComponent<MeshFilter>();
Bounds b = getBounds(obj1);
Vector3 lowerCenter = new Vector3(0, b.center.y - b.extents.y, 0);
obj1.transform.position = averagePoint - lowerCenter;
//Return shape at the given point
yield return(obj1);
}
}
//default behavior
else
{
GameObject obj = GameObject.CreatePrimitive(PrimitiveType.Cube);
obj.transform.position = averagePoint;
obj.transform.rotation = new Quaternion(0, 0, 0, 0);
obj.transform.localScale *= OBJSCALE;
//obj.AddComponent<MeshFilter>();
Bounds b = getBounds(obj);
Vector3 lowerCenter = new Vector3(0, b.center.y - b.extents.y, 0);
obj.transform.position = averagePoint - lowerCenter;
//Return shape at the given point
yield return(obj);
}
}
}
}
