AutoParameterExplorer.cpp

Go to the documentation of this file.

#include <face_contour_detector/AutoParameterExplorer.h>
#include <algorithm>
#include <queue>
#include <deque>
#include <limits>
#include <sstream>
#include <ros/assert.h>

namespace face_contour_detector {

        AutoParameterExplorer::AutoParameterExplorer(boost::function<void (const std::vector<double>&, std::map<std::string, double>&)> paramsFunction, boost::function<double (const std::map<std::string, double>&, const std::map<std::string, double>&)> costsFunction) : m_paramsFunction(paramsFunction), m_costsFunction(costsFunction) {}
        AutoParameterExplorer::~AutoParameterExplorer() {}

        void AutoParameterExplorer::AddParameter(const std::string& name, double min, double max) {
                ROS_ASSERT(min < max);

                m_paramNames.push_back(name);
                m_startEnd.push_back(std::pair<double,double>(min, max));
        }

        void AutoParameterExplorer::FindBest(std::map<std::string, double> targetValue, int steps, int numResults, std::vector<std::map<std::string, double> >& results) {

                //Ok let's do this
                //generate the starting area that encloses everything
                ParameterPoint base;
                {
                        std::vector<std::pair<double,double> >::iterator it;
                        for (it = m_startEnd.begin(); it != m_startEnd.end(); it++) {
                                base.AddParam(it->first);
                        }
                }
                Area a(base);
                {
                        std::vector<std::pair<double,double> >::iterator it;
                        for (it = m_startEnd.begin(); it != m_startEnd.end(); it++) {
                                a.AddDimension(it->second - it->first);
                        }
                }
                //check if the area should have the target value in it
                if (!M_IsTargetValueInside(targetValue, a)) {
                        ROS_ERROR("The main parameter area does not contain the target values");
                        return;
                }

                //get the costs for the Area
                M_GetCosts(targetValue, a);

                //initialize the min ordered list
                std::priority_queue<Area, std::vector<Area>, Area::GreaterCosts> activeQueue;
                activeQueue.push(a);

                //this is where the areas will be saved that could not be improved
                std::priority_queue<Area, std::vector<Area>, Area::GreaterCosts> inactiveQueue;
                //double lastBestCosts = std::numeric_limits<double>::infinity();
                //bool foundBetter = true;
                for (int i = 0; i < steps; i++) {
                        //foundBetter = false;
                        ROS_INFO("step %i", i);
                        Area currArea = activeQueue.top();
                        activeQueue.pop();
                        std::stringstream strS;
                        strS<<currArea;
                        ROS_INFO("%s", strS.str().c_str());
                        std::vector<Area> subAreas;
                        currArea.SubAreas(subAreas);
                        std::vector<Area>::iterator subIt;
                        bool foundGoodSubArea = false;
                        for (subIt = subAreas.begin(); subIt != subAreas.end(); subIt++) {
                                if (M_IsTargetValueInside(targetValue, *subIt)) {
                                        foundGoodSubArea = true;
                                        M_GetCosts(targetValue, *subIt);
                                        /*if (lastBestCosts > subIt->Costs()) {
                                                lastBestCosts = subIt->Costs();
                                                foundBetter = true;
                                        }*/
                                        activeQueue.push(*subIt);
                                }
                        }
                        if (!foundGoodSubArea) {
                                inactiveQueue.push(currArea);
                        }
                }

                //Ok let's read out all the points for the top numResults areas
                face_contour_detector::LimitedPriorityQueueSet<ParameterPoint, ParameterPoint::LessCosts> resultPoints(numResults);
                while (resultPoints.Size() < numResults) {
                        if (activeQueue.empty() && inactiveQueue.empty()) {
                                break;
                        }
                        if (activeQueue.empty()) {
                                Area top = inactiveQueue.top();
                                M_PushAllPoints(top, targetValue, resultPoints);
                                inactiveQueue.pop();
                        } else if (inactiveQueue.empty()) {
                                Area top = activeQueue.top();
                                M_PushAllPoints(top, targetValue, resultPoints);
                                activeQueue.pop();
                        } else {
                                Area activeTop = activeQueue.top();
                                Area inactiveTop = inactiveQueue.top();
                                if (activeTop.Costs() > inactiveTop.Costs()) {
                                        M_PushAllPoints(inactiveTop, targetValue, resultPoints);
                                        inactiveQueue.pop();
                                } else  {
                                        M_PushAllPoints(activeTop, targetValue, resultPoints);
                                        activeQueue.pop();
                                }
                        }

                }

                //save out the results
                for (int i = 0; i < numResults && !resultPoints.Empty(); i++) {
                        ParameterPoint currP = resultPoints.PopFront();

                        std::map<std::string, double> currResult;
                        std::vector<double> params = currP.Params();
                        std::vector<std::string>::iterator nameIt = m_paramNames.begin();
                        std::vector<double>::iterator valueIt = params.begin();

                        while (valueIt != params.end()) {
                                currResult[*nameIt] = *valueIt;
                                nameIt++;
                                valueIt++;
                        }
                        results.push_back(currResult);
                }

        }

        void AutoParameterExplorer::M_GetCosts(std::map<std::string, double>& targetValue, ParameterPoint& paramPoint) {
                /*std::map<std::string, double> vars;
                std::vector<double>& values = paramPoint.Params();
                std::vector<std::string>::iterator nameIt = m_paramNames.begin();
                std::vector<double>::iterator valueIt = values.begin();

                while (valueIt != values.end()) {

                        vars[*nameIt] = *valueIt;

                        valueIt++;
                        nameIt++;
                }
                ROS_ASSERT(valueIt == values.end());
                ROS_ASSERT(nameIt == m_paramNames.end());

                paramPoint.Costs(m_costsFunction(vars));
                ROS_ASSERT(paramPoint.HasCosts());*/

                std::map<std::string, double> results;
                m_paramsFunction(paramPoint.Params(), results);
                paramPoint.Costs(m_costsFunction(results, targetValue));
                ROS_ASSERT(paramPoint.HasCosts());

        }

        void AutoParameterExplorer::M_GetCosts(std::map<std::string, double>& targetValue, Area& area) {
                /*ParameterPoint middle = area.Middle();
                std::map<std::string, double> results;
                m_paramsFunction(middle.Params(), results);
                area.Costs(m_costsFunction(targetValue, results));*/

                face_contour_detector::LimitedPriorityQueueSet<ParameterPoint, ParameterPoint::LessCosts>  queue = face_contour_detector::LimitedPriorityQueueSet<ParameterPoint, ParameterPoint::LessCosts>(1);

                M_PushAllPoints(area, targetValue, queue);

                area.Costs(queue.Front().Costs());

                //alternatively: middle over all the costs of the edges
                /*std::vector<ParameterPoint> edges;
                area.EdgePoints(edges);
                double sum;
                std::vector<ParameterPoint>::iterator it;
                for (it = edges.begin(); it != edges.end(); it++) {
                        M_GetCosts(*it);
                        sum = it-
                }*/
        }

        bool AutoParameterExplorer::M_IsTargetValueInside(std::map<std::string, double>& targetValue, Area& area) {

                //initialize the dimension checks
                std::map<std::string, bool> foundMin;
                std::map<std::string, bool> foundMax;

                {
                        std::map<std::string, double>::iterator it;
                        for (it = targetValue.begin(); it != targetValue.end(); it++) {
                                foundMin[it->first] = false;
                                foundMax[it->first] = false;
                        }
                }

                //
                std::vector<ParameterPoint> edges;
                area.EdgePoints(edges);
                std::vector<ParameterPoint>::iterator edgeIt;
                for (edgeIt = edges.begin(); edgeIt != edges.end(); edgeIt++) {
                        //get result value for the point
                        std::map<std::string, double> result;
                        m_paramsFunction(edgeIt->Params(), result);

                        //let's go through the dimensions
                        std::map<std::string, double>::iterator reIt;
                        for (reIt = result.begin(); reIt != result.end(); reIt++) {
                                if (reIt->second < targetValue[reIt->first]) {
                                        foundMin[reIt->first] = true;
                                } else if (reIt->second > targetValue[reIt->first]) {
                                        foundMax[reIt->first] = true;
                                } else if (reIt->second == targetValue[reIt->first]) {
                                        foundMax[reIt->first] = true;
                                        foundMin[reIt->first] = true;
                                }
                        }
                }

                //check if all the dimensions were within
                std::map<std::string, bool>::iterator minIt;
                for (minIt = foundMin.begin(); minIt != foundMin.end(); minIt++) {
                        if (!(minIt->second)) return false;
                }

                std::map<std::string, bool>::iterator maxIt;
                for (maxIt = foundMax.begin(); maxIt != foundMax.end(); maxIt++) {
                        if (!(maxIt->second)) return false;
                }
                return true;

        }

        void AutoParameterExplorer::M_PushAllPoints(Area& area, std::map<std::string, double>& targetValues, face_contour_detector::LimitedPriorityQueueSet<AutoParameterExplorer::ParameterPoint, AutoParameterExplorer::ParameterPoint::LessCosts>& results) {
                std::vector<ParameterPoint> points;
                area.EdgePoints(points);
                points.push_back(area.Middle());
                std::vector<ParameterPoint>::iterator it;
                for (it = points.begin(); it != points.end(); it++) {
                        M_GetCosts(targetValues, *it);
                        results.Push(*it);
                }
        }

        //Parampoint

        AutoParameterExplorer::ParameterPoint::ParameterPoint() : m_costsSet(false) {}

        AutoParameterExplorer::ParameterPoint::ParameterPoint(const ParameterPoint& other) : m_params(other.m_params), m_costsSet(other.m_costsSet), m_costs(other.m_costs) {}

        void AutoParameterExplorer::ParameterPoint::AddParam(double value) {
                m_params.push_back(value);
        }

        AutoParameterExplorer::ParameterPoint AutoParameterExplorer::ParameterPoint::Midpoint(ParameterPoint& other) {
                ROS_ASSERT(m_params.size() == other.m_params.size());
                ParameterPoint re;
                std::vector<double>::iterator itThis = m_params.begin();
                std::vector<double>::iterator itOther = other.m_params.begin();
                while (itThis != m_params.end()) {

                        re.AddParam( (((*itOther) - (*itThis))/2.0) +(*itThis) );

                        itThis++;
                        itOther++;
                }
        }

        AutoParameterExplorer::Vector AutoParameterExplorer::ParameterPoint::VectorTo(const AutoParameterExplorer::ParameterPoint& other) {

                Vector v;

                std::vector<double>::const_iterator thisIt = m_params.begin();
                std::vector<double>::const_iterator otherIt = other.m_params.begin();

                while (thisIt != m_params.end()) {

                        v.AddParam((*otherIt) - (*thisIt));

                        thisIt++;
                        otherIt++;
                }
                ROS_ASSERT(thisIt == m_params.end());
                ROS_ASSERT(otherIt == other.m_params.end());

                return v;
        }

        AutoParameterExplorer::ParameterPoint AutoParameterExplorer::ParameterPoint::operator+ (const AutoParameterExplorer::Vector& vector) {

                ParameterPoint p;

                std::vector<double>::const_iterator thisIt = m_params.begin();
                std::vector<double>::const_iterator otherIt = vector.values.begin();

                while (thisIt != m_params.end()) {

                        p.AddParam((*otherIt) + (*thisIt));

                        thisIt++;
                        otherIt++;
                }
                ROS_ASSERT(thisIt == m_params.end());
                ROS_ASSERT(otherIt == vector.values.end());

                return p;

        }

        //Vector
        AutoParameterExplorer::Vector::Vector() {}

        AutoParameterExplorer::Vector::Vector(const Vector& other) : values(other.values) { }

        void AutoParameterExplorer::Vector::AddParam(double value) { values.push_back(value); }

        AutoParameterExplorer::Vector AutoParameterExplorer::Vector::operator+(const AutoParameterExplorer::Vector& other) {
                Vector v;

                std::vector<double>::const_iterator thisIt = values.begin();
                std::vector<double>::const_iterator otherIt = other.values.begin();

                while (thisIt != values.end()) {

                        v.AddParam((*otherIt) + (*thisIt));

                        thisIt++;
                        otherIt++;
                }
                ROS_ASSERT(thisIt == values.end());
                ROS_ASSERT(otherIt == other.values.end());

                return v;
        }

        AutoParameterExplorer::Vector AutoParameterExplorer::Vector::operator*(double factor) {
                Vector v;
                std::vector<double>::const_iterator thisIt;
                for (thisIt = values.begin(); thisIt != values.end(); thisIt++) {
                        v.AddParam((*thisIt) * factor);
                }
                return v;
        }

        //Area

        AutoParameterExplorer::Area::Area(const AutoParameterExplorer::ParameterPoint& base) : base(base), m_hasCosts(false) {

        }
        void AutoParameterExplorer::Area::AddDimension(double value) {
                dimensions.push_back(value);
        }

        AutoParameterExplorer::ParameterPoint AutoParameterExplorer::Area::Middle() {
                ROS_ASSERT(dimensions.size() > 0);

                ParameterPoint p;
                std::vector<double>::iterator it;
                Vector sumVector;

                for (it = dimensions.begin(); it != dimensions.end(); it++) {
                        sumVector.AddParam((*it) * 0.5);
                }
                return base + sumVector;

        }

        void AutoParameterExplorer::Area::SubAreas(std::vector<AutoParameterExplorer::Area>& re) {
                ROS_ASSERT(dimensions.size() > 0);


                for (unsigned int i = 0; i < M_UIntPower2(dimensions.size()); i++) {
                        //callculate the base
                        ParameterPoint subbase;
                        //iterate through dimensions
                        std::vector<double>::iterator dimIt;
                        int dim = 0;
                        for (dimIt = dimensions.begin(); dimIt != dimensions.end(); dimIt++) {
                                ROS_ASSERT(dim < 30);
                                if ((i) & (1<<(dim))) {
                                        subbase.Params().push_back(base.Params().at(dim) + dimensions.at(dim)/2.0);
                                } else {
                                        subbase.Params().push_back(base.Params().at(dim));
                                }
                                dim++;
                        }

                        Area subArea(subbase);
                        for (dimIt = dimensions.begin(); dimIt != dimensions.end(); dimIt++) {
                                subArea.AddDimension((*dimIt)/2.0);
                        }
                        re.push_back(subArea);
                }

        }

        void AutoParameterExplorer::Area::EdgePoints(std::vector<AutoParameterExplorer::ParameterPoint>& re) {
                ROS_ASSERT(dimensions.size() > 0);

                for (unsigned int i = 0; i < M_UIntPower2(dimensions.size()); i++) {
                        //callculate the edge
                        ParameterPoint edge;
                        //iterate through dimensions
                        std::vector<double>::iterator dimIt;
                        int dim = 0;
                        for (dimIt = dimensions.begin(); dimIt != dimensions.end(); dimIt++) {
                                ROS_ASSERT(dim < 30);
                                if ((i) & (1<<(dim))) {
                                        edge.Params().push_back(base.Params().at(dim) + dimensions.at(dim));
                                } else {
                                        edge.Params().push_back(base.Params().at(dim));
                                }
                                dim++;
                        }
                        re.push_back(edge);
                }

        }

        bool AutoParameterExplorer::Area::HasCosts() const {
                return m_hasCosts;
        }

        void AutoParameterExplorer::Area::Costs(double costs) {
                m_hasCosts = true;
                m_costs = costs;
        }

        double AutoParameterExplorer::Area::Costs() const {
                ROS_ASSERT(m_hasCosts);
                return m_costs;
        }

        bool AutoParameterExplorer::Area::GreaterCosts::operator() (const Area& x, const Area& y) const {
                return x.Costs()>y.Costs();
        }

} //namespace face_contour_detector