void BlobTracking::process(const cv::Mat &img_input, const cv::Mat &img_mask, cv::Mat &img_output)
{
if(img_input.empty() || img_mask.empty())
return;
loadConfig();
if(firstTime)
saveConfig();
IplImage* frame = new IplImage(img_input);
cvConvertScale(frame, frame, 1, 0);
IplImage* segmentated = new IplImage(img_mask);
IplConvKernel* morphKernel = cvCreateStructuringElementEx(5, 5, 1, 1, CV_SHAPE_RECT, NULL);
cvMorphologyEx(segmentated, segmentated, NULL, morphKernel, CV_MOP_OPEN, 1);
if(showBlobMask)
cvShowImage("Blob Mask", segmentated);
IplImage* labelImg = cvCreateImage(cvGetSize(frame), IPL_DEPTH_LABEL, 1);
cvb::CvBlobs blobs;
unsigned int result = cvb::cvLabel(segmentated, labelImg, blobs);
//cvb::cvFilterByArea(blobs, 500, 1000000);
cvb::cvFilterByArea(blobs, minArea, maxArea);
//cvb::cvRenderBlobs(labelImg, blobs, frame, frame, CV_BLOB_RENDER_BOUNDING_BOX);
if(debugBlob)
cvb::cvRenderBlobs(labelImg, blobs, frame, frame, CV_BLOB_RENDER_BOUNDING_BOX|CV_BLOB_RENDER_CENTROID|CV_BLOB_RENDER_ANGLE|CV_BLOB_RENDER_TO_STD);
else
cvb::cvRenderBlobs(labelImg, blobs, frame, frame, CV_BLOB_RENDER_BOUNDING_BOX|CV_BLOB_RENDER_CENTROID|CV_BLOB_RENDER_ANGLE);
cvb::cvUpdateTracks(blobs, tracks, 200., 5);
if(debugTrack)
cvb::cvRenderTracks(tracks, frame, frame, CV_TRACK_RENDER_ID|CV_TRACK_RENDER_BOUNDING_BOX|CV_TRACK_RENDER_TO_STD);
else
cvb::cvRenderTracks(tracks, frame, frame, CV_TRACK_RENDER_ID|CV_TRACK_RENDER_BOUNDING_BOX);
//std::map<CvID, CvTrack *> CvTracks
if(showOutput)
cvShowImage("Blob Tracking", frame);
cv::Mat img_result(frame);
img_result.copyTo(img_output);
//cvReleaseImage(&frame);
//cvReleaseImage(&segmentated);
cvReleaseImage(&labelImg);
delete frame;
delete segmentated;
cvReleaseBlobs(blobs);
cvReleaseStructuringElement(&morphKernel);
firstTime = false;
}
void AirLayerDetector::update()
{
// let us grab the current raw image from the kinect
cvZero(_tmp);
cvCopy(_kinect->rawDepthImage(), _tmp);//, _bgmodel->mask());
// subtraction
const unsigned short* ptr_sd; // bgmodel->standardDeviation
const unsigned short* ptr_bg; // bgmodel->image
const unsigned short* ptr_tmp; // rawDepthImage AND bgmodel->mask
const unsigned char* ptr_mask; // bg mask
unsigned short* ptr_tmp2; // result...
for(int y=0; y<_tmp->height; y++) {
ptr_sd = (const unsigned short*)(_bgmodel->standardDeviation()->imageData + y * _bgmodel->standardDeviation()->widthStep);
ptr_bg = (const unsigned short*)(_bgmodel->image()->imageData + y * _bgmodel->image()->widthStep);
//ptr_bg = (const unsigned short*)(_bgmodel->mean()->imageData + y * _bgmodel->mean()->widthStep);
ptr_mask = (const unsigned char*)(_bgmodel->mask()->imageData + y * _bgmodel->mask()->widthStep);
ptr_tmp = (const unsigned short*)(_tmp->imageData + y * _tmp->widthStep);
ptr_tmp2 = (unsigned short*)(_tmp2->imageData + y * _tmp2->widthStep);
for(int x=0; x<_tmp->width; x++) {
// perform the background subtraction
if(ptr_mask[x] > 0 && ptr_tmp[x] > 25 && ptr_tmp[x] < 2047) { // FIXME: magic numbers?
if(ptr_bg[x] >= ptr_tmp[x]) {
// inverse depth (kinect)
ptr_tmp2[x] = ptr_bg[x] - ptr_tmp[x];
// avoid jitter
if(ptr_tmp2[x] < 3 * ptr_sd[x])
ptr_tmp2[x] = 0;
}
else
ptr_tmp2[x] = 0;
}
else
ptr_tmp2[x] = 0;
}
}
// binary threshold
cvInRangeS(_tmp2, cvScalar(_lo), cvScalar(_hi), _blobs.getCvImage());
// noise...
cvDilate(_blobs.getCvImage(), _blobs.getCvImage(), NULL, 2);
cvSmooth(_blobs.getCvImage(), _blobs.getCvImage(), CV_MEDIAN, 3);
_blobs.flagImageChanged();
// extracting & filtering the blobs
cvReleaseBlobs(_cvb);
cvb::cvLabel(_blobs.getCvImage(), _labelImg, _cvb);
cvFilterByArea(_cvb, _minArea, _maxArea);
}
unsigned int cvLabel (IplImage const *img, IplImage *imgOut, CvBlobs &blobs)
{
CV_FUNCNAME("cvLabel");
__CV_BEGIN__;
{
CV_ASSERT(img&&(img->depth==IPL_DEPTH_8U)&&(img->nChannels==1));
CV_ASSERT(imgOut&&(imgOut->depth==IPL_DEPTH_LABEL)&&(imgOut->nChannels==1));
unsigned int numPixels=0;
cvSetZero(imgOut);
CvLabel label=0;
cvReleaseBlobs(blobs);
unsigned int stepIn = img->widthStep / (img->depth / 8);
unsigned int stepOut = imgOut->widthStep / (imgOut->depth / 8);
unsigned int imgIn_width = img->width;
unsigned int imgIn_height = img->height;
unsigned int imgIn_offset = 0;
unsigned int imgOut_width = imgOut->width;
unsigned int imgOut_height = imgOut->height;
unsigned int imgOut_offset = 0;
if(img->roi)
{
imgIn_width = img->roi->width;
imgIn_height = img->roi->height;
imgIn_offset = img->roi->xOffset + (img->roi->yOffset * stepIn);
}
if(imgOut->roi)
{
imgOut_width = imgOut->roi->width;
imgOut_height = imgOut->roi->height;
imgOut_offset = imgOut->roi->xOffset + (imgOut->roi->yOffset * stepOut);
}
unsigned char *imgDataIn = (unsigned char *)img->imageData + imgIn_offset;
CvLabel *imgDataOut = (CvLabel *)imgOut->imageData + imgOut_offset;
#define imageIn(X, Y) imgDataIn[(X) + (Y)*stepIn]
#define imageOut(X, Y) imgDataOut[(X) + (Y)*stepOut]
CvLabel lastLabel = 0;
CvBlob *lastBlob = NULL;
for (unsigned int y=0; y<imgIn_height; y++)
{
for (unsigned int x=0; x<imgIn_width; x++)
{
if (imageIn(x, y))
{
bool labeled = imageOut(x, y);
if ((!imageOut(x, y))&&((y==0)||(!imageIn(x, y-1))))
{
labeled = true;
// Label contour.
label++;
CV_ASSERT(label!=CV_BLOB_MAX_LABEL);
imageOut(x, y) = label;
numPixels++;
if (y>0)
imageOut(x, y-1) = CV_BLOB_MAX_LABEL;
CvBlob *blob = new CvBlob;
blob->label = label;
blob->area = 1;
blob->minx = x; blob->maxx = x;
blob->miny = y; blob->maxy = y;
blob->m10=x; blob->m01=y;
blob->m11=x*y;
blob->m20=x*x; blob->m02=y*y;
blob->internalContours.clear();
blobs.insert(CvLabelBlob(label,blob));
lastLabel = label;
lastBlob = blob;
blob->contour.startingPoint = cvPoint(x, y);
unsigned char direction=1;
unsigned int xx = x;
unsigned int yy = y;
bool contourEnd = false;
do
{
for (unsigned int numAttempts=0; numAttempts<3; numAttempts++)
{
bool found = false;
for (unsigned char i=0; i<3; i++)
{
int nx = xx+movesE[direction][i][0];
int ny = yy+movesE[direction][i][1];
if ((nx<imgIn_width)&&(nx>=0)&&(ny<imgIn_height)&&(ny>=0))
{
if (imageIn(nx, ny))
{
found = true;
blob->contour.chainCode.push_back(movesE[direction][i][3]);
xx=nx;
yy=ny;
direction=movesE[direction][i][2];
break;
}
else
{
imageOut(nx, ny) = CV_BLOB_MAX_LABEL;
}
}
}
if (!found)
direction=(direction+1)%4;
else
{
if (imageOut(xx, yy) != label)
{
imageOut(xx, yy) = label;
numPixels++;
if (xx<blob->minx) blob->minx = xx;
else if (xx>blob->maxx) blob->maxx = xx;
if (yy<blob->miny) blob->miny = yy;
else if (yy>blob->maxy) blob->maxy = yy;
blob->area++;
blob->m10+=xx; blob->m01+=yy;
blob->m11+=xx*yy;
blob->m20+=xx*xx; blob->m02+=yy*yy;
}
break;
}
if (contourEnd = ((xx==x) && (yy==y) && (direction==1)))
break;
}
}
while (!contourEnd);
}
if ((y+1<imgIn_height)&&(!imageIn(x, y+1))&&(!imageOut(x, y+1)))
{
labeled = true;
// Label internal contour
CvLabel l;
CvBlob *blob = NULL;
if (!imageOut(x, y))
{
l = imageOut(x-1, y);
imageOut(x, y) = l;
numPixels++;
if (l==lastLabel)
blob = lastBlob;
else
{
blob = blobs.find(l)->second;
lastLabel = l;
lastBlob = blob;
}
blob->area++;
blob->m10+=x; blob->m01+=y;
blob->m11+=x*y;
blob->m20+=x*x; blob->m02+=y*y;
}
else
{
l = imageOut(x, y);
if (l==lastLabel)
blob = lastBlob;
else
{
blob = blobs.find(l)->second;
lastLabel = l;
lastBlob = blob;
}
}
imageOut(x, y+1) = CV_BLOB_MAX_LABEL;
CvContourChainCode *contour = new CvContourChainCode;
contour->startingPoint = cvPoint(x, y);
unsigned char direction = 3;
unsigned int xx = x;
unsigned int yy = y;
do
{
for (unsigned int numAttempts=0; numAttempts<3; numAttempts++)
{
bool found = false;
for (unsigned char i=0; i<3; i++)
{
int nx = xx+movesI[direction][i][0];
int ny = yy+movesI[direction][i][1];
if (imageIn(nx, ny))
{
found = true;
contour->chainCode.push_back(movesI[direction][i][3]);
xx=nx;
yy=ny;
direction=movesI[direction][i][2];
break;
}
else
{
imageOut(nx, ny) = CV_BLOB_MAX_LABEL;
}
}
if (!found)
direction=(direction+1)%4;
else
{
if (!imageOut(xx, yy))
{
imageOut(xx, yy) = l;
numPixels++;
blob->area++;
blob->m10+=xx; blob->m01+=yy;
blob->m11+=xx*yy;
blob->m20+=xx*xx; blob->m02+=yy*yy;
}
break;
}
}
}
while (!(xx==x && yy==y));
blob->internalContours.push_back(contour);
}
if (!labeled)
{
CvLabel l = imageOut(x-1, y);
imageOut(x, y) = l;
numPixels++;
CvBlob *blob = NULL;
if (l==lastLabel)
blob = lastBlob;
else
{
blob = blobs.find(l)->second;
lastLabel = l;
lastBlob = blob;
}
blob->area++;
blob->m10+=x; blob->m01+=y;
blob->m11+=x*y;
blob->m20+=x*x; blob->m02+=y*y;
}
}
}
}
for (CvBlobs::iterator it=blobs.begin(); it!=blobs.end(); ++it)
{
cvCentroid((*it).second);
(*it).second->u11 = (*it).second->m11 - ((*it).second->m10*(*it).second->m01)/(*it).second->m00;
(*it).second->u20 = (*it).second->m20 - ((*it).second->m10*(*it).second->m10)/(*it).second->m00;
(*it).second->u02 = (*it).second->m02 - ((*it).second->m01*(*it).second->m01)/(*it).second->m00;
double m00_2 = (*it).second->m00 * (*it).second->m00;
(*it).second->n11 = (*it).second->u11 / m00_2;
(*it).second->n20 = (*it).second->u20 / m00_2;
(*it).second->n02 = (*it).second->u02 / m00_2;
(*it).second->p1 = (*it).second->n20 + (*it).second->n02;
double nn = (*it).second->n20 - (*it).second->n02;
(*it).second->p2 = nn*nn + 4.*((*it).second->n11*(*it).second->n11);
}
return numPixels;
}
__CV_END__;
}
void BlobTrackingNode::process(const cv::Mat &img_input, const cv::Mat &img_mask, cv::Mat &img_output)
{
//This is output event
QList<DetectedEvent> blobEvent;
cv::Mat newInput;
img_input.copyTo(newInput);
if(img_input.empty() || img_mask.empty()){
return;
}
loadConfig();
if(firstTime){
saveConfig();
}
IplImage* frame = new IplImage(img_input);
cvConvertScale(frame, frame, 1, 0);
IplImage* segmentated = new IplImage(img_mask);
IplConvKernel* morphKernel = cvCreateStructuringElementEx(5, 5, 1, 1, CV_SHAPE_RECT, NULL);
cvMorphologyEx(segmentated, segmentated, NULL, morphKernel, CV_MOP_OPEN, 1);
cv::Mat temp = cv::Mat(segmentated);
if(showBlobMask){
//cv::imshow("test",temp);
ownerPlugin->updateFrameViewer("Tracking Mask",convertToQImage(temp));
}
IplImage* labelImg = cvCreateImage(cvGetSize(frame), IPL_DEPTH_LABEL, 1);
cvb::CvBlobs blobs;
cvb::cvLabel(segmentated, labelImg, blobs);
cvb::cvFilterByArea(blobs, minArea, maxArea);
if(debugBlob){
cvb::cvRenderBlobs(labelImg, blobs, frame, frame, CV_BLOB_RENDER_BOUNDING_BOX|CV_BLOB_RENDER_CENTROID|CV_BLOB_RENDER_ANGLE|CV_BLOB_RENDER_TO_STD);
}
else{
cvb::cvRenderBlobs(labelImg, blobs, frame, frame, CV_BLOB_RENDER_BOUNDING_BOX|CV_BLOB_RENDER_CENTROID|CV_BLOB_RENDER_ANGLE);
}
cvb::cvUpdateTracks(blobs, tracks,threshold_distance, threshold_inactive);
//At this point, we have assigned each blob in current frame in to a track. so we can iterate through active tracks, and
// find out out blobs within one of those tracks. Following loop does that. This is helpfull to have more generalized idea about
// relationship between blobs with increasing time.
for (cvb::CvTracks::const_iterator track = tracks.begin(); track!=tracks.end(); ++track)
{
if((*track).second->active != 0){
for(std::map<cvb::CvLabel,cvb::CvBlob *>::iterator it = blobs.begin() ; it != blobs.end(); it++){
cvb::CvLabel label = (*it).first;
cvb::CvBlob * blob = (*it).second;
if((*track).second->label == label){
//qDebug()<< blob->minx <<","<<blob->miny;
//This is smoothed time tracked blob lables
blobEvent.append(DetectedEvent("blob",QString("%1,%2,%3,%4,%5,%6,%7,%8").arg(frameIndex).arg((*track).first).arg(blob->centroid.x).arg(blob->centroid.y).arg(blob->minx).arg(blob->miny).arg(blob->maxx).arg(blob->maxy),1.0));
}
}
}
}
if(debugTrack){
cvb::cvRenderTracks(tracks, frame, frame, CV_TRACK_RENDER_ID|CV_TRACK_RENDER_BOUNDING_BOX|CV_TRACK_RENDER_TO_STD);
}
else{
cvb::cvRenderTracks(tracks, frame, frame, CV_TRACK_RENDER_ID|CV_TRACK_RENDER_BOUNDING_BOX);
}
if(showFrameID){
cv::Mat temp = cv::Mat(frame);
cv::putText(temp,QString("%1").arg(frameIndex).toStdString(),cv::Point(40,120),cv::FONT_HERSHEY_PLAIN,2,cv::Scalar(0,255,0),2);
}
if(showOutput){
cv::Mat temp = cv::Mat(frame);
ownerPlugin->updateFrameViewer("Tracking Output",convertToQImage(temp));
//cvShowImage("Blob Tracking", frame);
}
cv::Mat img_result(frame);
cv::putText(img_result,QString("%1").arg(QString::number(frameIndex)).toUtf8().constData(), cv::Point(10,30), CV_FONT_HERSHEY_PLAIN,1.0, CV_RGB(255,255,255));
img_result.copyTo(img_output);
cvReleaseImage(&labelImg);
delete frame;
delete segmentated;
cvReleaseBlobs(blobs);
cvReleaseStructuringElement(&morphKernel);
firstTime = false;
frameIndex++;
QImage input((uchar*)newInput.data, newInput.cols, newInput.rows, newInput.step1(), QImage::Format_RGB888);
QImage output((uchar*)img_output.data, img_output.cols, img_output.rows, img_output.step1(), QImage::Format_RGB888);
QList<QImage> images;
images.append(input);
images.append(output);
emit generateEvent(blobEvent,images);
//emit generateEvent(blobEvent);
}
/**
* Runs the tracking.
*/
void Tracker::executeTracker()
{
#define PI (3.1415926535897932384626433832795028841)
// Kinect fow: 43° vertical, 57° horizontal
double verticalScalingFactor = tan(43 * PI / 180) / 240;
double horizontalScalingFactor = tan(57 * PI / 180) / 320;
ROS_DEBUG("Scaling factors: %lf/%lf", horizontalScalingFactor, verticalScalingFactor);
bool quadcopterTracked = false;
// Images
cv::Mat cameraImage(cv::Size(640, 480), CV_8UC3); // Only for showCameraImage == true.
cv::Mat maskedImage(cv::Size(640, 480), CV_8UC3); // Only for showMaskedImage == true.
cv::Mat image(cv::Size(640, 480), CV_8UC3); // The raw image from the camera.
cv::Mat mapImage(cv::Size(640, 480), CV_8UC1); // The color mapped image.
cv::Mat hsvImage(cv::Size(640, 480), CV_8UC3); // The raw image in hsv format.
// CvBlob
cvb::CvBlobs blobs;
IplImage *labelImg = cvCreateImage(image.size(), IPL_DEPTH_LABEL, 1);
cv::Mat morphKernel = cv::getStructuringElement(CV_SHAPE_RECT, cv::Size(5, 5));
cvb::CvTracks tracks;
IplImage iplMapImage;
while (!stopFlag) {
if (imageDirty == 0) {
usleep(100);
continue;
} else if (imageDirty > 1) {
ROS_WARN("Skipped %d frames!", imageDirty - 1);
}
START_CLOCK(trackerClock)
imageMutex.lock();
((cv::Mat*) this->image)->copyTo(image);
long int time = this->imageTime;
imageDirty = 0;
imageMutex.unlock();
if (showCameraImage)
image.copyTo(cameraImage);
createColorMapImage(image, mapImage, hsvImage);
if (showMaskedImage) {
// Convert to 3 channel image.
int target = 0;
for (int i = 0; i < mapImage.total(); ++i) {
maskedImage.data[target++] = mapImage.data[i];
maskedImage.data[target++] = mapImage.data[i];
maskedImage.data[target++] = mapImage.data[i];
}
}
cv::morphologyEx(mapImage, mapImage, cv::MORPH_OPEN, morphKernel);
// Finding blobs
// Only copies headers.
iplMapImage = mapImage;
unsigned int result = cvLabel(&iplMapImage, labelImg, blobs);
// ROS_DEBUG("Blob result: %d", result);
// Filter blobs
cvFilterByArea(blobs, 10, 1000000);
if (showCameraImage || showMaskedImage)
cvUpdateTracks(blobs, tracks, 200., 5);
if (showMaskedImage) {
// Only copies headers.
IplImage iplImage = maskedImage;
cvRenderBlobs(labelImg, blobs, &iplImage, &iplImage, CV_BLOB_RENDER_BOUNDING_BOX);
cvRenderTracks(tracks, &iplImage, &iplImage, CV_TRACK_RENDER_ID | CV_TRACK_RENDER_BOUNDING_BOX);
ROS_DEBUG("Exiting visual masked block"); // TODO Tracking down
// issue #7
}
if (showCameraImage) {
// Only copies headers.
IplImage iplImage = cameraImage;
cvRenderBlobs(labelImg, blobs, &iplImage, &iplImage, CV_BLOB_RENDER_BOUNDING_BOX);
cvRenderTracks(tracks, &iplImage, &iplImage, CV_TRACK_RENDER_ID | CV_TRACK_RENDER_BOUNDING_BOX);
ROS_DEBUG("Exiting visual masked block"); // TODO Tracking down
// issue #7
}
if (showCameraImage || showMaskedImage)
cvReleaseTracks(tracks);
if (blobs.size() != 0) {
// Find biggest blob
cvb::CvLabel largestBlob = cvLargestBlob(blobs);
CvPoint2D64f center = blobs.find(largestBlob)->second->centroid;
double x = center.x;
double y = center.y;
// Set (0, 0) to center.
x -= 320;
y -= 240;
// ROS_DEBUG("Center: %lf/%lf", x, y);
// Apply scaling
x *= horizontalScalingFactor;
y *= verticalScalingFactor;
dataReceiver->receiveTrackingData(cv::Scalar(x, y, 1.0), ((QuadcopterColor*) qc)->getId(), time);
if (showMaskedImage)
drawCross(maskedImage, center.x, center.y);
if (showCameraImage)
drawCross(cameraImage, center.x, center.y);
if (!quadcopterTracked) {
quadcopterTracked = true;
ROS_DEBUG("Quadcopter %d tracked", ((QuadcopterColor*) this->qc)->getId());
}
} else if (quadcopterTracked) {
quadcopterTracked = false;
ROS_DEBUG("Quadcopter %d NOT tracked", ((QuadcopterColor*) this->qc)->getId());
}
// Free cvb stuff.
cvReleaseBlobs(blobs);
// ROS_DEBUG("cvb stuff freed"); // TODO Tracking down issue #7
if (showMaskedImage) {
cv::imshow(maskedWindowName, maskedImage);
ROS_DEBUG("showed masked image"); // TODO Tracking down issue #7
}
if (showCameraImage) {
cv::imshow(cameraWindowName, cameraImage);
ROS_DEBUG("showed camera image"); // TODO Tracking down issue #7
}
STOP_CLOCK(trackerClock, "Calculation of quadcopter position took: ")
}
cvReleaseImage(&labelImg);
if (showMaskedImage)
cv::destroyWindow(maskedWindowName);
if (showCameraImage)
cv::destroyWindow(cameraWindowName);
ROS_INFO("Tracker with id %d terminated", ((QuadcopterColor*) this->qc)->getId());
}
