OmniTracking.hh

Go to the documentation of this file.

/*
 *  Copyright (C) Massimo Cora' 2005 <maxcvs@email.it>
 *
 *  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 2 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, write to the Free Software
 *  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 */


#ifndef __OMNITRACKING_H__
#define __OMNITRACKING_H__


#include <stdio.h>

// opencv
#include <cv.h>
#include <highgui.h>

#include "OmniConfig.hh"
#include "OmniTrackingTable.hh"
#include "OmniAngle.hh"


typedef struct _added_point_data {

        CvPoint point;
        OmniAngle<int> angle_int;
} added_point_data_t;



template <class ConverterT, class AngleT, class ImageT, class PointT>
class OmniTracking : public OmniTrackingTable<ConverterT, AngleT, ImageT, PointT> {

public:
        OmniTracking( IplImage* first_frame, 
                                  OmniConversion<ConverterT, AngleT, ImageT, PointT> *conv_table, 
                                  int max_tracked_points_count = 100,
                                  int freeze_frame_bound = FREEZE_FRAME_BOUND, 
                                  double track_window_hitrate = TRACK_WINDOW_HITRATE );
        virtual ~OmniTracking();

        bool add_point_to_track( CvPoint &point );
        
        void process_next_frame( IplImage *next_frame );
        
        void register_image_callbacks( on_create_image_context_cb* create_image_context,
                                                                   on_image_do_action_cb* image_show,
                                                                   on_destroy_image_context_cb* destroy_image_context,
                                                                   void* callback_data
                                                                  );                                               

private:                // functions
        void initialize_image_buffers( IplImage* first_video_frame );

        on_create_image_context_cb* create_image_context;
        
        on_image_do_action_cb* image_show;
        
        on_destroy_image_context_cb* destroy_image_context;
        
private:                // data

        IplImage *_image, 
                         *_grey, 
                         *_prev_grey,  
                         *_pyramid, 
                         *_prev_pyramid, 
                         *_swap_temp;

        void* _callback_data_on_image_cb;       
        
        int _win_size;

        CvPoint2D32f *_tracked_points[2], 
                                *_points[2], 
                                *_swap_points;

        int _max_tracked_points_count;
        char* _tracked_points_status;
        
        int _tracked_points_count;

        int _video_width;
        int _video_height;

        int _track_flags;

        bool _add_remove_pt;
        list<added_point_data_t*> _points_added_list;   
};





template <class ConverterT, class AngleT, class ImageT, class PointT>
OmniTracking<ConverterT, AngleT, ImageT, PointT>::OmniTracking( IplImage* first_video_frame, 
                                                    OmniConversion<ConverterT, AngleT, ImageT, PointT> *lu_table,
                                                        int max_tracked_points_count /*= 100*/,
                                                        int _freeze_frame_bound /* = FREEZE_FRAME_BOUND */, 
                                                        double _track_window_hitrate /* = TRACK_WINDOW_HITRATE*/ ) 
                        : OmniTrackingTable<ConverterT, AngleT, ImageT, PointT>( lu_table, _freeze_frame_bound, _track_window_hitrate ) {

        this->_max_tracked_points_count = max_tracked_points_count;
        _tracked_points_status = 0;
        _tracked_points_count = 0;

        _tracked_points[0] = 0;
        _tracked_points[1] = 0;

        _points[0] = 0;
        _points[1] = 0;

        _win_size = 10;

        _video_width = first_video_frame->width;
        _video_height = first_video_frame->height;

        // no points have been added right now.
        _add_remove_pt = false;

        // set the default callbacks. They can be changed with register_callbacks () call.
        // callback data to NULL to avoid strange behaviour on pointers.
        register_image_callbacks( omnistuff_create_image_context, omnistuff_image_do_action, 
                                                          omnistuff_destroy_image_context, NULL );

        initialize_image_buffers( first_video_frame );
}


//-------------------------------------------------------------------------
//

template <class ConverterT, class AngleT, class ImageT, class PointT>
OmniTracking<ConverterT, AngleT, ImageT, PointT>::~OmniTracking() {

        if ( !_points_added_list.empty () ) {
                added_point_data_t* tmp_data;

                while ( (tmp_data = _points_added_list.front ())) {
                        _points_added_list.pop_front ();
                        if ( tmp_data )
                                delete tmp_data;
                }
        }
}


//-------------------------------------------------------------------------
//

template <class ConverterT, class AngleT, class ImageT, class PointT>
void OmniTracking<ConverterT, AngleT, ImageT, PointT>::initialize_image_buffers( IplImage* first_video_frame ) {

        // allocate all the buffers
    _image = cvCreateImage( cvGetSize( first_video_frame ), 8, 3 );
        _image->origin = first_video_frame->origin;

        // grey image
    _grey = cvCreateImage( cvGetSize( first_video_frame ), 8, 1 );
    _prev_grey = cvCreateImage( cvGetSize( first_video_frame ), 8, 1 );

        // pyramid image
    _pyramid = cvCreateImage( cvGetSize( first_video_frame ), 8, 1 );
    _prev_pyramid = cvCreateImage( cvGetSize( first_video_frame ), 8, 1 );

        // alloc the space for the max number of tracked points
    _tracked_points[0] = (CvPoint2D32f*)
                        cvAlloc( _max_tracked_points_count * sizeof(_tracked_points[0][0]));
    _tracked_points[1] = (CvPoint2D32f*)
                        cvAlloc( _max_tracked_points_count * sizeof(_tracked_points[0][0]));

    _tracked_points_status = (char*)cvAlloc( _max_tracked_points_count );
    _track_flags = 0;
}



//-------------------------------------------------------------------------
// perform the real "tracking" on the n-next frame. The input frame should be
// subsequent of the previuos, in a "video succession" format fashion.

template <class ConverterT, class AngleT, class ImageT, class PointT>
void OmniTracking<ConverterT, AngleT, ImageT, PointT>::process_next_frame( IplImage *next_frame ) {

        int i, curr_face;

        if ( !next_frame )
                return;

        cvCopy( next_frame, _image, 0 );
    cvCvtColor( _image, _grey, CV_BGR2GRAY );
                
        // proceed only if we have some point tracked
        if( _tracked_points_count > 0 ) {

                // LK tracking algorithm
                cvCalcOpticalFlowPyrLK( _prev_grey, _grey, _prev_pyramid, _pyramid,
                        _tracked_points[0], _tracked_points[1], _tracked_points_count, 
                        cvSize( _win_size, _win_size ), 3, _tracked_points_status, 0,
            cvTermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03 ), _track_flags );

                _track_flags |= CV_LKFLOW_PYR_A_READY;


                // before the for loop we must set the update status of the tracking windows:
                this->update_begin();

                // store the tmp value of tracked points, and have loop for every point
                int curr_tracked_points_count = _tracked_points_count;

        for( i = curr_face = 0; i < curr_tracked_points_count; i++ ) {
                        if( _add_remove_pt ) {
/*/     rarely this happens. With tracking windows points are most far away one from the other
                                //
                                // let's test the difference [distance] between the points already 
                                // existing in tracking process: if it's minor than a tot throw away the point
                                // and go on. NOTE: we don't remove it coz it could happen that two points
                                // are tracking an object next to another one.
                                //

                                double dx = point_last_added.x - tracked_points[1][i].x;
                double dy = point_last_added.y - tracked_points[1][i].y;

                if( dx*dx + dy*dy <= 5 ) {
                                        // the new point added is a wrong one. So we set to false the adding flag,
                                        // discarding it.
                                        add_remove_pt = false;
                    continue;
                }
/*/
                        }

                        if( !_tracked_points_status[curr_face] )
                                continue;

                        CvPoint angle_point = cvPointFrom32f( _tracked_points[1][curr_face] );

                        float rad_angle = 
                                atan2f( angle_point.y - ((float)this->m_conv_table->get_omni_center_y()),
                                                angle_point.x - ((float)this->m_conv_table->get_omni_center_x()));

                        // get the angle interval we want to track [= a window]
                        OmniAngle<int> angle_int;
                        OmniAngle<double> angle_double( rad_angle - FACE_TRACK_ANGLE_WINDOW,
                                                                                        rad_angle + FACE_TRACK_ANGLE_WINDOW,
                                                                                        2 * FACE_TRACK_ANGLE_WINDOW );
                        this->m_conv_table->angle_type_conversion( angle_double, angle_int );

#ifdef SHOW_TRACKED_FACES
                        char title[30];
                        sprintf(title, "Tracking face id #%d", curr_face );
                        create_image_context( title, _callback_data_on_image_cb );
#endif

                        // do an update tracking window here
                        if ( this->update_tracking_window_bounds( curr_face, angle_int.angle_start, 
                                                                                                angle_int.angle_end ) == false ) {

                                //
                                // adjust the tracked points array:
                                //
                                // | P0 | P1 | P2 | P3 | X | P5 | P6 | P7 | 
                                //                  ^---  deleted point
                                //
                                // | P0 | P1 | P2 | P3 | P5 | P6 | P7 |
                                //

                                for ( int h = curr_face + 1; h < _tracked_points_count; h++ ) {
                                        _tracked_points[1][h - 1] = _tracked_points[1][h];
                                        _tracked_points_status[h - 1] = _tracked_points_status[h];
                                }

#ifdef SHOW_TRACKED_FACES
                                char title[30];
                                // remove the last window: their number is decreased by 1
                                sprintf( title, "Tracking face id #%d", _tracked_points_count -1  );
                                destroy_image_context( title, _callback_data_on_image_cb );
#endif

                                // remove the tracking window. We don't need it anymore
                                // This perform also a decrement in windows ids that stays
                                // after the one we had just deleted
                                this->remove_tracking_window( curr_face );

                                _tracked_points_count--;
                                continue;
                        }

                        // curr_face++ is incremented only here: this is the tracked face index
                        curr_face++;


#ifdef SHOW_TRACKED_FACES                       
                        IplImage *face;

#ifdef SHOW_GREEN_MARKERS
                        // paint a circle on the tracked point
                        cvCircle( next_frame, cvPointFrom32f( _tracked_points[1][i] ), 3, CV_RGB(0,255,0), -1, 8,0);
#endif
                        face = this->m_conv_table->omni2pano( next_frame, angle_int );

                        // as from header
                        // So !*DO NOT FREE*! the image when you call this function.
                        image_show( face, curr_face -1, title, _callback_data_on_image_cb );
#endif
                }
                
                // ok all done with the update. Signal it's ending and let the windows re-write their
                // status on the degree_table
                this->update_end();

                // set the correct number of tracked faces
        _tracked_points_count = curr_face;
        }

        // process the new point added: we can parse the following "if" only if the requested
        // adding point is a good one.
        if( _add_remove_pt && _tracked_points_count < _max_tracked_points_count )  {
                list<added_point_data_t*>::iterator iter;

                for ( iter = _points_added_list.begin(); iter != _points_added_list.end(); iter++ ) {
                        added_point_data_t* data = (added_point_data_t*)(*iter);

                        // do this for all the points added [with motion they can be more than one at
                        // the same time]
                        _tracked_points[1][_tracked_points_count] = cvPointTo32f( data->point );

                        // increment the total number of points
                        _tracked_points_count++;

                        // let's add a tracking window!
                        this->add_tracking_window( _tracked_points_count - 1, 
                                                data->angle_int.angle_start, data->angle_int.angle_end );
        
                    cvFindCornerSubPix( _grey, _tracked_points[1] + _tracked_points_count - 1, 1,
                                cvSize( _win_size, _win_size ), cvSize( -1, -1 ),
                                cvTermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03) );

                        // remove the point freeing its data
                        delete data;
                }

                // remove the elements
                // we don't need the data* anymore. So throw it away
                _points_added_list.erase( _points_added_list.begin(), _points_added_list.end() );

                // ok, switch off
                _add_remove_pt = false;
    }

        CV_SWAP( _prev_grey, _grey, _swap_temp );
    CV_SWAP( _prev_pyramid, _pyramid, _swap_temp );
    CV_SWAP( _tracked_points[0], _tracked_points[1], _swap_points );
}



//-------------------------------------------------------------------------
// perform an add of one or more points. These will be stored in a linked list
// and processed in the next tracking loop

template <class ConverterT, class AngleT, class ImageT, class PointT>
bool OmniTracking<ConverterT, AngleT, ImageT, PointT>::add_point_to_track( CvPoint &point_added ) {

        if ( _tracked_points_count < _max_tracked_points_count ) {

                // create a new list object
                added_point_data_t *data = new added_point_data_t;

                // set the point
                data->point.x = point_added.x;
                data->point.y = point_added.y;

                // and calculate the point angle: it's relative to the omni image center.
                float rad_angle = 
                        atan2f( (float)point_added.y - (float)(this->m_conv_table->get_omni_center_y()),
                                    (float)point_added.x - (float)(this->m_conv_table->get_omni_center_x()));

                OmniAngle<double> angle_double (rad_angle - FACE_TRACK_ANGLE_WINDOW, 
                                                                                 rad_angle + FACE_TRACK_ANGLE_WINDOW,
                                                                                 2 * FACE_TRACK_ANGLE_WINDOW);
                this->m_conv_table->angle_type_conversion( angle_double, data->angle_int );


                // add it to the list
                _points_added_list.push_back( data );

                // add a wannabe trackable window
                this->add_wannabe_trackable_marks( data->angle_int.angle_start, 
                        data->angle_int.angle_end );

                //
        // we will check in the next loop if this point is a suitable one for tracking:
                // i.e. it has a minimum distance between the others tracked points
                // So here we just signal that a point has been added, a 
                // _tracked_points_count++ will eventually happen later.
                //

                _add_remove_pt = true;
        }

        return true;
}


template <class ConverterT, class AngleT, class ImageT, class PointT>
void OmniTracking<ConverterT, AngleT, ImageT, PointT>::register_image_callbacks( 
                                                                   on_create_image_context_cb* create_image_context,
                                                                   on_image_do_action_cb* image_show,
                                                                   on_destroy_image_context_cb* destroy_image_context,
                                                                   void* callback_data
                                                                  )
{
        this->create_image_context = create_image_context;
        this->image_show = image_show;
        this->destroy_image_context = destroy_image_context;
        this->_callback_data_on_image_cb = callback_data;
}


#endif

---