main.cpp

/*
 * Copyright (C) 2011 Department of Robotics Brain and Cognitive Sciences - Istituto Italiano di Tecnologia
 * Author: Vadim Tikhanoff, Carlo Ciliberto
 * email:  vadim.tikhanoff@iit.it
 * 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
 * http://www.robotcub.org/icub/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 <yarp/os/Network.h>
#include <yarp/os/BufferedPort.h>
#include <yarp/os/RFModule.h>
#include <yarp/os/PeriodicThread.h>
#include <yarp/os/Stamp.h>
 
#include <yarp/sig/Image.h>
#include <yarp/sig/Vector.h>
 
#include <yarp/cv/Cv.h>
 
#include <opencv2/core.hpp>
#include <opencv2/opencv.hpp>
 
#include <string>
#include <vector>
#include <iostream>
#include <fstream>
#include <cmath>
#include <mutex>
 
using namespace std;
using namespace yarp::os;
using namespace yarp::sig;
using namespace yarp::cv;
 
class BlobDetectorThread: public PeriodicThread
{
private:
    ResourceFinder                   &rf;
 
    BufferedPort<ImageOf<PixelMono>> port_i_img;
    BufferedPort<Image>              port_i_propImg; // extra port just to propagate image
    Port                             port_o_img;
    Port                             port_o_propImg;
    Port                             port_o_blobs;
    Port                             port_o_clean;
    cv::Mat                          imgProp;
 
    int                              gaussian_winsize;
                                     
    double                           window_ratio;
    double                           thresh;
    int                              erode_itr;
    int                              dilate_itr;
    int                              maxHeight;
    int                              maxWidth;
                                     
    mutex                            mtx;
    mutex                            contours;
    Bottle                           blobs;
    Bottle                           non_blobs;
                                     
    int                              offset;
                                     
    Vector                           area, orientation, axe1, axe2;
    int                              numBlobs;
                                     
    cv::Point                        tmpCenter[500], pt1, pt2;
    int                              numObj;
    double                           thetatmp;
 
public:
    BlobDetectorThread(ResourceFinder &_rf)
        :PeriodicThread(0.005),rf(_rf) { }
    
    string details;
 
    virtual bool threadInit()
    {
        string name=rf.find("name").asString();
 
        port_i_img.open("/"+name+"/img:i");
        port_i_propImg.open("/"+name+"/propImg:i");
        port_o_img.open("/"+name+"/img:o");
        port_o_propImg.open("/"+name+"/propImg:o");
 
        port_o_blobs.open("/"+name+"/blobs:o");
        port_o_clean.open("/"+name+"/binary:o");
 
        gaussian_winsize=rf.check("gaussian_winsize",Value(9)).asInt32();
 
        thresh=rf.check("thresh",Value(10.0)).asFloat64();
        erode_itr=rf.check("erode_itr",Value(8)).asInt32();
        dilate_itr=rf.check("dilate_itr",Value(3)).asInt32();
        window_ratio=rf.check("window_ratio",Value(0.6)).asFloat64();
 
        maxHeight = rf.check("maxHeight",Value(150)).asInt32();
        maxWidth  = rf.check("maxWidth",Value(150)).asInt32();
        
        details=rf.check("details",Value("off")).asString();
 
        offset=rf.check("offset",Value(0)).asInt32();
        //details = false;
        numBlobs = 0;
        orientation.clear();
        axe1.clear();
        axe2.clear();
        area.clear();
        area.resize(500);
        orientation.resize(500);
        axe1.resize(500);
        axe2.resize(500);
 
        return true;
    }
 
    virtual void setThreshold(double newThreshold)
    {
        lock_guard<mutex> lg(mtx);
        thresh = newThreshold;
    }
 
    virtual void run()
    {
        if (ImageOf<PixelMono> *img=port_i_img.read(false))
        {
            Stamp ts;
            port_i_img.getEnvelope(ts);
 
            cv::Mat gray=toCvMat(*img);
            cv::Mat imgProp=gray.clone();
            imgProp.setTo(cv::Scalar::all(0));
 
            cv::GaussianBlur(gray,gray,cv::Size(gaussian_winsize,gaussian_winsize),0.0);
            cv::threshold(gray,gray,thresh,255.0,cv::THRESH_BINARY);
            cv::equalizeHist(gray,gray); //normalize brightness and increase contrast
            cv::erode(gray,gray,cv::Mat(),cv::Point(-1,-1),erode_itr);
            cv::dilate(gray,gray,cv::Mat(),cv::Point(-1,-1),dilate_itr);
 
            lock_guard<mutex> lg(mtx);
            blobs.clear();
            non_blobs.clear();
 
            int w_offset=cvRound(0.5*gray.size().width*(1.0-window_ratio));
            int h_offset=cvRound(0.5*gray.size().height*(1.0-window_ratio));
            int itr = 0;
            for (int row=h_offset; row<gray.size().height-h_offset; row++)
            {
                for (int col=w_offset; col<gray.size().width-w_offset; col++)
                {
                    if (gray.at<uchar>(row,col)==255)
                    {
                        cv::Rect comp;
                        cv::floodFill(gray,cv::Point(col,row),cv::Scalar(255-(blobs.size()+non_blobs.size()+1)),&comp);
 
                        if (5<comp.width && comp.width<maxWidth && 5<comp.height && comp.height<maxHeight)
                        {
                            Bottle &b=blobs.addList();
                            b.addFloat64(comp.x-offset);
                            b.addFloat64(comp.y-offset);
                            b.addFloat64(comp.x+comp.width+offset);
                            b.addFloat64(comp.y+comp.height+offset);
                            if (details=="on")
                            {
                                if (orientation.size() > 0 )
                                {
                                    b.addFloat64(orientation[itr+1]);
                                    b.addInt32((int)axe2[itr+1]);
                                    b.addInt32((int)axe1[itr+1]);
                                }                         
                            }
                            itr++;
                        }
                        else
                        {
                            Bottle &n=non_blobs.addList();
                            n.addFloat64(comp.x-offset);
                            n.addFloat64(comp.y-offset);
                            n.addFloat64(comp.x+comp.width+offset);
                            n.addFloat64(comp.y+comp.height+offset);
                            if (details=="on")
                            {
                                if (orientation.size() > 0 )
                                {
                                    n.addFloat64(orientation[itr+1]);
                                    n.addInt32((int)axe2[itr+1]);
                                    n.addInt32((int)axe1[itr+1]);
                                }                         
                            }
                            itr++;
                        }
                    }
                }
            }
 
            cleanBlobs(gray);
 
            if (details=="on")
            {
                lock_guard<mutex> lg(contours);
                processImg(gray);
            }
            
            port_o_img.setEnvelope(ts);
            port_o_img.write(*img);
 
            port_o_blobs.setEnvelope(ts);
            port_o_blobs.write(blobs);
            Image *prop= port_i_propImg.read(false);  
            
            if (prop!=NULL)
            {
                port_o_propImg.setEnvelope(ts);
                port_o_propImg.write(*prop);
            }
 
            if (!imgProp.empty())
            {
                ImageOf<PixelMono> sendImg;
                sendImg.resize(imgProp.size().width,imgProp.size().height);
                imgProp.copyTo(toCvMat(sendImg));
                port_o_clean.setEnvelope(ts);
                port_o_clean.write(sendImg);
            }
        }
    }
 
    virtual void threadRelease()
    {
        port_i_img.close();
        port_o_img.close();
        port_o_blobs.close();
        port_i_propImg.close();   
        port_o_clean.close();
        port_o_propImg.close();
    }
 
    bool execReq(const Bottle &command, Bottle &reply)
    {
        if(command.get(0).asVocab32()==Vocab32::encode("thresh"))
        {
            lock_guard<mutex> lg(mtx);
            thresh = command.get(1).asFloat64();
            reply.addVocab32("ok");
            return true;
        }
        if(command.get(0).asVocab32()==Vocab32::encode("erode"))
        {
            lock_guard<mutex> lg(mtx);
            erode_itr = command.get(1).asInt32();
            reply.addVocab32("ok");
            return true;
        }
        if(command.get(0).asVocab32()==Vocab32::encode("dilate"))
        {
            lock_guard<mutex> lg(mtx);
            dilate_itr = command.get(1).asInt32();
            reply.addVocab32("ok");
            return true;
        }
 
        return false;
    }
 
    void cleanBlobs(cv::Mat &image)
    {
        for (int i=0; i<blobs.size(); i++)
        {
            cv::Point cog(-1,-1);
            if ((i>=0) && (i<blobs.size()))
            {
                cv::Point tl,br;
                Bottle *item=blobs.get(i).asList();
                if (item==NULL)
                    cout << "ITEM IS NULL" << cog.x << cog.y <<endl;
 
                tl.x=(int)item->get(0).asFloat64() - 2;
                tl.y=(int)item->get(1).asFloat64() - 2;
                br.x=(int)item->get(2).asFloat64() + 2;
                br.y=(int)item->get(3).asFloat64() + 2;
            
                image(cv::Rect(tl.x,tl.y,br.x-tl.x,br.y-tl.y)).copyTo(imgProp(cv::Rect(tl.x,tl.y,br.x-tl.x,br.y-tl.y)));
            }
        }
    }
 
    void processImg(cv::Mat &image)
    {
        for (int i=0; i<blobs.size(); i++)
        {
            cv::Point cog(-1,-1);
            if ((i>=0) && (i<blobs.size()))
            {
                cv::Point tl,br;
                Bottle *item=blobs.get(i).asList();
                if (item==NULL)
                    cout << "ITEM IS NULL" << cog.x << cog.y <<endl;
 
                tl.x=(int)item->get(0).asFloat64() - 10;
                tl.y=(int)item->get(1).asFloat64() - 10;
                br.x=(int)item->get(2).asFloat64() + 10;
                br.y=(int)item->get(3).asFloat64() + 10;
 
                cog.x=(tl.x + br.x)>>1;
                cog.y=(tl.y + br.y)>>1;     
        
                cv::Mat imageRoi=image(cv::Rect(tl.x,tl.y,br.x-tl.x,br.y-tl.y));
                getOrientations(imageRoi);
            }
        }
        numBlobs = 0;
        numObj = 0;
    }
 
    void getOrientations(cv::Mat &image)
    {
        vector<vector<cv::Point>> cont;
        cv::findContours(image,cont,CV_RETR_LIST,CV_CHAIN_APPROX_NONE);
 
        //go first through all contours in order to find if there are some duplications
        for (auto &c:cont)
        {
            numObj++;
            cv::RotatedRect boxtmp = cv::minAreaRect(cv::Mat(c));
            tmpCenter[numObj].x = cvRound(boxtmp.center.x);
            tmpCenter[numObj].y = cvRound(boxtmp.center.y);
            area[numObj] = cv::contourArea(c);
        }
 
        //check for duplicate center points
        int inc = 0;
        Vector index(numObj);
        for (int x=1; x<numObj; x++)
        {
            if (abs( tmpCenter[x].x -tmpCenter[x+1].x ) < 10 )
            {
                if (abs( tmpCenter[x].y -tmpCenter[x+1].y) < 10)
                {
                    if ( area[x] < area[x+1] )
                    {
                        index[inc] = x;
                        inc++;
                    }
                    else
                    {
                        index[inc] = x+1;
                        inc++;
                    }
                } 
            }
        }
 
        for (auto &c:cont)
        {
            numBlobs++;
            for (int i= 0; i<inc; i++)
                if (numBlobs == index[i])
                    continue;
                    
            if (c.size()<6)
                continue;
 
            cv::RotatedRect box = cv::fitEllipse(cv::Mat(c));
            cv::Point center = box.center;
            cv::Size size(cvRound(box.size.width*0.5),cvRound(box.size.height*0.5));
 
            if ((box.size.width > 0) && (box.size.width < 300) && (box.size.height > 0) && (box.size.height < 300))
            {
                axe1[numBlobs] = size.width;
                axe2[numBlobs] = size.height;
            
                vector<float> line(4);
                cv::fitLine(c,line,CV_DIST_L2,0,0.01,0.01);
                float t = (box.size.width + box.size.height)/2;
                pt1.x = cvRound(line[2] - line[0] *t );
                pt1.y = cvRound(line[3] - line[1] *t );
                pt2.x = cvRound(line[2] + line[0] *t );
                pt2.y = cvRound(line[3] + line[1] *t );
                cv::line(image,pt1,pt2,cv::Scalar(255,255,255),2,cv::LINE_AA);
                thetatmp = 0.0;
                thetatmp = (180.0/M_PI) * atan2( (double)(pt2.y - pt1.y) , (double)(pt2.x - pt1.x) );
                
                thetatmp=(thetatmp<0.0?-thetatmp:180.0-thetatmp);
                orientation[numBlobs]=thetatmp;
            }
        }
    }
};
 
 
class BlobDetectorModule: public RFModule
{
private:
    BlobDetectorThread  *bdThr;
    Port                rpcPort;
 
public:
    BlobDetectorModule() { }
 
    virtual bool configure(ResourceFinder &rf)
    {
        bdThr=new BlobDetectorThread(rf);
 
        if(!bdThr->start())
        {
            delete bdThr;
            return false;
        }
 
        string name=rf.find("name").asString();
        rpcPort.open("/"+name+"/rpc");
        attach(rpcPort);
 
        return true;
    }
 
    virtual bool interruptModule()
    {
        rpcPort.interrupt();
 
        return true;
    }
 
    virtual bool close()
    {
        bdThr->stop();
        delete bdThr;
 
        rpcPort.close();
 
        return true;
    }
 
    virtual double getPeriod()
    {
        return 0.1;
    }
 
    virtual bool updateModule()
    {
        return true;
    }
 
    virtual bool respond(const Bottle &command, Bottle &reply)
    {
        reply.clear();
        if (command.get(0).asString()=="details") 
        {
            if (command.get(1).asString()=="on")
            { 
                bdThr->details = "on";
                reply.addString("setting details to ON");
                return true;
            }else
            {
                bdThr->details = "off";
                reply.addString("setting details to OFF");
                return true;
            }         
            return false;     
        } else if (command.get(0).asString()=="thresh") 
        {
            double newThresh = command.get(1).asFloat64();
            if (newThresh<0 || newThresh>255.0)
            {
                reply.addString("Invalid threshold (expecting a value between 0 and 255)");
                return false;
            }
            reply.addString("Setting threshold");
            bdThr->setThreshold(newThresh);
            return true;
        }
        if(bdThr->execReq(command,reply))
            return true;
        else
            return RFModule::respond(command,reply);
    }
};
 
 
int main(int argc, char *argv[])
{
    Network yarp;
 
    if (!yarp.checkNetwork())
        return 1;
 
    ResourceFinder rf;
    rf.setDefaultContext("blobExtractor");
    rf.setDefaultConfigFile("config.ini");
    rf.setDefault("name","blobExtractor");
    rf.configure(argc,argv);
 
    BlobDetectorModule mod;
 
    return mod.runModule(rf);
}