kinect v1 point cloud 1

kinect v1でのポイントクラウド。
/*
reference
 https://github.com/shiffman/OpenKinect-for-Processing
 http://graphics.stanford.edu/~mdfisher/Kinect.html
 example: AveragePointTracking
 example: PointCloud
*/

import org.openkinect.freenect.*;
import org.openkinect.processing.*;
import codeanticode.syphon.*;

Kinect kinect;
SyphonServer server;

PVector v;
PFont font;

float[] depthLookUp = new float[2048];
float rotateFlag = 0;

float switchAngle = 0;
int timer = 0;

/* option */
int skip = 5; // ポイントの密度: 小さいほど細かい [1-]
int pointSize = 2; // ポイントの大きさ[1-]
int nowFrameRate = 30; // フレームレート: 大きいほど細かい [1-60]
float zoomSize = 600; // ズーム:大きいほど拡大
int colorDraw = 100; // ポイント明度の最大値 HSB最大値100
int colorRange = 10; // 深度による明度の暗化割合:
boolean mirrorDraw = false; // アミドスクリーン用反転設定
float rotateSpeed = 0.005f; // 回転表示速度 [0.005f-0.0001fくらい]

void settings() {
  size(800, 600, P3D);
  PJOGL.profile = 1;
  font = loadFont("AndaleMono-48.vlw");
}

void setup() {
  server = new SyphonServer(this, "Processing Syphon");
  frameRate(nowFrameRate);
  colorMode(HSB, 100);
  kinect = new Kinect(this);
  kinect.initDepth();

  // Lookup table for all possible depth values (0 - 2047)
  for (int i = 0; i < depthLookUp.length; i++) {
    depthLookUp[i] = rawDepthToMeters(i);
  }
  kinect.enableMirror(mirrorDraw);
}

void draw() {
  server.sendScreen();
  background(0);
  textFont(font, 30);
  int[] depth = kinect.getRawDepth();
  // We're just going to calculate and draw every "skip" pixel (equivalent of 160x120)
  translate(width/2, height/2, -50);
  rotateY(rotateFlag);

  for (int x = 0; x < kinect.width; x += skip) {
    for (int y = 0; y < kinect.height; y += skip) {
      int offset = x + y*kinect.width;
      // Convert kinect data to world xyz coordinate
      int rawDepth = depth[offset];
      v = depthToWorld(x, y, rawDepth);
        drawPoint();
    }
  }
  // 回転処理
  if (switchAngle == 0) {
    if (rotateFlag > 1.0) {
      switchAngle = 1;
    }
    rotateFlag += rotateSpeed;
  } else if (switchAngle == 1) {
    if (rotateFlag < -1.0) {
      switchAngle = 0;
      zoomSize = random(600, 600);
    }
    rotateFlag -= rotateSpeed;
  }
  timer += 1;
}

// These functions come from: http://graphics.stanford.edu/~mdfisher/Kinect.html
float rawDepthToMeters(int depthValue) {
  if (depthValue < 2047) {
    return (float)(1.0 / ((double)(depthValue) * -0.0030711016 + 3.3309495161));
  }
  return 0.0f;
}

PVector depthToWorld(int x, int y, int depthValue) {
  final double fx_d = 1.0 / 5.9421434211923247e+02;
  final double fy_d = 1.0 / 5.9104053696870778e+02;
  final double cx_d = 3.3930780975300314e+02;
  final double cy_d = 2.4273913761751615e+02;

  PVector result = new PVector();
  double depth =  depthLookUp[depthValue];
  //rawDepthToMeters(depthValue);
  result.x = (float)((x - cx_d) * depth * fx_d);
  result.y = (float)((y - cy_d) * depth * fy_d);
  result.z = (float)(depth);
  return result;
}

void drawPoint() {
  pushMatrix();
  float factor = zoomSize;
  translate(v.x*factor, v.y*factor, factor-v.z*factor);
  rect(0, 0, pointSize, pointSize);
  if (v.z < 0.6) {
    fill(0, 0, colorDraw);
  } else if (v.z < 0.7) {
    fill(0, 0, colorDraw - colorRange);
  } else if (v.z < 0.8) {
    fill(0, 0, colorDraw - (colorRange * 1.4));
  } else if (v.z < 0.9) {
    fill(0, 0, colorDraw - (colorRange * 1.8));
  } else if (v.z < 1.0) {
    fill(0, 0, colorDraw - (colorRange * 2.2));
  } else if (v.z < 1.1) {
    fill(0, 0, colorDraw - (colorRange * 2.6));
  } else if (v.z < 1.2) {
    fill(0, 0, colorDraw - (colorRange * 3.0));
  } else if (v.z < 1.3) {
    fill(0, 0, colorDraw - (colorRange * 3.4));
  } else if (v.z < 1.4) {
    fill(0, 0, colorDraw - (colorRange * 3.8));
  } else if (v.z < 1.5) {
    fill(0, 0, colorDraw - (colorRange * 4.2));
  } else if (v.z < 1.6) {
    fill(0, 0, colorDraw - (colorRange * 4.6));
  } else if (v.z < 1.7) {
    fill(0, 0, colorDraw - (colorRange * 5.0));
  } else if (v.z < 1.8) {
    fill(0, 0, colorDraw - (colorRange * 5.4));
  } else if (v.z < 1.9) {
    fill(0, 0, colorDraw - (colorRange * 5.8));
  } else if (v.z < 2.0) {
    fill(0, 0, colorDraw - (colorRange * 6.2));
  } else if (v.z < 2.1) {
    fill(0, 0, 0);
  }
  popMatrix();
}

void keyPressed() {
  if (key == CODED) {
    if (keyCode == UP) {
      colorRange = colorRange + 5;
      text("colorRange = " + colorRange, 10, 10);
      println(colorRange);
    } else if (keyCode == DOWN) {
      colorRange = colorRange - 5;
      text("colorRange = " + colorRange, 10, 10);
      println(colorRange);
    }
  }
}

コメントを残す

メールアドレスが公開されることはありません。 * が付いている欄は必須項目です

このサイトはスパムを低減するために Akismet を使っています。コメントデータの処理方法の詳細はこちらをご覧ください