FLIROverlayExample.cpp

Go to the documentation of this file.

/* ---------------------------------------------------------------------------
    Phission : 
        Realtime Vision Processing System
    
    Copyright (C) 2003 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.

 ---------------------------------------------------------------------------*/
#include <FLIROverlayExample.h>
#include <stdio.h>
#include <math.h>
#include <phission.h>

/* ------------------------------------------------------------------------ */
int glbl_disable_displays   = 0;
int glbl_save_images        = 0;


/* ------------------------------------------------------------------------ */
int usage()
{
    printf("\n\n\tUsage:\n");
    printf("\t\t\t--help\t\t\tdisplay usage\n");
    printf("\t\t\t--nodisplay\tdisable the allocation, opening or any use of a display.\n");
    printf("\t\t\t--saveimages\tsave the frames to file\n");
    printf("\t\t\t--test <value>\tload 'value' number of frames/files/images\n");
    printf("\t\t\t--file <avi_data_file>\t\t\tavi source (required)\n");
    printf("\n\n");
    exit(1);
}
/* ------------------------------------------------------------------------ */
class key_matte_threshold_Filter : public phFilter
{
private:
    int m_value;
   
protected:
    int filter();

public:
    key_matte_threshold_Filter(int value = 0);
    ~key_matte_threshold_Filter();
    
    int set(int value = 0);
    
    phFilter *cloneFilter();

};


/* ---------------------------------------------------------------------- */
key_matte_threshold_Filter::key_matte_threshold_Filter(int value) :
    phFilter("key_matte_threshold_Filter")

{
    this->m_format = (phImageGREY8);

    this->set(value);
}

/* ---------------------------------------------------------------------- */
key_matte_threshold_Filter::~key_matte_threshold_Filter()
{
}

/* ------------------------------------------------------------------------ */
phFilter *key_matte_threshold_Filter::cloneFilter()
{
    return (phFilter *)new key_matte_threshold_Filter(     this->m_value
                );
}

/* ---------------------------------------------------------------------- */
int key_matte_threshold_Filter::set(int value)
{
    this->m_value = value;
    
    return phSUCCESS;
}

/* ---------------------------------------------------------------------- */
int key_matte_threshold_Filter::filter()
{
    phFUNCTION("key_matte_threshold_Filter::filter")
    
    unsigned int    row = 0;
    unsigned int    pixel = 0;
    unsigned int    index = 0; /* r:0 g:1 b:2 */
    const uint32_t  ai = 3;
    uint32_t        pv = 0;
    static float    scale_log_range_factor  = 255 / log(255);
    
    unsigned int x, y, d = 0;
   
    /* Begin filter */
    rc = this->m_workspaceImage->convert(phImageRGBA32);
    phPRINT_RC(rc,NULL,"Image->convert(outputFormat) failed.");

    /* rc == 0 means conversion happened */
    /* rc == 1 means no conversion */
    if (rc == phSUCCESS)
    {
        rc = this->setFilterVars();
        phPRINT_RC(rc,NULL,"setFilterVars");
    }

    for ( y = 0; y < height; y++ )
    {
        row = y * width;
        for ( x = 0; x < width; x++ ) 
        {
            pixel = (x+row)*depth;
            pv = (Image)[pixel+index];
            for (d = 0; d < depth; d++)
            {
                if (d != index) (Image)[pixel+d] = 0;
            }

            if (pv < this->m_value)
            {
                (Image)[pixel+ai/* alpha channel = 3 */] = 0;
            }
            else
            {
                pv -= this->m_value;
                //pv *= pv;
                pv = log(pv) * scale_log_range_factor;
                //pv += 100;
                //if (pv > 0) pv /= 255;
                if (pv > 255) pv = 255;
                (Image)[pixel+ai/* alpha channel = 3 */] = pv; /* 185 */
            }
        }
    }
    
    return phSUCCESS;
}


/* ------------------------------------------------------------------------ */
class invert_Filter : public phFilter
{
private:
    uint8_t *m_pixelbuf;
    uint32_t m_pixelbuf_size;

protected:
    int filter();

public:
    invert_Filter();
    ~invert_Filter();
    
    phFilter *cloneFilter();


};

/* ---------------------------------------------------------------------- */
/*
  
(int channel, int value) */
/* ---------------------------------------------------------------------- */
invert_Filter::invert_Filter() :
    phFilter("invert_Filter")

{
    this->m_format = phImageValidFormatMask;
    this->m_pixelbuf = NULL;
    this->m_pixelbuf_size = 0;
}

/* ---------------------------------------------------------------------- */
invert_Filter::~invert_Filter()
{
}

/* ------------------------------------------------------------------------ */
phFilter *invert_Filter::cloneFilter()
{
    return (phFilter *)new invert_Filter();
}

/* ---------------------------------------------------------------------- */
int invert_Filter::filter()
{
    phFUNCTION("invert_Filter::filter")
     
    uint8_t *ptr = NULL;
    uint8_t *ptr2 = NULL;
    uint32_t pixel_half = (width * height) / 2;
    /* defined before being called:
     *  width
     *  height
     *  depth
     *  Image
     */
    
    phDALLOC(this->m_pixelbuf,
             this->m_pixelbuf_size,
             depth,
             uint8_t);

    /* Begin Filter */
    ptr = Image;
    ptr2 = Image + ((width * height * depth ) - (depth));
    for (int32_t pixel = 0; pixel < pixel_half; pixel++ , ptr += depth, ptr2 -= depth)
    {
        phMemcpy(this->m_pixelbuf,ptr2,depth * sizeof(uint8_t));
        phMemcpy(ptr2,ptr,depth * sizeof(uint8_t));
        phMemcpy(ptr,this->m_pixelbuf,depth * sizeof(uint8_t));
    }
    
    /* End Filter */
    
    /* make sure to free up any space here, not including Image */
    
    return phSUCCESS;
    
/* Comment this back in if you use any macros that jump to here, ie phCHECK_RC,
 * or phCHECK_PTR */

error:
    /* make sure to free up any space here, not including Image */
    
    return phFAIL;
    
}


/* ------------------------------------------------------------------------ */
class phEffectsThread : public phThread
{
private:
    phImage *m_input[2];
    uint32_t m_input_count;
    
    float   m_alpha;

    uint32_t m_output_format;
    phImage *m_output;

    uint8_t *m_buf;
    uint32_t m_buf_size;

protected:
    int run();
    int wakeup();

public:
    phEffectsThread( phLiveObject *one, phLiveObject *two, uint32_t output_format );
    ~phEffectsThread();

    int mosaic_images( phImage *one, phImage *two, phImage *out);
    int overlay_images( phImage *one, phImage *two, 
                        phImage *out,
                        int32_t move_x,
                        int32_t move_y );

    phLiveObject *getLiveSourceOutput() { return this->m_output; };
};

/* ------------------------------------------------------------------------ */
phEffectsThread::phEffectsThread( phLiveObject *one, 
                                phLiveObject *two, 
                                uint32_t output_format )
{
    phFUNCTION("phEffectsThread::phEffectsThread")
    int locked = 0;

    phTHIS_LOOSE_LOCK(locked);
    
    this->setName("phEffectsThread");
    this->m_input[0] = (phImage *)one;
    this->m_input[1] = (phImage *)two;
    this->m_input_count = 2;

    this->m_alpha = 0.00;

    this->m_output_format = output_format;

    this->m_output = new phImage();

    this->m_buf = NULL;
    this->m_buf_size = 0;

    phTHIS_LOOSE_UNLOCK(locked);
}

/* ------------------------------------------------------------------------ */
phEffectsThread::~phEffectsThread()
{
    phFree(this->m_buf);
    this->m_buf_size = 0;
    phDelete(this->m_output);
}

/* ------------------------------------------------------------------------ */
int phEffectsThread::mosaic_images( phImage *one, phImage *two, phImage *out )
{
    phFUNCTION("phEffectsThread::mosaic_images")
    int locked = 0;
    uint8_t *ptr[2];
    uint32_t height[2];
    uint32_t width[2];
    uint32_t offset;
    uint32_t output_height = 0;
    uint32_t output_width = 0;

    uint32_t i = 0;
    uint32_t j = 0;

    uint32_t depth = 0;

    if ((one == NULL) ||
        (two == NULL) ||
        (one->isNull()) || 
        (two->isNull()))
    {
        return phFAIL;
    }
    
    phTHIS_LOCK(locked);

    /* Convert the input images to one color space */
    rc = one->convert(this->m_output_format);
    phPRINT_RC(rc,NULL,"one->convert(%s)",
            phImageFormatToString(this->m_output_format));
    
    rc = two->convert(this->m_output_format);
    phPRINT_RC(rc,NULL,"two->convert(%s)",
            phImageFormatToString(this->m_output_format));
   
    /* Make sure our buffer is large enough */ 
    phDALLOC_RESIZE(this->m_buf,
                    this->m_buf_size,
                    one->getSize() + two->getSize(),
                    uint8_t );
    
    depth = phImageFormatToDepth(this->m_output_format);
    
    /* Get the pointers to the image only after the conversions */
    ptr[0] = (uint8_t*)one->getImage();
    ptr[1] = (uint8_t*)two->getImage();
  
    height[0] = one->getHeight();
    height[1] = two->getHeight();

    width[0] = one->getWidth();
    width[1] = two->getWidth();

    output_height = (height[0] > height[1] ? height[0] : height[1]);
    output_width = width[0] + width[1];

    offset = 0;
    for (j = 0; j < this->m_input_count; j++)
    {
        for (i = 0; i < height[j]; i++ )
        {
            phMemcpy(&(this->m_buf[((i * output_width) + offset) * depth]),
                     &(ptr[j][i * width[j] * depth]),
                     width[j] * depth);
        }
        offset += width[j];
    }

    rc = out->setImage(output_width,
                       output_height,
                       this->m_output_format,
                       this->m_buf_size,
                       this->m_buf);
    phCHECK_RC(rc,NULL,"out->setImage");
    
    phTHIS_UNLOCK(locked);

    return phSUCCESS;
error:

    phTHIS_ERROR_UNLOCK(locked);
    
    return phFAIL;
}

/* ------------------------------------------------------------------------ */
int phEffectsThread::overlay_images( phImage *one, phImage *two, 
                                     phImage *out,
                                     int32_t move_x,
                                     int32_t move_y )
{
    phFUNCTION("phEffectsThread::overlay_images")
    int locked = 0;
    
    float alpha = 0.0;
    float alphainv = 0.0;
    const int32_t ai = 3;
    
    float tmp = 0;
    uint8_t *outptr         = NULL;
    uint8_t *ptr[2];
    int32_t height[2];
    int32_t width[2];
    int32_t output_width   = 0;
    int32_t output_height  = 0;
    int32_t offset_x       = 0;
    int32_t offset_y       = 0;
    int32_t offset_remainder_x[2];
    int32_t offset_remainder_y[2];
    int32_t offset_pixel_x[2];
    int32_t offset_pixel_y[2];

    int32_t i = 0;
    int32_t j = 0;
    int32_t d = 0;

    uint32_t depth = 0;
    int32_t width_limit = 0;
    int32_t height_limit = 0;

    if ((one == NULL) || (two == NULL) || (one->isNull()) || (two->isNull()))
    {
        return phFAIL;
    }
    
    phTHIS_LOCK(locked);

    /* Convert the input images to one color space */
    rc = one->convert(this->m_output_format);
    phPRINT_RC(rc,NULL,"one->convert(%s)",
            phImageFormatToString(this->m_output_format));
    
    two->resize(70,50);
    rc = two->convert(this->m_output_format);
    phPRINT_RC(rc,NULL,"two->convert(%s)",
            phImageFormatToString(this->m_output_format));
   
   /*
    rc = two->resize(one->getWidth(),one->getHeight());
    phPRINT_RC(rc,NULL,"two->resize");
   */
    /* Make sure our buffer is large enough */ 
    phDALLOC_RESIZE(this->m_buf,
                    this->m_buf_size,
                    one->getSize(),
                    uint8_t );
    
    depth = phImageFormatToDepth(this->m_output_format);
    
    outptr          = this->m_buf;
    /* Get the pointers to the image only after the conversions */
    ptr[0]          = (uint8_t*)one->getImage();
    ptr[1]          = (uint8_t*)two->getImage();
  
    height[0]       = one->getHeight();
    height[1]       = two->getHeight();

    width[0]        = one->getWidth();
    width[1]        = two->getWidth();

    output_width    = one->getWidth();
    output_height   = one->getHeight();

    /* half width diff == the offset of the X coord of the upper left 
     * corner of the smaller image within the larger image */
    offset_x = width[0] - width[1];
    if (offset_x > 0) offset_x = offset_x / 2;
   
    /* half height diff == the offset of the Y coord of the upper 
     * left corner of the smaller image within the larger image */
    offset_y = height[0] - height[1];
    if (offset_y > 0) offset_y = offset_y / 2;
    
    /* The offset pixel is the pixel within the larger image and the
     * output image where the overlayed smaller image will start */
    offset_pixel_x[0]       = offset_x + move_x;
    offset_pixel_y[0]       = offset_y + move_y;
    offset_remainder_x[0]   = offset_x - move_x;
    offset_remainder_y[0]   = offset_y - move_y;
    offset_pixel_y[1]       = 0;
    offset_pixel_x[1]       = 0;
    
    /* if the offset pixel in the larger image is less than 0, we'll
     * index into the smaller image instead of the larger image */
    width_limit  = width[1];
    if (offset_pixel_x[0] < 0)
    {
        offset_pixel_x[1] = -1 * offset_pixel_x[0];
        offset_pixel_x[0] = 0;
        width_limit -= offset_pixel_x[1];
        offset_remainder_x[1] = width_limit;
    }
    
    height_limit = height[1];
    if (offset_pixel_y[0] < 0)
    {
        offset_pixel_y[1] = -1 * offset_pixel_y[0];
        offset_pixel_y[0] = 0;
        height_limit -= offset_pixel_y[1];
    }
    
    /* Copy all the rows of the image that won't have overlayed data into the
     * output image buffer */
    phMemcpy(outptr,ptr[0],offset_pixel_y[0] * width[0] * depth);

    /* Index into the buffers.
     * Index into the larger buffer and output buffer to the row where 
     * the overlay will begin */
    ptr[0] += offset_pixel_y[0] * width[0]      * depth;
    ptr[1] += offset_pixel_y[1] * width[1]      * depth;
    outptr += offset_pixel_y[0] * output_width  * depth;

    /* This loops over the two input images and overlays them into 
     * the output image */
    /* we only need to loop through the entirety of the smaller image */
    for (j = 0; j < height_limit; j++)
    {
        phMemcpy(outptr,ptr[0],offset_pixel_x[0] * 2 * depth);
        ptr[0] += (offset_pixel_x[0] * depth);
        ptr[1] += (offset_pixel_x[1] * depth);
        outptr += (offset_pixel_x[0] * depth);
        
        for (i = 0; i < width_limit; i++ )
        {
            alpha = ((float)ptr[1][ai]) / 255.0;
            alphainv = 1.0 - alpha;
            
            for (d = 0; d < 3; d++ )
            {
                tmp = ((*ptr[0]) * alphainv) + ((*ptr[1]) * alpha);
                if (tmp < 0) tmp = 0;
                *outptr = (uint8_t)(tmp > 255 ? 255 : tmp);
                ptr[0]++; ptr[1]++; outptr++;
            }
            
            outptr++;
            ptr[0]++;
            ptr[1]++;
        }
        
        phMemcpy(outptr,ptr[0],offset_remainder_x[0] * 2 * depth);
        ptr[0] += (offset_remainder_x[0] * depth);
        outptr += (offset_remainder_x[0] * depth);
    }
    
    phMemcpy(outptr,ptr[0],offset_remainder_y[0] * width[0] * depth);

    rc = out->setImage(output_width,
                       output_height,
                       this->m_output_format,
                       this->m_buf_size,
                       this->m_buf);
    phCHECK_RC(rc,NULL,"out->setImage");
    
    phTHIS_UNLOCK(locked);

    return phSUCCESS;
error:

    phTHIS_ERROR_UNLOCK(locked);
    
    return phFAIL;
}

/* ------------------------------------------------------------------------ */
int phEffectsThread::run()
{   
    phFUNCTION("phEffectsThread::run") 
    phImage local_copy[2];
    int32_t offset_x = 5;
    int32_t offset_y = -45;
    uint32_t i = 0;
    uint32_t counter = 0;
    int32_t  direction = 1;
    
    for (i = 0; i < 2; i++ )
    {
        rc = local_copy[i].connect(this->m_input[i]);
        phCHECK_RC(rc,NULL,"this->local_copy[%d].connect(%p)",i,this->m_input[i]);
    }
    
    rc = this->signal_running();
    phCHECK_RC(rc,NULL,"this->signal_running");
   
    while (this->isRunning())
    {    
        for (i = 0; i < 2; i++ )
        {
            rc = local_copy[i].update();
            phPRINT_RC(rc,NULL,"this->local_copy[%d].update()",i);
        }
#if 0
        rc = this->mosaic_images(&(local_copy[0]),&(local_copy[1]),this->m_output);
        phPRINT_RC(rc,NULL,"this->mosaic_images");
#else
        rc = this->overlay_images(&(local_copy[0]),
                                  &(local_copy[1]),
                                  this->m_output,
                                  offset_x,
                                  offset_y );
        phPRINT_RC(rc,NULL,"this->mosaic_images");
#endif

        if (counter == 0)
        {
            if (this->m_alpha >= 1.00)
                direction = -1;
            if (this->m_alpha <= 0.00)
                direction = 1;
                
            this->m_alpha += 0.1 * direction;
            counter = 0;
        }
        else
        {
            counter++;
        }
    }

    for (i = 0; i < 2; i++ )
    {
        rc = local_copy[i].disconnect();
        phPRINT_RC(rc,NULL,"this->local_copy[%d].disconnect()",i);
    }
    
    return phSUCCESS;
error:
    rc = this->signal_error();
    phPRINT_RC(rc,NULL,"this->signal_error");

    return phFAIL;
}

/* ------------------------------------------------------------------------ */
int phEffectsThread::wakeup()
{
    phFUNCTION("phEffectsThread::wakeup")
    uint32_t i = 0;
    
    for (i = 0; i < 2; i++ )
    {
        rc = this->m_input[i]->wakeup_clients();
        phCHECK_RC(rc,NULL,"this->m_input[%d]->wakeup_clients();",i);
    }
    
    return phSUCCESS;
error:
    return phFAIL;
}

/* ------------------------------------------------------------------------ */
int main(int argc, char *argv[] )
{
    phFUNCTION("main")

    int             displaysOpen= 1;
    
    unsigned int    i           = 0;
    unsigned int    image_count = 50;
    char            *filename = NULL;
    char            savefilename[255];

    phDisplayInterface *display = NULL;

    int         count = 0;
    phImage     image;
    float       fps = 0.0;
    float       last_fps = 0.0;

    /* Utility class to ease the starting and stopping of displays, captures and
     * pipelines */
    phSystem        system;
    phPipeline      *pipeline = new phPipeline();
    phPipeline      *pipeline2 = new phPipeline();
    invert_Filter   *invert = new invert_Filter();
    key_matte_threshold_Filter   *key_matte = new key_matte_threshold_Filter(120);
    //phAvcodecSource *capture    = new phAvcodecSource();
    V4LCapture      *capture    = new V4LCapture();
    V4LCapture      *capture2   = new V4LCapture();

    phEffectsThread  effects_thread(pipeline->getLiveSourceOutput(),
                            pipeline2->getLiveSourceOutput(),
                            phImageRGBA32);
   
    /* Remove the code below when using this code as an example.
     * 
     * This just checks whether "--test" has been specified with
     * a time value argument. It's for testing all the examples
     * without the need for human intervention. */
    int             test = 0;

    phArgTable      arg_parser;

    /* Setup and parse all the arguments */
    rc = arg_parser.add("--test", &test, phARG_INT);
    phCHECK_RC(rc,NULL,"arg_parser.add");
    rc = arg_parser.add("--nodisplay", (void *)&glbl_disable_displays, phARG_BOOL);
    phCHECK_RC(rc,NULL,"arg_parser.add");
    rc = arg_parser.add("--saveimages", (void *)&glbl_save_images, phARG_BOOL);
    phCHECK_RC(rc,NULL,"arg_parser.add");
    rc = arg_parser.add("--help",(void *)&usage, phARG_FUNC);
    phCHECK_RC(rc,NULL,"arg_parser.add");
    
    rc = arg_parser.parse(argc,argv);
    phCHECK_RC(rc,NULL,"arg_parser.parse");

    /* Pipeline 1 */
    rc = pipeline->setLiveSourceInput(capture->getOutput());
    phCHECK_RC(rc,NULL,"pipeline->setLiveSourceInput");

    rc = pipeline->add(invert);
    phCHECK_RC(rc,NULL,"pipeline->add");

    rc = system.add(pipeline);
    phCHECK_RC(rc,NULL,"system.add");

    /* Pipeline 2 */
    rc = pipeline2->setLiveSourceInput(capture2->getOutput());
    phCHECK_RC(rc,NULL,"pipeline2->setLiveSourceInput");

    rc = pipeline2->add(key_matte);
    phCHECK_RC(rc,NULL,"pipeline2->add");

    rc = system.add(pipeline2);
    phCHECK_RC(rc,NULL,"system.add");


    /* Set up the capture parameters */
    capture->set        ( 320, 240, "/dev/video0" );
    capture->setChannel ( 0 );
    capture->setFormat  ( phImageRGBA32 );

    rc = system.add(capture);
    phCHECK_RC(rc,NULL,"system.add(capture:%p)",capture);
    
    capture2->set        ( 320, 240, "/dev/video2" );
    capture2->setChannel ( 0 );
    capture2->setFormat  ( phImageGREY8 );

    rc = system.add(capture2);
    phCHECK_RC(rc,NULL,"system.add(capture2:%p)",capture2);

    /* TODO: init capture */
    if (glbl_disable_displays == 0)
    {
#if 0
        display = new X11Display(capture->getWidth(),
                                 capture->getHeight(),
                                 "phAvcodecSourceTest");
        phCHECK_PTR(display,"new","new X11Display failed.");
        
        display->setLiveSourceInput(capture->getLiveSourceOutput());
#else
        display = new X11Display(320,240,
                                 "FLIROverlayExample");
        phCHECK_PTR(display,"new","new X11Display failed.");
        
        display->setLiveSourceInput(effects_thread.getLiveSourceOutput());
#endif
        rc = system.addDisplay(display);
    }

    rc = system.add(&effects_thread);
    phCHECK_RC(rc,NULL,"system.add(&effects_thread)");

    if (glbl_save_images)
    {
        rc = image.connect(effects_thread.getLiveSourceOutput());
        phCHECK_RC(rc,NULL,"image.connect");
    }

    /* startup */
    rc = system.startup();
    phPRINT_RC(rc,NULL,"system.startup()");
    
    // now loop continuously for as long as we have a display open and a frame available from the avi file
    while ((displaysOpen) && (system.capturesActive()))
    {
        if (glbl_disable_displays == 0)
        {
            displaysOpen = 0;
            if (system.displaysOpen())
            {
                displaysOpen = 1;
            }
        }

        /* Remove this if block when using this code as an example */
        /* Set the loop control value to end the loop when testing */
        if (test > 0)
        { 
            /* loop once through for this test */
            if (i == (image_count - 1))
            {
                displaysOpen = 0;
            }
        }

        if (glbl_save_images)
        {
            sprintf(savefilename,"jpegs/flir%09d.jpg",count);
            count++;
            
            rc = image.update();
            phPRINT_RC(rc,NULL,"image.update");
            
            if (rc == phLiveObjectUPDATED)
            {
                rc = image.save(savefilename);
                phPRINT_RC(rc,NULL,"image.save");
            }
        }
        else
        {
            phMSleep(100);
        }
        
        fps = display->getImageWindow()->getFps();
        if (fps != last_fps)
        {
            last_fps = fps;
            phPRINT("%f\n", fps);
        }
    }
    
 error:
    rc = system.shutdown();
    phPRINT_RC(rc,NULL,"system.shutdown()");
    
    phDelete(display);
    phDelete(capture);
    phDelete(capture2);

    // free up the filename string allocation
    phFree(filename);

    return phSUCCESS;
}

Copyright (C) 2002 - 2007

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

---