Simple Blob Detection

import cl.eye.*;

class TPoint {
  public float x, y, z, h, hh;
  TPoint (float x1, float y1, float z1, float h1, float hh1) {
    x = x1;
    y = y1;
    z = z1;
    h = h1;
    hh = hh1;
  }
  PVector Pos() {
    return new PVector((x/z), (y/z));
  }
  String Color() {
    if ((h/z)>(hh/z)) {
      return "Red";
    }else{
      return "Blue";
    }
  }
}


// Camera Variables
int numCams;
CLCamera myCameras[] = new CLCamera[2];
PImage myImages[] = new PImage[2];
int cameraWidth = 320;
int cameraHeight = 240;
int cameraRate = 125;
int thresh = 190;
ArrayList<TPoint> Blobs = new ArrayList<TPoint>();

void DetectBlob(int x, int y, float h, float hh) {
  Boolean found = true;
  for (TPoint Blub : Blobs) {
    if (dist(Blub.Pos().x, Blub.Pos().y, x, y)<sqrt(Blub.z/PI)*1.4+15) {
      found = false;
      Blub.x += x;
      Blub.y += y;
      Blub.h += h;
      Blub.hh += hh;
      Blub.z++;
    }
  }
  if (found) {
    Blobs.add(new TPoint(x, y, 1,h, hh));
  }
}

void Vision(int i) {
  // Loop through every pixel in the image.
  for (int y = 1; y < myImages[i].height-1; y++) { // Skip top and bottom edges
    for (int x = 1; x < myImages[i].width-1; x++) { // Skip left and right edges
      int Pos = y*myImages[i].width + x;
      if (brightness(myImages[i].pixels[Pos])>200) {
        DetectBlob(x, y, red(myImages[i].pixels[Pos]), blue(myImages[i].pixels[Pos]));
      }else{
        myImages[i].pixels[Pos] = color(0);
      }
    }
  }
  // State that there are changes to edgeImg.pixels[]
  myImages[i].updatePixels();
}
void setup() {
  //CLCamera.loadLibrary("C:/CLEyeMulticam.dll");
  frameRate(125);
  noFill();
  if (!setupCameras()) exit();
}

void draw() {
  background(0);
  // Loops through available cameras and updates
  for (int i = 0; i < numCams; i++) {
    myCameras[i].getCameraFrame(myImages[i].pixels, (i==0) ? 1000 : 0);
    Vision(i);
    image(myImages[i], cameraWidth*i, 0);
    for (TPoint Blub : Blobs) {
      text("POINT: "+floor(Blub.Pos().x)+", "+floor(Blub.Pos().y)+": "+Blub.Color(), Blub.Pos().x+(cameraWidth*i)-30, (Blub.Pos().y)-5);
      stroke(200, 0, 0);
      ellipse((Blub.x/Blub.z)+(cameraWidth*i), Blub.y/Blub.z, (sqrt(Blub.z/PI)*1)*2, (sqrt(Blub.z/PI))*2);
    }
    /*
    if(Blobs.size()>2){
      line(Blobs.get(0).Pos().x*4,Blobs.get(0).Pos().y*4,Blobs.get(1).Pos().x*4,Blobs.get(1).Pos().y*4);
      line(Blobs.get(1).Pos().x*4,Blobs.get(1).Pos().y*4,Blobs.get(2).Pos().x*4,Blobs.get(2).Pos().y*4);
      line(Blobs.get(2).Pos().x*4,Blobs.get(2).Pos().y*4,Blobs.get(0).Pos().x*4,Blobs.get(0).Pos().y*4);
    }
    */
    if(Blobs.size()==3){
      line(Blobs.get(0).Pos().x,Blobs.get(0).Pos().y,Blobs.get(1).Pos().x,Blobs.get(1).Pos().y);
      line(Blobs.get(1).Pos().x,Blobs.get(1).Pos().y,Blobs.get(2).Pos().x,Blobs.get(2).Pos().y);
      line(Blobs.get(2).Pos().x,Blobs.get(2).Pos().y,Blobs.get(0).Pos().x,Blobs.get(0).Pos().y);
    }
    /*
    for(int j = 0;j<Blobs.size()-1;j++){
     line((Blobs.get(j).x/Blobs.get(j).z)+(cameraWidth*i),Blobs.get(j).y/Blobs.get(j).z,(Blobs.get(j+1).x/Blobs.get(j+1).z)+(cameraWidth*i),Blobs.get(j+1).y/Blobs.get(j+1).z);
    }

     if(Blobs.size()==2){
     line((Blobs.get(0).x/Blobs.get(0).z)+(cameraWidth*i),Blobs.get(0).y/Blobs.get(0).z,(Blobs.get(1).x/Blobs.get(1).z)+(cameraWidth*i),Blobs.get(1).y/Blobs.get(1).z);
     text("D: "+dist((Blobs.get(0).x/Blobs.get(0).z)+(cameraWidth*i),Blobs.get(0).y/Blobs.get(0).z,(Blobs.get(1).x/Blobs.get(1).z)+(cameraWidth*i),Blobs.get(1).y/Blobs.get(1).z),(((Blobs.get(0).x/Blobs.get(0).z)+(cameraWidth*i))+((Blobs.get(1).x/Blobs.get(1).z)+(cameraWidth*i)))/2,(((Blobs.get(0).y/Blobs.get(0).z))+((Blobs.get(1).y/Blobs.get(1).z)))/2);
     }
     */
    Blobs = new ArrayList<TPoint>();
  }
  stroke(255);
}

boolean setupCameras() {
  println("Getting number of cameras");
  // Checks available cameras
  numCams = CLCamera.cameraCount();
  println("Found " + numCams + " cameras");
  if (numCams == 0)  return false;
  // create cameras and start capture
  for (int i = 0; i < numCams; i++)
  {
    // Prints Unique Identifier per camera
    println("Camera " + (i+1) + " UUID " + CLCamera.cameraUUID(i));
    // New camera instance per camera
    myCameras[i] = new CLCamera(this);
    // ----------------------(i, CLEYE_GRAYSCALE/COLOR, CLEYE_QVGA/VGA, Framerate)
    myCameras[i].createCamera(i, CLCamera.CLEYE_COLOR_PROCESSED, CLCamera.CLEYE_QVGA, cameraRate);
    // Starts camera captures
    myCameras[i].startCamera();
    //myCameras[i].setCameraParam(CLCamera.CLEYE_AUTO_GAIN, 1);
    //myCameras[i].setCameraParam(CLCamera.CLEYE_AUTO_EXPOSURE, 1);
    //myCameras[i].setCameraParam(CLCamera.CLEYE_AUTO_WHITEBALANCE, 1);
    myImages[i] = createImage(cameraWidth, cameraHeight, RGB);
  }
  // resize the output window
  size(cameraWidth*numCams, cameraHeight);
  println("Complete Initializing Cameras");
  return true;
}