CvBlobs MultiCursorAppCpp::labelingUserArea(Mat& src)
{
// Make image dilating for stable labeling
#ifdef USE_KINECT_V1
dilate(src, src, Mat(), Point(-1, -1), 3);
#else
//dilate(src, src, Mat(), Point(-1, -1), 1);
#endif
/* Use IplImage (Labeling for Mat is not fully implemented) */
// Convert to IplImage
IplImage srcIpl = (IplImage)src;
// Convert to gray scale
IplImage* srcIplBinary = cvCreateImage(cvGetSize(&srcIpl), IPL_DEPTH_8U, 1);
cvCvtColor(&srcIpl, srcIplBinary, CV_BGR2GRAY);
// Get binary image
cvThreshold(srcIplBinary, srcIplBinary, 100, 255, CV_THRESH_BINARY);
// Get blobs
IplImage* labelImg = cvCreateImage(cvGetSize(srcIplBinary), IPL_DEPTH_LABEL, 1);
CvBlobs blobs;
UINT result = cvLabel(srcIplBinary, labelImg, blobs);
Mat labelMatBuf(labelImg, true); // データをコピーする
labelMat = labelMatBuf;
CV_Assert(reinterpret_cast<uchar*>(labelImg->imageData) != labelMat.data);
// Filter noise / ノイズ点の消去
cvFilterByArea(blobs, 2000, 1000000);
// Render blobs
cvRenderBlobs(labelImg, blobs, &srcIpl, &srcIpl);
// Free unused IplImages
cvReleaseImage(&labelImg);
cvReleaseImage(&srcIplBinary);
return blobs;
}
int vehicle_det::do_iteration()
{
//cout<<__PRETTY_FUNCTION__<<endl;
cv::Mat img_input, src;
cap >> src;
if(!src.data)
{
printf("Exiting\n");
return -1;
}
Mat img_display = src.clone();
draw_ROI_poly(img_display);
src.copyTo(img_input, mask);
img_input = Mat(img_input, main_roi);
IplImage temp = img_input;
IplImage * frame = &temp;
//getting the polygon
// bgs->process(...) internally process and show the foreground mask image
cv::Mat img_mask;
//bgs->process(img_input, img_mask);
get_foreground(img_input, img_mask);
blur(img_mask, img_mask, Size(4, 4));
img_mask = img_mask > 10;
/*morphologyEx(img_mask, img_mask, MORPH_CLOSE, Mat(25, 2, CV_8U));
morphologyEx(img_mask, img_mask, MORPH_OPEN, Mat(10, 10, CV_8U));*/
morphologyEx(img_mask, img_mask, MORPH_CLOSE, Mat(2, 2, CV_8U));
//morphologyEx(img_mask, img_mask, MORPH_OPEN, Mat(10, 10, CV_8U));
//morphologyEx(img_mask, img_mask, MORPH_GRADIENT , Mat(5,5, CV_8U));
//bgs->operator()(img_input,img_mask,0.2);
//erode(img_mask, img_mask, Mat());
//dilate(img_mask, img_mask, Mat());
//imshow("fore", img_mask);
if(!img_mask.empty())
{
//vector<Rect> rois;// to be added all the ROIs
IplImage copy = img_mask;
IplImage * new_mask = ©
IplImage * labelImg = cvCreateImage(cvGetSize(new_mask), IPL_DEPTH_LABEL, 1);
CvBlobs blobs, filtered_blobs;
unsigned int result = cvb::cvLabel(new_mask, labelImg, blobs);
cvFilterByArea(blobs, 40, 2000);
int count = 0;
for(CvBlobs::const_iterator it = blobs.begin(); it != blobs.end(); ++it)
{
count++;
// cout << "Blob #" << it->second->label << ": Area=" << it->second->area << ", Centroid=(" << it->second->centroid.x << ", " << it->second->centroid.y << ")" << endl;
int x, y;
x = (int)it->second->centroid.x;
y = (int)it->second->centroid.y;
//cv::Point2f p(x,y );
// circle(img_input, p, (int)10, cv::Scalar(255, 0 , 0), 2, 8, 0);
int x_final = 0;
int y_final = 0;
if(x - (width_roi / 2) <= 0)
{
x_final = 1;
}
else if(x + (width_roi / 2) >= img_input.cols)
{
x_final = (x - (width_roi / 2)) - (x + (width_roi / 2) - (img_input.cols - 1));
}
else
{
x_final = x - (width_roi / 2);
}
if(y - (height_roi / 2) <= 0)
{
y_final = 1;
}
else if(y + (height_roi / 2) >= img_input.rows)
{
y_final = (y - (height_roi / 2)) - (y + (height_roi / 2) - (img_input.rows - 1));
}
else
{
y_final = y - (height_roi / 2);
}
//printf("resized x_final=%d y_final=%d cols=%d, rows=%d \n", x_final,y_final,img_input.cols,img_input.rows);
Rect roi(x_final, y_final, width_roi, height_roi);
//rois.push_back(roi);//adding ROIs using rectangles
// Mat image = imread("");
Mat image_roi = Mat(img_input, roi);
int vehicle_ct = detect(image_roi); //getting the vehicle count per ROI
if(vehicle_ct > 0)
{
filtered_blobs[it->first] = it->second;
int matched = 0;
int c1 = 255, c2 = 0;
if(matched)
{
c1 = 0;
c2 = 255;
}
else
{
//print something to debug
}//changing the colour of the rectanged depending on matched or not matched
rectangle(img_display,
Point(min_x + x - 5, min_y + y - 5),
Point(min_x + x + 5, min_y + y + 5),
CV_RGB(c1, c2, 0), 2, 8, 0);
/*rectangle(img_input,
Point(x - 5, y - 5),
Point(x + 5, y + 5),
CV_RGB(c1, c2, 0), 2, 8, 0);*/
}
}
//cvUpdateTracks(filtered_blobs, tracks, 5., 10);
cvUpdateTracks(filtered_blobs, tracks, 10., 5);
cvRenderBlobs(labelImg, filtered_blobs, frame, frame, CV_BLOB_RENDER_CENTROID | CV_BLOB_RENDER_BOUNDING_BOX);
//cvRenderTracks(tracks, frame, frame, CV_TRACK_RENDER_ID|CV_TRACK_RENDER_BOUNDING_BOX|CV_TRACK_RENDER_TO_LOG);
cvRenderTracks(tracks, frame, frame, CV_TRACK_RENDER_ID);
printf("num of active tracks %d\n", tracks.size());
process_equation(tracks.size());//number of people given as input
if(abstract_det::Total_Score<0){
destroyAllWindows();
}
}
if(!img_display.empty())
{
cv::imshow("vehicle_view", img_display);
}
waitKey(30);
return 1;
}
void Viewer::createImageHSV(const colorspaces::Image& imageDepth)
{
float r,g,b;
imgHSV.create(imgOrig.size(), CV_8UC1);
imgOrig.copyTo(imgHSV);
IplImage *threshy=cvCreateImage(imgOrig.size(),8,1);
for (int i=0;i< imgHSV.size().width*imgHSV.size().height; i++)
{
r = (float)(unsigned int)(unsigned char) imgOrig.data[i*3];
g = (float)(unsigned int)(unsigned char) imgOrig.data[i*3+1];
b = (float)(unsigned int)(unsigned char) imgOrig.data[i*3+2];
const HSV* hsvData = RGB2HSV_getHSV (r,g,b);
if( hmax >= hsvData->H*DEGTORAD && hmin <= hsvData->H*DEGTORAD
&& smax >= hsvData->S && smin <= hsvData->S
&& vmax >= hsvData->V && vmin <= hsvData->V )
{
threshy->imageData[i] = 1;
}
else
{
imgHSV.data[i*3] = imgHSV.data[i*3+1] = imgHSV.data[i*3+2] = 0;
threshy->imageData[i] = 0;
}
}
//Structure to hold blobs
CvBlobs blobs;
IplImage *iplOrig = new IplImage(imgOrig);
if (mFrameBlob)
cvReleaseImage(&mFrameBlob);
mFrameBlob=cvCreateImage(imgOrig.size(),8,3);
IplImage *labelImg=cvCreateImage(imgOrig.size(),IPL_DEPTH_LABEL,1);
cvResize(iplOrig,mFrameBlob,CV_INTER_LINEAR );
//Threshy is a binary image
cvSmooth(threshy,threshy,CV_MEDIAN,7,7);
//Finding the blobs
// unsigned int result=cvLabel(threshy,labelImg,blobs); // Unused
//Rendering the blobs
cvRenderBlobs(labelImg,blobs,mFrameBlob,mFrameBlob);
//Filter Blobs
cvFilterByArea(blobs,500,5000);
double area = 0.0;
int x=0;
int y=0;
for (CvBlobs::const_iterator it=blobs.begin(); it!=blobs.end(); ++it)
{
//std::cout << "BLOB found: " << it->second->area <<std::endl;
double moment10 = it->second->m10;
double moment01 = it->second->m01;
if (it->second->area >= area)
{
area = it->second->area;
x = moment10/area;
y = moment01/area;
}
}
std::cout << "Max BLOB: " << area << ": " << x << " , " << y <<std::endl;
//cvShowImage("Live",mFrameBlob);
if (area != 0)
{
Eigen::Vector3d pixel;
pixel(0) = x;
pixel(1) = y;
pixel(2) = 1.0;
Eigen::Vector4d target;
mCalibration->BackProjectWithDepth(pixel, imageDepth, target);
}
// Release and free memory
delete(iplOrig);
cvReleaseImage(&threshy);
cvReleaseImage(&labelImg);
}
/**
* 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());
}
