AreaParameterChooserWorker.cpp

Go to the documentation of this file.

#include "face_contour_detector/AreaParameterChooserWorker.h"

#include <stdio.h>
#include <stdlib.h>
#include <face_contour_detector/filters/List.h>
#include <face_contour_detector/filters/GaussianBlur.h>
#include <face_contour_detector/filters/Canny.h>
#include <ros/assert.h>

namespace face_contour_detector {

//AreaParameterChooserWorker

AreaParameterChooserWorker::AreaParameterChooserWorker(
                ros::NodeHandle& node,
                cv::Mat& image,
                cv::Mat& mask,
                const std::map<std::string, std::map<std::string, double> >& targetValue) :
                node(node), image(image), mask(mask), targetValue(targetValue), kdTree(
                                3) {
        cv::cvtColor(image, bwimage, CV_BGR2GRAY);
}

void AreaParameterChooserWorker::GetAreas() {
        ros::ServiceClient client = node.serviceClient<
                        face_contour_detector::FindFaceAreas>("find_face_areas");
        if (!client.exists()) {
                ROS_ERROR("the service find_face_areas does not exist");
                return;
        }
        face_contour_detector::FindFaceAreas srv;
        //Putting the result image into the response
        cv::Mat tmp = mask.clone();
        try {
                sensor_msgs::CvBridge cvBridge;
                IplImage ipl = tmp;
                srv.request.mask = *(cvBridge.cvToImgMsg(&ipl));
        } catch (sensor_msgs::CvBridgeException& error) {
                ROS_ERROR(
                                "could not convert the mask image with the cvBridge to a sensor_msgs/Image");
                return;
        }
        if (client.call(srv)) {
                if (targetValue.find(srv.response.complete_face.name)
                                != targetValue.end()) {
                        areas.push_back(srv.response.complete_face);
                }
                if (targetValue.find(srv.response.nose.name) != targetValue.end()) {
                        areas.push_back(srv.response.nose);
                }
                if (targetValue.find(srv.response.left_eye.name) != targetValue.end()) {
                        areas.push_back(srv.response.left_eye);
                }
                if (targetValue.find(srv.response.right_eye.name)
                                != targetValue.end()) {
                        areas.push_back(srv.response.right_eye);
                }
                if (targetValue.find(srv.response.mouth.name) != targetValue.end()) {
                        areas.push_back(srv.response.mouth);
                }

                //TAG Debug
                //ROS_INFO("Printing areas");
                std::vector<face_contour_detector::image_area>::iterator it;
                for (it = areas.begin(); it != areas.end(); it++) {
                        ROS_ASSERT(image.rows >= it->y + it->height);
                        ROS_ASSERT(image.cols >= it->x + it->width);
                        /*std::string name = std::string(it->name);
                         ROS_INFO("area %s x= %i , y= %i , width= %i , height= %i ",name.c_str(), it->x, it->y, it->width, it->height);*/
                }

        } else {
                ROS_ERROR("Failed to call find_face_areas service");
        }
}
void AreaParameterChooserWorker::ParamsFunction(
                const std::vector<double>& params,
                std::map<std::string, double>& result) {

        //Load from cache if possible
        const std::map<std::string, double>* cacheResult = kdTree.Find(params);
        if (cacheResult != 0) {
                //ROS_INFO("found cache entry");
                result = *cacheResult;
                return;
        }

        //callculate
        std::vector<face_contour_detector::filter_area_result> re;

        ApplyFilters(params, re);

        std::vector<face_contour_detector::filter_area_result>::iterator it;
        for (it = re.begin(); it != re.end(); it++) {
                if (targetValue.find(it->area.name) == targetValue.end()) {
                        ROS_ERROR(
                                        "could not find target values for area %s", it->area.name.c_str());
                        return;
                }
                if (currArea->name == it->area.name) {

                        std::map<std::string, double>::iterator it2;
                        for (it2 = targetValue[currArea->name].begin();
                                        it2 != targetValue[currArea->name].end(); it2++) {

                                std::vector<face_contour_detector::name_value_pair>::iterator it3;
                                for (it3 = it->result_properties.begin();
                                                it3 != it->result_properties.end(); it3++) {
                                        if (it3->name == it2->first) {
                                                result[it3->name] = atof(it3->value.c_str());
                                        }
                                }
                                if (result.find(it2->first) == result.end()) {
                                        ROS_ERROR(
                                                        "Could not find needed target value %s in response for area %s", it2->first.c_str(), it->area.name.c_str());
                                        SendStatusMessage(
                                                        "Could not find needed target value in response for the area, please send a better image");
                                }
                        }

                        break;
                }
        }

        //save to cache
        kdTree.Insert(params, result);
}
double AreaParameterChooserWorker::CostFunction(
                const std::map<std::string, double>& result
                , const std::map<std::string, double>& targetValue) {
        double re = 0.0;
        std::map<std::string, double>::const_iterator it;
        for (it = result.begin(); it != result.end(); it++) {
                std::string tmp = it->first;
                double tar = targetValue.find(tmp)->second;
                double seen = it->second;
                re += std::abs<double>(tar - seen);
        }
        return re;
}

void AreaParameterChooserWorker::PushFilterSetup(
                std::map<std::string, std::vector<std::map<std::string, double> > >& params
                , int numResults
                , std::vector<face_contour_detector::autoselect_result>& results) {

        for (int i = 0; i < numResults; i++) {

                face_contour_detector::autoselect_result re;

                //load image
                cv::Mat img = image.clone();

                try {
                        sensor_msgs::CvBridge cvBridge;
                        IplImage ipl = img;
                        re.image = *(cvBridge.cvToImgMsg(&ipl));
                } catch (sensor_msgs::CvBridgeException& error) {
                        ROS_ERROR(
                                        "could not convert the image with the cvBridge to a sensor_msgs/Image");
                        return;
                }

                std::vector<face_contour_detector::image_area>::iterator it;
                for (it = areas.begin(); it != areas.end(); it++) {

                        face_contour_detector::filter_area_setup areaSetup;

                        areaSetup.area = *it;

                        //blur
                        {
                                face_contour_detector::filter_setup filterSetup;
                                filterSetup.name = "GaussianBlur";
                                {
                                        ROS_ASSERT(
                                                        params.find(areaSetup.area.name) != params.end());
                                        ROS_ASSERT(params[areaSetup.area.name].size() > static_cast<unsigned int>(i));
                                        ROS_ASSERT(
                                                        params[areaSetup.area.name][i].find("blur") != params[areaSetup.area.name][i].end());
                                        std::stringstream s;
                                        s << (int(params[areaSetup.area.name][i]["blur"]));
                                        face_contour_detector::name_value_pair param;
                                        param.name = "blurwidth";
                                        param.type = "int";
                                        param.value = s.str().c_str();
                                        //ROS_INFO("blurwidth = %s", s.str().c_str());
                                        filterSetup.parameters.push_back(param);
                                }
                                {
                                        std::stringstream s;
                                        s << (int(params[areaSetup.area.name][i]["blur"]));
                                        face_contour_detector::name_value_pair param;
                                        param.name = "blurheight";
                                        param.type = "int";
                                        param.value = s.str().c_str();
                                        //ROS_INFO("blurheight = %s", s.str().c_str());
                                        filterSetup.parameters.push_back(param);
                                }
                                areaSetup.filters.push_back(filterSetup);
                        }
                        //canny
                        {
                                face_contour_detector::filter_setup filterSetup;
                                filterSetup.name = "Canny";
                                {
                                        std::stringstream s;
                                        s << (int(params[areaSetup.area.name][i]["threshold1"]));
                                        face_contour_detector::name_value_pair param;
                                        param.name = "threshold1";
                                        param.type = "float";
                                        param.value = s.str().c_str();
                                        filterSetup.parameters.push_back(param);
                                }
                                {
                                        std::stringstream s;
                                        s << (int(params[areaSetup.area.name][i]["threshold2"]));
                                        face_contour_detector::name_value_pair param;
                                        param.name = "threshold2";
                                        param.type = "float";
                                        param.value = s.str().c_str();
                                        filterSetup.parameters.push_back(param);
                                }
                                {
                                        face_contour_detector::name_value_pair param;
                                        param.name = "l2gradientent";
                                        param.type = "bool";
                                        param.value = "true";
                                        filterSetup.parameters.push_back(param);
                                }
                                areaSetup.filters.push_back(filterSetup);
                        }

                        //edge connector
                        {
                                face_contour_detector::filter_setup filterSetup;
                                filterSetup.name = "EdgeConnector";
                                {
                                        face_contour_detector::name_value_pair param;
                                        param.name = "searchRadius";
                                        param.type = "int";
                                        param.value = "5";
                                        filterSetup.parameters.push_back(param);
                                }
                                areaSetup.filters.push_back(filterSetup);
                        }
                        //delete short lines
                        {
                                face_contour_detector::filter_setup filterSetup;
                                filterSetup.name = "DeleteShortLines";
                                {
                                        face_contour_detector::name_value_pair param;
                                        param.name = "minNumPixels";
                                        param.type = "int";
                                        param.value = "10";
                                        filterSetup.parameters.push_back(param);
                                }
                                areaSetup.filters.push_back(filterSetup);
                        }

                        re.areas.push_back(areaSetup);
                }

                results.push_back(re);
        }
}

void AreaParameterChooserWorker::ApplyFilters(const std::vector<double>& params,
                std::vector<face_contour_detector::filter_area_result>& results) {

        cv::Mat areaImage = cv::Mat(currArea->height, currArea->width, CV_8UC1);

        for (int y = 0; y < currArea->height; y++) {
                for (int x = 0; x < currArea->width; x++) {
                        ROS_ASSERT(bwimage.cols > x);
                        ROS_ASSERT(bwimage.rows > y);
                        areaImage.at<unsigned char>(y, x) = bwimage.at<unsigned char>(
                                        currArea->y + y, currArea->x + x);
                }
        }

        cv::Mat currAreaImage = cv::Mat(currArea->height, currArea->width, CV_8UC1);
        face_contour_detector::filters::List list;
        {
                face_contour_detector::filters::Filter* currFilter =
                                new face_contour_detector::filters::GaussianBlur();
                std::vector<face_contour_detector::filters::Parameter> filterParams =
                                currFilter->GetParameters();
                std::vector<face_contour_detector::filters::Parameter>::iterator it;
                for (it = filterParams.begin(); it != filterParams.end(); it++) {
                        *(it->GetIntValuePtr()) = int(params[2]);
                }
                list.Add(currFilter);
        }
        {
                face_contour_detector::filters::Filter* currFilter =
                                new face_contour_detector::filters::Canny();
                std::vector<face_contour_detector::filters::Parameter> filterParams =
                                currFilter->GetParameters();
                std::vector<face_contour_detector::filters::Parameter>::iterator it;
                for (it = filterParams.begin(); it != filterParams.end(); it++) {
                        if (it->GetName() == std::string("threshold1")) {
                                *(it->GetDoubleValuePtr()) = params[0];
                        } else if (it->GetName() == std::string("threshold2")) {
                                *(it->GetDoubleValuePtr()) = params[1];
                        } else if (it->GetName() == std::string("l2gradientent")) {
                                *(it->GetBoolValuePtr()) = true;
                        }
                }
                list.Add(currFilter);
        }
        cv::Mat result = cv::Mat(currArea->height, currArea->width, CV_8UC1);
        list.Apply(areaImage, result);

        //callculating black to white pixel ratio
        double numWhite = 0.0;
        double numBlack = 0.0;
        double whiteFactor = double(image.rows) / 100.0
                        + double(image.cols) / 100.0;
        for (int y = 0; y < result.rows; y++) {
                for (int x = 0; x < result.cols; x++) {
                        if (result.at<unsigned char>(y, x) > 150) {
                                //numWhite += 1.0;
                                numWhite += whiteFactor;
                        } else {
                                numBlack += 1.0;
                        }
                }
        }
        //excluding pixels that will be overwritten by later areas
        {
                std::vector<face_contour_detector::image_area>::iterator areaIt2;
                bool following = false;
                for (areaIt2 = areas.begin(); areaIt2 != areas.end(); areaIt2++) {
                        if (following) {
                                for (int y = 0; y < areaIt2->height; y++) {
                                        for (int x = 0; x < areaIt2->width; x++) {
                                                int realX = areaIt2->x + x - currArea->x;
                                                int realY = areaIt2->y + y - currArea->y;
                                                if (realX >= 0 && realX < currArea->width && realY >= 0
                                                                && realY < currArea->height) {
                                                        ROS_ASSERT(
                                                                        result.rows > realY && result.cols > realX);
                                                        if (result.at<unsigned char>(realY, realX) > 150) {
                                                                numWhite -= 1.0;
                                                        } else {
                                                                numBlack -= 1.0;
                                                        }
                                                }
                                        }
                                }
                        } else if (areaIt2 == currArea) {
                                following = true;
                        }
                }
        }
        //writing out result
        {
                face_contour_detector::filter_area_result areaResult;
                areaResult.area = *currArea;
                face_contour_detector::name_value_pair nv;
                nv.name = "graphmass";
                std::stringstream sstr;
                sstr << (numWhite / numBlack);
                nv.value = sstr.str().c_str();
                nv.type = "float";
                areaResult.result_properties.push_back(nv);
                results.push_back(areaResult);
        }
        /*face_contour_detector::ApplyFilters req;
         std::vector<face_contour_detector::image_area>::iterator it;
         for (it = areas.begin(); it != areas.end(); it++) {

         face_contour_detector::filter_area_setup areaSetup;

         areaSetup.area = *it;

         //blur
         {
         face_contour_detector::filter_setup filterSetup;
         filterSetup.name = "GaussianBlur";
         {
         std::stringstream s;
         s<<(int(params[2]));
         face_contour_detector::name_value_pair param;
         param.name = "blurwidth";
         param.type = "int";
         param.value = s.str().c_str();
         filterSetup.parameters.push_back(param);
         }
         {
         std::stringstream s;
         s<<(int(params[2]));
         face_contour_detector::name_value_pair param;
         param.name = "blurheight";
         param.type = "int";
         param.value = s.str().c_str();
         filterSetup.parameters.push_back(param);
         }
         areaSetup.filters.push_back(filterSetup);
         }
         //canny
         {
         face_contour_detector::filter_setup filterSetup;
         filterSetup.name = "Canny";
         {
         std::stringstream s;
         s<<(int(params[0]));
         face_contour_detector::name_value_pair param;
         param.name = "threshold1";
         param.type = "float";
         param.value = s.str().c_str();
         filterSetup.parameters.push_back(param);
         }
         {
         std::stringstream s;
         s<<(int(params[1]));
         face_contour_detector::name_value_pair param;
         param.name = "threshold2";
         param.type = "float";
         param.value = s.str().c_str();
         filterSetup.parameters.push_back(param);
         }
         {
         face_contour_detector::name_value_pair param;
         param.name = "l2gradientent";
         param.type = "bool";
         param.value = "true";
         filterSetup.parameters.push_back(param);
         }
         areaSetup.filters.push_back(filterSetup);
         }

         //edge connector
         {
         face_contour_detector::filter_setup filterSetup;
         filterSetup.name = "EdgeConnector";
         {
         face_contour_detector::name_value_pair param;
         param.name = "searchRadius";
         param.type = "int";
         param.value = "3";
         filterSetup.parameters.push_back(param);
         }
         areaSetup.filters.push_back(filterSetup);
         }
         //delete short lines
         {
         face_contour_detector::filter_setup filterSetup;
         filterSetup.name = "DeleteShortLines";
         {
         face_contour_detector::name_value_pair param;
         param.name = "minNumPixels";
         param.type = "int";
         param.value = "5";
         filterSetup.parameters.push_back(param);
         }
         areaSetup.filters.push_back(filterSetup);
         }

         req.request.areas.push_back(areaSetup);
         }

         //Putting the mask image into the request
         cv::Mat tmp = image.clone();
         try {
         sensor_msgs::CvBridge cvBridge;
         IplImage ipl = tmp;
         req.request.input_image = *(cvBridge.cvToImgMsg(&ipl));

         ros::ServiceClient applyClient = node.serviceClient<face_contour_detector::ApplyFilters>("apply_filters");

         if (applyClient.call(req)) {
         results = req.response.area_results;
         } else {
         ROS_ERROR("Failed to call service apply_filters");
         return;
         }
         } catch (sensor_msgs::CvBridgeException error) {
         ROS_ERROR("could not convert the mask image with the cvBridge to a sensor_msgs/Image");
         return;
         }*/
}

void AreaParameterChooserWorker::SendStatusMessage(const std::string& message) {
        ros::ServiceClient client = node.serviceClient<
                        portrait_robot_msgs::alubsc_status_msg>("status_msg");
        portrait_robot_msgs::alubsc_status_msg srv;
        srv.request.ID = 2;
        srv.request.message = message;
        if (!client.call(srv)) {
                ROS_WARN(
                                "could not reach status message service \"%s\"", client.getService().c_str());
                ROS_INFO("%s", message.c_str());
        }
}

void AreaParameterChooserWorker::CallculateParameters(int numResults, int steps,
                std::vector<face_contour_detector::autoselect_result>& result) {
        //clear the kd tree

        //get the areas
        GetAreas();
        if (areas.size() <= 0) {
                ROS_ERROR("No areas found");
                SendStatusMessage(
                                "Could not find any areas, please send a better mask file");
                return;
        }

        //start the explorer for the different areas
        boost::function<
                        void(const std::vector<double>&, std::map<std::string, double>&)> paramsFunction =
                        boost::bind(&AreaParameterChooserWorker::ParamsFunction, this, _1,
                                        _2);

        boost::function<
                        double(const std::map<std::string, double>&
                                        , const std::map<std::string, double>&)> costsFunction =
                        boost::bind(&AreaParameterChooserWorker::CostFunction, this, _1,
                                        _2);

        std::map<std::string, std::vector<std::map<std::string, double> > > areaResults;

        for (currArea = areas.begin(); currArea != areas.end(); currArea++) {
                std::stringstream sstr;
                sstr << "exploring area \"" << currArea->name << "\" ...";
                SendStatusMessage(sstr.str());

                //new kd tree cache
                kdTree = face_contour_detector::KdTree<double,
                                std::map<std::string, double> >(3);

                std::map<std::string, double> targetValue =
                                this->targetValue[currArea->name];
                std::vector<std::map<std::string, double> > results;
                AutoParameterExplorer explorer = AutoParameterExplorer(paramsFunction,
                                costsFunction);
                explorer.AddParameter("threshold1", 0, 30000);
                explorer.AddParameter("threshold2", 0, 30000);
                explorer.AddParameter("blur", 15, 50);
                ROS_INFO("Callculating area \"%s\"", currArea->name.c_str());
                explorer.FindBest(targetValue, steps, numResults, results);
                if (results.size() < static_cast<unsigned int>(numResults)) {
                        ROS_ERROR(
                                        "Could not find enough results for this image in the area %s", currArea->name.c_str());
                        std::stringstream sstr;
                        sstr << "Could not find enough results for this image in the area"
                                        << currArea->name << std::endl;
                        SendStatusMessage(sstr.str());
                        return;
                }
                areaResults[currArea->name] = results;
        }

        //convert to autparameter results
        PushFilterSetup(areaResults, numResults, result);
}

}