Turntable Scan

thwump

Hi, I've written a bit of code based on the Making Things See book by Greg Borenstein that uses a Kinect on an Arduino powered turntable to take three depth scans at different points across the turntable's horizontal rotation before saving them as an STL (preferably a DXF, though I'm trying to get the code to work before I get around to that). I'm using the Modelbuilder library to create the STL meshes so I can open them with meshlab, but am struggling to get the three scans to be displayed as the three images side by side. At the moment, I'm getting two pairs of STL files (it should just be one), one with the three scans spaced out one on top of another, and the other with two of the scans facing each other across the origin.

I think I'm on to something by having three separate vertex lists, rotating them before I add faces to the scan, though

Code is below (I've omitted all of the UI code, as it's redundant here), and if anyone can help, it'd be much appreciated, as this is for a final year University project, and I'd really like something to show for myself. Sorry if I haven't explained what I'm after all that well. I can link to pictures of the meshes I get if that helps.

    import processing.opengl.*;
    import unlekker.util.*;
    import unlekker.modelbuilder.*;
    import SimpleOpenNI.*;
    import processing.serial.*;

    SimpleOpenNI kinect;
    UGeometry model;
    UVertexList vertexList;
    UVertexList vertexList2;
    UVertexList vertexList3;
    Serial myPort;

    boolean scanning = false;

    boolean turn1 = false;
    boolean turn2 = false;
    boolean turn3 = false;
    boolean resetturn = false;
    boolean rotscan1 = false;
    boolean rotscan2 = false;

    boolean finalscandone = false;
    float rotangle;

  //  int furthestValue = 0;
   // float maxZ;
    int spacing = 6;
    PVector rotatedPoint;

    void setup() {
      size(1024, 768, OPENGL);

      kinect = new SimpleOpenNI(this);
      kinect.enableDepth();

      model = new UGeometry();
      vertexList = new UVertexList();

      String portName = Serial.list()[0];
      myPort = new Serial(this, portName, 57600);
      myPort.bufferUntil('\n');
    }

    void draw () {

      background(0);

      kinect.update();

      translate(width/2, height/2, -1000);
      rotateX(radians(180));

      if (turn1 && !scanning)
      {
        kinect.update();
        scanning = true;
        delay(1000);
      }

      if (turn2)
      {
        myPort.write('2');
        myPort.readStringUntil('\n');
        kinect.update();  
        rotangle = radians(57);
        turn1 = false;
        scanning = false;
        delay(1000);
        rotscan1 = true;
      }

      if (turn3)
      {
        myPort.write('3');
        myPort.readStringUntil('\n');
        kinect.update();
        rotangle = radians(90);
        turn2 = false;
        rotscan1 = false;
        delay(1000);
        rotscan2 = true;
      }

      if (resetturn)
      {
        myPort.write('4');
        myPort.readStringUntil('\n');
        kinect.update();
        finalscandone = false;
      }

      if (scanning) {
        model.beginShape(TRIANGLES);
      }

      PVector[] depthPoints = kinect.depthMapRealWorld();

      for (int y = 0; y < 480 -spacing; y+=spacing) {
        for (int x = 0; x < 640 -spacing; x+= spacing) {

        if (scanning) {
            int nw = x + y * 640; 
            int ne = (x + spacing) + y * 640; 
            int sw = x + (y + spacing) * 640; 
            int se = (x + spacing) + (y + spacing) * 640;

            model.addFace(new UVec3(depthPoints[nw].x, depthPoints[nw].y, depthPoints[nw].z), 
            new UVec3(depthPoints[ne].x, depthPoints[ne].y, depthPoints[ne].z),
            new UVec3(depthPoints[sw].x, depthPoints[sw].y, depthPoints[sw].z));

            model.addFace(new UVec3(depthPoints[ne].x, depthPoints[ne].y,depthPoints[ne].z), 
            new UVec3(depthPoints[se].x, depthPoints[se].y, depthPoints[se].z), 
            new UVec3(depthPoints[sw].x, depthPoints[sw].y, depthPoints[sw].z));

          turn2 = true;
        }
            else {
              stroke(255); 
              int i = x + y * 640; 
              PVector currentPoint = depthPoints[i]; 
              point(currentPoint.x, currentPoint.y, currentPoint.z);
            }
          }
        } 

        for (int y = 0; y < 480 -spacing; y+=spacing) {
          for (int x = 0; x < 640 -spacing; x+= spacing) {

          if (rotscan1) {
            int nw = x + y * 640; 
            int ne = (x + spacing) + y * 640; 
            int sw = x + (y + spacing) * 640; 
            int se = (x + spacing) + (y + spacing) * 640;

            vertexList2 = new UVertexList();
            vertexList2.rotateX(cos(radians(57)) * sin(radians(57)));
            vertexList2.rotateY(sin(radians(57)) * sin(radians(57)));
            vertexList2.rotateZ(cos(radians(57)));

            model.addFace(new UVec3(depthPoints[nw].x, depthPoints[nw].y, depthPoints[nw].z), 
            new UVec3(depthPoints[ne].x, depthPoints[ne].y, depthPoints[ne].z),
            new UVec3(depthPoints[sw].x, depthPoints[sw].y, depthPoints[sw].z));

            model.addFace(new UVec3(depthPoints[ne].x, depthPoints[ne].y,depthPoints[ne].z), 
            new UVec3(depthPoints[se].x, depthPoints[se].y, depthPoints[se].z), 
            new UVec3(depthPoints[sw].x, depthPoints[sw].y, depthPoints[sw].z));

          turn3 = true;
          }  
            else {
              stroke(255); 
              int i = x + y * 640; 
              PVector currentPoint = depthPoints[i]; 
              point(currentPoint.x, currentPoint.y, currentPoint.z);
            }
          }
        }  

      for (int y = 0; y < 480 -spacing; y+=spacing) {
        for (int x = 0; x < 640 -spacing; x+= spacing) {

          if (rotscan2) {
            int nw = x + y * 640; 
            int ne = (x + spacing) + y * 640; 
            int sw = x + (y + spacing) * 640; 
            int se = (x + spacing) + (y + spacing) * 640;

            vertexList3 = new UVertexList();
            vertexList3.rotateX(cos(radians(57)) * sin(radians(57)));
            vertexList3.rotateY(sin(radians(57)) * sin(radians(57)));
            vertexList3.rotateZ(cos(radians(57)));

            model.addFace(new UVec3(depthPoints[nw].x, depthPoints[nw].y, depthPoints[nw].z), 
            new UVec3(depthPoints[ne].x, depthPoints[ne].y, depthPoints[ne].z),
            new UVec3(depthPoints[sw].x, depthPoints[sw].y, depthPoints[sw].z));

            model.addFace(new UVec3(depthPoints[ne].x, depthPoints[ne].y,depthPoints[ne].z), 
            new UVec3(depthPoints[se].x, depthPoints[se].y, depthPoints[se].z), 
            new UVec3(depthPoints[sw].x, depthPoints[sw].y, depthPoints[sw].z));

        finalscandone = true;
      }

            else {
              stroke(255); 
              int i = x + y * 640; 
              PVector currentPoint = depthPoints[i]; 
              point(currentPoint.x, currentPoint.y, currentPoint.z);
            }
         }
      }
        if (finalscandone) { 
        model.rotateY(radians(180));
        model.toOrigin();

        model.endShape();
        SimpleDateFormat logFileFmt = new SimpleDateFormat("'scan_'yyyyMMddHHmmss'.stl'");
        model.writeSTL(this, logFileFmt.format(new Date()));
        model.writeSTL(this, "scan_"+random(1000)+".stl"); scanning = false;
        resetturn = true;
      }

    }

    void keyPressed() {
      if (key == ' ') {
    //    scanning = true;
        turn1 = true;
      }
    }