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;
}
