tidied up last issues

[The-Artvertiser.git] / artvertiser / artvertiser.cpp

/*
 * Credits: Julian Oliver, 2008-2009 <julian@julianoliver.com>.
 *
 * This program is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the Free
 * Software Foundation, either version 3 of the License, or (at your option)
 * any later version.
 *
 * This program is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
 * more details.
 *
 * You should have received a copy of the GNU General Public License along with
 * this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 * This code builds upon BazAR, in particular 'multigl.cpp'. It has been
 * modified to support texture and video-texture mapping to an OpenGL plane over
 * the ROI. The ROI in the model image is now read in from a file generated by
 * the training process. Pose estimation stabilisation, augmentation fades,
 * fonts, mouse input hooks, augmentation over archival video and other bits have
 * pieces have been added also.
 *
 * I've fixed a bug in BazAR's planar_object_recognizer::build_with_cache where
 * corner values for the ROI were only being set immediately after training, not
 * on plain init.
 *
 * Usage:
 *
 * There are four ways to use Artvertiser.
 *
 * With video substitution of the ROI:
 *
 *       ./artvertiser -m <model file> -a <avi file>
 *
 * With video substitution of the ROI and capture from an AVI file
 *
 *  ./artvertiser -m <model file> -a <avi file> -b <avi file>
 *
 * With image substitution of the ROI and capture from a v4l device
 *
 *      ./artvertiser -m <model file>
 *
 * See defines below for setting capture window size and V4L device index
 *
 */

#include "multigrab.h"

// read from arduino
#include <stdio.h>    /* Standard input/output definitions */
#include <stdlib.h>
#include <stdint.h>   /* Standard types */
#include <string.h>   /* String function definitions */
#include <unistd.h>   /* UNIX standard function definitions */
#include <errno.h>    /* Error number definitions */
#include <fcntl.h>    /* File control definitions */
#include <errno.h>    /* Error number definitions */
#include <fcntl.h>    /* File control definitions */
#include <termios.h>  /* POSIX terminal control definitions */

#include <iostream>
#include <sstream> // for conv int->str
#include <vector>
#include <opencv/cv.h>
#include <highgui.h>
#include <map>

#include <stdio.h>
#include <time.h>

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include <calib/camera.h>

#ifdef __APPLE__
#define HAVE_APPLE_OPENGL_FRAMEWORK
#endif
#ifdef HAVE_APPLE_OPENGL_FRAMEWORK
#include <GLUT/glut.h>
#else
#include <GL/glut.h>
#endif

#include "/usr/include/freetype2/freetype/config/ftconfig.h"
#include <FTGL/ftgl.h>

#include "FProfiler/FProfiler.h"

// framerate counter
#include "framerate.h"

#include <list>
using namespace std;

// matrix tracker
#include "MatrixTracker/MatrixTracker.h"

#define IsRGB(s) ((s[0] == 'R') && (s[1] == 'G') && (s[2] == 'B'))
#define IsBGR(s) ((s[0] == 'B') && (s[1] == 'G') && (s[2] == 'R'))

#ifndef GL_CLAMP_TO_BORDER
#define GL_CLAMP_TO_BORDER 0x812D
#endif
#define GL_MIRROR_CLAMP_EXT 0x8742

#define DEFAULT_WIDTH 640
#define DEFAULT_HEIGHT 480
#define DEFAULT_V4LDEVICE 0

#define NUMARTVERTS 5

// buttons via arduino
// button_state is bitmapped so as to handle multiple button presses at once
char button_state = 0;
bool button_state_changed = false;
const char BUTTON_RED   = 0x01;
const char BUTTON_GREEN = 0x02;
const char BUTTON_BLUE  = 0x04;
// serial comms
int serialport_init(const char* serialport, int baud);
int serialport_read_until(int fd, char* buf, char until);
bool serial_thread_should_exit = false;
bool serial_thread_is_running = false;
pthread_t serial_thread;
void startSerialThread();
void shutdownSerialThread();
void* serialThreadFunc( void* );
// running on binoculars?
bool running_on_binoculars = false;
bool no_fullscreen = false;


// we continue tracking for 1 second, then fade for 3
static const float SECONDS_LOST_TRACK = 0.5f;
static const float SECONDS_LOST_FADE = 1.0f;
static const float MAX_FADE_SHOW = 0.9f;
static const float MAX_FADE_NORMAL = 1.0f;

static const int DEFAULT_CAPTURE_FPS = 20;

//#define WIDTH 320
//#define HEIGHT 240

MultiGrab *multi=0;
CamCalibration *calib=0;
CvPoint projPts[4];
IplTexture *raw_frame_texture=0;
FTime raw_frame_timestamp;
IplTexture *tex=0;
IplImage *image = 0;
CvCapture *capture = 0;
CvCapture *avi_capture = 0;
IplImage *avi_image = 0;
IplImage *avi_frame = 0;
//IplImage *model_image = 0;
IplImage *this_frame = 0;
IplImage *last_frame  = 0;
IplImage *diff= 0;
IplImage *bit_frame= 0;

int v4l_device = DEFAULT_V4LDEVICE;
int video_width = DEFAULT_WIDTH;
int video_height = DEFAULT_HEIGHT;
int detect_width = DEFAULT_WIDTH;
int detect_height = DEFAULT_HEIGHT;
int desired_capture_fps = DEFAULT_CAPTURE_FPS;

// load some images. hard-coded for know until i get the path parsing together.
IplImage *image1 = cvLoadImage("artvert1.png");
IplImage *image2 = cvLoadImage("artvert2.png");
IplImage *image3 = cvLoadImage("artvert3.png");
IplImage *image4 = cvLoadImage("artvert4.png");
IplImage *image5 = cvLoadImage("artvert5.png");
IplImage *fallback_artvert_image = cvLoadImage("artvert1.png");

// matrix tracker
MatrixTracker matrix_tracker;

// define a container struct for each artvert
struct artvert_struct
{
    const char *artvert;
    IplImage *image;
    const char *date;
    const char *author;
    const char *advert;
    const char *street;
};

typedef vector<artvert_struct> artverts_list;
artverts_list artverts(5);

// create a vector for the images and initialise it.
typedef vector<IplImage> imgVec;

bool frame_ok=false;
bool cache_light=false;
bool dynamic_light=false;
bool sphere_object=false;
bool avi_play=false;
bool avi_play_init=false;
bool lbutton_down = false;
bool label = false;

bool track_kalman = false;
bool delay_video = true;
// how many frames to delay the video
static const int VIDEO_DELAY_FRAMES=7;

double a_proj[3][4];
double old_a_proj[3][4];
float fade = 0.0;
FTime last_frame_caught_time;
FTime frame_timer;
float draw_fps;
int difference = 0;
int have_proj = 0;
int nb_light_measures=0;
int geom_calib_nb_homography;
int current_cam = 0;
int avi_init = 0;
int augment = 1;
int cnt=0;

vector<int> roi_vec;
CvPoint2D32f *c1 = new CvPoint2D32f[4];
vector<int> artvert_roi_vec;

char *image_path;

class Artvert
{
public:
        Artvert() { 
                artvert_image=0; 
                model_file="model.bmp"; 
                artvert_image_file="artvert1.png"; 
                artvert_is_movie= false;
                artist = "unknown artist";
                advert = "unknown advert";
                name = "unnamed artvert";
                avi_capture = NULL;
                avi_image = NULL;
                avi_play_init = false;
        }
        ~Artvert()
        {
                if ( artvert_image )
                        cvReleaseImage( &artvert_image );
                if ( avi_capture )
                        cvReleaseCapture( &avi_capture );
                if ( avi_image )
                        cvReleaseImage( &avi_image );
        }

        IplImage* getArtvertImage()
        {
                if ( artvert_is_movie )
                {

                        if ( !avi_capture )
                        {
                                avi_capture = cvCaptureFromAVI( artvert_movie_file.c_str() );
                                avi_play_init = false;
                        }       
        
                        IplImage *avi_frame = 0;
                        avi_frame = cvQueryFrame( avi_capture );
                        if ( avi_frame == 0 )
                                return fallback_artvert_image;
                        if ( avi_image == 0 )
                                avi_image = cvCreateImage( cvGetSize(avi_frame), avi_frame->depth, avi_frame->nChannels );
                        cvCopy( avi_frame, avi_image );
                        avi_image->origin = avi_frame->origin;
                        GLenum format = IsBGR(avi_image->channelSeq) ? GL_BGR_EXT : GL_RGBA;

                    if (!avi_play_init)
                    {
                        glGenTextures(1, &imageID);
                        glBindTexture(GL_TEXTURE_2D, imageID);
                        glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
                        avi_play_init=true;
                    }
                        return avi_image;
                }
                else
                {
                        if ( !artvert_image )
                        {
                                printf("loading artvert image '%s'\n", artvert_image_file.c_str() );
                                artvert_image = cvLoadImage( artvert_image_file.c_str() );
                        }
                        if ( !artvert_image )
                        {
                                fprintf(stderr, "couldn't load artvert image '%s'\n", artvert_image_file.c_str() );
                                artvert_image = fallback_artvert_image;
                        }
                        return artvert_image;
                }
        }

        string model_file;
        string artvert_image_file;
        string artist;
        string advert;
        string name;
        bool artvert_is_movie;
        string artvert_movie_file;
private:
        CvCapture* avi_capture;
        IplImage* avi_image;
        bool avi_play_init;
        GLuint imageID;

        IplImage* artvert_image;
};

vector< Artvert > artvert_list;
bool new_artvert_switching_in_progress = false;
int current_artvert_index=-1;
bool new_artvert_requested = false;
int new_artvert_requested_index = 0;
vector< bool > model_file_needs_training;
#include "ofxXmlSettings/ofxXmlSettings.h"

// detection thread
pthread_t detection_thread;
double detection_fps = 0.0;
static void shutdownDetectionThread();
static void startDetectionThread( int thread_priority = 0 /* only takes effect if root */ );
static void* detectionThreadFunc( void* _data );
bool detection_thread_should_exit = false;
bool detection_thread_running = false;


static void start();
static void geomCalibStart(bool cache);

// initialise a couple of fonts.
CvFont font, fontbold;

// Gl format for texturing
GLenum format;
GLuint imageID;

// ftgl font setup
static FTFont *ftglFont;

// interface
bool show_status = false;
bool show_profile_results = false;
string status_string = "";

// menu
// hide after 5s
#define MENU_HIDE_TIME 5.0f
bool menu_show = false;
FTime menu_timer;
bool menu_is_showing = false;
void updateMenu();
void drawMenu();

/* use this to read paths from the file system

   string getExtension(const string &file)
   {
   string::size_type dot = file.rfind('.');
   string lcpath = file;
   string suffix;
   transform(lcpath.begin(), lcpath.end(), lcpath.begin(), tolower);
   if(dot != string::npos)
   {
   suffix = lcpath.substr(dot + 1);
   }
   return suffix;
   }
 */



string getSettingsString()
{
    planar_object_recognizer &detector(multi->cams[current_cam]->detector);
    static char detector_settings_string[2048];
    sprintf( detector_settings_string, "1.ransac dist %4.2f  2.iter %i   detected points %i match count %i,\n"
            "3.refine %6.4f  4.score %6.4f  5.best_support thresh %2i  6.tau %2i\n"
            "smoothing: 7.position %5.3f  8.position_z %5.3f  \n  frames back: 9.raw %2i  0.returned %2i",
            detector.ransac_dist_threshold_ui,
            detector.max_ransac_iterations_ui,
            detector.detected_point_number,
            detector.match_number,
            detector.non_linear_refine_threshold_ui,
            detector.match_score_threshold_ui,
            detector.best_support_thresh_ui,
            detector.point_detector_tau_ui,
            matrix_tracker.getPositionSmoothing(),
            matrix_tracker.getPositionZSmoothing(),
            matrix_tracker.getFramesBackRaw(),
            matrix_tracker.getFramesBackReturned() );

    return detector_settings_string;
}


std::string date(int now)
{
    time_t rawtime;
    struct tm *timeinfo;
    char tBuffer[80];
    time ( &rawtime );
    timeinfo = localtime ( &rawtime );
    strftime (tBuffer,80,"%I:%M:%S:%p, %d %b %Y",timeinfo);
    string timeStr;
    stringstream _timeStr;
    _timeStr << tBuffer;
    timeStr = _timeStr.str();
    return timeStr;
}


// mouse input using GLUT.
void entry(int state)
{
    if (state == GLUT_ENTERED)
        cout << "Mouse Entered" << endl;
    else
        cout << "Mouse Left" << endl;
}

void mouse(int button, int state, int x, int y)
{
    if (button == GLUT_RIGHT_BUTTON)
    {
        if (state == GLUT_DOWN)
        {
            label = true;
        }

        else
        {
            label = false;
        }
    }
    else if (button == GLUT_LEFT_BUTTON)
    {
        if (state == GLUT_DOWN)
        {
            lbutton_down = true;
            if (cnt >= NUMARTVERTS-1)
            {
                cnt = 0;
            }
            else
            {
                cnt ++;
            }
        }
        else
            lbutton_down = false;
    }
    cout << "we are on image " << cnt << endl;
}

// text drawing function
static void drawText(IplImage *img, const char *text, CvPoint point, CvFont *font, CvScalar colour, double size)
{
    cvInitFont( font, CV_FONT_HERSHEY_DUPLEX, size, size, 0, 1, CV_AA);
    //cvInitFont( font, CV_FONT_HERSHEY_PLAIN, size, size, 0, 1, CV_AA);
    cvPutText(img, text, point, font, colour);
}

// read in ROI coords from txt file into vector.
static vector<int>  readROI(const char *filename)
{
    cout << filename << endl;
    string l;
    ifstream roi(filename);
    vector<int> v;
    int coord;
    char s[10];
    char *s1;
    int lines = 0;

    if (roi.is_open())
    {
        while (!roi.eof())
        {
            getline(roi, l);
            strcpy(s, l.c_str());
            s1 = strtok(s, " ");

            while (s1 != NULL)
            {
                //roi_vec.push_back(atoi(s1));
                v.push_back(atoi(s1));
                s1 = strtok(NULL, " ,");
            }
        }
        roi.close();
    }
    else
    {
        cout << "roi file not found" << endl;
    }
    return v;
}

//! GLUT callback on window size change
static void reshape(int width, int height)
{
    //GLfloat h = (GLfloat) height / (GLfloat) width;
    int winWidth  = video_width;
    int winHeight = video_height;
    glViewport(0,0,winWidth, winHeight);
    glutPostRedisplay();
}

//! Print a command line help and exit.
static void usage(const char *s)
{
    cerr << "usage:\n" << s
         << " [-m <model image>] [-m <model image>] ... \n "
         "  [-ml <model images file .xml>] [-r] [-t] [-g] [-a <path>] [-l] [-vd <num>] [-vs <width> <height>]\n"
         "  [-ds <width> <height>] [-fps <fps>] [-binoc [-nofullscreen]]\n\n"
         //"   -a <path>  specify path to AVI (instead of v4l device)\n"
         "   -b <path>  specify path to AVI (instead of v4l device), ignores -vs\n"
         "   -m  specify model image (may be used multiple times)\n"
         "   -ml <path>  load model images from <path> (xml) (respects additional -m paths)\n"
         "   -r  do not load any data\n"
         "   -t  train a new classifier\n"
         "   -g  recompute geometric calibration\n"
         "   -a <path>  load an AVI movie as an artvert\n"
         "   -i <path>  load an image as an artvert\n"
         "   -l  rebuild irradiance map from scratch\n"
         "   -vd <num>  V4L video device number (0-n)\n"
         "   -vs <width> <height>  video width and height (default 640x480)\n"
         "   -ds <width> <height>  frame size at which to run the detector (default to video width/height)\n"
         "   -fps <fps>  desired fps at which to run the image capture\n"
                 "   -binoc  run as if operating on binoculars (necessary for osx training)\n"
                 "      -nofullscreen  don't try to run fullscreen in -binoc mode\n\n";
    exit(1);
}



void exit_handler()
{
    printf("in exit_handler\n");
     // shutdown detection thread
    if ( detection_thread_running )
    {
        printf("stopping detection\n");
        shutdownDetectionThread();
    }
        // shutdown serial
    if ( serial_thread_is_running )
    {
        printf("stopping serial\n");
        shutdownSerialThread();
    }
        // shutdown binocular training
        if ( multi && multi->model.isInteractiveTrainBinocularsRunning() )
        {
                printf("stopping interactive train binoculars\n");
                multi->model.abortInteractiveTrainBinoculars();
        }

    // shutdown capture
    if ( multi )
    {
        printf("stopping multithread capture\n");


        multi->cams[0]->shutdownMultiThreadCapture();
    }
    // delete cameras
    if ( multi )
    {
        printf("deleteing multi\n");
        delete multi;
    }
}

int serialport_init(const char* serialport, int baud)
{
    struct termios toptions;
    int fd;

    //fprintf(stderr,"init_serialport: opening port %s @ %d bps\n",
    //        serialport,baud);

    fd = open(serialport, O_RDWR | O_NOCTTY | O_NDELAY);
    if (fd == -1)  {
        perror("init_serialport: Unable to open port ");
        return -1;
    }

    if (tcgetattr(fd, &toptions) < 0) {
        perror("init_serialport: Couldn't get term attributes");
        return -1;
    }
    speed_t brate = baud; // let you override switch below if needed
    switch(baud) {
    case 4800:   brate=B4800;   break;
    case 9600:   brate=B9600;   break;
#ifdef B14400
    case 14400:  brate=B14400;  break;
#endif
    case 19200:  brate=B19200;  break;
#ifdef B28800
    case 28800:  brate=B28800;  break;
#endif
    case 38400:  brate=B38400;  break;
    case 57600:  brate=B57600;  break;
    case 115200: brate=B115200; break;
    }
    cfsetispeed(&toptions, brate);
    cfsetospeed(&toptions, brate);

    // 8N1
    toptions.c_cflag &= ~PARENB;
    toptions.c_cflag &= ~CSTOPB;
    toptions.c_cflag &= ~CSIZE;
    toptions.c_cflag |= CS8;
    // no flow control
    toptions.c_cflag &= ~CRTSCTS;

    toptions.c_cflag |= CREAD | CLOCAL;  // turn on READ & ignore ctrl lines
    toptions.c_iflag &= ~(IXON | IXOFF | IXANY); // turn off s/w flow ctrl

    toptions.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); // make raw
    toptions.c_oflag &= ~OPOST; // make raw

    // see: http://unixwiz.net/techtips/termios-vmin-vtime.html
    toptions.c_cc[VMIN]  = 0;
    toptions.c_cc[VTIME] = 20;

    if( tcsetattr(fd, TCSANOW, &toptions) < 0) {
        perror("init_serialport: Couldn't set term attributes");
        return -1;
    }

    return fd;
}


// arduino serial port read
int serialport_read_until(int fd, char* buf, char until)
{
        char b[1];
        int i=0;
        // 1000ms timeout
        int timeout = 1000*1000;
        do {
                int n = read(fd, b, 1);  // read a char at a time
                if( n==0||n==-1 ) {
                        timeout -= 100;
                        usleep( 100 ); // wait 100 usec try again
                        continue;
                }
                buf[i] = b[0]; i++;
        } while( b[0] != until && timeout > 0 );

        if ( timeout<=0 )
                fprintf(stderr, "serialport_read_until timed out\n");

        buf[i] = 0;  // null terminate the string
        return 0;
}


bool loadOrTrain( int new_index )
{
    // fetch data
    if ( new_index < 0 || new_index >= artvert_list.size() )
    {
        fprintf(stderr,"loadOrTrain: invalid index %i (artvert_list has %i members)\n", new_index, (int)artvert_list.size() );

        return false;
    }

        // switching model..
        new_artvert_switching_in_progress = true;
    bool wants_training = model_file_needs_training[new_index];
    string model_file = artvert_list[new_index].model_file;
        // do we need to switch, or can we use the same model file?
        if ( ( current_artvert_index < 0 || current_artvert_index >= artvert_list.size() ) ||
                        model_file != artvert_list[current_artvert_index].model_file )
        {
                // load
            bool trained = multi->loadOrTrainCache( wants_training, model_file.c_str(), running_on_binoculars );
        if ( !trained )
                {
                        // fail
                        current_artvert_index = -1;
                        new_artvert_switching_in_progress = false;
                return false;
                }


                // copy char model_file before munging with strcat
                char s[1024];
                strcpy (s, model_file.c_str());
                strcat(s, ".roi");
                roi_vec = readROI(s);

                strcpy( s, model_file.c_str() );
                strcat(s, ".artvertroi");
                artvert_roi_vec = readROI(s);
                if ( artvert_roi_vec.empty() )
                {
                        // use roi_vec
                        artvert_roi_vec.insert( artvert_roi_vec.begin(), roi_vec.begin(), roi_vec.end() );
                }

                // load model_image for use with diffing, later
                // model_image = cvLoadImage(model_file.c_str());

                c1[0].x = roi_vec[0];
                c1[0].y = roi_vec[1];
                c1[1].x = roi_vec[2];
                c1[1].y = roi_vec[3];
                c1[2].x = roi_vec[4];
                c1[2].y = roi_vec[5];
                c1[3].x = roi_vec[6];
                c1[3].y = roi_vec[7];
        }

        // update current index
        current_artvert_index = new_index;

        // no longer switching
        new_artvert_switching_in_progress = false;

    return true;

}



/*!\brief Initialize everything
 *
 * - Parse the command line
 * - Initialize all the cameras
 * - Load or interactively build a model, with its classifier.
 * - Set the GLUT callbacks for geometric calibration or, if already done, for photometric calibration.
 */

static bool init( int argc, char** argv )
{
    // register exit function
    atexit( &exit_handler );

    // dump opencv version
    printf("built with opencv version %s\n", CV_VERSION );

    // more from before init should be moved here
    bool redo_lighting=false;
    bool redo_training = false;
    bool redo_geom = false;
    char *avi_bg_path=(char*)"";
    bool got_ds = false;
    bool got_fps = false;
    bool video_source_is_avi = false;
    char *model_file_list_file = NULL;

    // parse command line
    for (int i=1; i<argc; i++)
    {
        if (strcmp(argv[i], "-m") ==0)
        {
            if (i==argc-1)
                usage(argv[0]);
                        Artvert a;
                        a.model_file = argv[i+1];
                        a.advert = "cmdline "+a.model_file;
                        // store
                artvert_list.push_back( a );
            printf(" -m: adding model image '%s'\n", argv[i+1] );
            i++;
        }
        else if ( strcmp(argv[i], "-binoc")==0 )
        {
            running_on_binoculars = true;
            printf(" -binoc: running on binoculars\n");
        }
                else if ( strcmp(argv[i], "-nofullscreen")==0 )
                {
                        no_fullscreen = true;
                        printf(" -nofullscreen: won't go fullscreen\n");
                }
        else if ( strcmp(argv[i], "-ml" )== 0 )
        {
            if ( i==argc-1)
                usage(argv[0]);
            model_file_list_file = argv[i+1];
            printf(" -ml: loading model image list from '%s'\n", argv[i+1] );
            i++;
        }
        else if (strcmp(argv[i], "-r")==0)
        {
            redo_geom=redo_training=redo_lighting=true;
        }
        else if (strcmp(argv[i], "-g")==0)
        {
            redo_geom=true;
        }
        else if (strcmp(argv[i], "-l")==0)
        {
            redo_lighting=true;
        }
        else if (strcmp(argv[i], "-t")==0)
        {
            redo_training=true;
            printf( "-t: redoing training\n");
        }
        else if (strcmp(argv[i], "-a")==0)
        {
            avi_capture=cvCaptureFromAVI(argv[i+1]);
            avi_play=true;
        }
        else if (strcmp(argv[i], "-i")==0)
        {
                        IplImage *image1 = cvLoadImage(argv[i]+1);
        }
        else if (strcmp(argv[i], "-b")==0)
        {
            video_source_is_avi = true;
            avi_bg_path=argv[i+1];
            printf(" -b: loading from avi '%s'\n", avi_bg_path );
        }
        else if (strcmp(argv[i], "-i")==0)
        {
            image_path=argv[i+1];
        }
        else if ( strcmp(argv[i], "-fps")==0 )
        {
            desired_capture_fps=atoi(argv[i+1]);
            got_fps = true;
            i++;
        }
        else if (strcmp(argv[i], "-vd")==0)
        {
            v4l_device=atoi(argv[i+1]);
            printf(" -vd: using v4l device %i\n", v4l_device);
            i++;
        }
        else if (strcmp(argv[i], "-vs")==0)
        {
            video_width=atoi(argv[i+1]);
            video_height=atoi(argv[i+2]);
            if ( !got_ds )
            {
                // also set detect size (if not already set)
                detect_width = video_width;
                detect_height = video_height;
            }
            printf(" -vs: video size is %ix%i\n", video_width, video_height );
            if ( video_width == 0 || video_height == 0 )
            {
                usage(argv[0]);
                exit(1);
            }
            i+=2;
        }
        else if ( strcmp(argv[i], "-ds")==0 )
        {
            detect_width = atoi(argv[i+1]);
            detect_height = atoi(argv[i+2]);
            printf(" -ds: detection frame size is %ix%i\n", detect_width, detect_height );
            if ( detect_width == 0 || detect_height == 0 )
            {
                usage(argv[0]);
                exit(1);
            }
            got_ds = true;
            i+=2;
        }
        else if (argv[i][0]=='-')
        {
            usage(argv[0]);
        }

    }


    // read model files from model_file_list_file
    if ( model_file_list_file != NULL )
    {
        // try to open
        ofxXmlSettings data;
        data.loadFile( model_file_list_file );

        if ( data.getNumTags( "artverts" ) == 1 )
        {
            data.pushTag( "artverts" );
            int num_filenames = data.getNumTags( "advert" );
            printf("   -ml: opened %s, %i adverts\n", model_file_list_file, num_filenames );
            for ( int i=0; i<num_filenames; i++ )
            {
                data.pushTag("advert", i);
                                Artvert a;
                a.model_file = data.getValue( "model_filename", "model.bmp" );
                                a.advert = data.getValue( "advert", "unknown advert" );
                                int num_artverts = data.getNumTags( "artvert" );
                printf("   -ml: got advert, model file '%s', advert '%s', %i artverts\n", a.model_file.c_str(), a.advert.c_str(), num_artverts );
                                for ( int j=0; j<num_artverts; j++ )
                                {
                                        data.pushTag("artvert", j );
                                        a.name = data.getValue( "name", "unnamed" );
                                        a.artist = data.getValue( "artist", "unknown artist" );
                                        if ( data.getNumTags("movie_filename") != 0 )
                                        {
                                                // load a movie
                                                a.artvert_is_movie = true;
                                                a.artvert_movie_file = data.getValue("movie_filename", "artvertmovie1.mp4" );
                                        }
                                        else
                                        {
                                                // load an image
                                                a.artvert_image_file = data.getValue( "image_filename", "artvert1.png" );
                                        }
                        printf("     %i: %s:%s:%s\n", j, a.name.c_str(), a.artist.c_str(),
                           a.artvert_is_movie?(a.artvert_movie_file+"( movie)").c_str() : a.artvert_image_file.c_str() );

                        artvert_list.push_back( a );
                                        data.popTag();
                                }
                data.popTag();
            }
            data.popTag();
        }
        else
        {
            printf("   -ml: error reading '%s': couldn't find 'artverts' tag\n", model_file_list_file );
        }
    }

    // check if model file list is empty
    if ( artvert_list.empty() )
    {
        // add default
                Artvert a;
        artvert_list.push_back( a );
    }

    // set up training flags
    for ( int i=0; i<artvert_list.size(); i++ )
        model_file_needs_training.push_back( redo_training );

    // check for video size arg if necessary
    if ( video_source_is_avi )
    {
        // try to read from video
        CvCapture* temp_cap = cvCaptureFromAVI(avi_bg_path);
        video_width = (int)cvGetCaptureProperty( temp_cap, CV_CAP_PROP_FRAME_WIDTH );
        video_height = (int)cvGetCaptureProperty( temp_cap, CV_CAP_PROP_FRAME_HEIGHT );
        int video_fps = (int)cvGetCaptureProperty( temp_cap, CV_CAP_PROP_FPS );
        printf(" -b: read video width/height %i/%i from avi (ignoring -vs)\n", video_width, video_height );
        if ( !got_ds )
        {
            detect_width = video_width;
            detect_height = video_height;
        }
        if ( !got_fps )
        {
            desired_capture_fps = video_fps;
        }
        cvReleaseCapture(&temp_cap);
    }

    //cout << avi_bg_path << endl;
    cache_light = !redo_lighting;

    glutReshapeWindow( video_width, video_height );

    multi = new MultiGrab();

    if( multi->init(avi_bg_path, video_width, video_height, v4l_device,
                    detect_width, detect_height, desired_capture_fps ) ==0)
    {
        cerr <<"Initialization error.\n";
        return false;
    }


    artvert_struct artvert1 = {"Arrebato, 1980", image1, "Feb, 2009", "Iván Zulueta", "Polo", "Madrid, Spain"};
    artvert_struct artvert2 = {"name2", image2, "2008", "simon innings", "Helmut Lang", "Parlance Avenue"};
    artvert_struct artvert3 = {"name3", image3, "2008", "simon innings", "Loreal", "Parlance Avenue"};
    artvert_struct artvert4 = {"name4", image4, "2008", "simon innings", "Hugo Boss", "Parlance Avenue"};
    artvert_struct artvert5 = {"name5", image5, "2008", "simon innings", "Burger King", "Parlance Avenue"};

    artverts[0] = artvert1;
    artverts[1] = artvert2;
    artverts[2] = artvert3;
    artverts[3] = artvert4;
    artverts[4] = artvert5;


    // load geometry
    loadOrTrain(0);

    // try to load geom cache + start the run loop
    geomCalibStart(!redo_geom);

    // start detection
    startDetectionThread( 1 /* priority, only if running as root */ );

    last_frame_caught_time.SetNow();
    frame_timer.SetNow();

    // start serial
    startSerialThread();

    printf("init() finished\n");
    return true;
}

/*! The keyboard callback: reacts to '+' and '-' to change the viewed cam, 'q' exits.
 * 'd' turns on/off the dynamic lightmap update.
 * 'f' goes fullscreen.
 */
static void keyboard(unsigned char c, int x, int y)
{
        char old_button_state = button_state;
    const char* filename;
    switch (c)
    {
    case 'n' :
        if (augment == 1)
            augment = 0;
        else
            augment = 1;
    case '+' :
        if (current_cam < multi->cams.size()-1)
            current_cam++;
        break;
    case '-':
        if (current_cam >= 1)
            current_cam--;
        break;
    case 'q':
    case 27 /*esc*/:
        exit(0);
        break;
    case 'l':
        dynamic_light = !dynamic_light;
        break;
    case 'd':
        delay_video = !delay_video;
        break;
    case 'a':
        if (avi_play == true)
            avi_play = false;
        else
            avi_play = true;
    case 'f':
        glutFullScreen();
        break;
    case 'k':
        track_kalman = !track_kalman;
        break;
    case 'S':
        show_status = !show_status;
        break;
    case 'p':
        show_profile_results = true;
        break;
    case 'P':
        FProfiler::Clear();
        break;
    case 'i':
        if (cnt >= NUMARTVERTS-1)
            cnt = 0;
        else
            cnt ++;
        cout << "we are on image " << cnt << endl;
        break;
        case '[':
                button_state |= BUTTON_RED;
                break;
        case ']':
                button_state |= BUTTON_GREEN;
                break;
        case '\\':
                button_state |= BUTTON_BLUE;
                break;

    default:
        break;
    }

    if ( multi && show_status )
    {
        planar_object_recognizer &detector(multi->cams[current_cam]->detector);
        bool something = true;
        switch (c)
        {
        // detector settings
        case '1':
            detector.ransac_dist_threshold_ui*=1.02f;
            break;
        case '!':
            detector.ransac_dist_threshold_ui/=1.02f;
            break;
        case '2':
            detector.max_ransac_iterations_ui+=10;
            break;
        case '@':
            detector.max_ransac_iterations_ui-=10;
            break;
        case '3':
            detector.non_linear_refine_threshold_ui*=1.02f;
            break;
        case '#':
            detector.non_linear_refine_threshold_ui/=1.02f;
            break;
        case '4':
            detector.match_score_threshold_ui*=1.02f;
            break;
        case '$':
            detector.match_score_threshold_ui/=1.02f;
            break;
        case '5':
            detector.best_support_thresh_ui++;
            break;
        case '%':
            detector.best_support_thresh_ui--;
            break;
        case '6':
            detector.point_detector_tau_ui++;
            break;
        case '^':
            detector.point_detector_tau_ui--;
            break;
        case '7':
            matrix_tracker.increasePositionSmoothing();
            break;
        case '&':
            matrix_tracker.decreasePositionSmoothing();
            break;
        case '8':
            matrix_tracker.increasePositionZSmoothing();
            break;
        case '*':
            matrix_tracker.decreasePositionZSmoothing();
            break;
        case '9':
            matrix_tracker.increaseFramesBackRaw();
            break;
        case '(':
            matrix_tracker.decreaseFramesBackRaw();
            break;
        case '0':
            matrix_tracker.increaseFramesBackReturned();
            break;
        case ')':
            matrix_tracker.decreaseFramesBackReturned();
            break;


        default:
            something = false;
            break;
        }
        if ( something )
        {
            printf("%s\n", getSettingsString().c_str());
        }

    }

        glutPostRedisplay();

        if ( old_button_state != button_state )
                button_state_changed = true;
}

static void keyboardReleased(unsigned char c, int x, int y)
{
        char old_button_state = button_state;
        switch ( c )
        {
        case '[':
                button_state &= (BUTTON_GREEN | BUTTON_BLUE);
                break;
        case ']':
                button_state &= (BUTTON_RED | BUTTON_BLUE);
                break;
        case '\\':
                button_state &= (BUTTON_GREEN | BUTTON_RED);
                break;
        default:
                break;
        }
        if ( old_button_state != button_state )
                button_state_changed = true;
}
static void emptyWindow()
{
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
}

int main(int argc, char *argv[])
{
    glutInit(&argc, argv);
    glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE);
    //glutInitWindowSize(video_width,video_height); // hard set the init window size
    //glutInitWindowSize(800,450); // hard set the init window size
    glutDisplayFunc(emptyWindow);

        bool start_fullscreen = false;
#ifdef __APPLE__
        // if on apple, don't ever go fullscreen
#else
        // look for -binoc flag on commandline
        // also look for -nofullscreen flag
        for ( int i=0; i<argc; i++ )
        {
                if ( strcmp( argv[i], "-binoc" )== 0 )
                {
                        // if found, start_fullscreen is true
                        start_fullscreen = true;
                }
                // if found, -nofullscreen cancels fullscreen 
                if ( strcmp( argv[i], "-nofullscreen" )==0 )
                {
                        start_fullscreen = false;
                }
        }
#endif

        if ( !start_fullscreen )
        {
        glutReshapeFunc(reshape);
        glutCreateWindow("The Artvertiser 0.4");
        }
    glutMouseFunc(mouse);
    glutEntryFunc(entry);

        if ( start_fullscreen )
        {
                glutGameModeString("1024x768:16@60");
                glutEnterGameMode();
        glutSetCursor(GLUT_CURSOR_NONE);
        }

    if (!init(argc,argv)) return -1;


    //ftglFont = new FTBufferFont("/usr/share/fonts/truetype/freefont/FreeMono.ttf");
    ftglFont = new FTBufferFont("fonts/FreeSans.ttf");
    ftglFont->FaceSize(12);
    ftglFont->CharMap(ft_encoding_unicode);

    //cvDestroyAllWindows();
    //cvWaitKey(0);

    glutKeyboardFunc(keyboard);
        glutKeyboardUpFunc(keyboardReleased);
    glutMainLoop();
    glutLeaveGameMode();
    return 0; /* ANSI C requires main to return int. */
}

//!\brief  Draw a frame contained in an IplTexture object on an OpenGL viewport.
static bool drawBackground(IplTexture *tex)
{
    //printf("draw background\n");
    if (!tex || !tex->getIm()) return false;
    //printf("drawBackground: drawing frame with timestamp %f\n", raw_frame_timestamp.ToSeconds() );

    IplImage *im = tex->getIm();
    int w = im->width-1;
    int h = im->height-1;

    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();

    glDisable(GL_BLEND);
    glDisable(GL_DEPTH_TEST);

    tex->loadTexture();

    glBegin(GL_QUADS);
    glColor4f(1,1,1,1);

    glTexCoord2f(tex->u(0), tex->v(0));
    glVertex2f(-1, 1);
    glTexCoord2f(tex->u(w), tex->v(0));
    glVertex2f(1, 1);
    glTexCoord2f(tex->u(w), tex->v(h));
    glVertex2f(1, -1);
    glTexCoord2f(tex->u(0), tex->v(h));
    glVertex2f(-1, -1);
    glEnd();

    tex->disableTexture();

    return true;
}

/*! \brief Draw all the points
 *
 */
static void drawDetectedPoints(int frame_width, int frame_height)
{
    if (!multi) return;

    planar_object_recognizer &detector(multi->cams[current_cam]->detector);

    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    glOrtho(0, frame_width-1, frame_height-1, 0, -1, 1);

    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();

    glDisable(GL_BLEND);
    glDisable(GL_LIGHTING);
    glDisable(GL_DEPTH_TEST);


    glPointSize(2);
    glBegin(GL_POINTS);
    // draw all detected points
    glColor4f(0,1,0,1);
    for ( int i=0; detector.isReady() && i<detector.detected_point_number; ++i)
    {
        keypoint& kp = detector.detected_points[i];
        int s = kp.scale;
        float x =PyrImage::convCoordf(kp.u, s, 0);
        float y =PyrImage::convCoordf(kp.v, s, 0);
        glVertex2f(x,y);
    }
    // draw matching points red
    if ( detector.object_is_detected )
    {
        glColor4f( 1, 0, 0, 1 );
        for (int i=0; i<detector.match_number; ++i)
        {
            image_object_point_match * match = detector.matches+i;
            if (match->inlier)
            {
                int s = (int)(match->image_point->scale);
                float x=PyrImage::convCoordf(match->image_point->u, s, 0);
                float y=PyrImage::convCoordf(match->image_point->v, s, 0);
                glVertex2f(x,y);
            }
        }
    }
    glEnd();


}


/*! \brief A draw callback during camera calibration
 *
 * GLUT calls that function during camera calibration when repainting the
 * window is required.
 * During geometric calibration, no 3D is known: we just plot 2d points
 * where some feature points have been recognized.
 */
static void geomCalibDraw(void)
{
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    glDisable(GL_LIGHTING);
    drawBackground(raw_frame_texture);

    if (!multi) return;

    IplImage *im = multi->cams[current_cam]->getLastProcessedFrame();
    planar_object_recognizer &detector(multi->cams[current_cam]->detector);
    if (!im) return;

    if (!detector.isReady()) return;
    detector.lock();

    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    glOrtho(0, im->width-1, im->height-1, 0, -1, 1);

    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();

    glDisable(GL_BLEND);
    glDisable(GL_LIGHTING);
    glDisable(GL_DEPTH_TEST);

    if (detector.object_is_detected)
    {
        glPointSize(2);
        glBegin(GL_POINTS);
        glColor4f(0,1,0,1);
        for (int i=0; i<detector.match_number; ++i)
        {
            image_object_point_match * match = detector.matches+i;
            if (match->inlier)
            {
                int s = (int)(match->image_point->scale);
                float x=PyrImage::convCoordf(match->image_point->u, s, 0);
                float y=PyrImage::convCoordf(match->image_point->v, s, 0);
                glVertex2f(x,y);
            }
        }
        glEnd();
    }

    detector.unlock();

    glutSwapBuffers();
}

/*!\brief Called when geometric calibration ends. It makes
 * sure that the CamAugmentation object is ready to work.
 */
static void geomCalibEnd()
{

    if (!multi->model.augm.LoadOptimalStructureFromFile((char*)"camera_c.txt", (char*)"camera_r_t.txt"))
    {
        cout << "failed to load camera calibration.\n";
        exit(-1);
    }
    glutIdleFunc(0);
    //glutDisplayFunc(0);
    delete calib;
    calib=0;
}

/*! Called by GLUT during geometric calibration when there's nothing else to do.
 * This function grab frames from camera(s), run the 2D detection on every image,
 * and keep the result in memory for calibration. When enough homographies have
 * been detected, it tries to actually calibrate the cameras.
 */
static void geomCalibIdle(void)
{
    // detect the calibration object in every image
    // (this loop could be paralelized)
    int nbdet=0;
    for (int i=0; i<multi->cams.size(); ++i)
    {
        bool dummy = false;
        if (multi->cams[i]->detect(dummy, dummy)) nbdet++;
    }


    if(!raw_frame_texture) raw_frame_texture = new IplTexture;
    IplImage* raw_frame = raw_frame_texture->getImage();
    multi->cams[current_cam]->getLastDrawFrame( &raw_frame );
    raw_frame_texture->setImage(raw_frame);
    //raw_frame_texture->setImage(multi->cams[current_cam]->frame);

    if (nbdet>0)
    {
        for (int i=0; i<multi->cams.size(); ++i)
        {
            if (multi->cams[i]->detector.object_is_detected)
            {
                add_detected_homography(i, multi->cams[i]->detector, *calib);
            }
            else
            {
                calib->AddHomography(i);
            }
        }
        geom_calib_nb_homography++;
    }

    printf("geom calib: %.2f%%\n", 100.0f*geom_calib_nb_homography/150.0f );

    if (geom_calib_nb_homography>=150)
    {
        if (calib->Calibrate(
                    50, // max hom
                    (multi->cams.size() > 1 ? 1:2),   // padding or random
                    (multi->cams.size() > 1 ? 0:3),
                    1,   // padding ratio 1/2
                    0,
                    0,
                    0.0078125,  //alpha
                    0.9,                //beta
                    0.001953125,//gamma
                    10,   // iter
                    0.05, //eps
                    3   //postfilter eps
                ))
        {
            calib->PrintOptimizedResultsToFile1();
            geomCalibEnd();
            start();
            return;
        }
    }
    glutPostRedisplay();
}

/*!\brief Start geometric calibration. If the calibration can be loaded from disk,
 * continue directly with photometric calibration.
 */
static void geomCalibStart(bool cache)
{
    if (cache && multi->model.augm.LoadOptimalStructureFromFile((char*)"camera_c.txt", (char*)"camera_r_t.txt"))
    {
        start();
        return;
    }

    // construct a CamCalibration object and register all the cameras
    calib = new CamCalibration();

    for (int i=0; i<multi->cams.size(); ++i)
    {
        calib->AddCamera(multi->cams[i]->width, multi->cams[i]->height);
    }

    geom_calib_nb_homography=0;
    glutDisplayFunc(geomCalibDraw);
    glutIdleFunc(geomCalibIdle);
}



static void drawAugmentation()
{

    // we know that im is not NULL already
//    IplImage *im = multi->model.image;

    //for ( int tracked_or_raw=0; tracked_or_raw<2; tracked_or_raw++ )
    {

        // Fetch object -> image, world->image and world -> object matrices

        CvMat *world;
        /*if ( tracked_or_raw == 1 )
        {
            // fetch from model:
            world = multi->model.augm.GetObjectToWorld();
        }
        else*/
        {

            // or fetch interpolated position
            world = cvCreateMat( 3, 4, CV_64FC1 );

            //printf(". now we want interpolated pose for %f\n", raw_frame_timestamp.ToSeconds() );
            if ( track_kalman )
                matrix_tracker.getInterpolatedPoseKalman( world,
                    multi->cams[0]->getFrameIndexForTime( raw_frame_timestamp ) );
            else
                matrix_tracker.getInterpolatedPose( world, raw_frame_timestamp );
        }

        /*// apply a scale factor
        float scalef = 1.0f;
        for ( int i=0; i<3; i++ )
            cvmSet(world, i, i, scalef*cvmGet( world, i, i ));*/


        // instead of this:
           /*CvMat *proj = multi->model.augm.GetProjectionMatrix(current_cam);
           CvMat *old_proj = multi->model.augm.GetProjectionMatrix(current_cam);*/
        // we make our own project matrix:
        // fetch the pre-projection matrix from model.augm
        CvMat* proj = multi->model.augm.GetPreProjectionMatrix(current_cam);
        // multiply by the object-to-world matrix
        CamCalibration::Mat3x4Mul( proj, world, proj );

        Mat3x4 moveObject, rot, obj2World, movedRT_;
        moveObject.setTranslate( multi->model.getImageWidth()/2, multi->model.getImageHeight()/2,
                                -120*3/4);
        // apply rotation
        rot.setRotate(Vec3(1,0,0),2*M_PI*180.0/360.0);
        //rot.setIdentity();
        moveObject.mul(rot);
        //moveObject.scale

        CvMat cvMoveObject = cvMat(3,4,CV_64FC1, moveObject.m);
        CvMat movedRT=cvMat(3,4,CV_64FC1,movedRT_.m);


        // pose only during movement
        //if (pixel_shift >= 200 || !have_proj)
        if ( true )
        {
            // memcpy or vectorisation speedup?
            for( int i = 0; i < 3; i++ )
            {
                for( int j = 0; j < 4; j++ )
                {
                    a_proj[i][j] = cvmGet( proj, i, j );
                    obj2World.m[i][j] = cvmGet(world, i, j);

                }
            }
            have_proj = 1;
            memcpy(old_a_proj, a_proj, sizeof(a_proj));
        }
        else // copy last known good projection over current
        {
            memcpy(a_proj, old_a_proj, sizeof(old_a_proj));
        }
        // dump the matrix
        /*
        printf("found matrix: %8.4f %8.4f %8.4f %8.4f\n"
               "              %8.4f %8.4f %8.4f %8.4f\n"
               "              %8.4f %8.4f %8.4f %8.4f\n",
               a_proj[0][0], a_proj[0][1], a_proj[0][2], a_proj[0][3],
               a_proj[1][0], a_proj[1][1], a_proj[1][2], a_proj[1][3],
               a_proj[2][0], a_proj[2][1], a_proj[2][2], a_proj[2][3]
                );*/


        CamCalibration::Mat3x4Mul( world, &cvMoveObject, &movedRT);
        // translate into OpenGL PROJECTION and MODELVIEW matrices
        PerspectiveCamera c;
        //c.loadTdir(a_proj, multi->cams[0]->frame->width, multi->cams[0]->frame->height);
        c.loadTdir(a_proj, multi->cams[0]->detect_width, multi->cams[0]->detect_height );
        c.flip();
        c.setPlanes(100,1000000); // near/far clip planes
        cvReleaseMat(&proj);

        // must set the model view after drawing the background.
        c.setGlProjection();
        c.setGlModelView();

        /*! this is the beginning of prototype code for an occlusion mask built
         *  by comparison of the tracked plane with that of the model image

        // create a copy of frame texture and warp to model image perspective
        CvPoint2D32f *c2 = new CvPoint2D32f[4];
        // update corner points of ROI in pixel space
        c2[0].x = cvRound(multi->cams[0]->detector.detected_u_corner1);
        c2[0].y = cvRound(multi->cams[0]->detector.detected_v_corner1);
        c2[1].x = cvRound(multi->cams[0]->detector.detected_u_corner2);
        c2[1].y = cvRound(multi->cams[0]->detector.detected_v_corner2);
        c2[2].x = cvRound(multi->cams[0]->detector.detected_u_corner3);
        c2[2].y = cvRound(multi->cams[0]->detector.detected_v_corner3);
        c2[3].x = cvRound(multi->cams[0]->detector.detected_u_corner4);
        c2[3].y = cvRound(multi->cams[0]->detector.detected_v_corner4);

        CvMat* mmat = cvCreateMat(3,3, CV_32FC1);
        IplImage *warped = cvCreateImage(cvSize(model_image->width, model_image->height), 8, 3);
        mmat = cvGetPerspectiveTransform(c2, c1, mmat);
        cvWarpPerspective(raw_frame_texture->getIm(), warped, mmat);
        cvReleaseMat(&mmat);

        // find difference between model image and frame
        IplImage *i1=cvCreateImage(cvSize(im->width,im->height),im->depth,1);
        IplImage *i2=cvCreateImage(cvSize(im->width,im->height),im->depth,1);
        IplImage *diff=cvCreateImage(cvSize(im->width,im->height),im->depth,1);
        IplImage *display=cvCreateImage(cvSize(im->width,im->height),im->depth,1);

        cvCvtColor(im, i1,CV_BGR2GRAY);
        cvCvtColor(warped, i2,CV_BGR2GRAY);
        cvAbsDiff(i2,i1,diff);
        cvThreshold(diff, display, 35, 255, CV_THRESH_BINARY);

        cvReleaseImage(&warped);
        cvSaveImage("checkdiff.png", display);
        */

        /* circles at corners of ROI. useful for debugging.
        cvCircle(raw_frame_texture->getIm(), c1, 10, CV_RGB(255, 0, 0), 2);
        cvCircle(raw_frame_texture->getIm(), c2, 10, CV_RGB(255, 0, 0), 2);
        cvCircle(raw_frame_texture->getIm(), c3, 10, CV_RGB(255, 0, 0), 2);
        cvCircle(raw_frame_texture->getIm(), c4, 10, CV_RGB(255, 0, 0), 2);

        tex = new IplTexture;
        tex->setImage(raw_frame_texture->getIm());
        drawBackground(tex);
        */



#ifndef DEBUG_SHADER

        glEnable(GL_DEPTH_TEST);
        glEnable(GL_BLEND);
        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
        glDisable(GL_CULL_FACE);
        // multiply texture colour by surface colour of poly
        glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE);
        glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);

        if (avi_play == true)
        {
            IplImage *avi_frame = 0;
            IplImage *avi_image = 0;
            avi_frame = cvQueryFrame( avi_capture );
            avi_image = cvCreateImage(cvSize(video_width/2, video_height/2), 8, 3);
            cvResize(avi_frame, avi_image, 0);
            avi_image->origin = avi_frame->origin;
            GLenum format = IsBGR(avi_image->channelSeq) ? GL_BGR_EXT : GL_RGBA;

            if (!avi_play_init)
            {
                glGenTextures(1, &imageID);
                glBindTexture(GL_TEXTURE_2D, imageID);
                glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
                glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
                glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
                glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, avi_image->width, avi_image->height, 0, format, GL_UNSIGNED_BYTE, avi_image->imageData);
                avi_play_init=true;
            }
            else
            {
                glBindTexture(GL_TEXTURE_2D, imageID);
                glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, avi_image->width, avi_image->height, format, GL_UNSIGNED_BYTE, avi_image->imageData);
            }
        }
        else
        {
                        if ( current_artvert_index >= 0 &&
                                        current_artvert_index < artvert_list.size() )
                        {
                                glBindTexture(GL_TEXTURE_2D, imageID);
                                glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
                                glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
                                IplImage* image = artvert_list.at(current_artvert_index).getArtvertImage();
                                GLenum format = IsBGR(image->channelSeq) ? GL_BGR_EXT : GL_RGBA;
                                glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, image->width, image->height, 0, format, GL_UNSIGNED_BYTE, image->imageData);
                        }
        }

        glEnable(GL_TEXTURE_2D);

        glHint(GL_POLYGON_SMOOTH, GL_NICEST);
        glEnable(GL_POLYGON_SMOOTH);

/*
#ifndef DEBUG_SHADER
        // apply the object transformation matrix
        Mat3x4 w2e(c.getWorldToEyeMat());
        w2e.mul(moveObject);
        c.setWorldToEyeMat(w2e);
        c.setGlModelView();
#endif

        if (multi->model.map.isReady())
        {
            glDisable(GL_LIGHTING);
#ifdef DEBUG_SHADER
            multi->model.map.enableShader(current_cam, world);
#else
            multi->model.map.enableShader(current_cam, &movedRT);
#endif
        }

*/
        glColor4f(1.0, 1.0, 1.0, fade);

        glBegin(GL_QUADS);
        glTexCoord2f(0, 0);
        glVertex3f(artvert_roi_vec[0], artvert_roi_vec[1], 0);
        glTexCoord2f(1, 0);
        glVertex3f(artvert_roi_vec[2], artvert_roi_vec[3], 0);
        glTexCoord2f(1, 1);
        glVertex3f(artvert_roi_vec[4], artvert_roi_vec[5], 0);
        glTexCoord2f(0, 1);
        glVertex3f(artvert_roi_vec[6], artvert_roi_vec[7], 0);
        glEnd();

        glDisable(GL_TEXTURE_2D);

        /*! 'label' is a boolean set by the right mouse button and toggles the
         * in-scene artvert label.
         */

        if (label)
        {
            glBegin(GL_LINE_LOOP);
            glColor3f(0.0, 1.0, 0.0);
            glVertex3f(roi_vec[0]-10, roi_vec[1]-10, 0);
            glVertex3f(roi_vec[2]+10, roi_vec[3]-10, 0);
            glVertex3f(roi_vec[4]+10, roi_vec[5]+10, 0);
            glVertex3f(roi_vec[6]-10, roi_vec[7]+10, 0);
            glVertex3f(roi_vec[0]-10, roi_vec[1]-10, 0);
            glEnd();

            glTranslatef(roi_vec[2]+12, roi_vec[3], 0);
            glRotatef(180, 1.0, 0.0, 0.0);
            glRotatef(-45, 0.0, 1.0, 0.0);
            glColor4f(0.0, 1.0, 0.0, 1);

            glBegin(GL_LINE_LOOP);
            glVertex3f(0, 10, -.2);
            glVertex3f(150, 10, -.2);
            glVertex3f(150, -60, -.2);
            glVertex3f(0, -60, -.2);
            glEnd();

            glColor4f(0.0, 1.0, 0.0, .5);

            glBegin(GL_QUADS);
            glVertex3f(0, 10, -.2);
            glVertex3f(150, 10, -.2);
            glVertex3f(150, -60, -.2);
            glVertex3f(0, -60, -.2);
            glEnd();

            // render the text in the label
            glColor4f(1.0, 1.0, 1.0, 1);
            ftglFont->Render(artverts[cnt].artvert);
            glTranslatef(0, -12, 0);
            ftglFont->Render(artverts[cnt].date);
            glTranslatef(0, -12, 0);
            ftglFont->Render(artverts[cnt].author);
            glTranslatef(0, -12, 0);
            ftglFont->Render(artverts[cnt].advert);
            glTranslatef(0, -12, 0);
            ftglFont->Render(artverts[cnt].street);
        }

#else
#endif
        //glEnd();

        /*cvReleaseMat(&world);
        {
            CvScalar c =cvGet2D(multi->model.image, multi->model.image->height/2, multi->model.image->width/2);
            glColor3d(c.val[2], c.val[1], c.val[0]);
        }
        if (multi->model.map.isReady())
            multi->model.map.disableShader();
        else
            glDisable(GL_LIGHTING);*/

        if ( avi_play == true  )
        {
            cvReleaseImage(&avi_image);
            cvReleaseImage(&avi_frame);
        }
    }

}

//#define DEBUG_SHADER
/*! The paint callback during photometric calibration and augmentation. In this
 * case, we have access to 3D data. Thus, we can augment the calibration target
 * with cool stuff.
 */
static void draw()
{
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    glDisable(GL_LIGHTING);

    if ( multi->model.isInteractiveTrainBinocularsRunning() )
        multi->model.interactiveTrainBinocularsDraw();
    else
    {

        drawBackground(raw_frame_texture);

        string cnt_str;
        stringstream cnt_out;
        cnt_out << cnt;
        cnt_str = cnt_out.str();

        //IplImage *pre_mask = cvCreateImage(cvSize(WIDTH, HEIGHT), 8, 3);

        if (!multi)
            return;

        int now = glutGet(GLUT_ELAPSED_TIME);
        /* elapsed time
        cout << now/1000.0 << endl;
        */


        // fade
        double elapsed = frame_timer.Update();
        if ( frame_ok )
        {
            last_frame_caught_time.SetNow();
            // increase fade
            if ( fade < (show_status?MAX_FADE_SHOW:MAX_FADE_NORMAL) )
            {
                fade += (1.0f/SECONDS_LOST_FADE)*elapsed;
            }
            else
                fade = show_status?MAX_FADE_SHOW:MAX_FADE_NORMAL;
            //printf("frame_ok: fade %f\n", fade );
        }
        else
        {
            FTime now;
            now.SetNow();
            double elapsed_since_last_caught = (now-last_frame_caught_time).ToSeconds();
            if ( elapsed_since_last_caught > SECONDS_LOST_TRACK )
            {
                if ( fade > 0.0f )
                    fade -= (1.0f/SECONDS_LOST_FADE)*elapsed;
                else
                    fade = 0.0f;
            }
            //printf("frame_ok: fade %f, elapsed since last caught %f\n", fade, elapsed_since_last_caught );
        }
        //printf("frame %s, lost_count %f -> fade pct %4.2f, fade %4.2f\n", frame_ok?"y":"n", frame_lost_count, fade_pct, fade );

        // draw augmentation
        if ( fade > 0 && augment == 1)
        {
            drawAugmentation();
        }

        // calculate fps
        draw_fps = (draw_fps*7.0+1.0/elapsed)/8.0f;

        glLoadIdentity();
        // we need to setup a new projection matrix for the title font.
        glMatrixMode(GL_PROJECTION);
        glLoadIdentity();
        glMatrixMode(GL_MODELVIEW);
        glLoadIdentity();
        glTranslatef(-.98, 0.9, 0.0);
        glScalef(.003, .003, .003);
        ftglFont->FaceSize(32);
        glColor4f(1.0, 1.0, 1.0, 1);
        ftglFont->Render("the artvertiser 0.4");
        glTranslatef(0, -(video_height+30), 0);
        glColor4f(1.0, 1.0, 1.0, .6);
        //ftglFont->FaceSize(16);
        //ftglFont->Render(date(now).c_str());
        if (frame_ok == 1 and (now/1000)%2== 0)
        {
            glTranslatef(video_width-295, video_height+35, 0);
            glColor4f(0.0, 1.0, 0.0, .8);
            glBegin(GL_TRIANGLES);
            glVertex3f(140, 0, 0);
            glVertex3f(150, 10, 0);
            glVertex3f(140, 20, 0);
            glEnd();
            glTranslatef(70, 5, 0);
            ftglFont->FaceSize(16);
            ftglFont->Render("tracking");
        }

        // reset the ftgl font size for next pass
        ftglFont->FaceSize(12);


        if ( show_status )
        {
            //printf("draw status\n");

            drawDetectedPoints( raw_frame_texture->getIm()->width, raw_frame_texture->getIm()->height );

            char detect_fps_string[256];
            sprintf(detect_fps_string, "draw fps: %4.2f\ndetection fps: %4.2f", draw_fps, detection_fps );
            drawGlutString( detect_fps_string, 1.0f, 0.2f, 0.2f, 0.7, 0.94 );

            // now status string
            string draw_string = status_string;
            if ( multi )
            {
                // show detector settings
                draw_string += "\n" + getSettingsString();
            }
            drawGlutString( draw_string.c_str(), 1.0f, 0.2f, 0.2f, 0.01f, 0.2f );
        }

        drawMenu();
    }

    glutSwapBuffers();
    //cvReleaseImage(&image); // cleanup used image
    glFlush();
}



void startSerialThread()
{
    pthread_attr_t thread_attr;
    pthread_attr_init(&thread_attr);
    pthread_attr_setdetachstate(&thread_attr, PTHREAD_CREATE_JOINABLE);
    // launch the thread
    pthread_create( &serial_thread, &thread_attr, serialThreadFunc, NULL );
    serial_thread_is_running = true;
    pthread_attr_destroy( &thread_attr );
}

void shutdownSerialThread()
{
    // kill the thread
    serial_thread_should_exit = true;
    void* ret;
    pthread_join( serial_thread, &ret );
    serial_thread_is_running = false;

}

void* serialThreadFunc( void* data )
{
    // arduino vars
    int fd = 0;
    char serialport[256];
    int baudrate = B9600;  // default
    char buf[256];
        char buf2[256];
    int rc,n;

    fd = serialport_init( "/dev/ttyUSB0", 9600 );
    printf("fd said %i, errno %i\n", fd, errno );

    while ( !serial_thread_should_exit )
    {
        int read = serialport_read_until(fd, buf, '\n');
        //printf("read: %s then %s\n",buf2, buf);
        if ( (read==0) && strlen( buf ) >= 4 /*includes final \n*/ )
        {
            bool button_red = (buf[0]=='1');
            bool button_green = (buf[1]=='1');
            bool button_blue = (buf[2]=='1');
            // printf("buttons: %s %s %s", button_red?"red":"   ", button_green?"green":"     ", button_blue?"blue":"    ");
            // bitmapped, to access all 7 press combinations
            char new_button_state =
                ( button_green ? BUTTON_GREEN : 0 ) |
                ( button_blue  ? BUTTON_BLUE : 0 ) |
                ( button_red   ? BUTTON_RED : 0 );
                        // deal with debounce
                        if ( new_button_state != button_state )
                        {
                                printf( "serialport read %s -> 0x%x (old was 0x%x)\n",
                                        buf, new_button_state, button_state );
                                button_state_changed = true;
                                button_state = new_button_state;
                        }
        }
        usleep(3*1000);
    }

    close(fd);
}

static void startDetectionThread( int thread_priority )
{
    pthread_attr_t thread_attr;
    pthread_attr_init(&thread_attr);
    pthread_attr_setdetachstate(&thread_attr, PTHREAD_CREATE_JOINABLE);
    // launch the thread
    pthread_create( &detection_thread, &thread_attr, detectionThreadFunc, NULL );
    if ( thread_priority > 0 )
    {
        printf("attempting to set detection thread priority to %i\n", thread_priority );
        struct sched_param param;
        param.sched_priority = thread_priority;

        int res = pthread_setschedparam( detection_thread, SCHED_RR, &param );
        if ( res != 0 )
        {
            fprintf(stderr,"pthread_setschedparam failed: %s\n",
                   (res == ENOSYS) ? "ENOSYS" :
                   (res == EINVAL) ? "EINVAL" :
                   (res == ENOTSUP) ? "ENOTSUP" :
                   (res == EPERM) ? "EPERM" :
                   (res == ESRCH) ? "ESRCH" :
                   "???"
                   );
        }
    }
    detection_thread_running = true;
    pthread_attr_destroy( &thread_attr );
}

static void shutdownDetectionThread()
{
    // kill the thread
    detection_thread_should_exit = true;
    void* ret;
    pthread_join( detection_thread, &ret );
    detection_thread_running = false;
}

static void* detectionThreadFunc( void* _data )
{

    FTime detection_thread_timer;
    detection_thread_timer.SetNow();

    while ( !detection_thread_should_exit )
    {
        PROFILE_THIS_BLOCK("detection_thread");

        if ( new_artvert_requested )
        {
            // no longer draw
            frame_ok = false;
            // go with the loading
            loadOrTrain(new_artvert_requested_index);
            new_artvert_requested = false;
        }

        bool frame_retrieved = false;
        bool frame_retrieved_and_ok = multi->cams[0]->detect( frame_retrieved, frame_ok );
        if( frame_retrieved )
        {
            double elapsed = detection_thread_timer.Update();
            detection_fps = (detection_fps*0.0 + (1.0/elapsed))/1.0;
        }
        if ( !frame_retrieved_and_ok )
        {
            PROFILE_THIS_BLOCK("sleep till next");
            usleep( 10000 );
            continue;
        }

        if ( detection_thread_should_exit )
            break;

        multi->model.augm.Clear();
        if (multi->cams[0]->detector.object_is_detected)
        {
            add_detected_homography(0, multi->cams[0]->detector, multi->model.augm);
        }
        else
        {
            multi->model.augm.AddHomography();
        }

        frame_ok = multi->model.augm.Accomodate(4, 1e-4);

        if (frame_ok)
        {
            // fetch surface normal in world coordinates
            CvMat *mat = multi->model.augm.GetObjectToWorld();
            float normal[3];
            for (int j=0; j<3; j++)
                normal[j] = cvGet2D(mat, j, 2).val[0];

            // continue to track
            if ( track_kalman )
                matrix_tracker.addPoseKalman( mat, multi->cams[0]->getFrameIndexForTime(
                        multi->cams[0]->getLastProcessedFrameTimestamp() ) );
            else
                matrix_tracker.addPose( mat, multi->cams[0]->getLastProcessedFrameTimestamp() );

            cvReleaseMat(&mat);

        }
    }

    printf("detection thread exiting\n");

    pthread_exit(0);
}


/*! GLUT calls this during photometric calibration or augmentation phase when
 * there's nothing else to do. This function does the 2D detection and bundle
 * adjusts the 3D pose of the calibration pattern.
 */
static void idle()
{
        if ( running_on_binoculars && !no_fullscreen )
        {
            static int fullscreen_timer = 30;
        if ( fullscreen_timer > 0 )
                {
                        fullscreen_timer --;
                        if( fullscreen_timer <= 0 )
                                glutFullScreen();
                }
        }

    // detect the calibration object in every image
    // (this loop could be paralelized)
    int nbdet=1;

    if(!raw_frame_texture) raw_frame_texture = new IplTexture;

    PROFILE_SECTION_PUSH("getting last frame");

    if ( delay_video )
    {
        IplImage* captured_frame;
        multi->cams[current_cam]->getLastDrawFrame( &captured_frame, &raw_frame_timestamp );

        static list< pair<IplImage*, FTime> > frameRingBuffer;
        while ( frameRingBuffer.size()<VIDEO_DELAY_FRAMES )
        {
            IplImage* first_frame =  cvCreateImage( cvGetSize( captured_frame ), captured_frame->depth, captured_frame->nChannels );
            cvCopy( captured_frame, first_frame );
            frameRingBuffer.push_back( make_pair( first_frame, raw_frame_timestamp ) );
        }

        IplImage* ringbuffered = frameRingBuffer.front().first;
        cvCopy( captured_frame, ringbuffered );
        frameRingBuffer.push_back( make_pair( ringbuffered, raw_frame_timestamp ) );

        frameRingBuffer.pop_front();

        IplImage* raw_frame = frameRingBuffer.front().first;
        raw_frame_timestamp = frameRingBuffer.front().second;

        raw_frame_texture->setImage(raw_frame);
    }
    else
    {
        IplImage* raw_frame = raw_frame_texture->getImage();
        multi->cams[current_cam]->getLastDrawFrame( &raw_frame, &raw_frame_timestamp );
        raw_frame_texture->setImage(raw_frame);
    }

    PROFILE_SECTION_POP();


    if ( multi->model.isInteractiveTrainBinocularsRunning() )
    {
        bool button_red = button_state   & BUTTON_RED;
        bool button_green = button_state & BUTTON_GREEN;
        bool button_blue = button_state  & BUTTON_BLUE;
        multi->model.interactiveTrainBinocularsUpdate( raw_frame_texture->getImage(),
                                                      button_red, button_green, button_blue );
    }
    else
    {
        updateMenu();
        //doDetection();
    }
    glutPostRedisplay();

    PROFILE_SECTION_POP();

    if ( show_profile_results )
    {
        // show profiler output
        printf("showing results\n");
        FProfiler::Display( FProfiler::SORT_TIME/*SORT_EXECUTION*/ );
        show_profile_results = false;
    }
}

//! Starts photometric calibration.
static void start()
{
    glutIdleFunc(idle);
    glutDisplayFunc(draw);
}



/*
// menu
bool menu_show = false;
FTime menu_timer;
bool menu_is_showing = false;
bool menu_up = false;
bool menu_down = false;
void updateMenu();
void drawMenu();
*/
int menu_index = 0;

void updateMenu()
{
        // update timer
        if ( menu_is_showing )
        {
                FTime now;
                now.SetNow();
                if ( (now-menu_timer).ToSeconds() > MENU_HIDE_TIME )
                {
                        menu_is_showing = false;
                }
        }

        if ( !button_state_changed || new_artvert_switching_in_progress )
                return;

        printf("menu sees new button state: %s %s %s\n", 
                        button_state&BUTTON_RED?"red":"   ",
                        button_state&BUTTON_GREEN?"green":"     ",
                        button_state&BUTTON_BLUE?"blue":"    ");

        // clear changed flag
        button_state_changed = false;

        if ( ( button_state == BUTTON_GREEN ) && !menu_is_showing )
        {
                menu_is_showing = true;
                menu_timer.SetNow();
                if ( menu_index >= artvert_list.size() )
                        menu_index = artvert_list.size()-1;
                // done
                return;
        }

        // only process rest of buttons if menu is showing
        if ( !menu_is_showing )
        return;

        // navigation
        if( button_state == BUTTON_BLUE )
        {
                menu_index++;
                if ( menu_index >= artvert_list.size() )
                        menu_index = 0;
                menu_timer.SetNow();
        }
        if ( button_state == BUTTON_RED )
        {
                menu_index--;
                if ( menu_index < 0 )
                        menu_index = artvert_list.size()-1;
                menu_timer.SetNow();
        }

        // accept?
        if ( button_state == BUTTON_GREEN )
        {

                new_artvert_requested_index = menu_index;
                new_artvert_requested = true;

            menu_is_showing = false;

        }





}

void drawMenu()
{
        if ( !menu_is_showing )
        {
                // draw switching text?
                if ( new_artvert_switching_in_progress )
                {
                        glMatrixMode(GL_PROJECTION);
                        glLoadIdentity();
                        glMatrixMode(GL_MODELVIEW);
                        glLoadIdentity();
                        glTranslatef(-.8, 0.65, 0.0);
                        glScalef(.003, .003, .003);
                        ftglFont->FaceSize(24);
                        glColor4f(0.0, 1.0, 0.0, 1);

                        if ( multi->model.isLearnInProgress() )
                        {       
                                // must manually tokenize
                                char message[2048];
                            strncpy( message, multi->model.getLearnProgressMessage(), 2048 );
                                char* ptr = strtok( message,"\n");
                                while( ptr != NULL) 
                                {
                                ftglFont->Render(ptr);
                                glTranslatef(0, -26, 0 );
                                        ptr = strtok( NULL, "\n" );
                                }
                        }
                        else
                                ftglFont->Render("changing artvert...");
                        

                }

                return;
        }

        // draw menu header

        // draw loop
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();
        glEnable(GL_BLEND);
    glTranslatef(-.85, 0.65, 0.0);
    glScalef(.003, .003, .003);
    ftglFont->FaceSize(24);
    glColor4f(.25, 1.0, 0.0, 1);

    ftglFont->Render("Select artvert:");
    glTranslatef( 0, -26, 0 );

        for ( int i=0; i<artvert_list.size(); i++ )
        {
                string advert = artvert_list[i].advert;
                string name = artvert_list[i].name;
                string artist = artvert_list[i].artist;
                string line = string("   ") + advert + " : '" + name + "' by " + artist;

        if ( i == menu_index )
        {
            glColor4f( 1,0.37,0,1 );
        }
        else
        {
            glColor4f( .25f, 1.f, 0.0f, 1 );
        }
        ftglFont->Render(line.c_str());
        glTranslatef(0, -26, 0 );

    }


}



//EOF