FilterParameterSamplerMain.cpp

Go to the documentation of this file.

#include <ros/ros.h>

#include <face_contour_detector/filters/List.h>
#include <face_contour_detector/filters/GaussianBlur.h>
#include <face_contour_detector/filters/Canny.h>
#include <face_contour_detector/filters/EdgeConnectorGraphBased.h>
#include <face_contour_detector/filters/DeleteShortLines.h>
#include <face_contour_detector/ImageForest.h>

#include <face_contour_detector/ParameterIterator.h>
#include <face_contour_detector/MultiParameterIterator.h>

#include <face_contour_detector/XMLWriter.h>
#include <face_contour_detector/XMLReader.h>
#include <face_contour_detector/ImageInformationBridge.h>

#include <face_contour_detector/xml_image.h>
#include <face_contour_detector/xml_image_entry.h>

#include <boost/shared_ptr.hpp>

#include <opencv/cv.h>
#include <sensor_msgs/Image.h>
#include <cv_bridge/CvBridge.h>

#include <string>
#include <map>
#include <utility>
#include <sstream>

face_contour_detector::filters::Parameter GetParameter(const std::string& name, face_contour_detector::filters::Filter& filter) {
        std::vector<face_contour_detector::filters::Parameter> parameters = filter.GetParameters();
        std::vector<face_contour_detector::filters::Parameter>::iterator it;
        for (it = parameters.begin(); it != parameters.end(); it++) {
                if (it->GetName() == name) {
                        return *it;
                }
        }
        std::cout<<"could not find parameter "<<name<<std::endl;
        throw "error";
}

void GenerateFilterParameterMessageEntries(face_contour_detector::filters::List& filterList, face_contour_detector::xml_image::_entries_type& entries) {
        face_contour_detector::filters::List::Iterator it;
        std::stringstream filterSetupStream;
        int i = 0;
        for (it = filterList.Begin(); it != filterList.End(); it++) {
                filterSetupStream<<(*it)->GetFilterName()<<",";
                std::vector<face_contour_detector::filters::Parameter> params = (*it)->GetParameters();
                std::vector<face_contour_detector::filters::Parameter>::iterator it2;
                for (it2 = params.begin(); it2 != params.end(); it2++) {
                        face_contour_detector::xml_image_entry paramEntry;
                        std::stringstream paramName;
                        paramName<<"filter"<<i<<"_"<<it2->GetName();
                        paramEntry.entry_name = paramName.str();
                        std::stringstream paramValue;
                        if (it2->GetType() == face_contour_detector::filters::TBOOL) {
                                paramEntry.type = "bool";
                                paramValue<<(*(it2->GetBoolValuePtr()));
                                break;
                        } else if (it2->GetType() == face_contour_detector::filters::TDOUBLE) {
                                paramEntry.type = "float";
                                paramValue<<(*(it2->GetDoubleValuePtr()));
                        } else if (it2->GetType() == face_contour_detector::filters::TINT) {
                                paramEntry.type = "int";
                                paramValue<<(*(it2->GetIntValuePtr()));
                        } else if (it2->GetType() == face_contour_detector::filters::TSTRING) {
                                paramEntry.type = "string";
                                paramValue<<(*(it2->GetStringValuePtr()));
                        } else {
                                throw "unknown type";
                        }
                        paramEntry.content = paramValue.str();
                        entries.push_back(paramEntry);
                }
                i++;
        }
        face_contour_detector::xml_image_entry filterSetupEntry;
        filterSetupEntry.entry_name = "filtersetup";
        filterSetupEntry.type = "string";
        filterSetupEntry.content = filterSetupStream.str();
        entries.push_back(filterSetupEntry);

}

void PushParams(std::vector<face_contour_detector::xml_image_entry>& toCopy, std::vector<face_contour_detector::xml_image_entry>& copyTarget) {
        std::vector<face_contour_detector::xml_image_entry>::iterator it;
        for (it = toCopy.begin(); it != toCopy.end(); it++) {
                face_contour_detector::xml_image_entry entry;
                entry.entry_name = it->entry_name;
                entry.type = it->type;
                entry.content = it->content;
                copyTarget.push_back(entry);
        }
}

void PushEndParams(cv::Mat& result, std::vector<face_contour_detector::xml_image_entry>& target) {
        //creating forest out of image
        face_contour_detector::ImageForest forest(result, 200);
        std::map<int, face_contour_detector::ImageForest::Graph> graphs = forest.GetGraphs();

        face_contour_detector::xml_image_entry entry;
        std::stringstream valuestream;

        //num of graphs
        {
                face_contour_detector::xml_image_entry entry;
                std::stringstream valuestream;
                entry.type = "int";
                entry.entry_name = "result_numgraphs";
                valuestream<<graphs.size();
                entry.content = valuestream.str().c_str();
                target.push_back(entry);
        }

        //num of connectingpoints
        {
                face_contour_detector::xml_image_entry entry;
                std::stringstream valuestream;
                int count = 0;
                std::map<int, face_contour_detector::ImageForest::Graph>::iterator it;
                for (it = graphs.begin(); it != graphs.end(); it++) {
                        count += it->second.connectingPoints.size();
                }
                entry.type = "int";
                entry.entry_name = "result_numconnectingpoints";
                valuestream<<count;
                entry.content = valuestream.str().c_str();
                target.push_back(entry);
        }

        //num of endpoints
        {
                face_contour_detector::xml_image_entry entry;
                std::stringstream valuestream;
                int count = 0;
                std::map<int, face_contour_detector::ImageForest::Graph>::iterator it;
                for (it = graphs.begin(); it != graphs.end(); it++) {
                        count += it->second.endPoints.size();
                }
                entry.type = "int";
                entry.entry_name = "result_numendpoints";
                valuestream<<count;
                entry.content = valuestream.str().c_str();
                target.push_back(entry);
        }

        //edgyness
        {
                face_contour_detector::xml_image_entry entry;
                std::stringstream valuestream;
                entry.type = "float";
                entry.entry_name = "result_edgyness";
                valuestream<<forest.CallculateEdgyness();
                entry.content = valuestream.str().c_str();
                target.push_back(entry);
        }

        //black to whitepixel ratio (num blackpixels/num whitepixels)
        {
                face_contour_detector::xml_image_entry entry;
                std::stringstream valuestream;
                entry.type = "float";
                entry.entry_name = "result_graphmass";
                double graphMass = forest.CallculateGraphMass();
                valuestream<<graphMass;
                entry.content = valuestream.str().c_str();
                target.push_back(entry);
        }
        //
}

int main(int argc, char* argv[]) {

        //Ros Setup
        ros::init(argc, argv, "filter_parameter_sampler");
        ros::NodeHandle n;

        //check if there is an image
        std::string filename;
          if (!n.getParam("/image", filename)) {
            ROS_ERROR("image has not been set! Typical command-line usage:\n"
                     "rosparam set /image '<filename>'");
            return 1;
          }

          std::string xmlFilename;

          if (!n.getParam("xmlfile", xmlFilename)) {
                ROS_ERROR("xmlfile has not been set! Typical command-line usage:\n"
                                 "rosparam set /xmlfile '<filename>'");
                return 1;
          }

        ros::Publisher chatter_pub = n.advertise<sensor_msgs::Image>("images", 5);
        sensor_msgs::CvBridge cvbridge;

        //loading base image
        cv::Mat image = cv::imread(filename);

        //get the image properties vars
        std::vector<face_contour_detector::xml_image_entry> imageProperties;
        ros::ServiceClient imageInformationClient = n.serviceClient<face_contour_detector::ImageInformationBridge>("image_information");
        face_contour_detector::ImageInformationBridge imageInformationSrv;
        IplImage ipl = image;
        imageInformationSrv.request.input_image = *cvbridge.cvToImgMsg(&ipl);
        ROS_INFO("Triying to get properties of the image from the image_information service");
        if (imageInformationClient.call(imageInformationSrv)) {
                //imageProperties = readerSrv.response.images;
                imageProperties = imageInformationSrv.response.image.entries;
                ROS_INFO("Successfully loaded image information");
        } else {
                ROS_ERROR("Failed to call service image_information");
                return 1;
        }


        cv::Mat grayImage;
        cv::cvtColor(image, grayImage, CV_BGR2GRAY);

        //Setting up filters
        face_contour_detector::filters::List list;
        face_contour_detector::filters::GaussianBlur* gaussBlur = new face_contour_detector::filters::GaussianBlur();
        list.Add(gaussBlur);
        face_contour_detector::filters::Canny* canny = new face_contour_detector::filters::Canny();
        list.Add(canny);
        list.Add(new face_contour_detector::filters::EdgeConnectorGraphBased());
        list.Add(new face_contour_detector::filters::DeleteShortLines());

        //Setting up multiparameter iterator
        face_contour_detector::MultiParameterIterator mp;

        face_contour_detector::filters::Parameter gaussBlurWidth = GetParameter("blurwidth", *gaussBlur);
        face_contour_detector::filters::Parameter gaussBlurHeight = GetParameter("blurheight", *gaussBlur);
        face_contour_detector::filters::Parameter cannyTh1 = GetParameter("threshold1", *canny);
        face_contour_detector::filters::Parameter cannyTh2 = GetParameter("threshold2", *canny);

        face_contour_detector::ParallelParameterIterator<int>* blurIt = new face_contour_detector::ParallelParameterIterator<int>(3, 61, 2);
        blurIt->Add(gaussBlurWidth.GetIntValuePtr());
        blurIt->Add(gaussBlurHeight.GetIntValuePtr());
        face_contour_detector::ParameterIterator<double>* cannyTh1It = new face_contour_detector::ParameterIterator<double>(400.0, 20000.0, 200.0, cannyTh1.GetDoubleValuePtr());
        face_contour_detector::ParameterIterator<double>* cannyTh2It = new face_contour_detector::ParameterIterator<double>(400.0, 20000.0, 200.0, cannyTh2.GetDoubleValuePtr());

        mp.Add(boost::shared_ptr<face_contour_detector::ParameterIteratorI>(blurIt));
        mp.Add(boost::shared_ptr<face_contour_detector::ParameterIteratorI>(cannyTh1It));
        mp.Add(boost::shared_ptr<face_contour_detector::ParameterIteratorI>(cannyTh2It));

        mp.Reset();

        //

        //Loading old xml data
        ros::ServiceClient readerClient = n.serviceClient<face_contour_detector::XMLReader>("xml_file_bridge_loader");
        std::vector<face_contour_detector::xml_image> imagesdata;
        face_contour_detector::XMLReader readerSrv;
        readerSrv.request.file_name = xmlFilename;
        ROS_INFO("Triying to load xml data");
        if (readerClient.call(readerSrv)) {
                if (!readerSrv.response.success) {
                        ROS_ERROR("Service failed to load the data");
                } else {
                        imagesdata = readerSrv.response.images;
                        ROS_INFO("Successfully loaded old xml file");
                }
        } else {
                ROS_ERROR("Failed to call service XMLReader");
                return 1;
        }



        std::cout<<"Starting sampeling"<<std::endl;

        while (mp.HasNext() && ros::ok()) {
                std::cout<<"\r"<<"["<<(*gaussBlurWidth.GetIntValuePtr())<<", "<<(*cannyTh1.GetDoubleValuePtr())<<", "<<(*cannyTh2.GetDoubleValuePtr())<<"]                       "<<std::flush;
                mp.Next();
                cv::Mat result;
                //aplies the filters with the parameters to the image
                list.Apply(grayImage, result);
                //write out parameters + properties
                face_contour_detector::xml_image dataMsg;

                //filename
                face_contour_detector::xml_image_entry filenameEntry;
                filenameEntry.entry_name = "filename";
                filenameEntry.type = "string";
                filenameEntry.content = filename;
                dataMsg.entries.push_back(filenameEntry);

                //image properties
                PushParams(imageProperties, dataMsg.entries);

                //filterparams
                GenerateFilterParameterMessageEntries(list, dataMsg.entries);

                //result descriptions
                PushEndParams(result, dataMsg.entries);

                //adding the data
                imagesdata.push_back(dataMsg);

                //Sending image message for debug
                IplImage ipl = result;
                chatter_pub.publish(cvbridge.cvToImgMsg(&ipl));

        }

        //sending dataMsg
        ros::ServiceClient writerClient = n.serviceClient<face_contour_detector::XMLWriter>("xml_file_bridge");
        face_contour_detector::XMLWriter srv;
        srv.request.file_name = xmlFilename;
        srv.request.images = imagesdata;

        if (writerClient.call(srv)) {
                if (!srv.response.success) {
                        ROS_ERROR("Service failed to save the data");
                }
        } else {
                ROS_ERROR("Failed to call service XMLWriter");
        }

        std::cout<<"\r"<<"done!"<<std::endl;
        return 0;
}