Solution intermediaire

ColorTracker.pde

// Inspired by Learning Processing
// Daniel Shiffman

import processing.video.*;
import controlP5.*;
import processing.net.*; 

// Variable for capture device
Capture video;

// Variable for Graphical interface
ControlP5 cp5;

PImage colorTrackingImage;

// A variable for the color we are searching for.
color trackColor; 

int VIDEO_WIDTH=640;
int VIDEO_HEIGHT=480;
float POST_IT_WIDTH=76; // mm 
Server server;

void settings() {
  size(VIDEO_WIDTH + 400, VIDEO_HEIGHT * 2);
}

void setup() {

  video = new Capture(this, VIDEO_WIDTH, VIDEO_HEIGHT);
  video.start();
  // Start off tracking for red
  trackColor = color(255, 0, 0);

  // Create an image buffer to store image data
  colorTrackingImage = createImage(VIDEO_WIDTH, VIDEO_HEIGHT, RGB);

  initGui();

  try {
    server = new Server(this, 12345); // Start a simple server on a port
  }
  catch(Exception e) {
    println("Serveur issue... " + e);
  }
}

// ControlP5 automatically fills the error field. 
float errorHue, errorSat, errorBright;

// ControlP5 object is drawn automatically.
void initGui() {
  cp5 = new ControlP5(this);

  cp5.addSlider("errorHue")
    .setPosition(VIDEO_WIDTH + 20, 80)
    .setSize(180, 20)
    .setValue(8)
    .setRange(0, 40); 
  cp5.addSlider("errorSat")
    .setPosition(VIDEO_WIDTH + 20, 110)
    .setSize(180, 20)
    .setValue(32)
    .setRange(0, 40); 
  cp5.addSlider("errorBright")
    .setPosition(VIDEO_WIDTH + 20, 140)
    .setSize(180, 20)
    .setValue(24)
    .setRange(0, 40);
}


void captureEvent(Capture video) {
  // Read image from the camera
  video.read();
}

void draw() {
  clear();

  video.loadPixels();
  colorTrackingImage.loadPixels();

  // Draw the original video
  image(video, 0, 0);

  float trackedHue = hue(trackColor);
  float trackedSat = saturation(trackColor);
  float trackedBright = brightness(trackColor);

  for (int x = 0; x < video.width; x ++ ) {
    for (int y = 0; y < video.height; y ++ ) {
      int loc = x + y*video.width;
      // What is current color
      color currentColor = video.pixels[loc];

      float detectedHue = hue(currentColor);
      float detectedSat = saturation(currentColor);
      float detectedBright = brightness(currentColor);

      boolean isGoodHue = abs(trackedHue - detectedHue) < errorHue;
      boolean isGoodSat = abs(trackedSat - detectedSat) < errorSat;
      boolean isGoodBright = abs(trackedBright - detectedBright) < errorBright;

      if (isGoodHue && isGoodSat && isGoodBright) { 
        colorTrackingImage.pixels[loc] = trackColor;
      } else {
        colorTrackingImage.pixels[loc] = color(10);
      }
    }
  }

  int maxX = 0;
  int minX = VIDEO_WIDTH-1;
  int maxY = 0;
  int minY = VIDEO_HEIGHT-1;

  for (int x = 0; x < video.width; x ++ ) {
    for (int y = 0; y < video.height; y ++ ) {
      int loc = x + y*video.width;

      if (colorTrackingImage.pixels[loc] == trackColor) {
        if (x < minX) {
          minX = x;
        }
        if (x > maxX) {
          maxX = x;
        }        
        if (y < minY) {
          minY = y;
        }
        if (y > maxY) {
          maxY = y;
        }
      }
      // What is current color
    }
  }
  rectMode(CENTER);
  rect(minX, minY, 20, 20);
  rect(maxX, minY, 20, 20);
  rect(minX, maxY, 20, 20);
  rect(maxX, maxY, 20, 20);

  // A: minX
  // B: maxX
  float oc = fx;
  float ca = minX - cx;

  float oa = sqrt(oc * oc + ca * ca);

  float dPostIt = (oa * POST_IT_WIDTH) / (maxX - minX);

  PVector posMm = pixelToWorld(new PVector(minX, minY, dPostIt));

  send(posMm);

  text("Distance: " + dPostIt, VIDEO_WIDTH + 20, 180);
  text("PosX mm: " + posMm.x, VIDEO_WIDTH + 20, 210);
  line(VIDEO_WIDTH/2, 0, VIDEO_WIDTH/2, VIDEO_HEIGHT);  
  // Send the image to the video buffer.
  colorTrackingImage.updatePixels();

  // Draw the buffer
  image(colorTrackingImage, 0, VIDEO_HEIGHT);
}


void send(PVector pos) {
  try {  
    String message = pos.x + " " + pos.y + " " + pos.z + "\n"; 
    server.write(message);
  }
  catch(Exception e) {
    println("Serveur issue " + e);
  }
}

float cx = VIDEO_WIDTH/2;
float cy = VIDEO_HEIGHT/2;
float fx = 800;

PVector pixelToWorld(PVector p) {
  PVector result = new PVector();
  float a = p.x;
  float c1 = cx;
  float dx = p.z; // OC

  // Notations du TP
  result.x = ((a - c1) * dx) / fx;

  result.x = ((p.x - cx) * p.z) / fx;
  result.y = ((p.y - cy) * p.z) / fx;
  result.z = p.z;

  return result;
}



void mousePressed() {

  // Pick inside the window
  if (mouseX < VIDEO_WIDTH) {
    // Save color where the mouse is clicked in trackColor variable
    int px = constrain(mouseX, 0, VIDEO_WIDTH-1);
    int py = constrain(mouseY, 0, VIDEO_HEIGHT-1);

    int loc = px + py*video.width;
    trackColor = video.pixels[loc];

    println("Color picked: " + red(trackColor) + " " + green(trackColor) + " " + blue(trackColor));
  }
}