src/controllers/sensorFusion/detectRobotVisually.cpp

/* *************************************************************************
 * Check if we need this file in <defines.h>
 * *************************************************************************/

#include <defines.h>

#ifdef WITH_ROBOT_DETECTION

/* *************************************************************************
 * List dependencies
 * *************************************************************************/

// General libraries
#include <iostream>
#include <fstream>
#include <math.h>
#include <time.h>
#include <sstream>

// Symbricator3D library
//#include <controller/almControllers/sensorFusionComponent/saliencyDetector/imageFilter/intensityFilter.h>

// Plugin files
#include <controllers/sensorFusion/detectRobotVisually.h>
#include <controllers/positionControllers/defaultWheelController.h>

/* *************************************************************************
 * Shortcuts
 * *************************************************************************/

using namespace srCore;
using namespace std;
using namespace almendeSensorFusion;

/* *************************************************************************
 * Implementations
 * *************************************************************************/

using namespace srAlmende;

#define NO_LOCATION     DefaultWheelController::NO_LOCATION

DetectRobotVisually::DetectRobotVisually(const std::string &name,
                RobotActorBase *robotActor): SubController(name, robotActor),

                useAttention(true),                                             // A condition for the experiment
                useFusion(true),                                                // experiment condition

                saliencyD("Camera"),                                    // Create Saliency Detector for the camera image
                imageLocHist(3, NO_LOCATION),                   // Number of saliency history used for localisation
                energyClassHistImage(2, NO_LOCATION),   // Keeps track of the energy class that is classified
                energyClassHistAI(2, NO_LOCATION),              // Keeps track of the energy class that is classified

                localCounter(0),                                                // Local counter (used for counting camera images)
                imageScale(2),                                                  // Scale factor for images
                trackingGoal(false),
                experiment("sound-image-experiment") {

        LOG_INFO("Create visual robot detector");
        //Remove audio
        fusionInput.audio = NULL;

        // Init Saliency Detectors with a predefined set of filters used for certain task
        initSaliencyDetector();

        // Save learned weights using a timer
        saveTimer = time(NULL);
        trackTimer = time(NULL);

        lastImage = NULL;

        counter = 0;
}

void DetectRobotVisually::initSaliencyDetector() {
        std::vector<int> rFSizes                        = std::vector<int>(0);
        std::vector<ImageFilter*>* iFilters     = new std::vector<ImageFilter*>(0);

        rFSizes.push_back(2);
        rFSizes.push_back(4);
        rFSizes.push_back(6);
        rFSizes.push_back(8);
        rFSizes.push_back(12);
        rFSizes.push_back(16);
        rFSizes.push_back(20);
        rFSizes.push_back(26);
        rFSizes.push_back(32);

        iFilters->push_back(new IntensityFilter(rFSizes, 1, true, 0.1)); //0.1
        iFilters->push_back(new ColorFilter(rFSizes, 1,true, 1, ON_RED_OFF_GREEN)); //0.4
        //iFilters->push_back(new ColorFilter(rFSizes, 1,true, 0.1, ON_GREEN_OFF_RED));
        iFilters->push_back(new OrientationFilter(rFSizes, 1,true, 1, DEGREES_0)); //0.1
        //iFilters->push_back(new OrientationFilter(rFSizes, 1,false, 0.5, DEGREES_0)); //0.1

        saliencyD.setreceptiveFieldSize(rFSizes);
        saliencyD.setFilters(iFilters);
        saliencyD.setGaussianSmoothVariance(1);
}

DetectRobotVisually::~DetectRobotVisually() { }


bool DetectRobotVisually::salientImage(osg::ref_ptr<osg::Image> camImage, float* location) {

//      LOG_INFO("Detect saliency in image!");

        if(camImage->getTotalSizeInBytes() > 0) {

                int x, y, xMax, yMax;
                float value;
                bool salientImg = true;

                if(lastImage == NULL) {
                        LOG_INFO("Initialise the audio-visual fusion class...");
                        AVFusion.initVisual(camImage->s(), camImage->t(), 3, 2, 0.93);
                } else {
                        for (int x = 0; x < fusionInput.imageWidth; ++x) {
                                delete [] lastImage[x];
                        }
                        delete [] lastImage;
                }

                lastImage = saliencyD.convertImage(camImage);

                fusionInput.image = lastImage;
                fusionInput.imageHeight = camImage->t();
                fusionInput.imageWidth =  camImage->s();

                if(useAttention) {
//                      LOG_INFO("Use attention...");
                        // Set the image using a predefined scale
                        saliencyD.setImage(camImage,imageScale);
                        salientImg = saliencyD.detect(&x, &y, &xMax, &yMax, &value);

                        fusionInput.imgRegionWidth  = fusionInput.imageWidth/3;
                        fusionInput.imgRegionHeight = fusionInput.imageHeight/3;
                        fusionInput.imgRegionX = (x*imageScale)-(fusionInput.imgRegionWidth/2);
                        fusionInput.imgRegionY = (x*imageScale)-(fusionInput.imgRegionHeight/2);
                        if(fusionInput.imgRegionX < 0)
                                fusionInput.imgRegionX = 0;
                        if(fusionInput.imgRegionY < 0)
                                fusionInput.imgRegionY = 0;
                        if(fusionInput.imgRegionWidth+fusionInput.imgRegionX > fusionInput.imageWidth)
                                fusionInput.imgRegionWidth = (fusionInput.imageWidth - fusionInput.imgRegionX)-1;
                        if(fusionInput.imgRegionHeight+fusionInput.imgRegionY > fusionInput.imageHeight)
                                fusionInput.imgRegionHeight = (fusionInput.imageHeight - fusionInput.imgRegionY)-1;

                        if ((fusionInput.imgRegionWidth*fusionInput.imgRegionHeight) < ((xMax - x)*imageScale)*((yMax - y)*imageScale))
                        {
                                fusionInput.imgRegionX = x*imageScale;
                                fusionInput.imgRegionY = y*imageScale;
                                fusionInput.imgRegionWidth  = (xMax - x)*imageScale;
                                fusionInput.imgRegionHeight = (yMax - y)*imageScale;

                        }
                        // things on the ground are no target
                        if(fusionInput.imgRegionY < float(fusionInput.imageHeight)*float(3/4))
                                salientImg = false;
                        //TODO: the saliency value of a location together with history information needs
                        // to be considered so that objects don't stay salient

                        if(value < 10 || (fusionInput.imgRegionWidth < 2 && fusionInput.imgRegionHeight < 2 ))
                                salientImg = false;
                        if (salientImg)
                        {
                                int width  = saliencyD.getWidth();
                                // What is the camera it's field of view?
                                // We take 120 FOV, left = [-60, 0] right = [0,60]
                                // take the center of the square
                                x += (int) (float(xMax-x)/2.0);
                                *location = (float(x)*(120.0/float(width))) - 60.0;
                        }
                }
                else
                {
                        fusionInput.imgRegionX = 0;
                        fusionInput.imgRegionY = 0;
                        fusionInput.imgRegionWidth  = fusionInput.imageWidth-1;
                        fusionInput.imgRegionHeight = fusionInput.imageHeight-1;
                        // center
                        *location = 0;
                }
                return salientImg;
        }
        else
                return false;
}

#ifdef WITH_UNITTESTING

int DetectRobotVisually::extractClassId() {
        const std::string name = camImage->getFileName();
        std::string extract; size_t pos;
        pos = name.find("norobot");
//      std::cout << pos << std::endl;
        if (pos == string::npos) return 0;
        return 1;
}
#endif

int DetectRobotVisually::classifyObject(bool learning)
{
        if(!learning) {
                LOG_INFO("Start classification with previously learned data.");
                fusionInput.classId = -1;
        } else {
#ifdef WITH_UNITTESTING
                fusionInput.classId = extractClassId();
                ostringstream msg; msg.clear(); msg.str("");
                msg << "Learn to classify this image as class id " << fusionInput.classId;
                LOG_INFO(msg.str());
#else
                LOG_WARNING("How do you want to do supervised learning in a non-unit test mode?");
//              LOG_INFO("The existence of energy at robot/outlet can be as supervising signal");
//              fusionInput.classId = energyAtPos(!nonlearning);
#endif
        }

        int classId = -1;
        // we are close so use the whole image
        if(learning) {
                fusionInput.imgRegionX = 0;
                fusionInput.imgRegionY = 0;
                fusionInput.imgRegionWidth  = fusionInput.imageWidth-1;
                fusionInput.imgRegionHeight = fusionInput.imageHeight-1;
                LOG_DEBUG("In the learning phase");
        }
        vector<vector<ART_TYPE>*>* classes = AVFusion.classify(&fusionInput, !learning);

        for (unsigned int x = 0; x < classes->size(); ++x) {
                if((*classes)[x] != NULL)
                {
                        vector<ART_TYPE>* output  = (*classes)[x];
                        for (unsigned int y = 0; y < output->size(); ++y)
                        {
                                if(x == 0 && learning)
                                        LOG_ERROR("Audio class as one of the ART maps, why!?");
                                if(x == 1 && learning) {
//                                      std::ostringstream msg;
//                                      msg.clear(); msg.str("");
//                                      msg << "Currently identified ART node is: " << (*output)[y];
//                                      LOG_INFO(msg.str());
                                }
                                if(x == 2) {
//                                      LOG_ERROR("Energy class as one of the ART maps, why!?");
                                        if((*output)[y] > -1) {
                                                classId = (int)(AVFusion.d_artEnergy.getF2()->at((*output)[y])->at(0));
//                                              std::cout << (*output)[y] << ", "<< classId << std::endl;
                                        }
                                }
                        }
                        delete (*classes)[x];
                }
        }

        if(classes != NULL)
                delete classes;

        // Only for navigating, take the history of classification into account
//      if(fusionInput.image != NULL) {
//              std::ostringstream msg;
//              msg.clear(); msg.str("");
//              msg << "Currently identified image class is: " << classId;
//              LOG_INFO(msg.str());
//              classId = ControllerUtil::computeAverageClassification(&energyClassHistImage, classId);
//      }

        return classId;
}

/*
 * This function is run every tick and manages the main sensor fusion parts.
 * For now it does auditory-visual sensor fusion in the spatial domain.
 * It calls the audio capturing, saliency detection, and controls the motor speed
 * This function is primarily to test the different modules, eventually
 * the function needs to be split up.
 */
CtrlMsg* DetectRobotVisually::Act(CtrlMsg *message) {

        LOG_INFO("Visual robot detector gets a message...");

        if (message->type != CAMERA_IMAGE) {
                LOG_ERROR("Received the wrong message type as input! This class does not know\
                                how to handle this type");
                outgoingMsg.type = INVALID;
                return &outgoingMsg;
        }

        camImage = *(osg::ref_ptr<osg::Image>*)message->value;

        // Writes also the osg::Image to internal structure
        salientImage(camImage, &location);
//      std::ostringstream msg;
//      msg.clear(); msg.str("");
//      msg << "Found salient entity at: " << location;
//      LOG_DEBUG(msg.str());

        outgoingMsg.type = LOCATION_IN_VIEW;
        outgoingMsg.value = &location;

#ifndef WITH_UNITTESTING
        if (counter < 64) {
                // learning on
                classifyObject(true);
        }
        else {
                // learning off, only classification
                classifyObject(false);
        }

        if (counter == 64) {
                AVFusion.saveMemory("output/robotDetection");
        }
        counter++;
#else
        if (counter == 0) {
                AVFusion.loadMemory("output/robotDetection");
        }
        // learning off, only classification
        int classId = classifyObject(false);
        if (classId == 0) {
                LOG_ALWAYS("Robot detected!");
        } else if (classId == 1) {
                LOG_ALWAYS("Background...");
        } else {
                LOG_ALWAYS("Huh?");
        }
        counter++;

        // overwrite location on background or huh classes...
        if (classId != 0) {
                outgoingMsg.type = NO_LOCATION;
        }
#endif


        return &outgoingMsg;
}

#endif

---