projection mapping with AR tracking - almost there

bontempos

My objective was to use the AR tracking to get the 3d world position of a marker. Than, using a projector, I wanted to project a cross in the center of this marker, while I move it.

My code is "almost" working. The projection follows the object on axis X, but still, the projection is not over the marker. The Y axis seems to be a little odd.

The code is fully commented.. Can someone please point me some directions?

/**
 * Projection Mapping Matrix Corrspondence Study
 *
 * Turn the projection to a position where camera cam capture the whole frame 
 * Put the marker in front of the camera so it can be detected and beamed by the projection
 * On rojection window click at the center of the marker as you move it for at least 6 times.
 *
 * by Anderson Sudario, 2015
 *
 */

import jp.nyatla.nyar4psg.*;
import processing.video.*;
import gab.opencv.*;

import org.opencv.core.Mat;
import org.opencv.core.CvType;
import org.opencv.core.Core;

import java.awt.Frame;
import java.awt.BorderLayout;

int maxCPoints = 10; //min required is 6;

Mat A, B, P;
PVector [] p3d = new PVector[maxCPoints]; //stores 3D points in space on camera coord sys.
PVector [] p2dp = new PVector[maxCPoints];//stores projected 2D points correspondent to p3d position

ControlFrame projector;//new frame window for projection

Capture camera;
MultiMarker nya;
OpenCV opencv;
int markerSize = 70;
float tx, ty, tz, rx, ry, rz; //position/orientation of AR marker

boolean calibrationMode = true;
boolean allCPselected = false; //all 6 correspondent points selected
int currentCPoint = 0; //current correspondent point in calibration mode


//projection window setting
ControlFrame addControlFrame() {
  Frame f = new Frame("projectorWindow");
  ControlFrame p = new ControlFrame(this, 800, 600); //projector resolution
  f.add(p);
  p.init();
  f.setUndecorated(true); //hides title bar. Can I hide/show it dynamicaly?
  f.setSize(p.w, p.h);
  f.setLocation(1680, 0); //1680 is the main window resolution
  f.setResizable(false);
  f.setVisible(true);
  return p;
}


void setup() {

  //control window
  size(640, 480, P3D);

  opencv = new OpenCV(this, 800, 600);
  camera = new Capture(this, width, height);
  camera.start();

  nya=new MultiMarker(this, width, height, "camera_para.dat", NyAR4PsgConfig.CONFIG_PSG);
  nya.setLostDelay(1);
  nya.addARMarker("patt.kanji", markerSize);

  //dummy values to initiate variables
  for (int i = 0; i < maxCPoints; i++) {
    p2dp[i] = new PVector(0, 0);
    p3d[i] = new PVector(0, 0, 0);
  }

  //projector window
  projector = addControlFrame();
}



public void draw() {  

  if (!camera.available()) {
    fill(0);
    rect(0, 0, width, 25);
    fill(255);
    textSize(12);
    textAlign(CENTER);
    text("NO CAMERA AVAILABLE", 320, 17);
    return;
  }

  //detect marker and set background from camera input
  nya.detect(camera);
  background(camera);

  if ((!nya.isExistMarker(0))) {
    fill(0);
    rect(0, 0, width, 25);
    fill(255);
    textSize(12);
    textAlign(CENTER);
    text("NO MARKER AVAILABLE", 320, 17);
    return;
  }

  getInfo();
  showInfo();
}



void getInfo() {
  //get marker 3D information
  float ax = nya.getMarkerMatrix(0).m00;
  float ay = nya.getMarkerMatrix(0).m10;
  float az = nya.getMarkerMatrix(0).m20;
  float bz = nya.getMarkerMatrix(0).m21;
  float cz = nya.getMarkerMatrix(0).m22;
  tx = nya.getMarkerMatrix(0).m03;
  ty = nya.getMarkerMatrix(0).m13;
  tz = nya.getMarkerMatrix(0).m23;  
  rx = degrees(atan2(bz, cz));
  ry = degrees(atan2(-az, sqrt( pow(bz, 2)+pow(cz, 2) ) ) );
  rz = degrees(atan2(ay, ax));

  if (calibrationMode) {
    fill(0);
    rect(0, 0, width, 25);
    fill(255);
    textSize(12);
    textAlign(CENTER);
    text("CALIBRATION MODE", 320, 17);
  }
}

void showInfo() {
  noStroke();
  fill(0, 0, 0, 100);
  rect(0, height-80, 250, height);
  fill(255);
  textAlign(LEFT);
  textSize(12);
  text("press c for calibration mode", 10, height - 50);
  text(String.format("rx = %.2f, ry = %.2f, rz = %.2f", rx, ry, rz), 10, height - 35);
  text(String.format("tx = %.1f, ty = %.1f, tz = %.1f", tx, ty, tz), 10, height - 20);
  stroke(0, 0, 255);
  line(0, 0, width, height);
  line(0, height, width, 0);
  noStroke();
  text("FPS: " + (int)frameRate, 10, height - 5);

  scale( width/640, height/480);

  //vertexes info
  textSize(10);
  for (int i=0; i < 1; i++ ) { 
    PVector[] pos2d = nya.getMarkerVertex2D(i);
    for (int j=0; j < pos2d.length; j++ ) {
      String s = "(" + int(pos2d[j].x) + "," + int(pos2d[j].y) + ")";
      fill(255);
      rect(pos2d[j].x, pos2d[j].y - textAscent()/2, textWidth(s) + 3, textAscent() + textDescent());
      fill(0);
      text(s, pos2d[j].x + 2, pos2d[j].y + 2);
      fill(255, 0, 0);
      ellipse(pos2d[j].x, pos2d[j].y, 5, 5);
    }
  }
}

void resetCalibration() {
  allCPselected = false;
  calibrationMode = true;
  currentCPoint = 0;
}


void keyPressed() {
  if (key == 'c') {
    resetCalibration();
  }
}


void calculateMatrix() {
  //setting matrices A and B
  double [] tmpA = new double[11 * maxCPoints * 2];
  int j = 0;
  for (int i = 0; i < maxCPoints; i++) { 
    float X, Y, Z, x, y;
    //println(i);
    X = p3d[i].x; 
    Y = p3d[i].y;
    Z = p3d[i].z;
    x = p2dp[i].x;
    y = p2dp[i].y;

    double[] Cx = {
      X, Y, Z, 1, 0, 0, 0, 0, -X*x, -Y*x, -Z*x
    };
    for (int k = 0; k < 11; k++) {
      tmpA[j] = Cx[k];
      j++;
    }
    double[] Cy = { 
      0, 0, 0, 0, X, Y, Z, 1, -X*y, -Y*y, -Z*y
    };
    for (int k = 0; k < 11; k++) {
      tmpA[j] = Cy[k];
      j++;
    }
  }
  int row = 0, col = 0;
  Mat a = new Mat(maxCPoints*2, 11, CvType.CV_64F);
  a.put( row, col, tmpA );
  j=0;


  double [] tmpB = new double[maxCPoints*2];
  for (int i = 0; i < maxCPoints; i++) {
    tmpB[j] =  p2dp[i].x;
    j++;
    tmpB[j] =  p2dp[i].x;
    j++;
  }
  Mat b = new Mat(maxCPoints*2, 1, CvType.CV_64F);
  b.put( 0, 0, tmpB );

  //creating empty to hold next operations
  A = new Mat();
  B = new Mat();
  P = new Mat();


  //below is this equation C = ( At * A ).inv() * ( At * B ):
  Core.gemm(a.t(), a, 1, new Mat(), 0, A); //<- a.transp * a ; 1 = alpha, null, flag off, matrix to hold result
  Core.gemm(a.t(), b, 1, new Mat(), 0, B);
  Core.gemm(A.inv(1), B, 1, new Mat(), 0, P);

  //adding value to equalize row and column then reshape
  Mat z = new Mat(1, 1, CvType.CV_64F);
  z.put(0, 0, 1);
  P.push_back(z);
  P = P.reshape(1, 3);

  calibrationMode = false;
}



//-------------------------------------------------------------------------------



public class ControlFrame extends PApplet {
  int w, h;
  float fx, fy;
  Object parent;

  public void setup() {
    size(w, h, P3D);
    //frameRate(25);
  }

  public void draw() {
    background(30);

    if (calibrationMode) {
      drawCalibrationProjectionLines();
    } else {
      if (nya.isExistMarker(0)) {
        drawProjectionMappingCross();
      }
    }
  }


  void drawCalibrationProjectionLines() {
    stroke( (nya.isExistMarker(0))?#666666:#FF0000 );
    strokeWeight(3);
    line(0, mouseY, w, mouseY);
    line(mouseX, 0, mouseX, h);
    fill(50);
    rect(10, 10, w-20, h-20);
    noStroke();

    fill( (nya.isExistMarker(0))?#666666:#FF0000 );
    textSize(10);
    textAlign(CENTER, CENTER);
    text( currentCPoint + " ("+ mouseX + ","+ (mouseY)+")", (mouseX < w/2)? mouseX + 30 : mouseX - 30, (mouseY > h/2)? mouseY - 10 : mouseY +10);

    for ( int i = 0; i < currentCPoint; i++ ) {
      textAlign(LEFT, TOP);
      text( i +": " + p2dp[i] + " : " + p3d[i], 20, (11 * i) + 35);
    }
  }

  void drawProjectionMappingCross() {
    double [] tmp3D = new double[] { 
      tx, ty, tz, 1
    };
    int row = 0, col = 0;
    Mat new3D = new Mat(1, 4, CvType.CV_64F);
    new3D.put( row, col, tmp3D );

    Mat XY = new Mat();

    Core.gemm(P, new3D.t(), 1, new Mat(), 0, XY);

    float px = (float) XY.get(0, 0)[0];
    float py = (float) XY.get(1, 0)[0];

    stroke(0, 70, 0);
    line(px, 0, px, h);
    line(0, py, w, py);

  }

  void mousePressed( ) {
    if (!allCPselected && nya.isExistMarker(0) ) {
      p2dp[currentCPoint] = new PVector(mouseX, mouseY);
      p3d[currentCPoint] = new PVector(tx, ty, tz);

      currentCPoint++;
      if (currentCPoint>=p2dp.length)allCPselected = true;
    }
  }

  void mouseReleased() {
    if ( allCPselected ) {
      print(currentCPoint);
      calculateMatrix();
    }
  }


  void keyPressed() {
    if (key == 'c') {
      resetCalibration();
    }
  }



  private ControlFrame() {
  }

  public ControlFrame(Object theParent, int theWidth, int theHeight) {
    parent = theParent;
    w = theWidth;
    h = theHeight;
  }
}
 [-O<