double* KalmanFilter::getApprox(double speedAnguGyro,double angleAccelero) { Y[0] = speedAnguGyro; Y[1] = angleAccelero; predictionKalman(); updateKalman(); return X; }
/// /// \brief LogicInitiator::processObservations main function which processes new observations /// to initiation candidates. And compares already existing candidates and observation with use of /// cascaded logic. /// \param observations new observations /// \return new tracks to be initated to tracker /// InitiatorCandidates LogicInitiator::processObservations(const Observations observations) { // declare structure for new tracks which should be initiated by tracker InitiatorCandidates newTracks; if(observations.empty()) return newTracks; // declare structure for accepted candidates InitiatorCandidates acceptedCandidates; // Get delta time double timeDiff = observations.front()->createdAt - m_lastObservationTime; m_lastObservationTime = observations.front()->createdAt; // Get observations which could not be matched by the tracker Observations newObs; foreach(Observation::Ptr ob, observations) { if(!ob->matched) { newObs.push_back(ob); } } // Associate new observations with existing candidates for initiation foreach(InitiatorCandidate::Ptr candidate, m_candidates) { // initialize flag if pairing could be found for candidate bool pairingFound(false); // If selected number of scans will be reached tracks are created if(candidate->observations.size() == m_numberScans-1) { predictKalman(candidate->state, timeDiff); candidate->extrapolation = linearExtrapolation(*(candidate->observations.rbegin()+1), candidate->observations.back(),timeDiff); foreach(Observation::Ptr observation,newObs) { if(compareWithExtrapolation(candidate,observation)) { if(compareWithCandidate(candidate, observation)) { if(updateKalman(candidate->state,observation)) { pairingFound = true; candidate->missedObs = 0; observation->matched = true; candidate->observations.push_back(observation); newTracks.push_back(candidate); } } } } }
void ofxBlobTracker::track(vector<ofVec3f> &blobs) { blobSmoother.setSmoothness(smoothing); smoothedBlobs.clear(); untouchLastBlobs(); for(int i = 0; i < blobs.size() && i < MAX_NUM_BLOBS; i++) { // skip any NaN's (causes ofxTuioWrapper to run out of memory) if(blobs[i].x!=blobs[i].x || blobs[i].y!=blobs[i].y) { continue; } ofxBlob *blob = getClosestBlob(blobs[i]); // new blob! if(blob==NULL || blob->distance(blobs[i])>minTrackDistance) { ofxBlob *b = newBlob(blobs[i]); updateKalman(b->id, b); notifyAllListeners(*b, b->id, ofxBlobTracker_entered); // updated blob } else { blob = updateKalman(blob->id, blob); updateBlob(blob->id, &blobs[i]); notifyAllListeners(*blob, blob->id, ofxBlobTracker_moved); } } // dead blob // delete any untouched blobs for(int i = 0; i < lastBlobs.size(); i++) { if(lastBlobs[i]->touched==false) { notifyAllListeners(*lastBlobs[i], lastBlobs[i]->id, ofxBlobTracker_exited); delete lastBlobs[i]; lastBlobs.erase(lastBlobs.begin()+i); i--; } } }
//-------------------------------------------------------------- void TuioKinect::update() { ofBackground(10, 10, 10); //Kinect + Feeds updating kinect.update(); grayImage.setFromPixels(kinect.getDepthPixels(), kinect.width, kinect.height); grayImage.mirror(false, true); unsigned char * pix = grayImage.getPixels(); int numPixels = grayImage.getWidth() * grayImage.getHeight()-1; depthImage.setFromPixels(pix, kinect.width, kinect.height); depthImage.flagImageChanged(); colorImage.setFromPixels(kinect.getPixels(), kinect.width, kinect.height); colorImage.mirror(false, true); colorImage.convertToGrayscalePlanarImage(redImage, 0); for(int i = numPixels; i > 0 ; i--){ if( pix[i] > nearThreshold && pix[i] < farThreshold ){ pix[i] = 255; }else{ pix[i] = 0; } } //update the cv image grayImage.flagImageChanged(); unsigned char * red = redImage.getPixels(); numPixels = redImage.getWidth() * redImage.getHeight(); // contourFinder.findContours(grayImage, 900, (kinect.width*kinect.height)/8, 20, false); TuioTime frameTime = TuioTime::getSessionTime(); tuioServer->initFrame(frameTime); int numBlobs = contourFinder.blobs.size() ; int count = 0 ; if ( numBlobs == 0 ) holdingLength = 0 ; //detect all blobs std::vector<ofxCvBlob>::iterator blob; for (blob=contourFinder.blobs.begin(); blob!= contourFinder.blobs.end(); blob++) { float xpos = (*blob).centroid.x; float ypos = (*blob).centroid.y; TuioPoint tp(xpos/kinect.width,ypos/kinect.height); TuioCursor *tcur = tuioServer->getClosestTuioCursor(tp.getX(),tp.getY()); //Adjust here to change minumum distance for new points if ((tcur==NULL) || (tcur->getDistance(&tp)>0.1)) { tcur = tuioServer->addTuioCursor(tp.getX(), tp.getY()); updateKalman(tcur->getCursorID(),tcur); } else { TuioPoint kp = updateKalman(tcur->getCursorID(),tp); tuioServer->updateTuioCursor(tcur, kp.getX(), kp.getY()); } xpos = tp.getX() * ofGetWidth() ; ypos = tp.getY() * ofGetHeight() ; if ( numBlobs < 3 ) { kCursors[count].x = xpos ; kCursors[count].y = ypos ; } count++ ; } float strength = 5.0f; float damping = 0.5f; if ( numBlobs == 2 ) { if ( holdingLength < 40 ) holdingLength++ ; float holdingRatio = 0.05f + (float)holdingLength/40.0f ; float minDis = 100.0f; ofPoint c1 = kCursors[1] ; ofPoint c0 = kCursors[0] ; for(int i=0; i< customParticles.size() ; i++) { CustomParticle& p = customParticles[i] ; if ( i % 2 == 0 ) { p.addAttractionPoint( c0.x, c0.y, (float)holdingLength*1.05 * strength, minDis); } else { p.addAttractionPoint( c1.x, c1.y, (float)holdingLength*1.05 * strength, minDis); } p.addDamping(damping, damping); } } //remove non moving cursors tuioServer->stopUntouchedMovingCursors(); std::list<TuioCursor*> dead_cursor_list = tuioServer->getUntouchedCursors(); std::list<TuioCursor*>::iterator dead_cursor; for (dead_cursor=dead_cursor_list.begin(); dead_cursor!= dead_cursor_list.end(); dead_cursor++) { clearKalman((*dead_cursor)->getCursorID()); } tuioServer->removeUntouchedStoppedCursors(); tuioServer->commitFrame(); box2d.update() ; }
//-------------------------------------------------------------- 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; }