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; }
/** * 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()); }