void update()
{
// Update our little offset thingy.
offset += 0.01;
if (offset > 1)
{
offset = 0;
}
// Update our camera.
grabber.update();
// If the camera has a new frame to offer us ...
if (grabber.isFrameNew())
{
// Make a copy of our grabber pixels in the colorImage.
colorImage.setFromPixels(grabber.getPixelsRef());
// When we assign a color image to a grayscale image, it is converted automatically.
grayscaleImage = colorImage;
// If we set learnBackground to true using the keyboard, we'll take a snapshot of
// the background and use it to create a clean foreground image.
if (learnBackground == true)
{
// We assign the grayscaleImage to the grayscaleBackgroundImage.
grayscaleBackgroundImage = grayscaleImage;
// Now we set learnBakground so we won't set a background unless
// explicitly directed to with a keyboard command.
learnBackground = false;
}
// Create a difference image by comparing the background and the current grayscale images.
grayscaleAbsoluteDifference.absDiff(grayscaleBackgroundImage, grayscaleImage);
// Assign grayscaleAbsoluteDifference to the grayscaleBinary image.
grayscaleBinary = grayscaleAbsoluteDifference;
// Then threshold the grayscale image to create a binary image.
grayscaleBinary.threshold(threshold, invert);
// Find contours (blobs) that are between the size of 20 pixels and
// 1 / 3 * (width * height) of the camera. Also find holes.
contourFinder.findContours(grayscaleBinary, 100, (width * height) / 3.0, 10, true);
// Get the biggest blob and use it to draw.
if (contourFinder.nBlobs > 0)
{
holePositions.addVertex(contourFinder.blobs[0].boundingRect.getCenter());
}
else
{
holePositions.clear();
}
}
}
void ofApp::update() {
if (ofGetElapsedTimeMillis() - lastTime >= timeToReset) {
lastTime = ofGetElapsedTimeMillis();
bLearnBackground = true;
bLearnBackground1 = true;
}
micLevelsTopNew[4] = contourFinder.nBlobs;
micLevelsTopNew[5] = contourFinder1.nBlobs;
/* NEW CAMERA CODE */
bool bNewFrame = false;
bool bNewFrame1 = false;
vidGrabber.update();
bNewFrame = vidGrabber.isFrameNew();
vidGrabber1.update();
bNewFrame1 = vidGrabber.isFrameNew();
if (bNewFrame){
colorImg.setFromPixels(vidGrabber.getPixels(), 320,240);
grayImage = colorImg;
if (bLearnBackground == true){
grayBg = grayImage; // the = sign copys the pixels from grayImage into grayBg (operator overloading)
bLearnBackground = false;
}
grayDiff.absDiff(grayBg, grayImage);
grayDiff.threshold(threshold);
contourFinder.findContours(grayDiff, 20, (340*240)/3, 10, true);
}
if (bNewFrame1){
colorImg1.setFromPixels(vidGrabber1.getPixels(), 320,240);
grayImage1 = colorImg1;
if (bLearnBackground1 == true){
grayBg1 = grayImage1;
bLearnBackground1 = false;
}
grayDiff1.absDiff(grayBg1, grayImage1);
grayDiff1.threshold(threshold);
contourFinder1.findContours(grayDiff1, 20, (340*240)/3, 10, true);
}
switch (ANIMATION_STATE) {
case ACTIVATED: {
int max_pos = 0;
int max_element = -1000;
for (int i = 0; i < 12; i++) {
if (micLevelsTopNew[i] > max_element) {
max_pos = i;
max_element = micLevelsTopNew[i];
}
}
for (int x = 0; x < 1280; x++) {
float top = pow(x-bottomSwarm.b,2);
float bottom = 2*pow(bottomSwarm.c,2);
bottomSwarm.curve[x] = bottomSwarm.a*exp(-(top/bottom));
}
ofVec2f norm = swarmPosition;
bottomSwarm.b = norm.normalize().x*1280-160;
// ofVec2f btm = absColumnPositionTop[max_pos];
ofVec2f btm = cameraPositionsTop[max_pos];
ofVec2f desired = btm - swarmPosition;
float d = sqrt((desired.x*desired.x) + (desired.y+desired.y));
desired.normalize();
if (d < 100) {
float m = ofMap(d, 0.0, 100.0, 0.0, 4.0);
desired *= m;
} else {
desired *= 4.0;
}
swarmPosition += desired;
/* UPDATE WAVES */
for (int x = 0; x < 1280; x++) {
gaussianBottom[x] = ofMap(bottomSwarm.curve[x], 0.0, 1.1, ambientLevel, 255.0);
}
break;
}
case DEACTIVATED: {
for (int x = 0; x < 1280; x++) {
float top = pow(x-bottomSwarm.b,2);
float bottom = 2*pow(bottomSwarm.c,2);
bottomSwarm.curve[x] = bottomSwarm.a*exp(-(top/bottom));
}
swarmPosition += swarmVector;
if (swarmPosition.x >= 300+770 || swarmPosition.x <= 300) {
swarmVector *= -1.0;
}
ofVec2f norm = swarmPosition;
bottomSwarm.b = norm.normalize().x*1280.0-160;
for (int x = 0; x < 1280; x++) {
gaussianBottom[x] = ofMap(bottomSwarm.curve[x], 0.0, 1.1, ambientLevel, 255.0);
}
break;
}
}
int cnt = 0;
for (int i = 0; i < 12; i++) {
if (i == 4 || i == 5) {
} else {
micLevelsTopNew[i] = 0.0;
}
}
if (simulationIsOn) {
int cntN = 0;
for (int region = 3; region < 6; region++) {
string reg = "region" + ofToString(region);
int numCols;
if (region == 4) {
numCols = 3;
} else {
numCols = 4;
}
/* TODO: Did this get f****d up? */
for (int pointPos = 0; pointPos < numCols; pointPos++) {
string point = "point" + ofToString(pointPos);
float colX = columnGeometry[reg][point][0].asFloat();
for (int i = 0; i < 20; i++) {
if (i == 4 || i == 5) {
} else {
if (abs(spheresXPos[i]-columnGeometry[reg][point][0].asFloat()) < 100) {
micLevelsTopNew[cntN]++;
}
}
}
cntN++;
}
}
}
}
//--------------------------------------------------------------
void testApp::draw(){
// Display background
ofSetColor(255, 255, 255, 50);
img.draw(0, 0, ofGetWidth(), ofGetHeight());
// Draw ball
ofSetColor(255, 150, 0);
ofCircle(ballPositionX, ballPositionY, 10);
/************ DRAW PARTICLE SYSTEM ***********************/
particleSystem.setTimeStep(timeStep);
ofEnableAlphaBlending();
ofSetColor(251, 236, 93, lineOpacity);
particleSystem.setupForces();
// apply per-particle forces
glBegin(GL_LINES);
for(int i = 0; i < particleSystem.size(); i++) {
Particle& cur = particleSystem[i];
// global force on other particles
particleSystem.addRepulsionForce(cur, particleNeighborhood, particleRepulsion);
// forces on this particle
cur.bounceOffWalls(0, 0, ofGetWidth(), ofGetHeight());
cur.addDampingForce();
}
glEnd();
// Apply attraction to selected points
for (int i=0; i < attractPts.size(); i++) {
particleSystem.addAttractionForce(attractPts[i]->x, attractPts[i]->y, ofGetWidth(), centerAttraction);
}
// Add repulsion force by mouse click
if(isMousePressed)
particleSystem.addRepulsionForce(mouseX, mouseY, 100, 10);
// Set the ball to repulse
particleSystem.addRepulsionForce(ballPositionX, ballPositionY, 25, 10);
particleSystem.update();
ofSetColor(255, 255, 255, pointOpacity);
particleSystem.draw();
ofDisableAlphaBlending();
ofSetColor(255, 255, 255);
//ofDrawBitmapString(ofToString(kParticles) + "k particles", 32, 32);
//ofDrawBitmapString(ofToString((int) ofGetFrameRate()) + " fps", 32, 52);
// draw depth and color view
//kinect.drawDepth(10, 10, 200, 150);
//kinect.draw(220, 10, 200, 150);
//colorImg.mirror(false, true);
//colorImg.draw(100,100);
/************ DRAW POINT CLOUD ***********************/
// draw point cloud
pointCloudX = 800;
pointCloudY = 800;
ofSetColor(0, 0, 0);
ofPushMatrix();
ofTranslate(pointCloudX, pointCloudY);
drawPointCloud();
ofPopMatrix();
// Make attraction with point
/*for (int i=0; i < cloudPts.size(); i++) {
particleSystem.addAttractionForce(cloudPts[i]->x + 800, cloudPts[i]->y + 650, 25, 1);
//printf("Point Clouds: %f %f \n", cloudPts[i]->x, cloudPts[i]->y);
} */
// Rotate drawing
if(depthContours.blobs.size() == 2) {
ofxVec2f center = depthContours.blobs[0].centroid + depthContours.blobs[1].centroid;
ofxVec2f targetViewRot;
targetViewRot.x = ofMap(center.x, 0, ofGetWidth(), -360, 360);
targetViewRot.y = ofMap(center.y, 0, ofGetHeight(), 360, -360);
viewRot += (targetViewRot - viewRot) * 0.05;
} else {
viewRot -= viewRot * 0.05;
}
ofxVec3f camToWorld(ofGetWidth()/depthOrig.getWidth(), ofGetHeight()/depthOrig.getHeight(), 1);
ofxVec3f up(0, 1, 0);
gui.draw();
/********** Calculate 2D Coordinate from 3D Space ********/
GLdouble screenX = 0;
GLdouble screenY = 0;
GLdouble screenZ = 0;
GLint viewport[4];
GLdouble mvmatrix[16], projmatrix[16];
glGetIntegerv(GL_VIEWPORT, viewport);
glGetDoublev(GL_MODELVIEW_MATRIX, mvmatrix);
glGetDoublev(GL_PROJECTION_MATRIX, projmatrix);
gluProject(pointCloudX, pointCloudY, -3000,
mvmatrix, projmatrix, viewport,
&screenX, &screenY, &screenZ);
screenY = ofGetHeight() - screenY;
// Draws contours on the screen
depthContours.draw(screenX, screenY, 640, 480);
}
//--------------------------------------------------------------
void testApp::update(){
/************ UPDATE BALL ***********************/
//Update ball position
ballPositionX += ballVelocityX;
ballPositionY += ballVelocityY;
if(ballPositionX < 0 || ballPositionX > ofGetWidth()) {
ballVelocityX *= -1;
}
if (ballPositionY < 0 || ballPositionY > ofGetHeight()) {
ballVelocityY *= -1;
}
/************ UPDATE KINECT ***********************/
kinect.update();
// get color pixels
colorImageRGB = kinect.getPixels();
// get depth pixels
depthOrig = kinect.getDepthPixels();
// save original depth, and do some preprocessing
depthProcessed = depthOrig;
if(invert) depthProcessed.invert();
if(mirror) {
depthOrig.mirror(false, true);
depthProcessed.mirror(false, true);
colorImageRGB.mirror(false, true);
}
if(preBlur) cvSmooth(depthProcessed.getCvImage(), depthProcessed.getCvImage(), CV_BLUR , preBlur*2+1);
if(topThreshold) cvThreshold(depthProcessed.getCvImage(), depthProcessed.getCvImage(), topThreshold * 255, 255, CV_THRESH_TRUNC);
if(bottomThreshold) cvThreshold(depthProcessed.getCvImage(), depthProcessed.getCvImage(), bottomThreshold * 255, 255, CV_THRESH_TOZERO);
if(dilateBeforeErode) {
if(dilateAmount) cvDilate(depthProcessed.getCvImage(), depthProcessed.getCvImage(), 0, dilateAmount);
if(erodeAmount) cvErode(depthProcessed.getCvImage(), depthProcessed.getCvImage(), 0, erodeAmount);
} else {
if(erodeAmount) cvErode(depthProcessed.getCvImage(), depthProcessed.getCvImage(), 0, erodeAmount);
if(dilateAmount) cvDilate(depthProcessed.getCvImage(), depthProcessed.getCvImage(), 0, dilateAmount);
}
depthProcessed.flagImageChanged();
// find contours
depthContours.findContours(depthProcessed,
minBlobSize * minBlobSize * depthProcessed.getWidth() * depthProcessed.getHeight(),
maxBlobSize * maxBlobSize * depthProcessed.getWidth() * depthProcessed.getHeight(),
maxNumBlobs, findHoles, useApproximation);
// Clear old attraction points
attractPts.clear();
// Find centroid point for each blob area and add an attraction force to it
for (int i=0; i<depthContours.blobs.size(); i++) {
attractPts.push_back(new ofPoint(depthContours.blobs[i].centroid));
//printf("Blob %d: %f %f \n", i, depthContours.blobs[i].centroid.x, depthContours.blobs[i].centroid.y);
}
// if one blob found, find nearest point in blob area
static ofxVec3f newPoint;
if(depthContours.blobs.size() == 1) {
ofxCvBlob &blob = depthContours.blobs[0];
depthOrig.setROI(blob.boundingRect);
double minValue, maxValue;
CvPoint minLoc, maxLoc;
cvMinMaxLoc(depthOrig.getCvImage(), &minValue, &maxValue, &minLoc, &maxLoc, NULL);
depthOrig.resetROI();
newPoint.x = maxLoc.x + blob.boundingRect.x;
newPoint.y = maxLoc.y + blob.boundingRect.y;
// newPoint.z = (maxValue + offset) * depthScale; // read from depth map
//printf("Min: %f %f Max: %f %f \n", minLoc.x, minLoc.y, maxLoc.x, maxLoc.y);
// read directly from distance (in cm)
// this doesn't seem to work, need to look into it
newPoint.z = (kinect.getDistanceAt(newPoint) + depthOffset) * depthScale;
// apply kalman filtering
if(doKalman) {
newPoint.x = updateKalman(0, newPoint.x);
newPoint.y = updateKalman(1, newPoint.y);
newPoint.z = updateKalman(2, newPoint.z);
}
} else {
clearKalman(0);
clearKalman(1);
clearKalman(2);
}
pointHead = (pointHead + 1) % kNumPoints;
curPoint += (newPoint - curPoint) * lerpSpeed;
points[pointHead] = curPoint;
}
void draw()
{
ofSetColor(255);
// A few helper variables for layout.
int hw = width / 2; // Half width
int hh = height / 2; // Half height.
int qw = width / 4; // Quarter width.
int qh = height / 4; // Quarter height.
int lx = 14; // Label offset x.
int ly = 20; // Label offset y.
grayscaleImage.draw(0, 0, qw, qh);
ofDrawBitmapStringHighlight("0. Grayscale", lx, ly);
grayscaleBackgroundImage.draw(qw, 0, qw, qh);
ofDrawBitmapStringHighlight("1. Background\n (spacebar)", lx + qw, ly);
grayscaleAbsoluteDifference.draw(0, qh, qw, qh);
ofDrawBitmapStringHighlight("2. Grayscale - Background", lx, ly + qh);
grayscaleBinary.draw(qw, qh, qw, qh);
ofDrawBitmapStringHighlight("3. Threshold " + ofToString(threshold) + "\n (-/+: change threshold)\n ( i: invert)", lx + qw, ly + qh);
// Here we use ofPushMatrix(), ... to scale all of the contours and bounding boxes.
ofPushStyle();
ofPushMatrix();
ofTranslate(hw, 0);
ofScale(0.5, 0.5, 1);
grayscaleBinary.draw(0, 0);
contourFinder.draw(); // Draw all of the contours and their bounding boxes.
// Draw our line.
ofSetColor(ofColor::yellow);
holePositions.draw();
ofPopMatrix();
ofDrawBitmapStringHighlight("4. Contours and Bounding Boxes\n Draw a yellow line to follow\n the center of the largest blob.", lx + hw, ly);
ofPopStyle();
colorImage.draw(0, 0);//, hw, hh);
// ofDrawBitmapStringHighlight("5. Original", lx, ly + hh);
for (int i = 0; i < contourFinder.nBlobs; ++i)
{
ofPolyline contour(contourFinder.blobs[i].pts);
// Resample to reduce the resolution.
contour = contour.getResampledBySpacing(5);
float interpolatedIndex = offset * contour.size();
ofPoint position = contour.getPointAtIndexInterpolated(interpolatedIndex);
ofPoint normal = contour.getNormalAtIndexInterpolated(interpolatedIndex);
// Make a line pointing normal to the contour.
ofPoint lineEnd = position - normal * 30;
ofSetColor(ofColor::yellow);
contour.draw();
ofLine(position, lineEnd);
ofCircle(lineEnd, 2);
}
}
