bool processFrame(const cv::Mat& cameraFrame, std::auto_ptr<MarkerDetectionFacade>& detector, ARDrawingContext& drawingCtx)
{
// Clone image used for background (we will draw overlay on it)
cv::Mat img = cameraFrame.clone();
// Draw information:
// if (pipeline.m_patternDetector.enableHomographyRefinement)
// cv::putText(img, "Pose refinement: On ('h' to switch off)", cv::Point(10,15), CV_FONT_HERSHEY_PLAIN, 1, CV_RGB(0,200,0));
// else
// cv::putText(img, "Pose refinement: Off ('h' to switch on)", cv::Point(10,15), CV_FONT_HERSHEY_PLAIN, 1, CV_RGB(0,200,0));
// cv::putText(img, "RANSAC threshold: " + ToString(pipeline.m_patternDetector.homographyReprojectionThreshold) + "( Use'-'/'+' to adjust)", cv::Point(10, 30), CV_FONT_HERSHEY_PLAIN, 1, CV_RGB(0,200,0));
// Set a new camera frame:
drawingCtx.updateBackground(img);
// Find a pattern and update it's detection status:
BGRAVideoFrame cameraImage(cameraFrame);
drawingCtx.isPatternPresent = detector->processFrame(cameraImage);
std::vector<Transformation> vec_trans = detector->getTransformations();
if(!vec_trans.empty())
{
// Update a pattern pose:
drawingCtx.patternPose = vec_trans[0];
// Request redraw of the window:
drawingCtx.updateWindow();
}
// Read the keyboard input:
int keyCode = cv::waitKey(5);
bool shouldQuit = false;
//if (keyCode == '+' || keyCode == '=')
//{
// pipeline.m_patternDetector.homographyReprojectionThreshold += 0.2f;
// pipeline.m_patternDetector.homographyReprojectionThreshold =/* std::*/min(10.0f, pipeline.m_patternDetector.homographyReprojectionThreshold);
//}
//else if (keyCode == '-')
//{
// pipeline.m_patternDetector.homographyReprojectionThreshold -= 0.2f;
// pipeline.m_patternDetector.homographyReprojectionThreshold = /*std::*/max(0.0f, pipeline.m_patternDetector.homographyReprojectionThreshold);
//}
//else if (keyCode == 'h')
//{
// pipeline.m_patternDetector.enableHomographyRefinement = !pipeline.m_patternDetector.enableHomographyRefinement;
//}
//else if (keyCode == 27 || keyCode == 'q')
//{
// shouldQuit = true;
//}
return shouldQuit;
}
/**
* 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());
}
