module.cpp

Go to the documentation of this file.

/*
 * Copyright (C) 2015 WYSIWYD Consortium, European Commission FP7 Project ICT-612139
 * Authors: Ugo Pattacini, Tobias Fischer
 * email:   ugo.pattacini@iit.it, t.fischer@imperial.ac.uk
 * Permission is granted to copy, distribute, and/or modify this program
 * under the terms of the GNU General Public License, version 2 or any
 * later version published by the Free Software Foundation.
 *
 * A copy of the license can be found at
 * icub-client/license/gpl.txt
 *
 * 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
*/

#include <cmath>
#include <limits>
#include <sstream>
#include <cstdio>
#include <cstdarg>
#include <algorithm>
#include <set>

#include "module.h"


/**********************************************************/
void IOL2OPCBridge::yInfoGated(const char *msg, ...) const
{
    if (verbose)
    {
        va_list arg;
        char buf[512];
        va_start(arg,msg);        
        vsnprintf(buf,sizeof(buf),msg,arg);
        va_end(arg);

        yInfo("%s",buf);
    }
}


/**********************************************************/
string IOL2OPCBridge::findName(const Bottle &scores,
                               const string &tag)
{
    string retName=OBJECT_UNKNOWN;
    double maxScore=0.0;

    Bottle *blobScores=scores.find(tag.c_str()).asList();
    if (blobScores==nullptr)
        return retName;

    // first find the most likely object for the given blob
    for (unsigned int i=0; i<blobScores->size(); i++)
    {
        Bottle *item=blobScores->get(i).asList();
        if (item==nullptr)
            continue;

        string name=item->get(0).asString().c_str();
        double score=item->get(1).asDouble();
        if (score>maxScore)
        {
            maxScore=score;
            retName=name;
        }
    }

    // then double-check that the found object remains the best
    // prediction over the remaining blobs
    if (retName!=OBJECT_UNKNOWN)
    {
        for (unsigned int i=0; i<scores.size(); i++)
        {
            if (Bottle *blob=scores.get(i).asList())
            {
                // skip the blob under examination
                string name=blob->get(0).asString().c_str();
                Bottle *blobScores=blob->get(1).asList();
                if ((name==tag) || (blobScores==nullptr))
                    continue;

                if (blobScores->find(retName.c_str()).asDouble()>=maxScore)
                    return OBJECT_UNKNOWN;
            }
        }
    }

    return retName;
}


/**********************************************************/
Bottle IOL2OPCBridge::skimBlobs(const Bottle &blobs)
{
    Bottle skimmedBlobs;
    for (unsigned int i=0; i<blobs.size(); i++)
    {
        CvPoint cog=getBlobCOG(blobs,i);
        if ((cog.x==RET_INVALID) || (cog.y==RET_INVALID))
            continue;

        // skim out blobs that are too far in the cartesian space
        Vector x;
        if (get3DPosition(cog,x))
        {
            if ((x[0]>skim_blobs_x_bounds[0]) && (x[0]<skim_blobs_x_bounds[1]) &&
                (x[1]>skim_blobs_y_bounds[0]) && (x[1]<skim_blobs_y_bounds[1]))
                skimmedBlobs.add(blobs.get(i));
        }
    }

    return skimmedBlobs;
}


/**********************************************************/
bool IOL2OPCBridge::thresBBox(CvRect &bbox, const Image &img)
{
    CvPoint tl=cvPoint(bbox.x,bbox.y);
    CvPoint br=cvPoint(tl.x+bbox.width,tl.y+bbox.height);

    tl.x=std::min((int)img.width(),std::max(tl.x,0));
    tl.y=std::min((int)img.height(),std::max(tl.y,0));
    br.x=std::min((int)img.width(),std::max(br.x,0));
    br.y=std::min((int)img.height(),std::max(br.y,0));

    bbox=cvRect(tl.x,tl.y,br.x-tl.x,br.y-tl.y);
    if ((bbox.width>tracker_min_blob_size[0]) &&
        (bbox.height>tracker_min_blob_size[1]))
        return true;
    else
        return false;
}


/**********************************************************/
Bottle IOL2OPCBridge::getBlobs()
{
    // grab resources
    mutexResources.lock();

    if (Bottle *pBlobs=blobExtractor.read(false))
    {
        lastBlobsArrivalTime=Time::now();
        lastBlobs=skimBlobs(*pBlobs);
        yInfoGated("Received blobs list: %s",lastBlobs.toString().c_str());

        if (lastBlobs.size()==1)
        {
            if (lastBlobs.get(0).asVocab()==Vocab::encode("empty"))
                lastBlobs.clear();
        }        
    }
    else if (Time::now()-lastBlobsArrivalTime>blobs_detection_timeout)
        lastBlobs.clear();

    // release resources
    mutexResources.unlock();

    return lastBlobs;
}


/**********************************************************/
CvPoint IOL2OPCBridge::getBlobCOG(const Bottle &blobs, const unsigned int i)
{
    CvPoint cog=cvPoint(RET_INVALID,RET_INVALID);
    if (i<blobs.size())
    {
        CvPoint tl,br;
        Bottle *item=blobs.get(i).asList();
        if (item==nullptr)
            return cog;

        tl.x=(int)item->get(0).asDouble();
        tl.y=(int)item->get(1).asDouble();
        br.x=(int)item->get(2).asDouble();
        br.y=(int)item->get(3).asDouble();

        cog.x=(tl.x+br.x)>>1;
        cog.y=(tl.y+br.y)>>1;
    }

    return cog;
}


/**********************************************************/
bool IOL2OPCBridge::getClickPosition(CvPoint &pos)
{    
    if (Bottle *bPos=getClickPort.read(false))
    {
        if (bPos->size()>=2)
        {
            clickLocation.x=bPos->get(0).asInt();
            clickLocation.y=bPos->get(1).asInt();
            yInfoGated("Received new click location: (%d,%d)",
                  clickLocation.x,clickLocation.y);
        }
        else
            yError("Mis-sized bottle for click location");
    }

    pos=clickLocation;
    return ((clickLocation.x!=RET_INVALID) &&
            (clickLocation.y!=RET_INVALID));
}


bool IOL2OPCBridge::getBlobPoints(const CvPoint &cog, deque<CvPoint> &blobPoints)
{
    Bottle cmd,reply;
    blobPoints.clear();

    cmd.addString("get_component_around");
    cmd.addInt(cog.x); cmd.addInt(cog.y);

    if (rpcGetBlobPoints.write(cmd,reply))
    {
        if (Bottle *blob_list=reply.get(0).asList())
        {
            for (unsigned int i=0; i<blob_list->size();i++)
            {
                if (Bottle *blob_pair=blob_list->get(i).asList())
                {
                    blobPoints.push_back(cv::Point(blob_pair->get(0).asInt(),blob_pair->get(1).asInt()));
                }
                else
                {
                    yError("[iol2opc] getBlobPoints: Some problems in blob pixels!");
                    return false;
                }
            }
        }
        else
        {
            yError("[iol2opc] getBlobPoints: Some problem in object blob!");
            return false;
        }
    }
    else
    {
        yError("[iol2opc] lbpExtract query failed!");
        return false;
    }
    return true;
}


/**********************************************************/
bool IOL2OPCBridge::getSuperQuadric(const CvPoint &cog, Vector &pos, Vector &dim)
{
    pos.resize(3,0.0);
    dim.resize(3,0.0);
    Vector orientation(4,0.0);

    // Get points of a blob, defined by cog
    deque<CvPoint> blobPoints;
    if (!getBlobPoints(cog, blobPoints))
    {
        yDebug("[iol2opc] getSuperQuadric can't get points from blob of cog point");
        return false;
    }

    Bottle cmd, reply;
    cmd.addString("get_superq");

    Bottle &in1=cmd.addList();

    for (size_t i=0; i<blobPoints.size(); i++)
    {
        Bottle &in=in1.addList();
        in.addDouble(blobPoints[i].x);
        in.addDouble(blobPoints[i].y);
    }
    cmd.addInt(0);  //0 is for getting the estimated superquadric, 1 is for getting the filtered estimated superquadric

    mutexResourcesSPQ.lock();
    rpcGetSPQ.write(cmd, reply);
    mutexResourcesSPQ.unlock();
    // read reply to get position and dimension of blob by cog
    if (reply.size()>0)
    {
        if (Bottle *b=reply.get(0).asList())
        {
            if (Bottle *b1=b->find("dimensions").asList())
            {
                yDebug("[iol2opc] b1 dim: %s",b1->toString().c_str());
                for (int i=0; i<3; i++)
                {
                    dim[i]=b1->get(i).asDouble();
                }
            }
            if(Bottle *b1=b->find("center").asList())
            {
                yDebug("[iol2opc] b1 center: %s",b1->toString().c_str());
                for (int i=0; i<3; i++)
                {
                    pos[i]=b1->get(i).asDouble();
                }
            }
            if(Bottle *b1=b->find("orientation").asList())
            {
                yDebug("[iol2opc] b1 orientation: %s",b1->toString().c_str());
                for (int i=0; i<4; i++)
                {
                    orientation[i]=b1->get(i).asDouble();
                }
            }
            Matrix orientMatrix = axis2dcm(orientation);
            Vector dimTemp = dim;
            dimTemp.push_back(1.0);
            dimTemp = orientMatrix*dimTemp;
            dim = dimTemp.subVector(0,2);
            for (uint8_t i=0; i<dim.size();i++)
                dim[i] = fabs(dim[i]);  // This is due to the disTemp computation can generate negative value
            dim = dim*2.0;  // This is due to the outputs from superquadric are semi-axis lengths
        }
        else
        {
            yError("[iol2opc] getSuperQuadric receives wrong format");
            return false;
        }
    }
    else
    {
        yError("[iol2opc] getSuperQuadric with wrong cmd");
        return false;
    }
    return true;

}

/**********************************************************/
bool IOL2OPCBridge::get3DPosition(const CvPoint &point, Vector &x)
{
    x.resize(3,0.0);
    if (rpcGet3D.getOutputCount()>0)
    {
        // thanks to SFM we are here
        // safe against borders checking
        // command format: Rect tlx tly w h step
        Bottle cmd,reply;
        cmd.addString("Rect");
        cmd.addInt(point.x-3);
        cmd.addInt(point.y-3);
        cmd.addInt(7);
        cmd.addInt(7);
        cmd.addInt(2);

        mutexResourcesSFM.lock();
        rpcGet3D.write(cmd,reply);
        mutexResourcesSFM.unlock();

        int sz=reply.size();
        if ((sz>0) && ((sz%3)==0))
        {
            Vector tmp(3);
            int cnt=0;

            for (int i=0; i<sz; i+=3)
            {
                tmp[0]=reply.get(i+0).asDouble();
                tmp[1]=reply.get(i+1).asDouble();
                tmp[2]=reply.get(i+2).asDouble();

                if (norm(tmp)>0.0)
                {
                    x+=tmp;
                    cnt++;
                }
            }

            if (cnt>0)
                x/=cnt;
            else
                yWarning("get3DPosition failed");
        }
        else
            yError("SFM replied with wrong size");
    }

    return (norm(x)>0.0);
}


/**********************************************************/
bool IOL2OPCBridge::get3DPositionAndDimensions(const CvRect &bbox,
                                               Vector &x,
                                               Vector &dim)
{
    if (rpcGet3D.getOutputCount()>0)
    {
        Bottle cmd,reply;
        cmd.addString("Rect");
        cmd.addInt(bbox.x);
        cmd.addInt(bbox.y);
        cmd.addInt(bbox.width);
        cmd.addInt(bbox.height);
        cmd.addInt(2);
        mutexResourcesSFM.lock();
        rpcGet3D.write(cmd,reply);
        mutexResourcesSFM.unlock();

        x.resize(3);
        dim.resize(3);

        // find mean and standard deviation
        Vector tmp(3);
        for (unsigned int i=0; i<reply.size(); i+=3)
        {
            tmp[0]=reply.get(i+0).asDouble();
            tmp[1]=reply.get(i+1).asDouble();
            tmp[2]=reply.get(i+2).asDouble();

            x+=tmp;
            dim+=tmp*tmp;
        }
        
        double N=reply.size()/3.0;
        x/=N; dim=dim/N-x*x;
        dim[0]=2.0*sqrt(dim[0]);
        dim[1]=2.0*sqrt(dim[1]);
        dim[2]=2.0*sqrt(dim[2]);

        return true;
    }

    return false;
}


/**********************************************************/
void IOL2OPCBridge::acquireImage()
{
    // grab resources
    mutexResources.lock();

    // wait for incoming image
    if (ImageOf<PixelBgr> *tmp=imgIn.read())
        imgRtLoc=*tmp;

    // release resources
    mutexResources.unlock();
}


/**********************************************************/
void IOL2OPCBridge::drawBlobs(const Bottle &blobs, const unsigned int i,
                              const Bottle &scores)
{
    // grab resources
    mutexResources.lock();

    if (imgRtLocOut.getOutputCount()>0)
    {
        CvFont font;
        cvInitFont(&font,CV_FONT_HERSHEY_SIMPLEX,0.5,0.5,0,1);

        // latch image
        ImageOf<PixelBgr> imgLatch=this->imgRtLoc;
        for (unsigned int j=0; j<blobs.size(); j++)
        {
            CvPoint tl,br,txtLoc;
            Bottle *item=blobs.get(j).asList();
            tl.x=(int)item->get(0).asDouble();
            tl.y=(int)item->get(1).asDouble();
            br.x=(int)item->get(2).asDouble();
            br.y=(int)item->get(3).asDouble();
            txtLoc.x=tl.x;
            txtLoc.y=tl.y-5;

            ostringstream tag;
            tag<<"blob_"<<j;

            // find the blob name (or unknown)
            string object=findName(scores,tag.str());
            tag.str("");
            tag.clear();
            tag<<object;

            CvScalar highlight=cvScalar(0,255,0);
            CvScalar lowlight=cvScalar(150,125,125);
            cvRectangle(imgLatch.getIplImage(),tl,br,(j==i)?highlight:lowlight,2);
            cvPutText(imgLatch.getIplImage(),tag.str().c_str(),txtLoc,&font,(j==i)?highlight:lowlight);
        }

        imgRtLocOut.prepare()=imgLatch;
        imgRtLocOut.write();
    }

    // release resources
    mutexResources.unlock();
}


/**********************************************************/
void IOL2OPCBridge::rotate(cv::Mat &src, const double angle,
                           cv::Mat &dst)
{
    int len=std::max(src.cols,src.rows);
    cv::Point2f pt(len/2.0f,len/2.0f);
    cv::Mat r=cv::getRotationMatrix2D(pt,angle,1.0);
    cv::warpAffine(src,dst,r,cv::Size(len,len));
}


/**********************************************************/
void IOL2OPCBridge::drawScoresHistogram(const Bottle &blobs,
                                        const Bottle &scores,
                                        const int i)
{
    if (imgHistogram.getOutputCount()>0)
    {
        // grab resources
        mutexResources.lock();

        // create image containing histogram
        ImageOf<PixelBgr> imgConf;
        imgConf.resize(600,600);
        imgConf.zero();

        ostringstream tag;
        tag<<"blob_"<<i;

        // process scores on the given blob
        if (Bottle *blobScores=scores.find(tag.str().c_str()).asList())
        {
            // set up some variables and constraints
            size_t maxHeight=(size_t)(imgConf.height()*0.8);
            size_t minHeight=imgConf.height()-20;
            size_t widthStep=(blobScores->size()>0)?(size_t)(imgConf.width()/blobScores->size()):0;
            set<string> gcFilters;

            // cycle over classes
            for (unsigned int j=0; j<blobScores->size(); j++)
            {
                Bottle *item=blobScores->get(j).asList();
                if (item==nullptr)
                    continue;

                string name=item->get(0).asString().c_str();
                double score=std::max(std::min(item->get(1).asDouble(),1.0),0.0);

                // smooth out quickly varying scores
                map<string,Filter*>::iterator it=histFiltersPool.find(name);

                // create filter if not available
                if (it==histFiltersPool.end())
                {
                    Vector num(histFilterLength,1.0);
                    Vector den(histFilterLength,0.0); den[0]=histFilterLength;
                    histFiltersPool[name]=new Filter(num,den,Vector(1,score));
                }
                else
                {
                    Vector scoreFilt=it->second->filt(Vector(1,score));
                    score=scoreFilt[0];
                }

                // put the class name in a convenient set for garbage collection
                gcFilters.insert(name);

                int classHeight=std::min((int)minHeight,(int)imgConf.height()-(int)(maxHeight*score));

                cvRectangle(imgConf.getIplImage(),cvPoint(j*widthStep,classHeight),cvPoint((j+1)*widthStep,minHeight),
                            histColorsCode[j%(int)histColorsCode.size()],CV_FILLED);

                cv::Mat textImg=cv::Mat::zeros(imgConf.height(),imgConf.width(),CV_8UC3);
                cv::putText(textImg,name.c_str(),cvPoint(imgConf.width()-580,(j+1)*widthStep-10),
                            cv::FONT_HERSHEY_SIMPLEX,0.8,cv::Scalar(255,255,255),2);
                rotate(textImg,90.0,textImg);

                cv::Mat orig=cv::cvarrToMat((IplImage*)imgConf.getIplImage());
                orig=orig+textImg;
            }

            // draw the blob snapshot
            CvPoint tl,br,sz;
            Bottle *item=blobs.get(i).asList();
            tl.x=(int)item->get(0).asDouble();
            tl.y=(int)item->get(1).asDouble();
            br.x=(int)item->get(2).asDouble();
            br.y=(int)item->get(3).asDouble();
            sz.x=br.x-tl.x;
            sz.y=br.y-tl.y;

            // copy the blob
            ImageOf<PixelBgr> imgTmp1;
            imgTmp1.resize(sz.x,sz.y);
            cvSetImageROI((IplImage*)imgRtLoc.getIplImage(),cvRect(tl.x,tl.y,sz.x,sz.y));
            cvCopy(imgRtLoc.getIplImage(),imgTmp1.getIplImage());
            cvResetImageROI((IplImage*)imgRtLoc.getIplImage());

            // resize the blob
            ImageOf<PixelBgr> imgTmp2;
            int f=2;    // magnifying factor
            imgTmp2.resize(f*imgTmp1.width(),f*imgTmp1.height());
            cvResize(imgTmp1.getIplImage(),imgTmp2.getIplImage());

            // superimpose the blob on the histogram
            cvSetImageROI((IplImage*)imgConf.getIplImage(),cvRect(0,0,imgTmp2.width(),imgTmp2.height()));
            cvCopy(imgTmp2.getIplImage(),imgConf.getIplImage());
            cvRectangle(imgConf.getIplImage(),cvPoint(0,0),cvPoint(imgTmp2.width(),imgTmp2.height()),cvScalar(255,255,255),3);

            // give chance for disposing filters that are no longer used (one at time)
            if (histFiltersPool.size()>blobScores->size())
            {
                for (auto& it : histFiltersPool)
                {
                    if (gcFilters.find(it.first)==gcFilters.end())
                    {
                        delete it.second;
                        histFiltersPool.erase(it.first);
                        break;
                    }
                }
            }

        }

        imgHistogram.prepare()=imgConf;
        imgHistogram.write();

        // release resources
        mutexResources.unlock();
    }
}


/**********************************************************/
int IOL2OPCBridge::findClosestBlob(const Bottle &blobs,
                                   const CvPoint &loc)
{
    int ret=RET_INVALID;
    double min_d2=std::numeric_limits<double>::max();

    for (unsigned int i=0; i<blobs.size(); i++)
    {
        CvPoint cog=getBlobCOG(blobs,i);
        if ((cog.x==RET_INVALID) || (cog.y==RET_INVALID))
            continue;

        double dx=loc.x-cog.x;
        double dy=loc.y-cog.y;
        double d2=dx*dx+dy*dy;

        if (d2<min_d2)
        {
            min_d2=d2;
            ret=i;
        }
    }

    return ret;
}


/**********************************************************/
int IOL2OPCBridge::findClosestBlob(const Bottle &blobs,
                                   const Vector &loc)
{
    int ret=RET_INVALID;
    double curMinDist=std::numeric_limits<double>::max();
    
    for (unsigned int i=0; i<blobs.size(); i++)
    {
        CvPoint cog=getBlobCOG(blobs,i);
        if ((cog.x==RET_INVALID) || (cog.y==RET_INVALID))
            continue;

        Vector x;
        if (get3DPosition(cog,x))
        {
            double dist=norm(loc-x);
            if (dist<curMinDist)
            {
                ret=i;
                curMinDist=dist;
            }
        }
    }

    return ret;
}


/**********************************************************/
Bottle IOL2OPCBridge::classify(const Bottle &blobs)
{
    // grab resources
    mutexResources.lock();

    imgClassifier.write(imgRtLoc);

    Bottle cmd,reply;
    cmd.addVocab(Vocab::encode("classify"));
    Bottle &options=cmd.addList();
    for (unsigned int i=0; i<blobs.size(); i++)
    {
        ostringstream tag;
        tag<<"blob_"<<i;
        Bottle &item=options.addList();
        item.addString(tag.str().c_str());
        item.addList()=*blobs.get(i).asList();
    }
    yInfoGated("Sending classification request: %s",cmd.toString().c_str());
    rpcClassifier.write(cmd,reply);
    yInfoGated("Received reply: %s",reply.toString().c_str());

    // release resources
    mutexResources.unlock();

    return reply;
}


/**********************************************************/
void IOL2OPCBridge::train(const string &object, const Bottle &blobs,
                          const int i)
{
    // grab resources
    mutexResources.lock();

    imgClassifier.write(imgRtLoc);

    Bottle cmd,reply;
    cmd.addVocab(Vocab::encode("train"));
    Bottle &options=cmd.addList().addList();
    options.addString(object.c_str());

    if (i<0)
    {
        Bottle &noBlob=options.addList();
        noBlob.addDouble(0.0);
        noBlob.addDouble(0.0);
        noBlob.addDouble(0.0);
        noBlob.addDouble(0.0);
    }
    else
        options.add(blobs.get(i));

    yInfoGated("Sending training request: %s",cmd.toString().c_str());
    rpcClassifier.write(cmd,reply);
    yInfoGated("Received reply: %s",reply.toString().c_str());

    // release resources
    mutexResources.unlock();
}


/**********************************************************/
void IOL2OPCBridge::doLocalization()
{
    if (state==Bridge::localization)
    {
        // acquire image for classification/training
        acquireImage();
        // grab the blobs
        Bottle blobs=getBlobs();
        // get the scores from the learning machine
        Bottle scores=classify(blobs);
        // update location of histogram display
        if (Bottle *loc=histObjLocPort.read(false))
        {
            if (loc->size()>=2)
            {
                Vector x;
                if (get3DPosition(cvPoint(loc->get(0).asInt(),loc->get(1).asInt()),x))
                    histObjLocation=x;
            }
        }
        // find the closest blob to the location of histogram display
        int closestBlob=findClosestBlob(blobs,histObjLocation);
        // draw the blobs
        drawBlobs(blobs,closestBlob,scores);
        // draw scores histogram
        drawScoresHistogram(blobs,scores,closestBlob);

        // data for opc update
        mutexResourcesOpc.lock();
        opcBlobs=blobs;
        opcScores=scores;
        mutexResourcesOpc.unlock();
    }
}

ObjectArea IOL2OPCBridge::getReachableArea(const Vector &objpos) {
    if ((objpos[0]>human_area_x_bounds[0]) && (objpos[0]<human_area_x_bounds[1]) &&
        (objpos[1]>human_area_y_bounds[0]) && (objpos[1]<human_area_y_bounds[1])) {
        return ObjectArea::HUMAN;
    } else if ((objpos[0]>shared_area_x_bounds[0]) && (objpos[0]<shared_area_x_bounds[1]) &&
               (objpos[1]>shared_area_y_bounds[0]) && (objpos[1]<shared_area_y_bounds[1])) {
        return ObjectArea::SHARED;
    } else if ((objpos[0]>robot_area_x_bounds[0]) && (objpos[0]<robot_area_x_bounds[1]) &&
               (objpos[1]>robot_area_y_bounds[0]) && (objpos[1]<robot_area_y_bounds[1])) {
        return ObjectArea::ROBOT;
    } else {
        return ObjectArea::NOTREACHABLE;
    }
}

/**********************************************************/
void IOL2OPCBridge::updateOPC()
{
    if ((state==Bridge::localization) && opc->isConnected())
    {
        Bottle& bStreamObjLoc = objLocOut.prepare();
        bStreamObjLoc.clear();

        // grab resources
        LockGuard lg(mutexResources);
        opc->checkout();

        // latch image
        ImageOf<PixelBgr> &imgLatch=imgTrackOut.prepare();
        imgLatch=imgRtLoc;

        mutexResourcesOpc.lock();
        Bottle blobs=opcBlobs;
        Bottle scores=opcScores;
        mutexResourcesOpc.unlock();

        // reset internal tracking state
        for (auto& it : db)
            it.second.prepare();

        // check detected objects
        bool unknownObjectInScene=false;
        for (unsigned int j=0; j<blobs.size(); j++)
        {
            Bottle *item=blobs.get(j).asList();
            if (item==nullptr)
                continue;

            ostringstream tag;
            tag<<"blob_"<<j;

            // find the blob name (or unknown)
            string object=findName(scores,tag.str());
            if (object!=OBJECT_UNKNOWN)
            {
                auto it=db.find(object);
                if (it!=db.end())
                {
                    CvPoint cog=getBlobCOG(blobs,j);
                    if ((cog.x==RET_INVALID) || (cog.y==RET_INVALID))
                        continue;

                    // compute the bounding box
                    CvPoint tl,br;
                    tl.x=(int)item->get(0).asDouble();
                    tl.y=(int)item->get(1).asDouble();
                    br.x=(int)item->get(2).asDouble();
                    br.y=(int)item->get(3).asDouble();

                    CvRect bbox=cvRect(tl.x,tl.y,br.x-tl.x,br.y-tl.y);
                    if (thresBBox(bbox,imgLatch))
                    {
                        it->second.latchBBox(bbox);
                        it->second.heartBeat();
                    }
                }
            }
            else
                unknownObjectInScene=true;
        }

        // cycle over objects to handle tracking
        for (auto& it : db)
            it.second.track(imgLatch);

        CvFont font;
        cvInitFont(&font,CV_FONT_HERSHEY_SIMPLEX,0.5,0.5,0,1);                

        // perform operations
        for (auto& it : db)
        {
            string object=it.first;
            Object *obj=opc->addOrRetrieveEntity<Object>(object);

            // garbage collection
            if (it.second.isDead())
            {
                if (object_persistence)
                {
                    if (obj->m_present!=0.0)
                        obj->m_present=0.5; 
                }
                else
                    obj->m_present=0.0; 
                continue;
            }

            CvRect bbox;
            if (it.second.is_tracking(bbox))
            {
                CvPoint cog;
                cog.x=bbox.x+(bbox.width>>1);
                cog.y=bbox.y+(bbox.height>>1);

                // find 3d position
                Vector  dim(3,0.05);
                Vector  x;
                bool    foundObj = false;

                if (useSPQ && connectedSPQ)
                {
                    // get dim and x directly from superquadric
                    // reading should be here
                    yInfo("CvPoint [%d, %d]",cog.x,cog.y);
                    foundObj = getSuperQuadric(cog, x, dim);
                    yInfo("dim from superquadratic %s",dim.toString(3,3).c_str());
                    yInfo("pos from superquadratic %s",x.toString(3,3).c_str());
                }
                else
                {
                    // otherwise use SFM only
                    foundObj = get3DPosition(cog,x);
                }

                if (foundObj)
                {
                    Bottle bObjNameLoc;
                    Vector x_filtered,dim_filtered;
                    it.second.filt(x,x_filtered,dim,dim_filtered);

                    Vector objpos;
                    objpos=x_filtered;
                    obj->m_ego_position=objpos;
                    obj->m_dimensions=dim_filtered;
                    obj->m_present=1.0;
                    obj->m_objectarea = getReachableArea(obj->m_ego_position);

                    bObjNameLoc.addString(obj->name());
                    bObjNameLoc.addString(obj->m_ego_position.toString());

                    bStreamObjLoc.addList() = bObjNameLoc;

                    // threshold bbox
                    if (thresBBox(bbox,imgLatch))
                    {
                        // Extract color information from blob
                        // create temporary image, and copy blob in there
                        ImageOf<PixelBgr> imgTmp;
                        imgTmp.resize(bbox.width,bbox.height);
                        cvSetImageROI((IplImage*)imgLatch.getIplImage(),bbox);
                        cvCopy(imgLatch.getIplImage(),imgTmp.getIplImage());
                        cvResetImageROI((IplImage*)imgLatch.getIplImage());

                        // now get mean color of blob, and fill the OPC object with the information
                        // be careful: computation done in BGR => save in RGB for displaying purpose
                        CvScalar meanColor=cvAvg(imgTmp.getIplImage());
                        obj->m_color[0]=(int)meanColor.val[2];
                        obj->m_color[1]=(int)meanColor.val[1];
                        obj->m_color[2]=(int)meanColor.val[0];

                        cvRectangle(imgLatch.getIplImage(),cvPoint(bbox.x,bbox.y),
                                    cvPoint(bbox.x+bbox.width,bbox.y+bbox.height),cvScalar(255,0,0),2);

                        cvPutText(imgLatch.getIplImage(),object.c_str(),
                                  cvPoint(bbox.x,bbox.y-5),&font,cvScalar(255,0,0));
                    }
                }
            }
        }

        if (!unknownObjectInScene)
            onlyKnownObjects.heartBeat();

        objLocOut.write();
        opc->commit();

        if (imgTrackOut.getOutputCount()>0)
            imgTrackOut.write();
        else
            imgTrackOut.unprepare();
    }
}

void IOL2OPCBridge::setBounds(ResourceFinder &rf, Vector& bounds, string configName, double std_lower, double std_upper) {
    bounds.resize(2);
    bounds[0]=std_lower;
    bounds[1]=std_upper;
    if (rf.check(configName))
    {
        if (Bottle *rf_bounds=rf.find(configName).asList())
        {
            if (rf_bounds->size()==2)
            {
                bounds[0]=rf_bounds->get(0).asDouble();
                bounds[1]=rf_bounds->get(1).asDouble();
            }
            else
            {
                yWarning() << configName << " does not have correct length, using default values!";
            }
        }
        else
        {
            yWarning() << configName << " is not a list, using default values!";
        }
    }
    else
    {
        yWarning() << configName << " not specified, using default values!";
    }
}


/**********************************************************/
bool IOL2OPCBridge::configure(ResourceFinder &rf)
{
    string name=rf.check("name",Value("iol2opc")).asString().c_str();
    period=rf.check("period",Value(0.1)).asDouble();
    verbose=rf.check("verbose");
    empty=rf.check("empty");
    object_persistence=(rf.check("object_persistence",Value("off")).asString()=="on");
    useSPQ=(rf.check("use_superquadric",Value("off")).asString()=="on");
    if (useSPQ)
        yInfo("[%s] useSPQ set on",name.c_str());
    else
        yInfo("[%s] useSPQ set off",name.c_str());

    opc=new OPCClient(name);
    if (!opc->connect(rf.check("opcName",Value("OPC")).asString().c_str()))
    {
        yError("OPC doesn't seem to be running!");
        return false;
    }
    opc->checkout();

    imgIn.open(("/"+name+"/img:i").c_str());
    blobExtractor.open(("/"+name+"/blobs:i").c_str());
    imgRtLocOut.open(("/"+name+"/imgLoc:o").c_str());
    imgTrackOut.open(("/"+name+"/imgTrack:o").c_str());
    imgSelBlobOut.open(("/"+name+"/imgSel:o").c_str());
    imgClassifier.open(("/"+name+"/imgClassifier:o").c_str());
    imgHistogram.open(("/"+name+"/imgHistogram:o").c_str());
    histObjLocPort.open(("/"+name+"/histObjLocation:i").c_str());

    //new port to stream obj localisation
    objLocOut.open(("/"+name+"/objLoc:o").c_str());

    rpcPort.open(("/"+name+"/rpc").c_str());
    rpcClassifier.open(("/"+name+"/classify:rpc").c_str());
    rpcGet3D.open(("/"+name+"/get3d:rpc").c_str());
    getClickPort.open(("/"+name+"/getClick:i").c_str());

    // new port connections for superqudric-model
    if (rpcGetBlobPoints.open(("/"+name+"/blob/rpc").c_str()))
        yInfo("[%s] Open port rpcGetBlobPoints to connect to lbpExtract rpc port", name.c_str());
    std::string blobrpc = "/lbpExtract/rpc:i";
    connectedSPQ = yarp::os::Network::connect(rpcGetBlobPoints.getName().c_str(),blobrpc);
    if (!connectedSPQ)
        yError("[%s] Unable to connect to lbpExtract rpc port", name.c_str());
    else
        yInfo("[%s] Connected to lbpExtract rpc port", name.c_str());

    if (rpcGetSPQ.open(("/"+name+"/superquadric-model/rpc").c_str()))
        yInfo("[%s] Open port rpcGetSPQ to connect to superquadric-model rpc port", name.c_str());
    std::string SPQrpc = "/superquadric-model/rpc";
    connectedSPQ &= yarp::os::Network::connect(rpcGetSPQ.getName().c_str(),SPQrpc);
    if (!connectedSPQ)
        yError("[%s] Unable to connect to superquadric-model rpc port", name.c_str());
    else
        yInfo("[%s] Connected to superquadric-model rpc port", name.c_str());


    setBounds(rf, skim_blobs_x_bounds,  "skim_blobs_x_bounds",  -0.70, -0.10);
    setBounds(rf, skim_blobs_y_bounds,  "skim_blobs_y_bounds",  -0.30, -0.30);
    setBounds(rf, human_area_x_bounds,  "human_area_x_bounds",  -0.70, -0.50);
    setBounds(rf, human_area_y_bounds,  "human_area_y_bounds",  -0.30, 0.30);
    setBounds(rf, shared_area_x_bounds, "shared_area_x_bounds", -0.50, -0.35);
    setBounds(rf, shared_area_y_bounds, "shared_area_y_bounds", -0.30, 0.30);
    setBounds(rf, robot_area_x_bounds,  "robot_area_x_bounds",  -0.35, -0.10);
    setBounds(rf, robot_area_y_bounds,  "robot_area_y_bounds",  -0.30, 0.30);

    // location used to display the
    // histograms upon the closest blob
    histObjLocation.resize(3);
    histObjLocation[0]=-0.3;
    histObjLocation[1]=0.0;
    histObjLocation[2]=-0.1;

    rtLocalization.setBridge(this);
    rtLocalization.setRate(rf.check("rt_localization_period",Value(30)).asInt());

    opcUpdater.setBridge(this);
    opcUpdater.setRate(rf.check("opc_update_period",Value(60)).asInt());
    opcMedianFilterOrder=rf.check("opc_median_window",Value(5)).asInt();

    classifierReporter.setBridge(this);

    blobs_detection_timeout=rf.check("blobs_detection_timeout",Value(0.2)).asDouble();
    histFilterLength=std::max(1,rf.check("hist_filter_length",Value(10)).asInt());
    presence_timeout=std::max(0.0,rf.check("presence_timeout",Value(1.0)).asDouble());
    tracker_type=rf.check("tracker_type",Value("BOOSTING")).asString().c_str();
    tracker_timeout=std::max(0.0,rf.check("tracker_timeout",Value(5.0)).asDouble());

    tracker_min_blob_size.resize(2,0);
    if (rf.check("tracker_min_blob_size"))
    {
        if (Bottle *size=rf.find("tracker_min_blob_size").asList())
        {
            if (size->size()>=2)
            {
                tracker_min_blob_size[0]=size->get(0).asInt();
                tracker_min_blob_size[1]=size->get(1).asInt();
            }
        }
    }

    imgRtLoc.resize(320,240);
    imgRtLoc.zero();

    histColorsCode.push_back(cvScalar( 65, 47,213));
    histColorsCode.push_back(cvScalar(122, 79, 58));
    histColorsCode.push_back(cvScalar(154,208, 72));
    histColorsCode.push_back(cvScalar( 71,196,249));
    histColorsCode.push_back(cvScalar(224,176, 96));
    histColorsCode.push_back(cvScalar( 22,118,238));

    clickLocation=cvPoint(RET_INVALID,RET_INVALID);

    attach(rpcPort);

    lastBlobsArrivalTime=0.0;
    rtLocalization.start();
    opcUpdater.start();

    state=Bridge::idle;
    rpcClassifier.setReporter(classifierReporter);
    return true;
}


/**********************************************************/
bool IOL2OPCBridge::interruptModule()
{
    yDebug() << "Interrupt iol2opc";
    rtLocalization.stop();
    opcUpdater.stop();
    yDebug() << "iol2opc threads stopped";

    imgIn.interrupt();
    imgRtLocOut.interrupt();
    imgTrackOut.interrupt();
    imgSelBlobOut.interrupt();
    imgClassifier.interrupt();
    imgHistogram.interrupt();
    histObjLocPort.interrupt();
    rpcPort.interrupt();

    yDebug() << "iol2opc half way through interrupt";

    objLocOut.interrupt();
    blobExtractor.interrupt();
    rpcClassifier.interrupt();
    getClickPort.interrupt();
    rpcGet3D.interrupt();
    rpcGetBlobPoints.interrupt();
    rpcGetSPQ.interrupt();
    opc->interrupt();
    yDebug() << "Interrupt finished";

    return true;
}


/**********************************************************/
bool IOL2OPCBridge::close()
{
    yDebug() << "Close iol2opc";
    LockGuard lg(mutexResources);

    rtLocalization.stop();
    opcUpdater.stop();

    rpcPort.interrupt();
    rpcPort.close();
    imgIn.interrupt();
    imgIn.close();
    imgRtLocOut.interrupt();
    imgRtLocOut.close();
    imgTrackOut.interrupt();
    imgTrackOut.close();
    imgSelBlobOut.interrupt();
    imgSelBlobOut.close();
    imgClassifier.interrupt();
    imgClassifier.close();
    imgHistogram.interrupt();
    imgHistogram.close();
    histObjLocPort.interrupt();
    histObjLocPort.close();
    objLocOut.interrupt();
    objLocOut.close();
    blobExtractor.interrupt();
    blobExtractor.close();
    rpcClassifier.interrupt();
    rpcClassifier.close();
    getClickPort.interrupt();
    getClickPort.close();
    rpcGetBlobPoints.interrupt();
    rpcGetBlobPoints.close();
    rpcGetSPQ.interrupt();
    rpcGetSPQ.close();
    rpcGet3D.interrupt();
    rpcGet3D.close();
    opc->interrupt();
    opc->close();

    delete opc;

    // dispose filters used for scores histogram
    for (auto& it : histFiltersPool)
        delete it.second;

    return true;
}


/**********************************************************/
double IOL2OPCBridge::getPeriod()
{
    return period;
}


/**********************************************************/
bool IOL2OPCBridge::updateModule()
{
    if (state==Bridge::load_database)
    {
        Bottle cmd,reply;
        cmd.addVocab(Vocab::encode("list"));
        yInfoGated("Sending list request: %s",cmd.toString().c_str());
        rpcClassifier.write(cmd,reply);
        yInfoGated("Received reply: %s",reply.toString().c_str());

        if (reply.get(0).asString()=="ack")
        {
            if (Bottle *names=reply.get(1).asList())
                for (unsigned int i=0; i<names->size(); i++)
                    db[names->get(i).asString().c_str()]=IOLObject(opcMedianFilterOrder,presence_timeout,
                                                                   tracker_type,tracker_timeout);

            if (empty)
                remove_all();

            yInfoGated("Turning localization on");
            state=Bridge::localization;
            onlyKnownObjects=IOLObject(opcMedianFilterOrder,10.0);
        }
    }
    // highlight selected blob
    else if (state==Bridge::localization)
    {
        CvPoint loc;
        if (imgSelBlobOut.getOutputCount()>0)
        {
            mutexResourcesOpc.lock();
            Bottle blobs=opcBlobs;
            mutexResourcesOpc.unlock();

            mutexResources.lock();
            ImageOf<PixelBgr> imgLatch=this->imgRtLoc;
            mutexResources.unlock();

            if (getClickPosition(loc))
            {
                int i=findClosestBlob(blobs,loc);
                if (i!=RET_INVALID)
                {
                    // latch image
                    CvPoint tl,br;
                    Bottle *item=blobs.get(i).asList();
                    tl.x=(int)item->get(0).asDouble();
                    tl.y=(int)item->get(1).asDouble();
                    br.x=(int)item->get(2).asDouble();
                    br.y=(int)item->get(3).asDouble();

                    cvRectangle(imgLatch.getIplImage(),tl,br,cvScalar(0,255,0),2);
                }
            }

            imgSelBlobOut.prepare()=imgLatch;
            imgSelBlobOut.write();
        }

        if (onlyKnownObjects.isDead())
        {
            // grab resources
            mutexResourcesOpc.lock();
            Bottle blobs=opcBlobs;
            Bottle scores=opcScores;
            mutexResourcesOpc.unlock();

            for (unsigned int j=0; j<blobs.size(); j++)
            {
                ostringstream tag;
                tag<<"blob_"<<j;

                // find the blob name (or unknown)
                string object=findName(scores,tag.str());
                if (object==OBJECT_UNKNOWN)
                {
                    mutexResources.lock();
                    opc->checkout();
                    Object* obj=opc->addEntity<Object>("unknown_object");
                    opc->commit(obj);
                    mutexResources.unlock();

                    db[obj->name()]=IOLObject(opcMedianFilterOrder,presence_timeout,
                                              tracker_type,tracker_timeout);
                    train(obj->name(),blobs,j);                    
                    break;
                }
            }

            onlyKnownObjects.heartBeat();
        }
    }

    return true;
}


/**********************************************************/
bool IOL2OPCBridge::attach(RpcServer &source)
{
    return this->yarp().attachAsServer(source);
}


/**********************************************************/
bool IOL2OPCBridge::train_object(const string &name)
{
    if (!opc->isConnected())
    {
        yError("No connection to OPC");
        return false;
    }

    CvPoint loc;
    if (getClickPosition(loc))
    {
        mutexResourcesOpc.lock();
        Bottle blobs=opcBlobs;
        mutexResourcesOpc.unlock();

        int i=findClosestBlob(blobs,loc);
        if (i==RET_INVALID)
            return false;

        train(name,blobs,i);

        // grab resources
        LockGuard lg(mutexResources);

        // add a new object in the database
        // if not already existing
        if (db.find(name)==db.end())
            db[name]=IOLObject(opcMedianFilterOrder,presence_timeout,
                               tracker_type,tracker_timeout);

        return true;
    }
    else
    {
        yError("Could not retrieve click location");
        return false;
    }
}


/**********************************************************/
bool IOL2OPCBridge::remove_object(const string &name)
{
    if (!opc->isConnected())
    {
        yError("No connection to OPC");
        return false;
    }

    // grab resources
    LockGuard lg(mutexResources);

    Bottle cmdClassifier,replyClassifier;
    cmdClassifier.addVocab(Vocab::encode("forget"));
    cmdClassifier.addString(name.c_str());
    yInfoGated("Sending clearing request: %s",cmdClassifier.toString().c_str());
    rpcClassifier.write(cmdClassifier,replyClassifier);
    yInfoGated("Received reply: %s",replyClassifier.toString().c_str());

    opc->checkout();

    bool success = false;
    auto it=db.find(name);
    if (it!=db.end()) {
        success = opc->removeEntity(it->first);
        db.erase(it);
    }

    return success;
}


/**********************************************************/
bool IOL2OPCBridge::remove_all()
{
    if (!opc->isConnected())
    {
        yError("No connection to OPC");
        return false;
    }

    // grab resources
    LockGuard lg(mutexResources);

    Bottle cmdClassifier,replyClassifier;
    cmdClassifier.addVocab(Vocab::encode("forget"));
    cmdClassifier.addString("all");
    yInfoGated("Sending clearing request: %s",cmdClassifier.toString().c_str());
    rpcClassifier.write(cmdClassifier,replyClassifier);
    yInfoGated("Received reply: %s",replyClassifier.toString().c_str());

    opc->checkout();

    bool success = true;
    for (auto& it : db)
        success &= opc->removeEntity(it.first);

    db.clear();
    return success;
}


/**********************************************************/
bool IOL2OPCBridge::change_name(const string &old_name,
                                const string &new_name)
{
    if (!opc->isConnected())
    {
        yError("No connection to OPC");
        return false;
    }

    // grab resources
    LockGuard lg(mutexResources);

    Bottle cmdClassifier,replyClassifier;
    cmdClassifier.addVocab(createVocab('c','h','n','a'));
    cmdClassifier.addString(old_name);
    cmdClassifier.addString(new_name);
    yInfoGated("Sending change name request: %s",cmdClassifier.toString().c_str());
    rpcClassifier.write(cmdClassifier,replyClassifier);
    yInfoGated("Received reply: %s",replyClassifier.toString().c_str());

    if (replyClassifier.get(0).asString()=="nack")
    {
        yError("Classifier did not allow name change.");
        yError("Is there already an object with this name in the classifier database?");
        return false;
    }
    else if (new_name!=old_name)
    {   
        const auto it_new=db.find(new_name);
        if (it_new!=db.end())
        {
            yError("\"%s\" is already existing in the database",new_name.c_str());
            return false;
        }

        const auto it_old=db.find(old_name);
        if (it_old!=db.end())
        {
            db[new_name]=IOLObject(opcMedianFilterOrder,presence_timeout,
                                   tracker_type,tracker_timeout);
            opc->checkout();
            opc->removeEntity(it_old->first);
            db.erase(it_old);
            yInfoGated("Name change successful: reloading local cache");
        }
        else
        {
            yError("\"%s\" not present in the database",old_name.c_str());
            return false;
        }
    }
    else
    {
        yWarning("matching names: no operations performed");
        return true;
    }

    return true;
}


/**********************************************************/
bool IOL2OPCBridge::set_object_persistence(const string &sw)
{
    if (sw=="on")
        object_persistence=true;
    else if (sw=="off")
        object_persistence=false;
    else
        return false;

    return true;
}


/**********************************************************/
string IOL2OPCBridge::get_object_persistence()
{
    return (object_persistence?"on":"off");
}


/**********************************************************/
void IOL2OPCBridge::pause()
{
    mutexResources.lock();
}


/**********************************************************/
void IOL2OPCBridge::resume()
{
    mutexResources.unlock();
}