graphSegmentation_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

    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 <graphSegmentation_Filter.h>

#include <algorithm>
#include <cmath>

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

#define phGRAPH_USE_RANDOM_COLORS() 0

/* ---------------------------------------------------------------------------
 * Code in this file from "Efficient Graph Based Segmentation" source code
 *
 *  graphSegmentation_Filter::SegmentImage
 *  graphSegmentation_Filter::SegmentGraph
 *  graphSegmentation_Filter::ProcessGraph
 *  
 *  dist_rgb
 *
 *  class phUniverse
 *
 *  bool operator<(const ph_edge &a, const ph_edge &b) 
 *
 * 05/27/2007
 *  Code was edited by Philip Thoren (pthoren@cs.uml.edu)
 *  
 *  Formatted to my own crazy standards and optimized it for uint8_t images.
 *
 * ---------------------------------------------------------------------------
 * 
 * Copyright (C) 2006 Pedro Felzenszwalb
 * 
 * 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
 */
/* ------------------------------------------------------------------------ */
phUniverse::phUniverse( uint32_t elements ) 
{
    phFUNCTION("phUniverse::phUniverse")
    uint32_t i = 0;
    
    this->m_elts        = NULL;
    this->m_elts_size   = 0;

    if (elements > 0)
    {
        phDALLOC(this->m_elts,
                 this->m_elts_size,
                 elements,
                 ph_uni_elt);
        for (i = 0; i < elements; i++) 
        {
            this->m_elts[i].rank = 0;
            this->m_elts[i].size = 1;
            this->m_elts[i].p = i;
        }
    }

    this->m_elt_count   = elements;
    this->m_num         = elements;

error:
    return;
}

/* ------------------------------------------------------------------------ */
phUniverse::~phUniverse() 
{
    phFree(this->m_elts);
    this->m_elts_size   = 0;
    this->m_elt_count   = 0;
    this->m_num         = 0;
}

/* ------------------------------------------------------------------------ */
void phUniverse::resize( uint32_t elements )
{
    phFUNCTION("phUniverse::resize")
    if (elements > 0)
    {
        uint32_t i = 0;

        phDALLOC(this->m_elts,
                this->m_elts_size,
                elements,
                ph_uni_elt);
        for (i = 0; i < elements; i++) 
        {
            this->m_elts[i].rank = 0;
            this->m_elts[i].size = 1;
            this->m_elts[i].p = i;
        }

        this->m_elt_count   = elements;
        this->m_num         = elements;
    }
    
    return;
error:
    this->m_num         = 0;
    this->m_elt_count   = 0;
    this->m_elts_size   = 0;
}

/* ------------------------------------------------------------------------ */
int phUniverse::find(int x) 
{
    int y = x;
    
    while (y != this->m_elts[y].p)
    {
        y = this->m_elts[y].p;
    }
    
    this->m_elts[x].p = y;
    
    return y;
}

/* ------------------------------------------------------------------------ */
int phUniverse::size(int x) const 
{ 
    return this->m_elts[x].size; 
}

/* ------------------------------------------------------------------------ */
uint32_t phUniverse::num_sets() const 
{ 
    return this->m_num; 
}

/* ------------------------------------------------------------------------ */
void phUniverse::join(int x, int y)
{
    ph_uni_elt *elts = this->m_elts;
    
    /* compare the rank of x and y... */
    /* if the rank of x is greater than y, we'll merge y into x */
    if (elts[x].rank > elts[y].rank) 
    {
        /* Set the new parent of y */
        elts[y].p = x;
        /* Add the size of component y to new parent x */
        elts[x].size += elts[y].size;
    }
    else 
    {
        /* Set the new parent of x to y */
        elts[x].p = y;
        /* Add the size of component x to new parent y */
        elts[y].size += elts[x].size;
        /* increase parent y's rank */
        if (elts[x].rank == elts[y].rank)
            elts[y].rank++;
    }
    
    if (this->m_num > 0)    this->m_num--;
}

/* ---------------------------------------------------------------------- */
typedef float (*phDistFunctionType)( const uint8_t *ptr1,
                                     const uint8_t *ptr2 );

/* ---------------------------------------------------------------------- */
float dist_grey8(const uint8_t *ptr1,
                 const uint8_t *ptr2 )
{
    return ((float)ptr1[0]-(float)ptr2[0]);
}

/* ---------------------------------------------------------------------- */
float dist_hsv24(const uint8_t *ptr1,
                 const uint8_t *ptr2 )
{
    float hd = ((float)ptr1[0]-(float)ptr2[0]);
    float sd = ((float)ptr1[1]-(float)ptr2[1]);
    float vd = ((float)ptr1[2]-(float)ptr2[2]);
    return sqrt((hd*hd)+(sd*sd)+(vd*vd));
}

/* ---------------------------------------------------------------------- */
float dist_rgb24(const uint8_t *ptr1,
                 const uint8_t *ptr2 )
{
    float rd = ((float)ptr1[0]-(float)ptr2[0]);
    float gd = ((float)ptr1[1]-(float)ptr2[1]);
    float bd = ((float)ptr1[2]-(float)ptr2[2]);
    return sqrt((rd*rd)+(gd*gd)+(bd*bd));
}

/* ---------------------------------------------------------------------- */
graphSegmentation_Filter::graphSegmentation_Filter( uint32_t    min_size,
                                                    uint32_t    threshold,
                                                    int         color_blobs ):
    phFilter("graphSegmentation_Filter")

{
    //phImageValidFormatMask;
    this->m_format = phImageRGB24 | phImageHSV24 | phImageGREY8;

    this->m_num_edges   = 0;
    
    this->m_edges       = NULL;
    this->m_edges_size  = 0;
    
    this->m_output      = NULL;
    this->m_output_size = 0;

    this->m_thresholds      = NULL;
    this->m_thresholds_size = 0;

    this->m_universe    = new phUniverse(0);

    this->m_min_size    = min_size;
    this->m_thresh      = threshold;
    this->m_color_blobs = color_blobs;

    this->m_last_thresh = threshold == 0 ? 1 : 0;
    this->m_last_thresholds_size = 0;
}

/* ---------------------------------------------------------------------- */
graphSegmentation_Filter::~graphSegmentation_Filter()
{
    phFree(this->m_edges);
    this->m_edges_size = 0;
    phFree(this->m_output);
    this->m_output_size = 0;
    phFree(this->m_thresholds);
    this->m_thresholds_size = 0;
    phDelete(this->m_universe);
}

/* ------------------------------------------------------------------------ */
phFilter *graphSegmentation_Filter::cloneFilter()
{
    phFUNCTION("graphSegmentation_Filter::cloneFilter")
    int locked = 0;
    graphSegmentation_Filter *graphSegmentation = new graphSegmentation_Filter();
    
    phTHIS_LOOSE_LOCK(locked);
    
    /* graphSegmentation->set(this->m_value); */

    phTHIS_LOOSE_UNLOCK(locked);

    return (phFilter *)graphSegmentation;
}

/* ---------------------------------------------------------------------- */
int graphSegmentation_Filter::set( uint32_t   min_size,
                                   uint32_t   threshold,
                                   int        color_blobs )
{
    phFUNCTION("graphSegmentation_Filter::set")
    int locked = 0;
    phTHIS_LOOSE_LOCK(locked);
    
    this->m_min_size    = min_size;
    this->m_thresh      = threshold;
    this->m_color_blobs = color_blobs;

    phTHIS_LOOSE_UNLOCK(locked);
    
    return phSUCCESS;
}

/* ---------------------------------------------------------------------- */
int graphSegmentation_Filter::setMinSize( uint32_t min_size )
{
    phFUNCTION("graphSegmentation_Filter::set")
    int locked = 0;
    phTHIS_LOOSE_LOCK(locked);
    
    this->m_min_size = min_size;

    phTHIS_LOOSE_UNLOCK(locked);
    
    return phSUCCESS;
}

/* ---------------------------------------------------------------------- */
int graphSegmentation_Filter::setThreshold( uint32_t threshold )
{
    phFUNCTION("graphSegmentation_Filter::set")
    int locked = 0;
    phTHIS_LOOSE_LOCK(locked);
    
    this->m_thresh = threshold;

    phTHIS_LOOSE_UNLOCK(locked);
    
    return phSUCCESS;
}

/* ---------------------------------------------------------------------- */
int graphSegmentation_Filter::setColorBlobs( int color_blobs )
{
    phFUNCTION("graphSegmentation_Filter::set")
    int locked = 0;
    phTHIS_LOOSE_LOCK(locked);
    
    this->m_color_blobs = color_blobs;

    phTHIS_LOOSE_UNLOCK(locked);
    
    return phSUCCESS;
}
/* ---------------------------------------------------------------------- */
uint32_t graphSegmentation_Filter::getMinSize()
{
    phFUNCTION("graphSegmentation_Filter::set")
    int locked = 0;
    uint32_t min_size = 0;

    phTHIS_LOOSE_LOCK(locked);
    
    min_size = this->m_min_size;

    phTHIS_LOOSE_UNLOCK(locked);
    
    return min_size;

}

/* ---------------------------------------------------------------------- */
uint32_t graphSegmentation_Filter::getThreshold()
{
    phFUNCTION("graphSegmentation_Filter::set")
    int locked = 0;
    uint32_t threshold = 0;

    phTHIS_LOOSE_LOCK(locked);

    threshold = this->m_thresh;

    phTHIS_LOOSE_UNLOCK(locked);
    
    return threshold;

}
 
/* ---------------------------------------------------------------------- */
int graphSegmentation_Filter::getColorBlobs()
{
    phFUNCTION("graphSegmentation_Filter::set")
    int locked = 0;
    int color_blobs = 0;

    phTHIS_LOOSE_LOCK(locked);

    color_blobs = this->m_color_blobs;

    phTHIS_LOOSE_UNLOCK(locked);
    
    return color_blobs;
}
 
/* ---------------------------------------------------------------------- */
/* This algorithm was adapted from the "Efficient Graph Based Segmentation"
 * source code at:
 *                   http://people.cs.uchicago.edu/~pff/segment/
 *
 * It has been changed to work with a uint8_t buffer and use pointer arithmetic
 * to try and speed up the algorithm.
 *
 */
void graphSegmentation_Filter::SegmentImage( )
{
    ph_edge    *cur_edge = NULL;
    uint32_t num_edges= 0;

    phDistFunctionType distpixels = NULL;
    
          uint8_t *ptr= this->Image;
    const uint32_t w  = this->width;
    const uint32_t h  = this->height;
    const uint32_t d  = this->depth;

    register uint32_t x  = 0;
    register uint32_t y  = 0;
    register int a  = 0;
    //register int b  = 0;
    
    uint8_t *ptr_cur        = (uint8_t *)ptr;
    uint8_t *ptr_right      = NULL;
    uint8_t *ptr_down       = NULL;
    uint8_t *ptr_down_right = NULL;
    uint8_t *ptr_up_right   = NULL;

    switch (format)
    {
        case phImageRGB24:
            distpixels = dist_rgb24;
        break;
        case phImageHSV24:
            distpixels = dist_hsv24;
        break;
        case phImageGREY8:
            distpixels = dist_grey8;
        break;
    }
    
    this->m_num_edges   = 0;
    cur_edge            = this->m_edges;

    for (y = 0; y < h; y++)
    {
        ptr_right = &(ptr_cur[d]);
        if (y < (h-1))
        {
            ptr_down        = &(ptr_cur[w*d]);
            ptr_down_right  = &(ptr_cur[(w+1)*d]);
        }
        if (y > 0)
        {
            ptr_up_right = &(ptr[((((y-1) * w)+1) * d)]);
        }
        a = y * w;
        for (x = 0; x < w; x++, ptr_cur += d, a++)
        {
            /* right */
            if (x < w-1)
            {
                cur_edge->a = a;
                cur_edge->b = y * w + (x+1);
                cur_edge->w = (distpixels)(ptr_cur, ptr_right);
                
                cur_edge++;
                num_edges++;
                ptr_right += d;
            }

            /* down */
            if (y < h-1)
            {
                cur_edge->a = a;
                cur_edge->b = (y+1) * w + x;
                cur_edge->w = (distpixels)(ptr_cur, ptr_down);
                
                cur_edge++;
                num_edges++;
                ptr_down += d;
            }

            if ((x < w-1) && (y < h-1))
            {
                cur_edge->a = a;
                cur_edge->b = (y+1) * w + (x+1);
                cur_edge->w = (distpixels)(ptr_cur, ptr_down_right);

                cur_edge++;
                num_edges++;
                ptr_down_right += d;
            }

            if ((x < w-1) && (y > 0))
            {
                cur_edge->a = a;
                cur_edge->b = (y-1) * w + (x+1);
                cur_edge->w = (distpixels)(ptr_cur, ptr_up_right);

                cur_edge++;
                num_edges++;
                ptr_up_right += d;
            }
        }
    }

    this->m_num_edges   = num_edges;
}

/* ---------------------------------------------------------------------- */
bool operator<(const ph_edge &a, const ph_edge &b) 
{
    return a.w < b.w;
}

#define ph_GRAPHTHRESHOLD(size, c) (c/size)

/* ---------------------------------------------------------------------- */
int graphSegmentation_Filter::SegmentGraph()
{ 
    phFUNCTION("graphSegmentation_Filter::SegmentGraph")

    const uint32_t      num_vertices= (this->width * this->height);
    register uint32_t   i = 0;
    register int        a = 0;
    register int        b = 0;
    
    /* sort edges by weight */
    std::sort(this->m_edges, this->m_edges + this->m_num_edges);
    
    /* make a disjoint-set forest */
    this->m_universe->resize(num_vertices);

    /* init / alloc thresholds */
    phDALLOC(this->m_thresholds,
             this->m_thresholds_size,
             num_vertices,
             float);
    
    if ((this->m_thresh             != this->m_last_thresh          ) ||
        (this->m_thresholds_size    != this->m_last_thresholds_size ))
    {
        for (i = 0; i < num_vertices; i++)
        {
            this->m_thresholds[i] = ph_GRAPHTHRESHOLD(1,this->m_thresh);
        }
        this->m_last_thresh             = this->m_thresh;
        this->m_last_thresholds_size    = this->m_thresholds_size;
    }

    /* for each ph_edge, in non-decreasing weight order... */
    for (i = 0; i < this->m_num_edges; i++) 
    {
        ph_edge *pedge = &(this->m_edges[i]);

        /* components connected by this ph_edge */
        a = this->m_universe->find(pedge->a);
        b = this->m_universe->find(pedge->b);
        if (a != b) 
        {
            if ((pedge->w <= this->m_thresholds[a]) && 
                (pedge->w <= this->m_thresholds[b])) 
            {
                this->m_universe->join(a, b);
                
                a = this->m_universe->find(a);
                
                this->m_thresholds[a] = pedge->w + 
                    ph_GRAPHTHRESHOLD(this->m_universe->size(a),
                                      this->m_thresh);
            }
        }
    }

    return phSUCCESS;
error:
    return phFAIL;
}

/* ---------------------------------------------------------------------- */
/* 
 * This is the code that actually segments the graph from the output 
 * generated in the graphSegmentation_Filter::SegmentImage method.
 *
 * This is also take from the original segment_image function from the
 * "Efficient Graph Based Segmentation" source code.
 *
 */
void graphSegmentation_Filter::ProcessGraph( )
{
    register uint32_t       i           = 0;
    register uint32_t       y;
    uint8_t                *outptr      = this->m_output;
    uint32_t                d           = this->depth;
    register int            a           = 0;
    register int            b           = 0;
    const uint32_t          lim         = this->width * this->height;

    phUniverse             *u           = this->m_universe;

    // post process small components
    for (i = 0; i < this->m_num_edges; i++)
    {
        a = u->find(m_edges[i].a);
        b = u->find(m_edges[i].b);
        if ((a != b) && 
            ((u->size(a) < this->m_min_size) || 
             (u->size(b) < this->m_min_size)))
        {
            u->join(a, b);
        }
    }
    
    #if phGRAPH_USE_RANDOM_COLORS()
    uint8_t *colors = new uint8_t[w*h*d];
    uint8_t *col_ptr= colors;
    for (y = 0; y < lim; y++ )
    {
        for (x = 0; x < d; x++ )
        {
            colors[x+y] = (uint8_t)random();
        }
    }
    #endif

    /* Edited from the original code that colors the blobs in that this 
     * colors the image for an RGB image represented by a uint8_t buffer */
    if (this->m_color_blobs)
    {
        for (y = 0; y < lim; y++, outptr += d )
        {
            phMemcpy(outptr,&(this->Image[u->find(y) * d]),d);

            #if phGRAPH_USE_RANDOM_COLORS()
            comp = u->find(y);
            phMemcpy(outptr,&(colors[comp*d]),d);
            #endif
        }
    }

    #if phGRAPH_USE_RANDOM_COLORS()
    delete [] colors;
    #endif
}

/* ---------------------------------------------------------------------- */
int graphSegmentation_Filter::filter()
{
    phFUNCTION("graphSegmentation_Filter::filter")
   
    /* defined before being called:
     *  width, height, depth, format, Image
     */
    
    /* Begin Filter */
    phDALLOC(this->m_output,this->m_output_size,
             this->width*this->height*this->depth,
             uint8_t);

    phDALLOC(this->m_edges,this->m_edges_size,
             this->width*this->height*4,
             ph_edge)

    /* segment the image : populates this->m_edges */
    this->SegmentImage( );
    /* segment : Populates this->m_universe */
    this->SegmentGraph();
    /* process the graph : colors the image */
    this->ProcessGraph( );

    this->m_workspaceImage->setImage( this->width,
                                      this->height,
                                      this->format,
                                      0,
                                      this->m_output );
    
    /* End Filter */
    
    /* make sure to free up any space here, not including Image */
    
    return phSUCCESS;
    
error:
    /* make sure to free up any space here, not including Image */
    
    return phFAIL;
    
}

Copyright (C) 2002 - 2007

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

---