first commit of library base

examples/CALIBRATION/CALIBRATION.pde

@@ -0,0 +1,168 @@
+import javax.swing.JFrame;
+import SimpleOpenNI.*;
+import gab.opencv.*;
+import controlP5.*;
+
+// set resolution of your projector image/second monitor
+// and name of your calibration file-to-be
+int pWidth = 1024;
+int pHeight = 768;
+String calibFilename = "calib1.txt";
+
+SimpleOpenNI kinect;
+OpenCV opencv;
+ChessboardFrame frameBoard;
+ChessboardApplet ca;
+PVector[] depthMap;
+ArrayList<PVector> foundPoints = new ArrayList<PVector>();
+ArrayList<PVector> projPoints = new ArrayList<PVector>();
+ArrayList<PVector> ptsK, ptsP;
+PVector testPoint, testPointP;
+boolean isSearchingBoard = false;
+boolean calibrated = false;
+boolean testingMode = false;
+boolean viewRgb = true;
+int cx, cy, cwidth;
+
+void setup() 
+{
+  size(1150, 620);
+  textFont(createFont("Courier", 24));
+  frameBoard = new ChessboardFrame();
+
+  // set up kinect
+  kinect = new SimpleOpenNI(this);
+  kinect.setMirror(false);
+  kinect.enableDepth();
+  kinect.enableRGB();
+  kinect.alternativeViewPointDepthToImage();
+  opencv = new OpenCV(this, kinect.depthWidth(), kinect.depthHeight());
+
+  // matching pairs
+  ptsK = new ArrayList<PVector>();
+  ptsP = new ArrayList<PVector>();
+  testPoint = new PVector();
+  testPointP = new PVector();
+
+  cx = 20;
+  cy = 20;
+  cwidth = 200;
+  setupGui();
+}
+
+void draw() 
+{
+  // draw Chessboard onto scene
+  projPoints = drawChessboard(cx, cy, cwidth);
+
+  // update kinect and look for chessboard
+  kinect.update();
+  depthMap = kinect.depthMapRealWorld();
+  opencv.loadImage(kinect.rgbImage());
+  opencv.gray();
+
+  if (isSearchingBoard)
+    foundPoints = opencv.findChessboardCorners(4, 3);
+
+  drawGui();
+}
+
+void drawGui() 
+{
+  background(0, 100, 0);
+
+  // draw the RGB image
+  pushMatrix();
+  translate(10, 40);
+  textSize(22);
+  fill(255);
+  text("Kinect Image", 5, -5);
+  if (viewRgb)  image(kinect.rgbImage(),   0, 0);
+  else          image(kinect.depthImage(), 0, 0);
+
+  // draw chessboard corners, if found
+  if (isSearchingBoard) {
+    for (PVector p : foundPoints) {
+      if (getDepthMapAt((int)p.x, (int)p.y).z > 0)
+        fill(0, 255, 0);
+      else  fill(255, 0, 0);
+      ellipse(p.x, p.y, 4, 4);
+    }
+  }
+  if (calibrated && testingMode) {
+    fill(255, 0, 0);
+    ellipse(testPoint.x, testPoint.y, 10, 10);
+  }
+  popMatrix();
+
+  // draw GUI
+  pushMatrix();
+  pushStyle();
+  translate(kinect.depthWidth()+40, 40);
+  fill(0);
+  rect(0, 0, 420, 560);
+  fill(255);
+  text("number of point pairs: " + ptsP.size(), 12, 24);
+  popStyle();
+  popMatrix();
+}
+
+ArrayList<PVector> drawChessboard(int x0, int y0, int cwidth) {
+  ArrayList<PVector> projPoints = new ArrayList<PVector>();
+  int cheight = (int)(cwidth * 0.8);
+  ca.background(255);
+  ca.fill(0);
+  for (int j=0; j<4; j++) {
+    for (int i=0; i<5; i++) {
+      int x = int(x0 + map(i, 0, 5, 0, cwidth));
+      int y = int(y0 + map(j, 0, 4, 0, cheight));
+      if (i>0 && j>0)  projPoints.add(new PVector((float)x/pWidth, (float)y/pHeight));
+      if ((i+j)%2==0)  ca.rect(x, y, cwidth/5, cheight/4);
+    }
+  }  
+  ca.fill(0, 255, 0);
+  if (calibrated)  
+    ca.ellipse(testPointP.x, testPointP.y, 20, 20);  
+  ca.redraw();
+  return projPoints;
+}
+
+
+void addPointPair() {
+  if (projPoints.size() == foundPoints.size()) {
+    for (int i=0; i<projPoints.size(); i++) {
+      ptsP.add( projPoints.get(i) );
+      ptsK.add( getDepthMapAt((int) foundPoints.get(i).x, (int) foundPoints.get(i).y) );
+    }
+  }
+}
+
+PVector getDepthMapAt(int x, int y) {
+  PVector dm = depthMap[kinect.depthWidth() * y + x];
+  return new PVector(dm.x, dm.y, dm.z);
+}
+
+void clearPoints() {
+  ptsP.clear();
+  ptsK.clear();
+}
+
+void saveC() {
+  saveCalibration(calibFilename); 
+}
+
+void loadC() {
+  loadCalibration(calibFilename);
+}
+
+void mousePressed() {
+  if (calibrated && testingMode) {
+    testPoint = new PVector(constrain(mouseX-10, 0, kinect.depthWidth()-1), 
+                            constrain(mouseY-40, 0, kinect.depthHeight()-1));
+    //println("=====0======");
+    //println(testPoint);
+    int idx = kinect.depthWidth() * (int) testPoint.y + (int) testPoint.x;
+    testPointP = convertKinectToProjector(depthMap[idx]);
+  }
+}
+

examples/CALIBRATION/Calibrate.pde

@@ -0,0 +1,71 @@
+Jama.Matrix A, x, y;
+
+void calibrate() {
+  A = new Jama.Matrix(ptsP.size()*2, 11);
+  y = new Jama.Matrix(ptsP.size()*2, 1);
+  for(int i=0; i < ptsP.size()*2; i=i+2){
+    PVector kc = ptsK.get(i/2);
+    PVector projC = ptsP.get(i/2);
+    A.set(i, 0, kc.x);
+    A.set(i, 1, kc.y);
+    A.set(i, 2, kc.z);
+    A.set(i, 3, 1);
+    A.set(i, 4, 0);
+    A.set(i, 5, 0);
+    A.set(i, 6, 0);
+    A.set(i, 7, 0);
+    A.set(i, 8, -projC.x*kc.x);
+    A.set(i, 9, -projC.x*kc.y);
+    A.set(i,10, -projC.x*kc.z);
+
+    y.set(i, 0, projC.x);
+
+    A.set(i+1, 0, 0);
+    A.set(i+1, 1, 0);
+    A.set(i+1, 2, 0);
+    A.set(i+1, 3, 0);
+    A.set(i+1, 4, kc.x);
+    A.set(i+1, 5, kc.y);
+    A.set(i+1, 6, kc.z);
+    A.set(i+1, 7, 1);
+    A.set(i+1, 8, -projC.y*kc.x);
+    A.set(i+1, 9, -projC.y*kc.y);
+    A.set(i+1,10, -projC.y*kc.z);
+
+    y.set(i+1, 0, projC.y);
+  }
+  QRDecomposition problem = new QRDecomposition(A);
+  x = problem.solve(y);
+  calibrated = true;
+}
+
+PVector convertKinectToProjector(PVector kp) {
+  PVector out = new PVector();
+  float denom = (float)x.get(8,0)*kp.x + (float)x.get(9,0)*kp.y + (float)x.get(10,0)*kp.z + 1;
+  out.x = pWidth * ((float)x.get(0,0)*kp.x + (float)x.get(1,0)*kp.y + (float)x.get(2,0)*kp.z + (float)x.get(3,0)) / denom;
+  out.y = pHeight * ((float)x.get(4,0)*kp.x + (float)x.get(5,0)*kp.y + (float)x.get(6,0)*kp.z + (float)x.get(7,0)) / denom;
+  //println("===1====");
+  //println(kp);
+  //println("===2====");
+  //println(new PVector(out.x/pWidth, out.y/pHeight));
+  //println("===3====");
+
+  return out;
+}
+
+String[] getCalibrationString() {
+  String[] coeffs = new String[11];
+  for (int i=0; i<11; i++)
+    coeffs[i] = ""+x.get(i,0);
+  return coeffs;
+}
+
+void printMatrix(Jama.Matrix M) {
+  double[][] a = M.getArray();
+  for (int i=0; i<a.length; i++) {
+    for (int j=0; j<a[i].length; j++) {
+      println(i + " " + j + " : " + a[i][j]);
+    }
+  }
+}
+

examples/CALIBRATION/GUI.pde

@@ -0,0 +1,73 @@
+ControlP5 cp5;
+Slider2D cPos;
+PVector guiPos;
+
+void setupGui() {
+  cp5 = new ControlP5(this);
+  cp5.setFont(createFont("Courier", 16));
+
+  guiPos = new PVector(kinect.depthWidth()+60, 60);
+
+  cPos = cp5.addSlider2D("chessPosition")
+      .setPosition(guiPos.x, guiPos.y+30)
+      .setSize(360, 300)
+      .setArrayValue(new float[]{150, 120});
+
+  cp5.addSlider("cwidth")
+     .setPosition(guiPos.x, guiPos.y+360)
+     .setHeight(30)
+     .setWidth(345)
+     .setRange(2, 800)
+     .setLabel("Size"); 
+
+  cp5.addToggle("isSearchingBoard")
+     .setPosition(guiPos.x, guiPos.y+415)
+     .setSize(105, 32)
+     .setMode(ControlP5.SWITCH)
+     .setLabel("Search");
+
+  cp5.addButton("addPointPair")
+     .setPosition(guiPos.x+135, guiPos.y+415)
+     .setSize(105, 32)
+     .setLabel("Add pair");
+
+  cp5.addButton("clearPoints")
+     .setPosition(guiPos.x+270, guiPos.y+415)
+     .setSize(105, 32)
+     .setLabel("Clear");
+
+  cp5.addButton("calibrate")
+     .setPosition(guiPos.x, guiPos.y+480)
+     .setSize(105, 32);
+
+  cp5.addButton("saveC")
+     .setPosition(guiPos.x+135, guiPos.y+480)
+     .setSize(105, 32)
+     .setLabel("Save");
+
+   cp5.addButton("loadC")
+     .setPosition(guiPos.x+270, guiPos.y+480)
+     .setSize(105, 32)
+     .setLabel("Load");  
+
+  cp5.addToggle("testingMode")
+     .setPosition(15, 50+kinect.depthHeight())
+     .setSize(105, 32)
+     .setMode(ControlP5.SWITCH)
+     .setLabel("Test Mode");
+
+  cp5.addToggle("viewRgb")
+     .setPosition(140, 50+kinect.depthHeight())
+     .setSize(125, 32)
+     .setMode(ControlP5.SWITCH)
+     .setLabel("RGB");     
+}
+
+void controlEvent(ControlEvent theControlEvent) {
+  try {
+    if (theControlEvent.isFrom("chessPosition")) {
+      cx = (int) map(cPos.arrayValue()[0], 0, 100, 0, pWidth);
+      cy = (int) map(cPos.arrayValue()[1], 0, 100, 0, pHeight);
+    }  
+  } catch(Exception e) {};
+}

examples/CALIBRATION/Util.pde

@@ -0,0 +1,35 @@
+public class ChessboardFrame extends JFrame {
+  public ChessboardFrame() {
+    setBounds(displayWidth,0,pWidth,pHeight);
+    ca = new ChessboardApplet();
+    add(ca);
+    removeNotify(); 
+    setUndecorated(true); 
+    setAlwaysOnTop(false); 
+    setResizable(false);  
+    addNotify();     
+    ca.init();
+    show();
+  }
+}
+
+public class ChessboardApplet extends PApplet {
+  public void setup() {
+    noLoop();
+  }
+  public void draw() {
+  }
+}
+
+void saveCalibration(String filename) {
+  String[] coeffs = getCalibrationString();
+  saveStrings(dataPath(filename), coeffs);
+}
+
+void loadCalibration(String filename) {
+  String[] s = loadStrings(dataPath(filename));
+  x = new Jama.Matrix(11, 1);
+  for (int i=0; i<s.length; i++)
+    x.set(i, 0, Float.parseFloat(s[i]));
+  calibrated = true;
+}