float Tracker2D::getSampleProbability(int sample_id)
{
cv::Point2f sample = m_particleFilterTracker.getSampleByIndex(sample_id);
/*
*calculate the histogram of H value in the ROI
*/
int height = m_currentHSV.rows;
int width = m_currentHSV.cols;
// need to check the boundary
int leftUpX = (int)(sample.x - m_trackerSize /2 );
leftUpX = leftUpX < 0 ? 0: leftUpX;
int leftUpY = (int)(sample.y - m_trackerSize /2 );
leftUpY = leftUpY < 0 ? 0 : leftUpY;
int rightBottomX = (int)(sample.x + m_trackerSize / 2);
rightBottomX = rightBottomX >= width ? width - 1 : rightBottomX;
int rightBottomY = (int)(sample.y + m_trackerSize / 2);
rightBottomY = rightBottomY >= height ? height - 1 : rightBottomY;
int roiWidth = rightBottomX - leftUpX;
int roiHeight = rightBottomY - leftUpY;
if( roiWidth <= 5 && roiHeight <= 5){
return 0.0f;
}
//cv::Mat roiMat(m_currentHSV, cv::Rect((int)(sample.x - m_trackerSize / 2), (int)(sample.y - m_trackerSize / 2), m_trackerSize, m_trackerSize));
cv::Mat roiMat(m_currentHSV, cv::Rect(leftUpX, leftUpY, roiWidth, roiHeight));
cv::Mat histforH;
calculateHueHistogram(roiMat, histforH);
//std::cout << "Sample feature: " << std::endl;
//std::cout << histforH << std::endl;
cv::Mat histForHNorm;
cv::normalize(histforH, histForHNorm, 1.0, 0.0, cv::NORM_MINMAX);
/*
*Gaussian distribution.
*/
float differenceSquare = 0;
for (int i = 0; i < m_histogramSize ; i++)
{
float delta = m_objectHfeature[i] - histForHNorm.at<float>(i);
delta = delta * delta;
differenceSquare += delta;
}
//std::cout << differenceSquare << std::endl;
//std::cout << expf(-differenceSquare / m_sigma) << std::endl;
return expf(-differenceSquare / m_sigma);
}
//------------------------------------------------------------------------
ofPixelsRef CvProcessor::process ( ofBaseImage & image ){
if ( bTrackHaar ){
processHaar( cameraBabyImage );
}
if ( bTrackOpticalFlow ){
processOpticalFlow( cameraSmallImage );
}
differencedImage.setFromPixels(image.getPixelsRef());
ofxCv::threshold(differencedImage, threshold);
// find contours
contourFinder.setFindHoles( bFindHoles );
contourFinder.setMinArea( minBlobArea * tspsWidth * tspsHeight );
contourFinder.setMaxArea( maxBlobArea * tspsWidth * tspsHeight );
contourFinder.findContours( differencedImage );
// update people
RectTracker& rectTracker = contourFinder.getTracker();
cv::Mat cameraMat = toCv(cameraImage);
//optical flow scale
// float flowROIScale = tspsWidth/flow.getWidth();
for(int i = 0; i < contourFinder.size(); i++){
unsigned int id = contourFinder.getLabel(i);
if(rectTracker.existsPrevious(id)) {
CvPerson* p = (CvPerson *) getTrackedPerson(id);
//somehow we are not tracking this person, safeguard (shouldn't happen)
if(NULL == p){
ofLog(OF_LOG_WARNING, "Person::warning. encountered persistent blob without a person behind them\n");
continue;
}
p->oid = i; //hack ;(
//update this person with new blob info
// to-do: make centroid dampening dynamic
p->update(true);
//normalize simple contour
for (int i=0; i<p->simpleContour.size(); i++){
p->simpleContour[i].x /= tspsWidth;
p->simpleContour[i].y /= tspsHeight;
}
//find peak in blob (only useful with depth cameras)
cv::Point minLoc, maxLoc;
double minVal = 0, maxVal = 0;
cv::Rect rect;
rect.x = p->boundingRect.x;
rect.y = p->boundingRect.y;
rect.width = p->boundingRect.width;
rect.height = p->boundingRect.height;
cv::Mat roiMat(cameraMat, rect);
cv::minMaxLoc( roiMat, &minVal, &maxVal, &minLoc, &maxLoc, cv::Mat());
// set depth
p->depth = p->highest.z / 255.0f;
// set highest and lowest points: x, y, VALUE stored in .z prop
// ease vals unless first time you're setting them
if ( p->highest.x == -1 ){
p->highest.set( p->boundingRect.x + maxLoc.x, p->boundingRect.y + maxLoc.y, maxVal);
p->lowest.set( p->boundingRect.x + minLoc.x, p->boundingRect.y + minLoc.y, minVal);
} else {
p->highest.x = ( p->highest.x * .9 ) + ( p->boundingRect.x + maxLoc.x ) * .1;
p->highest.y = ( p->highest.y * .9 ) + ( p->boundingRect.y + maxLoc.y ) * .1;
p->highest.z = ( p->highest.z * .9) + ( maxVal ) * .1;
p->lowest.x = ( p->lowest.x * .9 ) + ( p->boundingRect.x + minLoc.x ) * .1;
p->lowest.y = ( p->lowest.y * .9 ) + ( p->boundingRect.y + minLoc.y ) * .1;
p->lowest.z = ( p->lowest.z * .9) + ( minVal ) * .1;
}
// cap highest + lowest
p->highest.x = (p->highest.x > tspsWidth ? tspsWidth : p->highest.x);
p->highest.x = (p->highest.x < 0 ? 0 : p->highest.x);
p->highest.y = (p->highest.y > tspsHeight ? tspsHeight : p->highest.y);
p->highest.y = (p->highest.y < 0 ? 0 : p->highest.y);
p->lowest.x = (p->lowest.x > tspsWidth ? tspsWidth : p->lowest.x);
p->lowest.x = (p->lowest.x < 0 ? 0 : p->highest.x);
p->lowest.y = (p->lowest.y > tspsHeight ? tspsHeight : p->lowest.y);
p->lowest.y = (p->lowest.y < 0 ? 0 : p->highest.y);
// ROI for opticalflow
ofRectangle roi = p->getBoundingRectNormalized(tspsWidth, tspsHeight);
roi.x *= flow.getWidth();
roi.y *= flow.getHeight();
roi.width *= flow.getWidth();
roi.height *= flow.getHeight();
// sum optical flow for the person
if ( bTrackOpticalFlow && bFlowTrackedOnce ){
// TO-DO!
p->opticalFlowVectorAccumulation = flow.getAverageFlowInRegion(roi);
} else {
p->opticalFlowVectorAccumulation.x = p->opticalFlowVectorAccumulation.y = 0;
}
//detect haar patterns (faces, eyes, etc)
if ( bTrackHaar ){
//find the region of interest, expanded by haarArea.
ofRectangle haarROI;
haarROI.x = (p->boundingRect.x - haarAreaPadding/2) * haarTrackingScale > 0.0f ? (p->boundingRect.x - haarAreaPadding/2) * haarTrackingScale : 0.0;
haarROI.y = (p->boundingRect.y - haarAreaPadding/2) * haarTrackingScale > 0.0f ? (p->boundingRect.y - haarAreaPadding/2) : 0.0f;
haarROI.width = (p->boundingRect.width + haarAreaPadding*2) * haarTrackingScale > cameraBabyImage.width ? (p->boundingRect.width + haarAreaPadding*2) * haarTrackingScale : cameraBabyImage.width;
haarROI.height = (p->boundingRect.height + haarAreaPadding*2) * haarTrackingScale > cameraBabyImage.height ? (p->boundingRect.height + haarAreaPadding*2) * haarTrackingScale : cameraBabyImage.height;
bool haarThisFrame = false;
for(int j = 0; j < haarObjects.size(); j++) {
ofRectangle hr = toOf(haarObjects[j]);
//check to see if the haar is contained within the bounding rectangle
if(hr.x > haarROI.x && hr.y > haarROI.y && hr.x+hr.width < haarROI.x+haarROI.width && hr.y+hr.height < haarROI.y+haarROI.height){
hr.x /= haarTrackingScale;
hr.y /= haarTrackingScale;
hr.width /= haarTrackingScale;
hr.height /= haarTrackingScale;
p->setHaarRect(hr);
haarThisFrame = true;
break;
}
}
if(!haarThisFrame){
p->noHaarThisFrame();
}
}
personUpdated(p, scene);
} else {
ofPoint centroid = toOf(contourFinder.getCentroid(i));
CvPerson* newPerson = new CvPerson(id, i, contourFinder);
personEntered(newPerson, scene);
}
}
//reset scene
if ( bTrackOpticalFlow && bFlowTrackedOnce ){
scene->averageMotion = flow.getAverageFlow();
} else {
scene->averageMotion = ofPoint(0,0);
}
scene->update( trackedPeople, tspsWidth, tspsHeight );
// delete old blobs
for (int i=trackedPeople->size()-1; i>=0; i--){
Person* p = (*trackedPeople)[i];
EventArgs args;
args.person = p;
args.scene = scene;
if (p == NULL){
personWillLeave(p, scene);
trackedPeople->erase(trackedPeople->begin() + i);
} else if ( !(rectTracker.existsPrevious( p->pid ) && rectTracker.existsCurrent(p->pid)) && !rectTracker.existsCurrent(p->pid) ){
personWillLeave(p, scene);
trackedPeople->erase(trackedPeople->begin() + i);
}
}
return differencedImage.getPixelsRef();
}
