FrameObserver.h

Go to the documentation of this file.

// -*- mode: c++; indent-tabs-mode: nil; c-basic-offset: 4; coding: iso-8859-1; -*-

/*
ACT-CV - Machine Vision for Cognitive Modeling
Copyright (c) 2008 Marc Halbruegge

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., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
*/


#pragma once

#include <vector>
#include <map>
#include <iostream>
#include <sstream>
#include <algorithm>

//#include <tcpclient/TcpClient.h>

#include <cv.h>
#include <highgui.h>
//#include <opencv2/core/core.hpp>

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

#include <mhexcptn.h>
#include <DynamicsHelpers.h>
#include <GCString.h>
#include <StoppUhr.h>

#include <Threads/Mutex.h>

#include <oflow/OpticalFlow.h>
#include "FrameSource.h"
#include "ActCvSource.h"
#include "RegionInfo.h"

#include <Matcher.h>

extern "C" {
#include "server/ACT-CV.i"
}

#include "server/ResultHolder.h"

class FrameObs : virtual public ReferenceCount {
public:
    virtual void FrameNotify(IplImage *img, int frameNr) =0;

    virtual void RegionInfoNotify(const std::vector<RegionInfo> &regions) {}

    virtual const char* GetName() const =0;
};

class FrameObsSupport {
    std::map<std::string, GCPointer<FrameObs> > observers;
    typedef std::map<std::string, GCPointer<FrameObs> >::const_iterator obs_iter;
protected:
    void FireFrameNotification(IplImage *img, int frameNr) const {
        for (obs_iter i=observers.begin();i!=observers.end();i++) {
            i->second->FrameNotify(img,frameNr);
        }
    }
    void FireRegionNotification(const std::vector<RegionInfo> &regions) const {
        for (obs_iter i=observers.begin();i!=observers.end();i++) {
            i->second->RegionInfoNotify(regions);
        }
    }
public:
    void AddObserver(const GCPointer<FrameObs> &obs) {
        if (observers.find(obs->GetName())==observers.end()) {
            observers[obs->GetName()]=obs;
        }
    }
    
    void RemoveObserver(const GCPointer<FrameObs> &obs) {
        std::map<std::string, GCPointer<FrameObs> >::iterator
        i = observers.find(obs->GetName());
        if (i!=observers.end()) {
            observers.erase(i);
        }
    }
    
    GCPointer<FrameObs> FindObserver(const std::string &s) const {
        obs_iter i=observers.find(s);
        if (i==observers.end()) {
            return nullptr;
        } else {
            return i->second;
        }
    }
};



class FpsFrameObs : public FrameObs {
    RundenZeitenClock zeiten;
    ExpGlaett<float> fps;
public:
    FpsFrameObs() : fps(-1.0f, 0.05f) {}
    
    void FrameNotify(IplImage *img, int frameNr) {
        zeiten.Messung();
        if (zeiten.GetSeconds()>0.0) {
            if (fps.get()<0) {
                fps.set(static_cast<float>(1.0/zeiten.GetSeconds()));
            } else {
                fps.add(static_cast<float>(1.0/zeiten.GetSeconds()));
            }
            std::cout << fps.get() << " fps" << std::endl;
        }
    }
    
    const char* GetName() const {
        return "frames per seconds";
    }
};


class WriterObs : public FrameObs {
    CvVideoWriter *pWriter;
    const char *videoFName;
public:
    WriterObs(const char* fname = "actcv-out.mpg") 
        : pWriter(nullptr),
          videoFName(fname) {
    }
    ~WriterObs() {
        if(pWriter) {
            cvReleaseVideoWriter(&pWriter);
        }
    }

    void FrameNotify(IplImage *img, int frameNr) {
        if(!pWriter) {
            pWriter = cvCreateVideoWriter(videoFName, CV_FOURCC('m','p','4','v'), 25, cvGetSize(img));
        }
        if(pWriter) {
            cvWriteFrame(pWriter, img);
        }
    }
    const char* GetName() const {
        return "writer";
    }
};


class FrameObsWindow : public FrameObs {
    std::string name;
public:
    FrameObsWindow(const char *s) 
        : name(s) {
        MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
        cvNamedWindow(name.c_str());
        cvWaitKey(100);
        MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
    }
    ~FrameObsWindow() {
        MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
        cvDestroyWindow(name.c_str());
        cvWaitKey(100); // doesn't help :(
    }
    void FrameNotify(IplImage *img, int frameNr) {
        MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
        MHDBGMSG_P(__FILE__,__LINE__,"cvShowImage", img);
        cvShowImage(name.c_str(), img);
        MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
    }
    const char* GetName() const {
        return "window";
    }
};

class MatchObs : public FrameObs,
    public ResultSource<ObjectPosition> {
    std::vector<GCPointer<MatchImage> > matchers;
    
    mhthreads::Mutex mutex;
    std::vector<ObjectPosition> lastResult;

public:
    MatchObs(const char *defFileName);
    void FrameNotify(IplImage *img, int frameNr) {
        MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
        if (img==nullptr) return;

        std::vector<ObjectPosition> result;
        
        for (unsigned int i=0;i<matchers.size();i++) {
            MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
            matchers[i]->Match(img);
            MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);

            for (unsigned int j=0;j<matchers[i]->GetPoints().size();j++) {
                result.push_back(
                    ObjectPosition(matchers[i]->GetPoints().at(j).x,
                        matchers[i]->GetPoints().at(j).y,
                        matchers[i]->GetWidth(),
                        matchers[i]->GetHeight(),
                        matchers[i]->GetVisiconName()));
            }

#ifdef DEBUG
            if (matchers[i]->TargetFound()) {
                CvPoint target = matchers[i]->GetRandomTarget();
                //robot.MouseMove(target.x,target.y);
                std::cout << matchers[i]->GetVisiconName()
                    << " found at position " << target << std::endl;
            }
#endif            
            MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
        }

        mutex.Enter();
        lastResult=result;
        mutex.Leave();
        MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
    }
    const char* GetName() const {
        return "match";
    }

    void EnterCriticalSection() {
        mutex.Enter();
    }
    std::vector<ObjectPosition>& GetData() {
        return lastResult;
    }
    void LeaveCriticalSection() {
        mutex.Leave();
    }
};


class ImageCopy {
    IplImage* copy;
    int cpFrameNr;

    const int divisor;
public:
    ImageCopy(int divi=1) : copy(nullptr), cpFrameNr(0), divisor(divi) {}
    ~ImageCopy() {
        cvReleaseImage(&copy);
    }
    IplImage* GetCurCopy() {
        return copy;
    }
    IplImage* GetCopy(IplImage*img, int frameNr) {
        if (cpFrameNr!=frameNr) {
            if (copy==nullptr) {
                if(divisor == 1) {
                    copy = cvCloneImage(img);
                } else {
                    copy = cvCreateImage(cvSize(img->width/divisor,img->height/divisor),img->depth,img->nChannels);
                    cvResize(img, copy, CV_INTER_LINEAR);
                }
            } else {
                // ACHTUNG: erst src, dann dst!
                if(divisor == 1) {
                    cvCopy(img, copy);
                } else {
                    cvResize(img, copy, CV_INTER_LINEAR);
                }
            }
            cpFrameNr=frameNr;
            return copy;
        } else {
            return copy;
        }
    }
};


class ColorObs : public FrameObs,
    public ResultSource<ObjectPosition>, private ImageCopy {
    
    mhthreads::Mutex mutex;
    std::vector<ObjectPosition> lastResult;

    GCPointer<FrameObs> win;

    std::vector<float> means;
    std::vector<float> sd;

public:
    ColorObs(const char *defFileName);

    void FrameNotify(IplImage *img, int frameNr) {
        IplImage* pCopy = GetCopy(img,frameNr);

#if 0
        cv::Mat mat(pCopy);
        cv::MatConstIterator_<cv::Vec3b> it = mat.begin<cv::Vec3b>(), it_end = mat.end<cv::Vec3b>();
        for(; it != it_end; ++it) {
            float maxDiff = .0f;
            for(int c=0;c<pCopy->nChannels;++c) {
                float curVal = (*it)[c];
                float curDiff = std::abs(curVal-means[c])/sd[c];
#ifdef max
                maxDiff = max(maxDiff, curDiff);
#else
                maxDiff = std::max(maxDiff, curDiff);
#endif
            }

            // overlay
            (*it) /= 2;
            const unsigned char overlay = (maxDiff < 1.5f) ? 255 : 0;
            for(int c=0;c<pCopy->nChannels;++c) {
                (*it)[c] += overlay/2;
            }
        }
#else
        unsigned char* p = reinterpret_cast<unsigned char*>(pCopy->imageData);
        for(int y=0; y<pCopy->height; ++y) {
            unsigned char* row = p + y*pCopy->widthStep;
            for(int x=0; x<pCopy->width; ++x, row += pCopy->nChannels) {
                float maxDiff = .0f;
                for(int c=0;c<pCopy->nChannels;++c) {
                    float curVal = row[c];
                    float curDiff = std::abs(curVal-means[c])/sd[c];
#ifdef max
                    maxDiff = max(maxDiff, curDiff);
#else
                    maxDiff = std::max(maxDiff, curDiff);
#endif
                }

                const unsigned char overlay = (maxDiff < 1.5f) ? 255 : 0;
                for(int c=0;c<pCopy->nChannels;++c) {
                    row[c] = row[c] / 2 + overlay/2;
                }
            }
        }
#endif

        win->FrameNotify(pCopy, frameNr);
    }
    const char* GetName() const {
        return "color";
    }

    void EnterCriticalSection() {
        mutex.Enter();
    }
    std::vector<ObjectPosition>& GetData() {
        return lastResult;
    }
    void LeaveCriticalSection() {
        mutex.Leave();
    }
};


class DiffObs : public FrameObs, private ImageCopy {
    
    GCPointer<FrameObs> win;
    GCPointer<FrameObs> writer;

    IplImage* pPrevImg;
    IplImage* pWorkImg;

public:
    DiffObs()
        : pPrevImg(nullptr),
          pWorkImg(nullptr),
          win(new FrameObsWindow("diff")),
          ImageCopy(1) {
        writer = new WriterObs("diff-out.avi");
    }
    ~DiffObs() {
        if(pPrevImg != nullptr) cvReleaseImage(&pPrevImg);
        if(pWorkImg != nullptr) cvReleaseImage(&pWorkImg);
    }

    void FrameNotify(IplImage *img, int frameNr) {
        IplImage* pCopy = GetCopy(img,frameNr);
        if(pPrevImg == nullptr) {
            pPrevImg = cvCloneImage(pCopy);
            pWorkImg = cvCloneImage(pCopy);
            return;
        } else {
            cvCopy(pPrevImg, pWorkImg);
            cvCopy(pCopy, pPrevImg);
        }

        unsigned char* p = reinterpret_cast<unsigned char*>(pCopy->imageData);
        const unsigned char* pPrev = reinterpret_cast<unsigned char*>(pWorkImg->imageData);
        for(int y=0; y<pCopy->height; ++y) {
            unsigned char* row = p + y*pCopy->widthStep;
            const unsigned char* rowPrev = pPrev + y*pWorkImg->widthStep;
            for(int x=0; 
                x<pCopy->width; 
                ++x, row += pCopy->nChannels, rowPrev += pWorkImg->nChannels) {

                for(int c=0;c<pCopy->nChannels;++c) {
                    int cur = row[c];
                    cur -= rowPrev[c];
#ifdef min
                    row[c] = min(abs(cur),255);
#else
                    row[c] = std::min(abs(cur),255);
#endif
                }
            }
        }

        win->FrameNotify(pCopy, frameNr);
        if(writer) {
            writer->FrameNotify(pCopy, frameNr);
        }
    }
    const char* GetName() const {
        return "diff";
    }
};


class TextObs : public FrameObs, private ImageCopy,
    public ResultSource<ObjectPosition> {
    
    mhthreads::Mutex mutex;
    std::vector<ObjectPosition> lastResult;

    GCPointer<FrameObs> win;

    int lastFrameNr;

public:
    TextObs(bool showWin = false) 
    : win(showWin ? new FrameObsWindow("text") : nullptr),
      lastFrameNr(0) {
        MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
    }
    void FrameNotify(IplImage *img, int frameNr) {
        MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
        if(img && win) {
            win->FrameNotify(GetCopy(img,frameNr), frameNr);
        }
        lastFrameNr = frameNr;
    }

    void RegionInfoNotify(const std::vector<RegionInfo> &regions) {
        MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);

        std::vector<ObjectPosition> result;
        for (const RegionInfo &region : regions) {
            result.push_back(
                ObjectPosition((region.right+region.left) / 2,
                               (region.bottom+region.top) / 2,
                               region.right-region.left,
                               region.bottom-region.top,
                               region.info.c_str()));

#if 0
            std::cout << "[" 
                        << region.left << ";" 
                        << region.top // << ";"  
                        << "] "
                        << region.info << std::endl;
#endif
        }

        mutex.Enter();
        lastResult=result;
        mutex.Leave();
        MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
    }
    const char* GetName() const {
        return "text";
    }

    void EnterCriticalSection() {
        mutex.Enter();
    }
    std::vector<ObjectPosition>& GetData() {
        return lastResult;
    }
    void LeaveCriticalSection() {
        mutex.Leave();
    }
};



class FrameObsAutoGray : public FrameObs {
    IplImage* gray;
protected:
    virtual void FrameNotifyGray(IplImage *img, IplImage *gray, int frameNr) = 0;
public:
    FrameObsAutoGray() : gray(0) {};
    ~FrameObsAutoGray() {
        cvReleaseImage(&gray);
    };
    void FrameNotify(IplImage *img, int frameNr) {
        if (img==nullptr) return;

        if (img->nChannels==3) {
            // assume rgb
            if (gray==0) {
                gray = cvCreateImage(cvSize(img->width,img->height), IPL_DEPTH_8U, 1);
            }
            cvCvtColor(img, gray, CV_BGR2GRAY);
            FrameNotifyGray(img,gray,frameNr);
        } else {
            // assume gray
            FrameNotifyGray(img,img,frameNr);
        }
    }
};

class AttendedLocObs : public FrameObs, private ImageCopy {
    GCPointer<FrameObs> win;
    CvPoint focus;
    CvPoint mouse;
    mhthreads::Mutex mutex;
    
#define ATTLOC_CROSS_LWIDTH 2

    void PrintCross(IplImage *img) {
        if(focus.x > 0 && focus.y > 0) {
            cvLine(img, 
                focus+cvPoint(-ATTLOC_CROSS_LWIDTH*3,0), 
                focus+cvPoint(ATTLOC_CROSS_LWIDTH*3,0), 
                CV_RGB(0,255,0), 
                ATTLOC_CROSS_LWIDTH, 
                CV_AA, 0 );
            cvLine(img, 
                focus+cvPoint(0,-ATTLOC_CROSS_LWIDTH*3), 
                focus+cvPoint(0,ATTLOC_CROSS_LWIDTH*3), 
                CV_RGB(0,255,0), 
                ATTLOC_CROSS_LWIDTH, 
                CV_AA, 0 );
        }

        if(mouse.x > 0 && mouse.y > 0) {
            cvLine(img, 
                mouse+cvPoint(-ATTLOC_CROSS_LWIDTH*3,0), 
                mouse+cvPoint(ATTLOC_CROSS_LWIDTH*3,0), 
                CV_RGB(0,0,255), 
                ATTLOC_CROSS_LWIDTH, 
                CV_AA, 0 );
            cvLine(img, 
                mouse+cvPoint(0,-ATTLOC_CROSS_LWIDTH*3), 
                mouse+cvPoint(0,ATTLOC_CROSS_LWIDTH*3), 
                CV_RGB(0,0,255), 
                ATTLOC_CROSS_LWIDTH, 
                CV_AA, 0 );
        }
    }
public:    
    AttendedLocObs() 
    : focus(cvPoint(-10,-10)),
      mouse(cvPoint(-10,-10)) {
        win = new FrameObsWindow(GetName());
    }
    ~AttendedLocObs() {
        win = nullptr;
    }
    void FrameNotify(IplImage *img, int frameNr) {
        if (img==nullptr) return;
        MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
        mutex.Enter();
        MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
        PrintCross(GetCopy(img,frameNr));
        MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
        win->FrameNotify(GetCopy(img,frameNr),frameNr);
        MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
        mutex.Leave();
        MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
    }
    void UpdateAttendedLoc(int x, int y) {
        MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
        mutex.Enter();
        focus.x=x;
        focus.y=y;
        mutex.Leave();
        MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
    } 
    void UpdateMousePos(int x, int y) {
        MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
        mutex.Enter();
        mouse.x=x;
        mouse.y=y;
        mutex.Leave();
        MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
    } 
    const char* GetName() const {
        return "attended location";
    }
};

class OFlowObs : public FrameObsAutoGray, public FocusObserver, 
    private ImageCopy, public ActCvSource, public ActCvObsSupport {
    OpticalFlow oFlow;
    ExpGlaett<double> flowWinkel;
    
    GCPointer<FrameObs> win;
    
    std::string curResult;
public:
    OFlowObs() : flowWinkel(0.0f, 0.4f) {
        win = new FrameObsWindow(GetName());
    }
    void FrameNotifyGray(IplImage *img, IplImage *gray, int frameNr) {
        if (img==nullptr) return;

        if (frameNr==1) {
            oFlow.SetSize(cvSize(img->width,img->height));
#if 0
            oFlow.SetFocus(cvPoint(img->width/2,
                static_cast<int>(img->height - img->height*.5)));
#else
            oFlow.SetFocus(cvPoint(img->width/2,
                static_cast<int>(img->height - img->height*.75)));
#endif                
        }
        
        oFlow.AddImage(gray);

        if (frameNr>1) {
            oFlow.Calc();
            oFlow.PrintFlowField(GetCopy(img,frameNr));
            // still some work to be done
            //oFlow.PrintFocus(GetCopy(img,frameNr));
            flowWinkel.add(oFlow.GetWinkel());
            //std::cout << flowWinkel << std::endl;
            
            std::ostringstream ostr;
            ostr << "(" << flowWinkel << ")";
            curResult = ostr.str();
            FireVisionNotification(*this);
        }

        win->FrameNotify(GetCopy(img,frameNr),frameNr);
    }
    void FocusNotify(const CvPoint &p) {
        oFlow.FocusNotify(p);
    }
    const char* GetResult() const {
        return curResult.c_str();
    }
    const char* GetName() const {
        return "optical flow";
    }
    const char* GetTypeName() const {
        return "flowangle";
    }
};


class MatchContourObs : public FrameObsAutoGray {
    
    MatchContour matcher;
public:
    MatchContourObs() : matcher ("FG", "imgs/finger.png") {
    }
    void FrameNotifyGray(IplImage *img, IplImage *gray, int frameNr) {
        if (img==nullptr) return;
        matcher.Match(gray);
    }
    const char* GetName() const {
        return "match-contour";
    }
};

class LineObs : public FrameObsAutoGray, 
    private ImageCopy, public ResultSource<Line> {
    IplImage* canny;
    CvMemStorage* storage;
    
    GCPointer<FrameObs> win;
    
public:
    LineObs() : canny(0), storage(0) {
        MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
        win = new FrameObsWindow("lines");
        MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
    }
    LineObs(const LineObs &lo) : canny(0), storage(0), win(0) {
        MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
        win = new FrameObsWindow("lines");
    }
    LineObs& operator = (const LineObs &lo) {
        // release structures to allow resizing
        cvReleaseImage(&canny);
        cvReleaseMemStorage(&storage);
    }
    ~LineObs() {
        MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
        win = 0;
        cvReleaseImage(&canny);
        cvReleaseMemStorage(&storage);
        cvWaitKey(100);
    }
    void FrameNotifyGray(IplImage *img, IplImage *gray, int frameNr) {
        MHDBGMSG(__FILE__,__LINE__,__PRETTY_FUNCTION__);
        if (img==nullptr) return;

        if (canny==0) canny = cvCreateImage(cvGetSize(gray), 8, 1);
        if (storage==0) storage = cvCreateMemStorage();

        cvCanny(gray, canny, 50, 200, 3);
        CvSeq* lines = cvHoughLines2(canny, storage, 
            CV_HOUGH_PROBABILISTIC, 1, CV_PI/180, 
            40, 40, 10);

        mutex.Enter();
        lastResult.resize(lines->total);
        for(int i = 0; i < lines->total; i++) {
            CvPoint* line = (CvPoint*)cvGetSeqElem(lines,i);
            cvLine(GetCopy(img,frameNr), line[0], line[1], CV_RGB(255,0,0), 2, 8 );
            lastResult[i].end1x=line[0].x;            
            lastResult[i].end1y=line[0].y;            
            lastResult[i].end2x=line[1].x;            
            lastResult[i].end2y=line[1].y;            
        }
        mutex.Leave();

        cvClearMemStorage(storage); 
        
        win->FrameNotify(GetCopy(img,frameNr),frameNr);
    }
    
    const char* GetName() const {
        return "lines";
    }

protected:
    mhthreads::Mutex mutex;
    std::vector<Line> lastResult;

public:    
    void EnterCriticalSection() {
        mutex.Enter();
    }
    std::vector<Line>& GetData() {
        return lastResult;
    }
    void LeaveCriticalSection() {
        mutex.Leave();
    }
    
};