void cvRenderTracks(CvTracks const tracks, IplImage *imgSource, IplImage *imgDest, unsigned short mode, CvFont *font)
{
CV_FUNCNAME("cvRenderTracks");
__CV_BEGIN__;
CV_ASSERT(imgDest&&(imgDest->depth==IPL_DEPTH_8U)&&(imgDest->nChannels==3));
if ((mode&CV_TRACK_RENDER_ID)&&(!font))
{
if (!defaultFont)
{
font = defaultFont = new CvFont;
cvInitFont(font, CV_FONT_HERSHEY_DUPLEX, 0.5, 0.5, 0, 1);
// Other fonts:
// CV_FONT_HERSHEY_SIMPLEX, CV_FONT_HERSHEY_PLAIN,
// CV_FONT_HERSHEY_DUPLEX, CV_FONT_HERSHEY_COMPLEX,
// CV_FONT_HERSHEY_TRIPLEX, CV_FONT_HERSHEY_COMPLEX_SMALL,
// CV_FONT_HERSHEY_SCRIPT_SIMPLEX, CV_FONT_HERSHEY_SCRIPT_COMPLEX
}
else
font = defaultFont;
}
if (mode)
{
for (CvTracks::const_iterator it=tracks.begin(); it!=tracks.end(); ++it)
{
if (mode&CV_TRACK_RENDER_ID)
if (!it->second->inactive)
{
stringstream buffer;
buffer << it->first;
cvPutText(imgDest, buffer.str().c_str(), cvPoint((int)it->second->centroid.x, (int)it->second->centroid.y), font, CV_RGB(0.,255.,0.));
}
if (mode&CV_TRACK_RENDER_BOUNDING_BOX)
if (it->second->inactive)
cvRectangle(imgDest, cvPoint(it->second->minx, it->second->miny), cvPoint(it->second->maxx-1, it->second->maxy-1), CV_RGB(0., 0., 50.));
else
cvRectangle(imgDest, cvPoint(it->second->minx, it->second->miny), cvPoint(it->second->maxx-1, it->second->maxy-1), CV_RGB(0., 0., 255.));
if (mode&CV_TRACK_RENDER_TO_LOG)
{
clog << "Track " << it->second->id << endl;
if (it->second->inactive)
clog << " - Inactive for " << it->second->inactive << " frames" << endl;
else
clog << " - Associated with blob " << it->second->label << endl;
clog << " - Lifetime " << it->second->lifetime << endl;
clog << " - Active " << it->second->active << endl;
clog << " - Bounding box: (" << it->second->minx << ", " << it->second->miny << ") - (" << it->second->maxx << ", " << it->second->maxy << ")" << endl;
clog << " - Centroid: (" << it->second->centroid.x << ", " << it->second->centroid.y << ")" << endl;
clog << endl;
}
if (mode&CV_TRACK_RENDER_TO_STD)
{
cout << "Track " << it->second->id << endl;
if (it->second->inactive)
cout << " - Inactive for " << it->second->inactive << " frames" << endl;
else
cout << " - Associated with blobs " << it->second->label << endl;
cout << " - Lifetime " << it->second->lifetime << endl;
cout << " - Active " << it->second->active << endl;
cout << " - Bounding box: (" << it->second->minx << ", " << it->second->miny << ") - (" << it->second->maxx << ", " << it->second->maxy << ")" << endl;
cout << " - Centroid: (" << it->second->centroid.x << ", " << it->second->centroid.y << ")" << endl;
cout << endl;
}
}
}
__CV_END__;
}
void cvUpdateTracks(CvBlobs const &blobs, CvTracks &tracks, const double thDistance, const unsigned int thInactive, const unsigned int thActive)
{
CV_FUNCNAME("cvUpdateTracks");
__CV_BEGIN__;
unsigned int nBlobs = blobs.size();
unsigned int nTracks = tracks.size();
// Proximity matrix:
// Last row/column is for ID/label.
// Last-1 "/" is for accumulation.
CvID *close = new unsigned int[(nBlobs+2)*(nTracks+2)]; // XXX Must be same type than CvLabel.
try
{
// Inicialization:
unsigned int i=0;
for (CvBlobs::const_iterator it = blobs.begin(); it!=blobs.end(); ++it, i++)
{
AB(i) = 0;
IB(i) = it->second->label;
}
CvID maxTrackID = 0;
unsigned int j=0;
for (CvTracks::const_iterator jt = tracks.begin(); jt!=tracks.end(); ++jt, j++)
{
AT(j) = 0;
IT(j) = jt->second->id;
if (jt->second->id > maxTrackID)
maxTrackID = jt->second->id;
}
// Proximity matrix calculation and "used blob" list inicialization:
for (i=0; i<nBlobs; i++)
for (j=0; j<nTracks; j++)
if (C(i, j) = (distantBlobTrack(B(i), T(j)) < thDistance))
{
AB(i)++;
AT(j)++;
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Detect inactive tracks
for (j=0; j<nTracks; j++)
{
unsigned int c = AT(j);
if (c==0)
{
//cout << "Inactive track: " << j << endl;
// Inactive track.
CvTrack *track = T(j);
track->inactive++;
track->label = 0;
}
}
// Detect new tracks
for (i=0; i<nBlobs; i++)
{
unsigned int c = AB(i);
if (c==0)
{
//cout << "Blob (new track): " << maxTrackID+1 << endl;
//cout << *B(i) << endl;
// New track.
maxTrackID++;
CvBlob *blob = B(i);
CvTrack *track = new CvTrack;
track->id = maxTrackID;
track->label = blob->label;
track->minx = blob->minx;
track->miny = blob->miny;
track->maxx = blob->maxx;
track->maxy = blob->maxy;
track->centroid = blob->centroid;
track->lifetime = 0;
track->active = 0;
track->inactive = 0;
tracks.insert(CvIDTrack(maxTrackID, track));
}
}
// Clustering
for (j=0; j<nTracks; j++)
{
unsigned int c = AT(j);
if (c)
{
list<CvTrack*> tt; tt.push_back(T(j));
list<CvBlob*> bb;
getClusterForTrack(j, close, nBlobs, nTracks, blobs, tracks, bb, tt);
// Select track
CvTrack *track;
unsigned int area = 0;
for (list<CvTrack*>::const_iterator it=tt.begin(); it!=tt.end(); ++it)
{
CvTrack *t = *it;
unsigned int a = (t->maxx-t->minx)*(t->maxy-t->miny);
if (a>area)
{
area = a;
track = t;
}
}
// Select blob
CvBlob *blob;
area = 0;
//cout << "Matching blobs: ";
for (list<CvBlob*>::const_iterator it=bb.begin(); it!=bb.end(); ++it)
{
CvBlob *b = *it;
//cout << b->label << " ";
if (b->area>area)
{
area = b->area;
blob = b;
}
}
//cout << endl;
// Update track
//cout << "Matching: track=" << track->id << ", blob=" << blob->label << endl;
track->label = blob->label;
track->centroid = blob->centroid;
track->minx = blob->minx;
track->miny = blob->miny;
track->maxx = blob->maxx;
track->maxy = blob->maxy;
if (track->inactive)
track->active = 0;
track->inactive = 0;
// Others to inactive
for (list<CvTrack*>::const_iterator it=tt.begin(); it!=tt.end(); ++it)
{
CvTrack *t = *it;
if (t!=track)
{
//cout << "Inactive: track=" << t->id << endl;
t->inactive++;
t->label = 0;
}
}
}
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
for (CvTracks::iterator jt=tracks.begin(); jt!=tracks.end();)
if ((jt->second->inactive>=thInactive)||((jt->second->inactive)&&(thActive)&&(jt->second->active<thActive)))
{
delete jt->second;
tracks.erase(jt++);
}
else
{
jt->second->lifetime++;
if (!jt->second->inactive)
jt->second->active++;
++jt;
}
}
catch (...)
{
delete[] close;
throw; // TODO: OpenCV style.
}
delete[] close;
__CV_END__;
}
void processImage(cv::Mat& image) {
if (image.empty())
return;
#ifdef _OPENCV3
pMOG->apply(image, fgMaskMOG, 0.05);
#else
pMOG->operator()(image, fgMaskMOG, 0.05);
#endif
cv::dilate(fgMaskMOG,fgMaskMOG,cv::getStructuringElement(cv::MORPH_ELLIPSE,cv::Size(15,15)));
bin = new IplImage(fgMaskMOG);
frame = new IplImage(image);
labelImg = cvCreateImage(cvSize(image.cols,image.rows),IPL_DEPTH_LABEL,1);
unsigned int result = cvLabel(bin, labelImg, blobs);
cvRenderBlobs(labelImg, blobs, frame, frame, CV_BLOB_RENDER_BOUNDING_BOX|CV_BLOB_RENDER_CENTROID|CV_BLOB_RENDER_ANGLE);
cvFilterByArea(blobs, 1500, 40000);
cvUpdateTracks(blobs, tracks, 200., 5);
cvRenderTracks(tracks, frame, frame, CV_TRACK_RENDER_ID);
for (std::map<CvID, CvTrack*>::iterator track_it = tracks.begin(); track_it!=tracks.end(); track_it++) {
CvID id = (*track_it).first;
CvTrack* track = (*track_it).second;
cur_pos = track->centroid;
if (track->inactive == 0) {
if (last_poses.count(id)) {
std::map<CvID, CvPoint2D64f>::iterator pose_it = last_poses.find(id);
last_pos = pose_it -> second;
last_poses.erase(pose_it);
}
last_poses.insert(std::pair<CvID, CvPoint2D64f>(id, cur_pos));
if (line_pos+25>cur_pos.y && cur_pos.y>line_pos && line_pos-25<last_pos.y && last_pos.y<line_pos) {
count++;
countUD++;
}
if (line_pos-25<cur_pos.y && cur_pos.y<line_pos && line_pos+25>last_pos.y && last_pos.y>line_pos) {
count++;
countDU++;
}
if ( cur_pos.y<line_pos+50 && cur_pos.y>line_pos-50) {
avg_vel += abs(cur_pos.y-last_pos.y);
count_active++;
}
//update heatmapfg
heat_mapfg = cv::Mat::zeros(FR_H, FR_W, CV_8UC3);
count_arr[lmindex] = count;
avg_vel_arr[lmindex] = avg_vel/count_active ;
for (int i=0; i<landmarks.size(); i++) {
cv::circle(heat_mapfg, cv::Point((landmarks[i].y + 50)*2.4, (landmarks[i].x + 50)*2.4), count_arr[i]*3, cv::Scalar(0, 16*avg_vel_arr[i], 255 - 16*avg_vel_arr[i]), -1);
}
cv::GaussianBlur(heat_mapfg, heat_mapfg, cv::Size(15, 15), 5);
} else {
if (last_poses.count(id)) {
last_poses.erase(last_poses.find(id));
}
}
}
cv::line(image, cv::Point(0, line_pos), cv::Point(FR_W, line_pos), cv::Scalar(0,255,0),2);
cv::putText(image, "COUNT: "+to_string(count), cv::Point(10, 15), cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(255,255,255));
cv::putText(image, "UP->DOWN: "+to_string(countUD), cv::Point(10, 30), cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(255,255,255));
cv::putText(image, "DOWN->UP: "+to_string(countDU), cv::Point(10, 45), cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(255,255,255));
cv::imshow("BLOBS", image);
cv::imshow("HEATMAP", heat_map + heat_mapfg);
cv::waitKey(33);
}
void cvRenderTracks(CvTracks const tracks, IplImage *imgSource, IplImage *imgDest, unsigned short mode, CvFont *font)
{
CV_FUNCNAME("cvRenderTracks");
__CV_BEGIN__;
CV_ASSERT(imgDest&&(imgDest->depth==IPL_DEPTH_8U)&&(imgDest->nChannels==3));
if ((mode&CV_TRACK_RENDER_ID)&&(!font))
{
if (!defaultFont)
{
font = defaultFont = new CvFont;
hudFont = new CvFont;
cvInitFont(font, CV_FONT_HERSHEY_DUPLEX, 0.5, 0.5, 0, 1);
cvInitFont(hudFont, CV_FONT_HERSHEY_PLAIN, 0.8, 0.8 ,0.5);
// Other fonts:
// CV_FONT_HERSHEY_SIMPLEX, CV_FONT_HERSHEY_PLAIN,
// CV_FONT_HERSHEY_DUPLEX, CV_FONT_HERSHEY_COMPLEX,
// CV_FONT_HERSHEY_TRIPLEX, CV_FONT_HERSHEY_COMPLEX_SMALL,
// CV_FONT_HERSHEY_SCRIPT_SIMPLEX, CV_FONT_HERSHEY_SCRIPT_COMPLEX
}
else
font = defaultFont;
}
if (mode)
{
for (CvTracks::const_iterator it=tracks.begin(); it!=tracks.end(); ++it)
{
if (mode&CV_TRACK_RENDER_ID)
if (!it->second->inactive)
{
stringstream buffer, mouseloc;
buffer << it->first;
mouseloc << "Mouse Location : " << "(" << (int)it->second->centroid.x << " , " << (int)it->second->centroid.y << ")"; // // Mouse Location String
//my << it->second->centroid.y;
//cvPutText(imgDest, buffer.str().c_str(), cvPoint((int)it->second->centroid.x, (int)it->second->centroid.y), font, CV_RGB(0.,255.,0.));
// Drawing functions for the Draw-conture
/** C++: void circle(Mat& img, Point center, int radius, const Scalar& color, int thickness=1, int lineType=8, int shift=0) **/
/** C: void cvCircle(CvArr* img, CvPoint center, int radius, CvScalar color, int thickness=1, int line_type=8, int shift=0 ) **/
//cvPutText(imgDest, buffer.str().c_str(), cvPoint((int)it->second->centroid.x, (int)it->second->centroid.y), font, CV_RGB(0.0, 0.0, 0.0));
cvPutText(imgDest, buffer.str().c_str(), cvPoint((int)it->second->maxx, (int)it->second->miny), font, CV_RGB(255.0, 255.0, 0.0)); // // puts the blob id on the largest blob
//cout << buffer.str().c_str() << endl;
//cvPutText(imgDest, mouseloc.str().c_str(), cvPoint(0, 9), hudFont, CV_RGB(0.0, 230.0, 230.0));
cvCircle(imgDest, cvPoint((int)it->second->centroid.x, (int)it->second->centroid.y), 13, CV_RGB(0.0, 255.0, 255.0), 4, CV_AA, 0); // // Draws the origin-Circle
cvCircle(imgDest, cvPoint((int)it->second->centroid.x, (int)it->second->centroid.y), 4, CV_RGB(130.815, 0.0, 255.0), 1, CV_AA, 0);
//cout << "Blob minx = " << it->second->minx << endl;
}
if (mode&CV_TRACK_RENDER_BOUNDING_BOX)
if (it->second->inactive)
cvRectangle(imgDest, cvPoint(it->second->minx, it->second->miny), cvPoint(it->second->maxx-1, it->second->maxy-1), CV_RGB(0., 0., 50.));
else
cvRectangle(imgDest, cvPoint(it->second->minx, it->second->miny), cvPoint(it->second->maxx-1, it->second->maxy-1), CV_RGB(0., 0., 255.));
if (mode&CV_TRACK_RENDER_TO_LOG)
{
clog << "Track " << it->second->id << endl;
if (it->second->inactive)
clog << " - Inactive for " << it->second->inactive << " frames" << endl;
else
clog << " - Associated with blob " << it->second->label << endl;
clog << " - Lifetime " << it->second->lifetime << endl;
clog << " - Active " << it->second->active << endl;
clog << " - Bounding box: (" << it->second->minx << ", " << it->second->miny << ") - (" << it->second->maxx << ", " << it->second->maxy << ")" << endl;
clog << " - Centroid: (" << it->second->centroid.x << ", " << it->second->centroid.y << ")" << endl;
clog << endl;
}
if (mode&CV_TRACK_RENDER_TO_STD)
{
cout << "Track " << it->second->id << endl;
if (it->second->inactive)
cout << " - Inactive for " << it->second->inactive << " frames" << endl;
else
cout << " - Associated with blobs " << it->second->label << endl;
cout << " - Lifetime " << it->second->lifetime << endl;
cout << " - Active " << it->second->active << endl;
cout << " - Bounding box: (" << it->second->minx << ", " << it->second->miny << ") - (" << it->second->maxx << ", " << it->second->maxy << ")" << endl;
cout << " - Centroid: (" << it->second->centroid.x << ", " << it->second->centroid.y << ")" << endl;
cout << endl;
}
}
}
__CV_END__;
}
int blobslib_doall( blobtype *pblobs, int *num_blobs, blobparamstype *parms )
{
//while (cvGrabFrame(capture))
#ifdef OCVCAM
if (!cvGrabFrame(capture) )
return -1;
#endif
{
#ifdef OCVCAM
// Get raw cam image...
IplImage *img = cvRetrieveFrame(capture);
#else
// Show raw cam image...
if ( parms->drawmode == LOGO )
{
if ( _fullscreen && logohigh )
_show( logohigh, "gw_object_tracking", ".",0,0,0);
else if ( !_fullscreen && logonormal )
_show( logonormal, "gw_object_tracking", ".",0,0,0);
}
// Set camera parameters first and be smart about it!!
if ( _lastgain != parms->gain )
{
CLEyeSetCameraParameter(_cam, CLEYE_GAIN, parms->gain );
_lastgain = parms->gain;
}
if ( _lastexposure != parms->exposure )
{
CLEyeSetCameraParameter(_cam, CLEYE_EXPOSURE, parms->exposure );
_lastexposure = parms->exposure;
}
// Get raw image...
CLEyeCameraGetFrame(_cam, pCapBuffer);
CvSize imgSize = cvGetSize( pCapImage );
cvSplit( pCapImage, pb, pg, pr, NULL );
IplImage *img = cvCreateImage(imgSize, 8, 3);
cvMerge( pb, pg, pr, NULL, img );
#endif
// Possibly flip raw image here...
if ( parms->flipmode) cvFlip( img, NULL, 1);
// What is this ?
IplImage *frame = cvCreateImage(imgSize, img->depth, img->nChannels);
cvConvertScale(img, frame, 1, 0);
// Show raw cam image...
if ( parms->drawmode == RAW )
{
_show( img, "gw_object_tracking", "raw", 255,255,0);
#if 0
IplImage *display = cvCloneImage(img);
cvPutText (display,"raw",cvPoint(20,20), &font, cvScalar(255,255,0));
cvShowImage("gw_object_tracking", display);
cvReleaseImage( &display );
#endif
}
// BG mask, first time init only...
if ( ! bg_mask )
{
bg_mask = cvCreateImage(imgSize, 8, 1);
cvZero( bg_mask );
inv_bg_mask = cvCreateImage(imgSize, 8, 1);
cvNot( bg_mask, inv_bg_mask );
}
// Show BG mask...
if ( parms->drawmode==BGMASK )
{
_show( inv_bg_mask, "gw_object_tracking", "bg_mask", 0,0,0);
#if 0
IplImage *display = cvCloneImage(inv_bg_mask);
cvPutText (display,"bg mask",cvPoint(20,20), &font, cvScalar(0,0,0));
cvShowImage("gw_object_tracking", display);
cvReleaseImage( &display );
#endif
}
// Figure out who does key processing...
char k = 0;
if ( (parms->keyp==0) || (parms->keyp=='0') ) // processed here...
{
k = cvWaitKey(1);
}
else // caller sets these parameters
{
k = parms->keyp;
}
if ((k&0xff)==27)
{
return 0;
}
else if ( k=='r' ) // show raw cam image...
{
parms->drawmode = RAW;
}
else if ( k=='h' ) // show thresholding...
{
parms->drawmode = THRESHOLD;
}
else if ( k=='s') // show segmentation...
{
parms->drawmode = SEGMENTATION;
}
else if ( k=='l') // show lables...
{
parms->drawmode = LABELS;
}
else if ( k=='t' ) // show tracks...
{
parms->drawmode = TRACKS;
}
else if ( k=='b' ) // show bg mask...
{
parms->drawmode = BGMASK;
}
else if ( k=='e' ) // decrease erosion...
{
if (parms->erode>0) parms->erode--;
printf("INFO: erode=%d\n", parms->erode);
}
else if ( k=='E' ) // increase erosion...
{
parms->erode++;
printf("INFO: erode=%d\n", parms->erode);
}
else if ( k=='d' ) // decrease dilation...
{
if (parms->dilate>0) parms->dilate--;
printf("INFO: dilate=%d\n", parms->dilate);
}
else if ( k=='D' ) // increase dilation...
{
parms->dilate++;
printf("INFO: dilate=%d\n", parms->dilate);
}
else if ( k=='p' ) // print info...
{
printf("INFO: erode=%d\n", parms->erode);
printf("INFO: dilate=%d\n", parms->dilate);
//cout << "yo" << "\n";
}
else if ( k=='w' ) // indicate desire to set bg mask here...
{
parms->sig_setmask = 1;
}
else if ( k=='z' ) // reset bg mask...
{
parms->sig_resetmask = 1;
}
else if ( k=='x' )
{
parms->flipmode = !parms->flipmode;
}
else if ( k=='j' )
{
if ( parms->e_rows>2) parms->e_rows--;
}
else if ( k=='J' )
{
parms->e_rows++;
}
else if ( k=='k' )
{
if ( parms->e_cols>2) parms->e_cols--;
}
else if ( k=='K' )
{
parms->e_cols++;
}
else if ( k=='u' )
{
if ( parms->d_rows>2) parms->d_rows--;
}
else if ( k=='U' )
{
parms->d_rows++;
}
else if ( k=='i' )
{
if ( parms->d_cols>2) parms->d_cols--;
}
else if ( k=='I' )
{
parms->d_cols++;
}
else if ( k=='q' )
{
_fullscreen = !_fullscreen;
parms->drawmode = LOGO;
}
else if ( k=='a' )
{
parms->drawmode = LOGO;
}
else if ( k=='g' )
{
parms->drawmode = NODISPLAY;
}
else if ( k=='7' )
{
parms->sig_savemask = 1;
}
else if ( k=='8' )
{
parms->sig_loadmask = 1;
}
// Thresholding...
IplImage *thresholded = cvCreateImage(imgSize, 8, 1);
cvInRangeS( img, cvScalar(
parms->bmin, parms->gmin,parms->rmin,0),
cvScalar( parms->bmax,parms->gmax,parms->rmax),
thresholded );
if ( parms->drawmode==THRESHOLD )
{
_show( thresholded, "gw_object_tracking", "threshold", 255, 255, 0 );
#if 0
IplImage *display = cvCloneImage(thresholded);
cvPutText (display,"threshold",cvPoint(20,20), &font, cvScalar(255,255,0));
cvShowImage("gw_object_tracking", display);
cvReleaseImage( &display );
#endif
}
// BG mask programmed here (note, it gets programmed after it gets calculated right above...
if (parms->sig_setmask)
{
if (bg_mask) cvReleaseImage( &bg_mask);
if (inv_bg_mask) cvReleaseImage( &inv_bg_mask);
bg_mask = cvCloneImage( thresholded );
inv_bg_mask = cvCloneImage( thresholded );
cvNot( bg_mask, inv_bg_mask );
parms->sig_setmask = 0; // reset signal!!
}
if (parms->sig_resetmask)
{
cvZero( bg_mask );
cvNot( bg_mask, inv_bg_mask );
parms->sig_resetmask = 0; // reset signal!!
}
// load mask here...
if (parms->sig_loadmask)
{
#if 1
IplImage *loaded_mask = cvLoadImage( DEFAULT_MASK_PATH, 0);
if ( loaded_mask )
{
if (bg_mask) cvReleaseImage( &bg_mask );
if (inv_bg_mask) cvReleaseImage( &inv_bg_mask );
bg_mask = loaded_mask;
inv_bg_mask = cvCreateImage(imgSize, 8, 1);
cvNot( bg_mask, inv_bg_mask );
}
#endif
parms->sig_loadmask = 0;
}
// save mask here...
if (parms->sig_savemask)
{
int p[3];
p[0] = CV_IMWRITE_JPEG_QUALITY;
p[1] = 100;
p[2] = 0;
cvSaveImage( DEFAULT_MASK_PATH, bg_mask, p);
parms->sig_savemask = 0;
}
// Segmentation...
IplImage *segmentated = cvCreateImage(imgSize, 8, 1);
cvAnd( thresholded, inv_bg_mask, segmentated );
// Possibly deal with erode morph shape change...
if ( (parms->e_shape != _last_emorph_shape) ||
(parms->e_rows != _last_emorph_rows) ||
(parms->e_cols != _last_emorph_cols) )
{
if ( emorphKernel!=NULL) cvReleaseStructuringElement( &emorphKernel );
emorphKernel = cvCreateStructuringElementEx(
parms->e_rows, parms->e_cols,
1, 1,
CV_SHAPE_ELLIPSE,
NULL);
_last_emorph_rows = parms->e_rows;
_last_emorph_cols = parms->e_cols;
_last_emorph_shape = parms->e_shape;
}
// Possibly erode...
if ( parms->erode>0)
{
cvErode(segmentated, segmentated, emorphKernel,parms->erode);
}
// Possibly deal with dilate morph shape change...
if ( (parms->d_shape != _last_dmorph_shape) ||
(parms->d_rows != _last_dmorph_rows) ||
(parms->d_cols != _last_dmorph_cols) )
{
if ( dmorphKernel!=NULL) cvReleaseStructuringElement( &dmorphKernel );
dmorphKernel = cvCreateStructuringElementEx(
parms->d_rows, parms->d_cols,
1, 1,
CV_SHAPE_ELLIPSE,
NULL);
_last_dmorph_rows = parms->d_rows;
_last_dmorph_cols = parms->d_cols;
_last_dmorph_shape = parms->d_shape;
}
// Possibly dilate...
if ( parms->dilate > 0 )
{
cvDilate(segmentated, segmentated, dmorphKernel,parms->dilate);
}
if ( parms->drawmode==SEGMENTATION )
{
_show(segmentated, "gw_object_tracking", "segmentated", 255,255,0);
#if 0
IplImage *display = cvCloneImage(segmentated);
cvPutText (display,"segmentated",cvPoint(20,20), &font, cvScalar(255,255,0));
cvShowImage("gw_object_tracking", display);
cvReleaseImage( &display );
#endif
}
// Labeling and blobbing...
IplImage *labelImg = cvCreateImage(cvGetSize(frame), IPL_DEPTH_LABEL, 1);
CvBlobs blobs;
CvTracks tracks; //CB
unsigned int result = cvLabel(segmentated, labelImg, blobs); // secret sauce 3
cvFilterByArea(blobs, 100, 1000000); //CB secret sauce 4 (was 500 min)
if ( parms->drawmode==LABELS)
{
IplImage *display = cvCreateImage( cvGetSize(frame), IPL_DEPTH_8U, 3);
for (unsigned int y=0; y< (unsigned int)labelImg->height; y++)
for (unsigned int x=0; x<(unsigned int)labelImg->width; x++)
{
int lbl = cvGetLabel(labelImg, x, y);
unsigned char f = 255*lbl;
cvSet2D( display, y, x, CV_RGB( f,f,f) );
}
_show( display, "gw_object_tracking", "labels", 255,255,0 );
#if 0
cvPutText (display,"labels",cvPoint(20,20), &font, cvScalar(255,255,0));
cvShowImage("gw_object_tracking", display);
cvReleaseImage( &display );
#endif
cvReleaseImage( &display );
}
// Tracking...
// CB - GW put it back
cvUpdateTracks(blobs, tracks, 200., 5);
if ( parms->drawmode==TRACKS)
{
cvRenderTracks(tracks, frame, frame, CV_TRACK_RENDER_ID|CV_TRACK_RENDER_BOUNDING_BOX);
_show( frame, "gw_object_tracking", "tracks", 255,255,0);
#if 0
IplImage *display = cvCloneImage(frame);
cvPutText (display,"tracks",cvPoint(20,20), &font, cvScalar(255,255,0));
cvShowImage("gw_object_tracking", display);
cvReleaseImage( &display );
#endif
}
// Possibly return blob info to caller...
if (pblobs && num_blobs)
{
*num_blobs = 0;
for (CvTracks::const_iterator it=tracks.begin(); it!=tracks.end(); ++it)
{
(*num_blobs)++;
}
if ( *num_blobs > 0 )
{
int b = 0;
for (CvTracks::const_iterator it=tracks.begin(); it!=tracks.end(); ++it)
{
//cout << " - Centroid: (" << it->second->centroid.x << ", " << it->second->centroid.y << ")" << "\n";
//printf("centroid %f %f\n", it->second->centroid.x, it->second->centroid.y );
pblobs[ b ].x = (float)(it->second->centroid.x - imgSize.width/2.0f);
pblobs[ b ].y = (float)it->second->area;
pblobs[ b ].z = (float)(imgSize.height - it->second->centroid.y - imgSize.height/2.0f); // make it so that z goes up...
b++;
if (b==MAX_BLOBS)
{
printf("WARNING: max blobs reached (%d)\n", MAX_BLOBS);
}
}
}
}
// Release resources...
cvReleaseTracks(tracks);
cvReleaseBlobs(blobs);
cvReleaseImage(&labelImg);
cvReleaseImage(&segmentated);
cvReleaseImage(&thresholded);
cvReleaseImage(&frame);
#ifdef OCVCAM
#else
cvReleaseImage(&img);
#endif
frameNumber++;
}
return 1;
}
void cvUpdateTracks(CvBlobs &b, CvTracks &t, const double thDistance, const unsigned int thInactive)
{
CV_FUNCNAME("cvUpdateTracks");
__BEGIN__;
unsigned int nBlobs = b.size();
unsigned int nTracks = t.size();
// Proximity matrix:
// Last row/column is for ID/label.
// Last-1 "/" is for accumulation.
CvID *close = new unsigned int[(nBlobs+2)*(nTracks+2)]; // XXX Must be same type than CvLabel.
// Access to matrix
#define C(blob, track) close[((blob) + (track)*(nBlobs+2))]
// Access to accumulators
#define AB(label) C((label), (nTracks))
#define AT(id) C((nBlobs), (id))
// Access to identifications
#define IB(label) C((label), (nTracks)+1)
#define IT(id) C((nBlobs)+1, (id))
// Access to registers
#define B(label) b[IB(label)]
#define T(id) t[IT(id)]
try
{
// Inicialization:
unsigned int i=0;
for (CvBlobs::const_iterator it = b.begin(); it!=b.end(); ++it, i++)
{
AB(i) = 0;
IB(i) = it->second->label;
}
CvID maxTrackID = 0;
unsigned int j=0;
for (CvTracks::const_iterator jt = t.begin(); jt!=t.end(); ++jt, j++)
{
AT(j) = 0;
IT(j) = jt->second->id;
if (jt->second->id > maxTrackID)
maxTrackID = jt->second->id;
}
// Proximity matrix calculation:
for (i=0; i<nBlobs; i++)
for (j=0; j<nTracks; j++)
{
if (C(i, j) = (distantBlobTrack(B(i), T(j)) < thDistance))
{
AB(i)++;
AT(j)++;
}
}
// Run over tracks:
for (j=0; j<nTracks; j++)
{
//unsigned int c = C(nBlobs, j);
unsigned int c = AT(j);
if (c == 1)
{
// Match track-blob
// Search for the blob
for (i=0; (i<nBlobs)&&(!C(i, j)); i++) {}
// Update track
CvBlob *blob = B(i);
CvTrack *track = T(j);
track->label = blob->label;
track->centroid = blob->centroid;
track->minx = blob->minx;
track->miny = blob->miny;
track->maxx = blob->maxx;
track->maxy = blob->maxy;
track->inactive = 0;
}
else if (c > 1)
{
// Track divides
CvTrack *track = T(j);
track->inactive++;
track->label=0;
// Create new tracks
for (i=0; i<nBlobs; i++)
{
if (C(i, j))
{
maxTrackID++;
CvBlob *blob = B(i);
CvTrack *track = new CvTrack;
track->id = maxTrackID;
track->label = blob->label;
track->minx = blob->minx;
track->miny = blob->miny;
track->maxx = blob->maxx;
track->maxy = blob->maxy;
track->centroid = blob->centroid;
track->inactive = 0;
t.insert(CvIDTrack(maxTrackID, track));
}
}
}
else // if (c == 0)
{
// Inactive track
CvTrack *track = T(j);
track->inactive++;
track->label = 0;
}
}
// Run over blobs:
for (i=0; i<nBlobs; i++)
{
//unsigned int c = C(i, nTracks);
unsigned int c = AB(i);
if (c == 0)
{
// New track
maxTrackID++;
CvBlob *blob = B(i);
CvTrack *track = new CvTrack;
track->id = maxTrackID;
track->label = blob->label;
track->minx = blob->minx;
track->miny = blob->miny;
track->maxx = blob->maxx;
track->maxy = blob->maxy;
track->centroid = blob->centroid;
track->inactive = 0;
t.insert(CvIDTrack(maxTrackID, track));
}
else if (c > 1)
{
// Tracks joins
// New track
maxTrackID++;
CvBlob *blob = B(i);
CvTrack *track = new CvTrack;
track->id = maxTrackID;
track->label = blob->label;
track->minx = blob->minx;
track->miny = blob->miny;
track->maxx = blob->maxx;
track->maxy = blob->maxy;
track->centroid = blob->centroid;
track->inactive = 0;
t.insert(CvIDTrack(maxTrackID, track));
// Others tracks inactives
for (j=0; j<nTracks; j++)
{
T(j)->inactive++;
T(j)->label = 0;
}
}
}
for (CvTracks::iterator jt=t.begin(); jt!=t.end();)
if (jt->second->inactive>=thInactive)
{
delete jt->second;
t.erase(jt++);
}
else
++jt;
}
catch (...)
{
delete[] close;
throw; // TODO: OpenCV style.
}
delete[] close;
__END__;
}