blobify_Filter.cpp

Go to the documentation of this file.

/* ---------------------------------------------------------------------------
    Phission :
        Realtime Vision Processing System

    Copyright (C) 2003-2006 Philip D.S. Thoren (pthoren@cs.uml.edu)
    University of Massachusetts at Lowell,
    Laboratory for Artificial Intelligence and Robotics

 ---------------------------------------------------------------------------
    <Add other copyrights here> 
 ---------------------------------------------------------------------------

    This file is part of Phission.

    Phission is free software; you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    Phission 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 Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    along with Phission; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

 ---------------------------------------------------------------------------*/
#ifdef HAVE_CONFIG_H
    #include <phissionconfig.h>
#endif

#include <phStandard.h>

#include <blobify_Filter.h>

#include <phError.h>
#include <phMemory.h>
#include <phPrint.h>

#define TIMESTUFF() 0

/* ---------------------------------------------------------------------- */
#ifdef __cplusplus
extern "C" 
{
#endif
/* ---------------------------------------------------------------------- */
/* ----------------------- Blob Functions ------------------*/
/* ---------------------------------------------------------------------- */
static blobify_blob_type *initBlob( blobify_blob_type *b, unsigned int y, unsigned int x )
{
    b->mass = 1;
    b->ul.x=x;
    b->ul.y=y;
    b->lr.x=x;
    b->lr.y=y;
    b->cm.x=x;
    b->cm.y=y;
    b->next = 0;

    /* last is used for the returned results */
    b->last = 0;
    
    return (b);
}

/* ---------------------------------------------------------------------- */
static blobify_blob_type *addPixel( blobify_blob_type *b, unsigned int y, unsigned int x )
{
    if( x < b->ul.x )
        b->ul.x = x;  
    if( x > b->lr.x )
        b->lr.x = x;
    if( y < b->ul.y )
        b->ul.y = y;
    if( y > b->lr.y )
        b->lr.y = y;
    /* not correct */
    /*b->cm.x =( (float)(b->mass * b->cm.x + x) / (float)(b->mass+1) );
     * b->cm.y =( (float)(b->mass * b->cm.y + y) / (float)(b->mass+1) );*/
    b->mass++;

    return (b);
}

/* ---------------------------------------------------------------------- */
static void joinBlob( blobify_blob_type *self, blobify_blob_type *other )
{
    if(self->mass != 0 && other->mass != 0)
    {
        if( other->ul.x < self->ul.x )
            self->ul.x = other->ul.x;

        if( other->lr.x > self->lr.x )
            self->lr.x = other->lr.x ;

        if( other->ul.y < self->ul.y )
            self->ul.y = other->ul.y;

        if( other->lr.y > self->lr.y )
            self->lr.y = other->lr.y;

        /* Not Correct */
        /*
         * self->cm.x=( (self->mass * self->cm.x + other->mass * other->cm.x )/
         * (self->mass + other->mass));
         * self->cm.y=( (self->mass * self->cm.y + other->mass * other->cm.y)/
         * (self->mass + other->mass));
         * */ 
        self->mass += other->mass;      
        other->mass = 0;
    }
}

/* ---------------------------------------------------------------------- *
void blobify_Filter::deleteBlob( blobify_blob_type *b )
{
    * pthoren - What the? WHY IS THIS BEING DONE? It makes no SENSE! *
    * Where are the comments? :-) *
    *
    b->cm.x = width / 2;
    b->cm.y = height / 2;
    b->ul.x = width;
    b->ul.y = height;
    b->lr.x = 0;
    b->lr.x = 0;
    b->mass = 0;

}
*/
/* ---------------------------------------------------------------------- */
static int getBlobWidth( blobify_blob_type *b )
{
    return( b->lr.x - b->ul.x );
}

/* ---------------------------------------------------------------------- */
static int getBlobHeight( blobify_blob_type *b )
{
    return( b->lr.y - b->ul.y );
}

/* ---------------------------------------------------------------------- */
static int getBlobArea( blobify_blob_type *b )
{
    return( getBlobWidth( b ) * getBlobHeight( b ) );
}

/* not correct, 1 pixel is very dense.  */
/*
 * float getBlobDensity( blobify_blob_type *b )
 * {
 * return( (float)b->mass / (float)getBlobArea( b ) );
 * }
 * */


/* ---------------------------------------------------------------------- */
static void sortBlobs(  int             sortMethod, 
                        blobify_blob_type       bloblist[], 
                        unsigned int    indexes[], 
                        unsigned int    size )
{
    unsigned int i,j = 0;
    unsigned int rankTable[blobify_MAXBLOBS];


    for( i = 0; i < blobify_MAXBLOBS; i++) rankTable[i] = i;

    switch(sortMethod)
    {

        case 0:/* Mass */
            for( i = 1; i < size + 1; i++)
            {
                for(j = i + 1; j < blobify_MAXBLOBS; j++)
                {
                    if(bloblist[rankTable[i]].mass < bloblist[rankTable[j]].mass)
                    {
                        phSWAP(rankTable[i],rankTable[j]);
                    }
                }
            }
            break;

        case 1: /* Area */
            for( i = 1; i < size + 1; i++ )
            {
                for( j = i + 1; j < blobify_MAXBLOBS; j++ )
                {
                    /* automattically swap out 0 mass from i spot*/
                    if ((bloblist[rankTable[i]].mass == 0) 
                        ||
                        ((bloblist[rankTable[j]].mass != 0) 
                         &&
                         (getBlobArea(&bloblist[rankTable[i]]) <
                          getBlobArea(&bloblist[rankTable[j]]))
                         )
                        )
                    {
                        phSWAP(rankTable[i],rankTable[j]); 
                    }
                }
            }
            break;

    }

    for( i = 1; i < (size+1); i++ )
        indexes[i-1]= rankTable[i];

}
/* ---------------------------------------------------------------------- */
#ifdef __cplusplus
} /* extern "C" */
#endif


/* ---------------------------------------------------------------------- */
blobify_Filter::blobify_Filter(int          inChannel, 
                               int          low, 
                               int          high, 
                               int          sortmethod, 
                               unsigned int size, 
                               int          drawBox) :
    phFilter("blobify_Filter")
{
    unsigned int n = 0;

    phMemset(&(this->m_bloblist[n]),0,sizeof(blobify_blob_type)*blobify_MAXBLOBS);
    
    this->m_format = phImageRGB24;

    this->m_result_size         = 0;
    this->m_result_copy_size    = 0;
    this->m_result_array        = NULL;
    this->m_result_copy         = NULL;
    this->m_result_new          = 0;
    this->m_result_inited       = 0;
    
    this->m_blobdata                = NULL;
    this->m_blobdata_size           = 0;
    this->m_blobdataWidth           = 0;
    this->m_blobdataHeight          = 0;
    
    this->m_blobdata_sizes          = NULL;
    this->m_blobdata_sizes_size     = 0;

    this->set( inChannel, low, high, sortmethod, size, drawBox);
}

/* ---------------------------------------------------------------------- */
blobify_Filter::~blobify_Filter()
{
    phFUNCTION("blobify_Filter::~blobify_Filter")
    int         locked  = 0;
    uint32_t    i       = 0;
    
    phTHIS_LOOSE_LOCK(locked);
    
    phFree(this->m_result_array);
    phFree(this->m_result_copy);
    for (i = 0; i < this->m_blobdataWidth; i++ )
    {
        phFree(this->m_blobdata[i]);
    }
    phFree(this->m_blobdata);
    phFree(this->m_blobdata_sizes);
    this->m_result_size         = 0;
    this->m_result_copy_size    = 0;
    this->m_blobdata_size       = 0;
    this->m_blobdata_sizes_size = 0;

    phTHIS_LOOSE_UNLOCK(locked);
}

/* ------------------------------------------------------------------------ */
phFilter *blobify_Filter::cloneFilter()
{
    phFUNCTION("blobify_Filter::cloneFilter")
    int locked = 0;
    blobify_Filter *blob = new blobify_Filter( );
    
    phTHIS_LOOSE_LOCK(locked);
    
    blob->set( this->m_inChannel, 
               this->m_low, 
               this->m_high, 
               this->m_sortmethod, 
               this->m_size, 
               this->m_drawBox  );

    phTHIS_LOOSE_UNLOCK(locked);

    return (phFilter *)blob;
}

/* ---------------------------------------------------------------------- */
int blobify_Filter::set(int             inChannel, 
                        int             low,
                        int             high, 
                        int             sortmethod, 
                        unsigned int    size,
                        int             drawBox )
{
    phFUNCTION("blobify_Filter::set")
    int locked = 0;

    phTHIS_LOOSE_LOCK(locked);
    
    this->m_inChannel   = inChannel;
    this->m_low         = low;
    this->m_high        = high;
    this->m_sortmethod  = sortmethod;
    this->m_size        = size;
    this->m_drawBox     = drawBox;

    phTHIS_LOOSE_UNLOCK(locked);

    return phSUCCESS;
}

/* ---------------------------------------------------------------------- */
/* getResult: this can either return a safe-enough array that can
 * be looked at by one thread safely; or it returns NULL */
/* ---------------------------------------------------------------------- */
blobify_blob_type *blobify_Filter::getResult()
{
    phFUNCTION("blobify_Filter::blobify")
    int r_locked = 0;

    phMUTEX_LOCK(this->m_result_lock,r_locked);

    if (this->m_result_inited == 0)
    {
        phMUTEX_UNLOCK(this->m_result_lock,r_locked);

        while (this->m_result_inited == 0)
        {
            phYield();
        }

        phMUTEX_LOCK(this->m_result_lock,r_locked);
    }

    /* We'll have the lock here */
    /* If the result is new, then copy it into the copy array */
    if (this->m_result_new == 1)
    {
        phDALLOC_RESIZE(this->m_result_copy,
                        this->m_result_copy_size,
                        (this->m_result_size / sizeof(blobify_blob_type)),
                        blobify_blob_type);
        phMemcpy(this->m_result_copy,this->m_result_array,
                 this->m_result_copy_size);
    }
    
error:
    phMUTEX_ERROR_UNLOCK(this->m_result_lock,r_locked);
    
    return this->m_result_copy;
}

/* ---------------------------------------------------------------------- */
int blobify_Filter::filter()
{
    phFUNCTION("blobify_Filter::blobify")
    
    
    unsigned int w,h,i,j,n,k = 0;
    int offset, mark1, mark2;
    int count;
    /*int minBlobNum = 0;*/
    /*int maxBlobNum = 0;*/

    unsigned int maxIndex[blobify_MAXBLOBS] = {0};

    uint8_t *ImagePtr = NULL;
   
    unsigned long row = 0;
    unsigned long pixel = 0;
     
#if TIMESTUFF()
    const uint32_t  nStamps = 3;
    char            *tags[nStamps] = 
    {
        "matching & create blobs",
        "sortBlobs",
        "draw blob rects"
    };
    ph_time_db      timedb = NULL;
    rc = ph_timedb_alloc(&timedb,"blobify_Filter");
    phPRINT_RC(rc,NULL,"ph_timedb_alloc");
#endif /* TIMESTUFF() */

    /* Begin filter */

    /* Make sure the allocations are set correctly */
    if ((this->m_blobdata == NULL) || 
        (this->m_blobdataWidth != width) ||
        (this->m_blobdataHeight != height))
    {
        if ((this->m_blobdataWidth < width) &&
            (this->m_blobdata != NULL))
        {
            for ( i = width - 1; i < this->m_blobdataWidth; i++ )
            {
                phFree(this->m_blobdata[i]);
            }           
        }
        phDALLOC_RESIZE(this->m_blobdata,
                        this->m_blobdata_size,
                        width,
                        unsigned int *);
        phDALLOC_RESIZE(this->m_blobdata_sizes,
                        this->m_blobdata_sizes_size,
                        width,
                        unsigned int);
        for (w = 0; w < width; w++) 
        {
            phDALLOC_RESIZE(this->m_blobdata[w],
                            this->m_blobdata_sizes[w],
                            height,
                            unsigned int);
        }
        
        this->m_blobdataWidth = width;
        this->m_blobdataHeight = height;
    }
    
    /* Always need to initilize to zero*/
    for (w = 0; w < width; w++) 
    {
        phMemset(this->m_blobdata[w],0,height*sizeof(int));
    }

    if(this->m_inChannel == phCHANNEL_BLUE)
    {
        offset = phCHANNEL_BLUE; 
        mark1 = phCHANNEL_RED; 
        mark2 = phCHANNEL_GREEN;
    }
    else if(this->m_inChannel == phCHANNEL_GREEN)
    {
        offset = phCHANNEL_GREEN; 
        mark1 = phCHANNEL_RED; 
        mark2 = phCHANNEL_BLUE;
    }
    else if(this->m_inChannel == phCHANNEL_RED)
    {
        offset = phCHANNEL_RED; 
        mark1 = phCHANNEL_GREEN; 
        mark2 = phCHANNEL_BLUE;
    }
    else
    {
        phCHECK_RC(-1,NULL,"Invalid Channel.");
    }

    count = 1;

    ImagePtr = (uint8_t *)(Image);

#if TIMESTUFF()
    rc = ph_timedb_start(timedb,tags[0]);
    phPRINT_RC(rc,NULL,"ph_timedb_start");
#endif
    /*build the blobmap and construct unjoined blobify_blob_type objects*/
    for( h = 0; h < height; h++ )
    {
        for(w = 0; w < width; w++, ImagePtr += depth)
        {
            if ((*(ImagePtr+offset) >= this->m_low) && 
                (*(ImagePtr+offset) <= this->m_high) )
            {  
                /*in upper left corner - new blob */
                if(h == 0 && w == 0 && count < blobify_MAXBLOBS)
                { 
                    initBlob(&this->m_bloblist[count],h,w);
                    this->m_blobdata[w][h]= count;
                    count++;
                }
                /* if in first col */
                else if(w == 0)
                {
                    if( this->m_blobdata[w][h-1] != 0 )
                    {
                        addPixel(&this->m_bloblist[this->m_blobdata[w][h-1]],h,w);
                        this->m_blobdata[w][h]=this->m_blobdata[w][h-1];
                    }
                    /*above is off -- new blob*/
                    else if (count < blobify_MAXBLOBS) 
                    {
                        initBlob(&this->m_bloblist[count], h,w);                      
                        this->m_blobdata[w][h]=count;
                        count++;
                    }
                }
                /* in first row */
                else if(h == 0)
                {
                    if( this->m_blobdata[w-1][h] != 0 )
                    {
                        addPixel(&this->m_bloblist[this->m_blobdata[w-1][h]],h,w);
                        this->m_blobdata[w][h]= this->m_blobdata[w-1][h];
                    }
                    /* left is off -- new blob */
                    else if (count < blobify_MAXBLOBS)
                    {
                        initBlob(&this->m_bloblist[count], h,w);
                        this->m_blobdata[w][h]=count;
                        count++;
                    }                    
                }

                else if( this->m_blobdata[w-1][h] != 0 && this->m_blobdata[w][h-1] != 0 )
                {
                    /*
                     * see if the pixel to left and on the top are the same blob and add 
                     * this new pixel to the blob if they are 
                     * */
                    if(this->m_blobdata[w-1][h] == this->m_blobdata[w][h-1])
                    {
                        addPixel(&this->m_bloblist[this->m_blobdata[w-1][h]],h,w);
                        this->m_blobdata[w][h] = this->m_blobdata[w-1][h];
                    }
                    else 
                    {
                        addPixel(&this->m_bloblist[this->m_blobdata[w-1][h]],h,w);                      
                        joinBlob(&this->m_bloblist[this->m_blobdata[w-1][h]],
                                &this->m_bloblist[this->m_blobdata[w][h-1]]);
                        this->m_blobdata[w][h] = this->m_blobdata[w-1][h];
                        
                        n = this->m_blobdata[w][h-1];                      
                        for( i = 0; i <= h; i++ )
                        {
                            for( j = 0; j < width; j++ )
                            {
                                if(this->m_blobdata[j][i] == n)
                                {
                                    this->m_blobdata[j][i] = this->m_blobdata[w-1][h];
                                }
                            }
                        }

                        /*deleteBlob(&this->m_bloblist[this->m_blobdata[w][h-1]]);*/
                    }
                }
                else
                {
                    if( this->m_blobdata[w-1][h] != 0 )
                    {
                        addPixel(&this->m_bloblist[this->m_blobdata[w-1][h]],h,w);                  
                        this->m_blobdata[w][h]=this->m_blobdata[w-1][h];                      
                    }
                    /*top is on -- old blob */
                    else if( this->m_blobdata[w][h-1] != 0 )
                    {                      
                        addPixel(&this->m_bloblist[this->m_blobdata[w][h-1]],h,w);
                        this->m_blobdata[w][h]=this->m_blobdata[w][h-1];
                    }
                    else if (count < blobify_MAXBLOBS) /* neither left or top on. -- new blob.*/
                    {
                        initBlob(&this->m_bloblist[count],h,w);                      
                        this->m_blobdata[w][h]=count;                      
                        count++;
                    }
                }
            }
        }
    }
#if TIMESTUFF()
    rc = ph_timedb_stop(timedb);
    phPRINT_RC(rc,NULL,"ph_timedb_stop");
#endif

#if TIMESTUFF()
    rc = ph_timedb_start(timedb,tags[1]);
    phPRINT_RC(rc,NULL,"ph_timedb_start");
#endif
    sortBlobs(this->m_sortmethod, this->m_bloblist, maxIndex, this->m_size);
#if TIMESTUFF()
    rc = ph_timedb_stop(timedb);
    phPRINT_RC(rc,NULL,"ph_timedb_stop");
#endif

#if TIMESTUFF()
    rc = ph_timedb_start(timedb,tags[2]);
    phPRINT_RC(rc,NULL,"ph_timedb_start");
#endif
    if(this->m_drawBox)
    {
        for( i = 0; i < height; i++ )
        {
            row = i * width;
            for( j = 0; j < width; j++ )
            {
                pixel = (row + j) * depth;
                for( k = 0; k < this->m_size; k++ ) 
                {
                    if(this->m_blobdata[j][i] == maxIndex[k]) {
                        (Image)[pixel + offset] = 0;
                        (Image)[pixel + mark1] = 255;
                        (Image)[pixel + mark2] = 0;
                    }
                    if(this->m_bloblist[maxIndex[k]].mass > 0 )
                    {
                        if (
                            (
                             (j >= this->m_bloblist[maxIndex[k]].ul.x && 
                              j <= this->m_bloblist[maxIndex[k]].lr.x) 
                             &&
                             (i == this->m_bloblist[maxIndex[k]].ul.y || 
                              i == this->m_bloblist[maxIndex[k]].lr.y)
                            )||
                            (
                             (j == this->m_bloblist[maxIndex[k]].ul.x || 
                              j == this->m_bloblist[maxIndex[k]].lr.x) 
                             &&
                             (i >= this->m_bloblist[maxIndex[k]].ul.y && 
                              i <= this->m_bloblist[maxIndex[k]].lr.y)
                            )
                           )
                        {
                            (Image)[pixel + offset] = 255;
                            (Image)[pixel + mark1] = 255;
                            (Image)[pixel + mark2] = 255;
                        }
                    }
                }
            }
        }
    }      
#if TIMESTUFF()
    rc = ph_timedb_stop(timedb);
    phPRINT_RC(rc,NULL,"ph_timedb_stop");
#endif

    rc = this->m_result_lock.lock();
    phPRINT_RC(rc,NULL,"this->m_result_lock.lock()");

    /* Make sure there is memory for the results to be placed... */    
    phDALLOC(this->m_result_array,
             this->m_result_size,
             this->m_size,
             blobify_blob_type);
    
    /* Copy the results into the array */
    for (i = 0; i < this->m_size; i++) 
    {
        this->m_result_array[i] = this->m_bloblist[maxIndex[i]];
    }
    if (this->m_size > 0)
    {
        this->m_result_array[this->m_size - 1].last = 1;
    }
    /* Let the output function know there is new data to be copied */
    this->m_result_new = 1;
    /* this should happen once, sure it's a waste of cycles after the 
     * fact but it makes coding the loop that waits for the 
     * result data(to exist) easier to code. */
    if (this->m_result_inited == 0) this->m_result_inited = 1;

    rc = this->m_result_lock.unlock();
    phPRINT_RC(rc,NULL,"this->m_result_lock.unlock()");

#if TIMESTUFF() /* phTIME_REPORT_ALL & */
    rc = ph_timedb_report( timedb, phTIME_REPORT_SUM );
    phPRINT_RC(rc,NULL,"ph_timedb_report");
    
    if (timedb)
    {
        rc = ph_timedb_free(&timedb);
        phPRINT_RC(rc,NULL,"ph_timedb_free");
    }
#endif
    return phSUCCESS; 
error:

#if TIMESTUFF() /* phTIME_REPORT_ALL & */
    if (timedb)
    {
        rc = ph_timedb_free(&timedb);
        phPRINT_RC(rc,NULL,"ph_timedb_free");
    }
#endif

    return phFAIL;
}  

Copyright (C) 2002 - 2007

Philip D.S. Thoren ( pthoren@users.sourceforge.net ) University Of Massachusetts at Lowell Robotics Lab

---