phych.cpp

// C
#include <stdio.h>
#include <sys/timeb.h>

// opencv
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/video/background_segm.hpp>

//C++
#include <iostream>
#include <sstream>

using namespace cv;
using namespace std;

#include "store_phych.h"

#define WINDOW_NAME "Phych rules the world"
#define PHYCH_NAME "Phych I"
#define PHYCH_MISSING "Last known location of Phych I"

/* Make transparent filled rectangle */
void rectangle2 (Mat img, Point top_left, Point bottom_right, Scalar color) {

	Mat rect_to_blend, background, result;

	/* Fail if wrong values */
	if (bottom_right.x <= top_left.x || top_left.y <= bottom_right.y)
		return;

	background = img.operator ()(Range (bottom_right.y, top_left.y),
				     Range (top_left.x, bottom_right.x));
	rect_to_blend.create (background.rows, background.cols, background.type ());
	rect_to_blend = Scalar (color[0], color[1], color[2]);
	addWeighted (background, 1.0, rect_to_blend, ((double) color[3]) / 255.0, 0, result);
	result.copyTo (background.rowRange(Range::all()).colRange(Range::all()));
}

/* CLOCK START */

double CLOCK()
{
    struct timespec t;
    clock_gettime(CLOCK_MONOTONIC,  &t);
    return (t.tv_sec * 1000)+(t.tv_nsec*1e-6);
}

double _avgdur=0;
double _fpsstart=0;
double _avgfps=0;
double _fps1sec=0;

double avgdur(double newdur)
{
    _avgdur=0.98*_avgdur+0.02*newdur;
    return _avgdur;
}

double avgfps()
{
    if(CLOCK()-_fpsstart>1000)      
    {
        _fpsstart=CLOCK();
        _avgfps=0.7*_avgfps+0.3*_fps1sec;
        _fps1sec=0;
    }
    _fps1sec++;
    return _avgfps;
}

/* CLOCK END */

void bruteforceLocalisationPhych (Point pixel, Mat frame, Point *phych_topleft, Point *phych_bottomright) {

	int x, y;
	int init_x = max (pixel.x - 60, 0);
	int init_y = max (pixel.y - 10, 0);
	int end_x  = min (pixel.x + 60, frame.cols);
	int end_y  = min (pixel.y + 60, frame.rows);

	for (x = init_x ; x < end_x ; x++) {
		for (y = init_y ; y < end_y ; y++) {

			if (frame.at<uchar>(Point (x, y)) == 255) {

				phych_topleft->x = min (phych_topleft->x, x);
				phych_topleft->y = max (phych_topleft->y, y);
				phych_bottomright->x = max (phych_bottomright->x, x);
				phych_bottomright->y = min (phych_bottomright->y, y);
			}
		}
	}
}
		
void updateLocalisationPhych (Point pixel, Mat frame, Point *phych_topleft, Point *phych_bottomright) {

	frame.at<uchar>(pixel) = 0;

	if (pixel.x < frame.cols && frame.at<uchar>(Point (pixel.x + 1, pixel.y)) == 255) {

		phych_bottomright->x = max (phych_bottomright->x, pixel.x + 1);
		updateLocalisationPhych (Point (pixel.x + 1, pixel.y), frame, 
					 phych_topleft, phych_bottomright);
		
	}

	if (pixel.x > 0 && frame.at<uchar>(Point (pixel.x - 1, pixel.y)) == 255) {

		phych_topleft->x = min (phych_topleft->x, pixel.x - 1);
		updateLocalisationPhych (Point (pixel.x - 1, pixel.y), frame, 
					 phych_topleft, phych_bottomright);
		
	}

	if (pixel.y < frame.rows && frame.at<uchar>(Point (pixel.x, pixel.y + 1)) == 255) {

		phych_topleft->y = max (phych_topleft->y, pixel.y + 1);
		updateLocalisationPhych (Point (pixel.x, pixel.y + 1), frame, 
					 phych_topleft, phych_bottomright);
		
	}

	if (pixel.y > 0 && frame.at<uchar>(Point (pixel.x, pixel.y - 1)) == 255) {

		phych_bottomright->y = min (phych_bottomright->y, pixel.y - 1);
		updateLocalisationPhych (Point (pixel.x, pixel.y - 1), frame, 
					 phych_topleft, phych_bottomright);
		
	}
	
}

int locatePhych (Mat frame, Mat fgMaskMOG, Point *phych_bottomright, Point *phych_topleft) {

	Point phychLoc;
	
	/* Locate first white pixel */
	minMaxLoc (fgMaskMOG, NULL, NULL, NULL, &phychLoc);

	/* If phych is not detected */
	if (phychLoc.x == 0) return 0;

	/* Locate rest of Phych */
	phych_topleft->x = phychLoc.x;
	phych_topleft->y = phychLoc.y;
	phych_bottomright->x = phychLoc.x;
	phych_bottomright->y = phychLoc.y;
	//updateLocalisationPhych (phychLoc, fgMaskMOG, phych_topleft, phych_bottomright);
	bruteforceLocalisationPhych (phychLoc, fgMaskMOG, phych_topleft, phych_bottomright);

	/* Brute force zone detection */
	//rectangle (frame, Point (max (phychLoc.x - 60, 0), max (phychLoc.y - 10, 0)), Point (phychLoc.x + 60, phychLoc.y + 60), Scalar (255, 0, 0));

	return 1;
}

int main (void) {

	int res, frameno = 0;
	Ptr<BackgroundSubtractor> pMOG; //MOG Background subtractor
	pMOG = new BackgroundSubtractorMOG();
	Mat frame; // current frame
	Mat fgMaskMOG; // fg mask generated by MOG method
	double start, dur;
	Point phych_bottomright, phych_topleft;
	Point org;
	Scalar color;
	char *text = NULL;
	char frame_info[BUFSIZ];

	namedWindow (WINDOW_NAME, CV_WINDOW_AUTOSIZE);

	while (1) {

		start = CLOCK ();

        	/* Get a new image from cam */
		res = store_phych ();		
		if (!res) {
			fprintf (stderr, "Cannot take a picture of our beloved Phych");
			exit (EXIT_FAILURE);
		}
		
		/* Load the new image */
    		frame = cv::imread ("phych.jpg", CV_LOAD_IMAGE_COLOR);

		if (frameno % 20 == 0)
			pMOG->operator ()(frame, fgMaskMOG, 1.0/10.0);
		else 
			pMOG->operator ()(frame, fgMaskMOG);
    
		/* Locate Phych */
		res = locatePhych (frame, fgMaskMOG, &phych_bottomright, &phych_topleft);
		org.x = min (frame.cols, phych_topleft.x);
		org.y = max (0, phych_bottomright.y - 2);

		if (res) {
			color = Scalar (0, 0, 255, 100);
			text = (char *) PHYCH_NAME;
		} else {
			color = Scalar (0, 255, 0, 100);
			text = (char *) PHYCH_MISSING;
		}

		putText (frame, text, org, FONT_HERSHEY_SIMPLEX, 0.25, color);
		rectangle2 (frame, phych_topleft, phych_bottomright, color);
		rectangle (frame, phych_topleft, phych_bottomright, color);

		/* Display frame rate */
		dur = CLOCK () - start;
        	snprintf (frame_info, BUFSIZ, "AVG time per frame %.2f ms. FPS %.1f. frameno = %d", avgdur(dur), avgfps(), frameno++);
		color = Scalar (255, 0, 0);
		org.x = 10;
		org.y = 10;
		putText (frame, frame_info, org, FONT_HERSHEY_SIMPLEX, 0.25, color);

		
    		imshow (WINDOW_NAME, frame);

    		waitKey(1);
	}

	exit (EXIT_SUCCESS);
}