void testApp::update() {
kinect.update();
if(kinect.isFrameNew()) { // only recompute the mesh if necessary
int width = kinect.getWidth();
int height = kinect.getHeight();
float* distancePixels = kinect.getDistancePixels(); // distance in centimeters
mesh.clear();
mesh.setMode(OF_TRIANGLES_MODE);
for(int y = 0; y < height - 1; y++) { // don't go to the end
for(int x = 0; x < width - 1; x++) { // don't go to the end
// get indices for each corner
int nwi = (y + 0) * width + (x + 0);
int nei = (y + 0) * width + (x + 1);
int sei = (y + 1) * width + (x + 1);
int swi = (y + 1) * width + (x + 0);
// get z values for each corner
float nwz = distancePixels[nwi];
float nez = distancePixels[nei];
float sez = distancePixels[sei];
float swz = distancePixels[swi];
if(nwz > 0 && nez > 0 && sez > 0 && swz > 0) { // ignore empty depth pixels
// get real world locations for each corner
ofVec3f nwv = ConvertProjectiveToRealWorld(x + 0, y + 0, nwz);
ofVec3f nev = ConvertProjectiveToRealWorld(x + 1, y + 0, nez);
ofVec3f sev = ConvertProjectiveToRealWorld(x + 1, y + 1, sez);
ofVec3f swv = ConvertProjectiveToRealWorld(x + 0, y + 1, swz);
// compute normal for the upper left
ofVec3f normal = getNormal(nwv, nev, swv);
// add the upper left triangle
mesh.addNormal(normal);
mesh.addVertex(nwv);
mesh.addNormal(normal);
mesh.addVertex(nev);
mesh.addNormal(normal);
mesh.addVertex(swv);
// add the bottom right triangle
mesh.addNormal(normal);
mesh.addVertex(nev);
mesh.addNormal(normal);
mesh.addVertex(sev);
mesh.addNormal(normal);
mesh.addVertex(swv);
}
}
}
}
}
void testApp::updateTrianglesSimplify() {
// zero edges
for(int x = roiStart.x; x < roiEnd.x; x++) {
surface[getSurfaceIndex(x, roiStart.y)] = ConvertProjectiveToRealWorld(x, roiStart.y, zCutoff); // top
surface[getSurfaceIndex(x, roiEnd.y - 1)] = ConvertProjectiveToRealWorld(x, roiEnd.y - 1, zCutoff); // bottom
}
for(int y = roiStart.y; y < roiEnd.y; y++) {
surface[getSurfaceIndex(roiStart.x, y)] = ConvertProjectiveToRealWorld(roiStart.x, y, zCutoff); // left
surface[getSurfaceIndex(roiEnd.x - 1, y)] = ConvertProjectiveToRealWorld(roiEnd.x - 1, y, zCutoff); // right
}
triangles.resize((roiEnd.x - roiStart.x) * (roiEnd.y - roiStart.y) * 2);
int totalTriangles = 0;
Triangle* topTriangle;
Triangle* bottomTriangle;
float* z = kinect.getDistancePixels();
for(int y = roiStart.y; y < roiEnd.y - 1; y++) {
bool stretching = false;
for(int x = roiStart.x; x < roiEnd.x - 1; x++) {
bool endOfRow = (x == roiEnd.x - 2);
int nw = getSurfaceIndex(x, y);
int ne = getSurfaceIndex(x + 1, y);
int sw = getSurfaceIndex(x, y + 1);
int se = getSurfaceIndex(x + 1, y + 1);
bool flat = (z[nw] == z[ne] && z[nw] == z[sw] && z[nw] == z[se]);
if(endOfRow ||
(stretching && flat)) {
// stretch the quad to our new position
topTriangle->vert2 = surface[ne];
bottomTriangle->vert1 = surface[ne];
bottomTriangle->vert2 = surface[se];
} else {
topTriangle = &triangles[totalTriangles++];
topTriangle->vert1 = surface[nw];
topTriangle->vert2 = surface[ne];
topTriangle->vert3 = surface[sw];
bottomTriangle = &triangles[totalTriangles++];
bottomTriangle->vert1 = surface[ne];
bottomTriangle->vert2 = surface[se];
bottomTriangle->vert3 = surface[sw];
stretching = flat;
}
}
}
triangles.resize(totalTriangles);
}
void testApp::update() {
kinect.update();
if(kinect.isFrameNew()) {
registerDepth(kinect.getRawDepthPixels(), ®isteredDepth[0]);
cloud.clear();
cloud.setMode(OF_PRIMITIVE_POINTS);
int width = kinect.getWidth();
int height = kinect.getHeight();
for(int y = 0; y < height; y++) {
for(int x = 0; x < width; x++) {
int i = y * width + x;
float z = registeredDepth[i]; // in mm
if(z != 0) { // ignore empty depth pixels
ofColor curColor = kinect.getColorAt(x, y);
float absDiff = 0;
absDiff += abs((float) curColor.r - (float) trackColor.r);
absDiff += abs((float) curColor.g - (float) trackColor.g);
absDiff += abs((float) curColor.b - (float) trackColor.b);
absDiff /= 3;
trackImage.getPixels()[i] = absDiff;
if(absDiff < mouseX) {
cloud.addColor(kinect.getColorAt(x, y));
cloud.addVertex(ConvertProjectiveToRealWorld(x, y, z));
}
}
}
}
trackImage.update();
}
}
void ofxVirtualCamera::updateSurface() {
float* z = kinect.getDistancePixels();
int i = 0;
for(int y = 0; y < camHeight; y++) {
for(int x = 0; x < camWidth; x++) {
if(z[i] != 0) {
surface[i] = ConvertProjectiveToRealWorld(x, y, -z[i]);
}
i++;
}
}
}
void testApp::updateSurface() {
float* z = kinect.getDistancePixels();
float alpha = panel.getValueF("temporalBlur");
float beta = 1 - alpha;
Mat kinectMat = Mat(480, 640, CV_32FC1, z);
Mat kinectBuffer = Mat(480, 640, CV_32FC1, z);
cv::addWeighted(kinectBuffer, alpha, kinectMat, beta, 0, kinectBuffer);
ofxCv::copy(kinectMat, kinectBuffer);
int i = 0;
for(int y = 0; y < Yres; y++) {
for(int x = 0; x < Xres; x++) {
surface[i] = ConvertProjectiveToRealWorld(x, y, z[i]);
i++;
}
}
}
void testApp::update() {
kinect.update();
if(kinect.isFrameNew()) {
int width = kinect.getWidth();
int height = kinect.getHeight();
float* distancePixels = kinect.getDistancePixels(); // distance in centimeters
cloud.clear();
cloud.setMode(OF_PRIMITIVE_POINTS);
for(int y = 0; y < height; y++) {
for(int x = 0; x < width; x++) {
int i = y * width + x;
float z = distancePixels[i];
if(z != 0) { // ignore empty depth pixels
cloud.addVertex(ConvertProjectiveToRealWorld(x, y, z));
}
}
}
}
if(exportPly) {
exportPlyCloud("cloud.ply", cloud);
exportPly = false;
}
}
Mat Kinect::retrievePointCloud(Mat& depthImage)
{
ConvertProjectiveToRealWorld(depthImage);
return mPointCloud;
}
void testApp::draw() {
ofBackground(245);
ofSetColor(255); //everything drawn with white tint
kinect.drawDepth(0, 0);
easyCam.begin();
ofScale(1, -1, -1);
//ofTranslate(0, 0, -200);
ofSetColor(0);
int width = kinect.getWidth();
int height = kinect.getHeight();
float* distancePixels = kinect.getDistancePixels();
ofMesh cloud;
cloud.setMode(OF_PRIMITIVE_POINTS);
float minZ = 1500;
float minX = 0;
float minY = 0;
float dist = 1500;
float secondZ = 1500;
float secondX = 0;
float secondY = 0;
for(int y = 0; y < height; y++) {
for(int x = 0; x < width; x++) {
int i = y * width + x;
float z = distancePixels[i];
if(z !=0 && z < 1500 && drawCloud == true) cloud.addVertex(ConvertProjectiveToRealWorld(x, y, z));
if(z != 0 && z < minZ) {
minZ= z;
minX = x;
minY = y;
forepoint = ConvertProjectiveToRealWorld(minX, minY, minZ);
}
else if ( z !=0 && z > minZ && z-minZ < dist) {
dist = z - minZ;
secondZ = z;
secondX = x;
secondY = y;
}
}
}
if(drawCloud) cloud.draw();
//secondPoint = ConvertProjectiveToRealWorld(secondX, secondY, secondZ);
ofSetColor(50);
if(doDrawing) {
drawing.addVertex(forepoint);
//drawing2.addVertex(secondPoint);
}
//drawing.draw();
//drawing2.draw();
//ofCircle(secondPoint.x, secondPoint.y, 5);
if(drawCopy) {
meshCopy.draw();
}
easyCam.end();
//averaging the data in a region around the forepoint
ofNoFill();
int searchRadius = 64;
int searchDistance = 100;
int count = 0;
ofVec3f sum = ofVec3f(0, 0, 0);
ofRect(minX-searchRadius/2, minY+searchRadius/2, searchRadius, searchRadius);
for(int x = -searchRadius/2; x < searchRadius/2; x++) {
for (int y = -searchRadius/2; y < searchRadius/2; y++) {
int curx = x+minX;
int cury = y+ minY;
if( curx < width && curx >0 && y + cury >0 && cury < height) {
int i = cury*width + curx;
float z = distancePixels[i];
if(z!=0 && z-minZ < searchDistance) {
sum.x+= curx;
sum.y+= cury;
//unlike x and y, z is taken from distancePixels[i] so dont adjust for current (x, y) like curx cury
sum.z+= z;
count++;
}
}
}
}
//we are only looking at a single point. no perspective issue involed?
//ofVec3f avg = ConvertProjectiveToRealWorld(sum.x/count, sum.y/count, sum.z/count);
ofSetColor(0, 255, 0);
ofFill();
ofCircle(avg.x, avg.y, 5);
ofSetColor(255, 0, 0);
ofFill();
ofCircle(forepoint.x, forepoint.y, 5);
ofSetColor(0);
ofDrawBitmapString( "space bar to render / t to resume", 50, 100);
ofDrawBitmapString( ofToString(forepoint.x) + "/" + ofToString(avg.x), 50, 150);
}
void testApp::update() {
kinect.update();
if(!panel.getValueB("pause") && kinect.isFrameNew()) {
zCutoff = panel.getValueF("zCutoff");
float fovWidth = panel.getValueF("fovWidth");
float fovHeight = panel.getValueF("fovHeight");
int left = Xres * (1 - fovWidth) / 2;
int top = Yres * (1 - fovHeight) / 2;
int right = left + Xres * fovWidth;
int bottom = top + Yres * fovHeight;
roiStart = Point2d(left, top);
roiEnd = Point2d(right, bottom);
ofVec3f nw = ConvertProjectiveToRealWorld(roiStart.x, roiStart.y, zCutoff);
ofVec3f se = ConvertProjectiveToRealWorld(roiEnd.x - 1, roiEnd.y - 1, zCutoff);
float width = (se - nw).x;
float height = (se - nw).y;
globalScale = panel.getValueF("stlSize") / MAX(width, height);
backOffset = panel.getValueF("backOffset") / globalScale;
cutoffKinect();
if(panel.getValueB("useSmoothing")) {
smoothKinect();
}
if(panel.getValueB("useWatermark")) {
startTimer();
injectWatermark();
injectWatermarkTime = stopTimer();
}
startTimer();
updateSurface();
updateSurfaceTime = stopTimer();
bool exportStl = panel.getValueB("exportStl");
bool useRandomExport = panel.getValueB("useRandomExport");
startTimer();
if((exportStl && useRandomExport) || panel.getValueB("useRandom")) {
updateTrianglesRandom();
} else if(panel.getValueB("useSimplify")) {
updateTrianglesSimplify();
} else {
updateTriangles();
}
calculateNormals(triangles, normals);
updateTrianglesTime = stopTimer();
startTimer();
updateBack();
updateBackTime = stopTimer();
startTimer();
postProcess();
postProcessTime = stopTimer();
if(exportStl) {
string pocoTime = Poco::DateTimeFormatter::format(Poco::LocalDateTime(), "%Y-%m-%d at %H.%M.%S");
ofxSTLExporter exporter;
exporter.beginModel("Kinect Export");
addTriangles(exporter, triangles, normals);
addTriangles(exporter, backTriangles, backNormals);
exporter.saveModel("Kinect Export " + pocoTime + ".stl");
#ifdef USE_REPLICATORG
if(printer.isConnected()) {
printer.printToFile("/home/matt/MakerBot/repg_workspace/ReplicatorG/examples/Snake.stl", "/home/matt/Desktop/snake.s3g");
}
#endif
panel.setValueB("exportStl", false);
}
}
float diffuse = panel.getValueF("diffuseAmount");
redLight.setDiffuseColor(ofColor(diffuse / 2, diffuse / 2, 0));
greenLight.setDiffuseColor(ofColor(0, diffuse / 2, diffuse / 2));
blueLight.setDiffuseColor(ofColor(diffuse / 2, 0, diffuse / 2));
float ambient = 255 - diffuse;
redLight.setAmbientColor(ofColor(ambient / 2, ambient / 2, 0));
greenLight.setAmbientColor(ofColor(0, ambient / 2, ambient / 2));
blueLight.setAmbientColor(ofColor(ambient / 2, 0, ambient / 2));
float lightY = ofGetHeight() / 2 + panel.getValueF("lightY");
float lightZ = panel.getValueF("lightZ");
float lightDistance = panel.getValueF("lightDistance");
float lightRotation = panel.getValueF("lightRotation");
redLight.setPosition(ofGetWidth() / 2 + cos(lightRotation + 0 * TWO_PI / 3) * lightDistance,
lightY + sin(lightRotation + 0 * TWO_PI / 3) * lightDistance,
lightZ);
greenLight.setPosition(ofGetWidth() / 2 + cos(lightRotation + 1 * TWO_PI / 3) * lightDistance,
lightY + sin(lightRotation + 1 * TWO_PI / 3) * lightDistance,
lightZ);
blueLight.setPosition(ofGetWidth() / 2 + cos(lightRotation + 2 * TWO_PI / 3) * lightDistance,
lightY + sin(lightRotation + 2 * TWO_PI / 3) * lightDistance,
lightZ);
}
void testApp::draw() {
ofBackground(0);
easyCam.begin();
int width = kinect.getWidth();
int height = kinect.getHeight();
ofScale(1, -1, -1); // orient the point cloud properly
ofTranslate(0, 0, -1500); // rotate about z = 1500 mm
float* distancePixels = kinect.getDistancePixels(); // distance in millimeters
ofMesh cloud;
cloud.setMode(OF_PRIMITIVE_POINTS);
float minz = numeric_limits<float>::max();
float minx = 0, miny = 0;
for(int y = 0; y < height; y++) {
for(int x = 0; x < width; x++) {
int i = y * width + x;
float z = distancePixels[i];
if(z != 0) { // ignore empty depth pixels
ofVec3f cur = ConvertProjectiveToRealWorld(x, y, z);
cloud.addVertex(cur);
if(z < minz) {
minz = z;
minx = x;
miny = y;
}
}
}
}
//cloud.draw();
easyCam.end();
ofSetColor(255);
kinect.drawDepth(0, 0);
ofSetColor(255, 0, 0);
ofNoFill();
int searchRadius = 128;
//ofRect(minx - searchRadius / 2, miny - searchRadius / 2, searchRadius, searchRadius);
//ofCircle(minx, miny, 10);
ofVec3f sum;
int count = 0;
int maxSearchDistance = 100;
for(int y = -searchRadius / 2; y < +searchRadius / 2; y++) {
for(int x = -searchRadius / 2; x < +searchRadius / 2; x++) {
int curx = x + minx;
int cury = y + miny;
if(curx > 0 && curx < width &&
cury > 0 && cury < height) {
int i = cury * width + curx;
float curz = distancePixels[i];
if(curz != 0 && abs(curz - minz) < maxSearchDistance) {
sum.x += curx;
sum.y += cury;
sum.z += curz;
count++;
}
}
}
}
ofVec3f avg = sum / count;
if(avg.z > 650 && avg.z <820 && avg.x > 309 && avg.x < 400) {
ofFill();
ofSetColor(255, 0, 0);
}
else {
ofFill();
ofSetColor(0, 255, 0);
}
ofCircle(avg.x, avg.y, 10);
/*
if(smoothedForepoint == ofVec3f()) {
smoothedForepoint = avg;
} else {
smoothedForepoint.interpolate(avg, .1);
}
ofSetColor(255, 0, 255);
ofCircle(smoothedForepoint.x, smoothedForepoint.y, 10);
*/
ofSetColor(255);
ofDrawBitmapString(ofToString(minz) + "->" + ofToString(avg.x), 10, 20);
}
