TopologyManager.cpp

Go to the documentation of this file.

#include "learner/foreground/ocm/combinatorial_optimization/TopologyManager.h"

namespace ProbabilisticSceneRecognition {

TopologyManager::TopologyManager(std::vector<boost::shared_ptr<ISM::ObjectSet>> pExamplesList,
                const std::vector<std::string>& pObjectTypes,
                boost::shared_ptr<SceneModel::AbstractTopologyCreator> pTopologyCreator,
                boost::shared_ptr<AbstractTopologyEvaluator> pEvaluator):
    mEvaluator(pEvaluator), mTopologyCreator(pTopologyCreator), mExamplesList(pExamplesList), mObjectTypes(pObjectTypes), mHistoryIndex(0), mPrintHelper('#')
{
    ros::NodeHandle nodeHandle("~");

    // Try to get output target for the optimization history; from "none", "screen", "file".
    if(!nodeHandle.getParam("optimization_history_output", mHistoryOutput))
        throw std::runtime_error("Please specify parameter optimization_history_output when starting this node.");

    // Try to get the history file path.
    if(!nodeHandle.getParam("optimization_history_file_path", mHistoryFilePath))
        throw std::runtime_error("Please specify parameter optimization_history_file_path when starting this node.");

    // Try to get whether to revisit .
    if(!nodeHandle.getParam("revisit_topologies", mRevisitTopologies))
        throw std::runtime_error("Please specify parameter revisit_topologies when starting this node.");

    if (mHistoryOutput == "svg")
        mSVGHelper.reset(new ISM::SVGHelper(mHistoryFilePath));
}

TopologyManager::~TopologyManager()
{
    if (mHistoryOutput == "svg")
        mSVGHelper->writeResult();
}

boost::shared_ptr<SceneModel::Topology> TopologyManager::getNextNeighbour()
{
    // pop neighbour:
    boost::shared_ptr<SceneModel::Topology> nextNeighbour = mNeighbours[0];
    mNeighbours.erase(mNeighbours.begin());

    // initialize neighbour:
    if (!nextNeighbour->isTreeValid()) makeTree(nextNeighbour); // make sure the neighbour has a tree associated with it, which will be used by evaluator
    mEvaluator->evaluate(nextNeighbour);                // if nextNeighbour has already been evaluated, the Evaluator returns without rerunning the evaluation

    nextNeighbour->mUsedInOptimization = true;  // topology has been used in optimization

    mHistory[mHistoryIndex].push_back(std::pair<boost::shared_ptr<SceneModel::Topology>, bool>(nextNeighbour, false));   // add neighbour to history

    return nextNeighbour;
}

bool TopologyManager::hasNextNeighbour()
{
    return !(mNeighbours.empty());
}

void TopologyManager::setReferenceInstance(boost::shared_ptr<SceneModel::Topology> instance)
{
    if (!mTopologyCreator) throw std::runtime_error("In TopologyManager::setReferenceInstance(): TopologyCreator not initialized.");
    std::vector<boost::shared_ptr<SceneModel::Topology>> allNeighbours = mTopologyCreator->generateNeighbours(instance);
    std::vector<boost::shared_ptr<SceneModel::Topology>> selectedNeighbours;
    for (boost::shared_ptr<SceneModel::Topology> neighbour: allNeighbours)
    {
        if (!mSeenTopologies[neighbour->mIdentifier])   // no pointer assigned
        {
            selectedNeighbours.push_back(neighbour);
            mSeenTopologies[neighbour->mIdentifier] = neighbour;
        }
        else if (mRevisitTopologies || !mSeenTopologies[neighbour->mIdentifier]->mUsedInOptimization) // if topologies get revisited or if topology in list of seen topologies, but not yet used in optimization
        {
            selectedNeighbours.push_back(mSeenTopologies[neighbour->mIdentifier]);
        }
    }

    // add first step to history:
    if (mHistory.empty())
    {
        mHistory.push_back(std::vector<std::pair<boost::shared_ptr<SceneModel::Topology>, bool>>());
        mHistoryIndex = 0;
        // add starting topology to history and mark it as selected:
        mHistory[mHistoryIndex].push_back(std::pair<boost::shared_ptr<SceneModel::Topology>, bool>(instance, true));
    }

    // mark selected topology in last step:
    for (unsigned int i = 0; i < mHistory[mHistoryIndex].size(); i++)
        if (mHistory[mHistoryIndex][i].first->mIdentifier == instance->mIdentifier)
            mHistory[mHistoryIndex][i].second = true;

    // add a new step to history:
    mHistoryIndex++;
    mHistory.push_back(std::vector<std::pair<boost::shared_ptr<SceneModel::Topology>, bool>>());

    mNeighbours = selectedNeighbours;
}

boost::shared_ptr<SceneModel::Topology> TopologyManager::getFullyMeshedTopology()
{
    if (!mTopologyCreator) throw std::runtime_error("In TopologyManager::getFullyMeshedTopology(): TopologyCreator not initialized.");
    boost::shared_ptr<SceneModel::Topology> fullyMeshed = mTopologyCreator->generateFullyMeshedTopology();
    if (mSeenTopologies[fullyMeshed->mIdentifier])
        return mSeenTopologies[fullyMeshed->mIdentifier];   // only set up tree once
    makeTree(fullyMeshed);
    mSeenTopologies[fullyMeshed->mIdentifier] = fullyMeshed;

    return fullyMeshed;
}

std::vector<boost::shared_ptr<SceneModel::Topology>> TopologyManager::getStarTopologies()
{
    if (!mTopologyCreator) throw std::runtime_error("In TopologyManager::getStarTopologies(): TopologyCreator not initialized.");
    std::vector<boost::shared_ptr<SceneModel::Topology>> starTopologies = mTopologyCreator->generateStarTopologies();
    for (unsigned int i = 0; i < starTopologies.size(); i++)
    {
        boost::shared_ptr<SceneModel::Topology> star = starTopologies[i];
        if (!mSeenTopologies[star->mIdentifier])
        {
            makeTree(star);
            mSeenTopologies[star->mIdentifier] = star;
        }
        else starTopologies[i] = mSeenTopologies[star->mIdentifier];    // only set up tree once
    }
    return starTopologies;
}

boost::shared_ptr<SceneModel::Topology> TopologyManager::getRandomTopology()
{
    if (!mTopologyCreator) throw std::runtime_error("In TopologyManager::getRandomTopology(): TopologyCreator not initialized.");
    boost::shared_ptr<SceneModel::Topology> randomTopology = mTopologyCreator->generateRandomTopology();
    if (mSeenTopologies[randomTopology->mIdentifier])
        return mSeenTopologies[randomTopology->mIdentifier];   // only set up tree once
    makeTree(randomTopology);
    mSeenTopologies[randomTopology->mIdentifier] = randomTopology;

    return randomTopology;
}

void TopologyManager::makeTree(boost::shared_ptr<SceneModel::Topology> pTopology)
{

    mPrintHelper.printAsHeader("Generating tree from topology " + pTopology->mIdentifier);

    SceneModel::TopologyTreeTrainer tttrainer(pTopology->mRelations);
    tttrainer.addSceneGraphMessages(mExamplesList);

    tttrainer.loadTrajectoriesAndBuildTree();

    boost::shared_ptr<SceneModel::TreeNode> tree = tttrainer.getTree();

    // Print tree.
    ROS_INFO("Topology tree trainer successfully generated tree.");
    std::cout << "------------- TREE:" << std::endl;
    tree->printTreeToConsole(0);
    std::cout << "---------------------" << std::endl;

    pTopology->setTree(tree);
}

void TopologyManager::resetTopologies()
{
    for (std::pair<std::string, boost::shared_ptr<SceneModel::Topology>> topology: mSeenTopologies)
        topology.second->mUsedInOptimization = false;
    mHistory.clear();   // reset history
    mHistoryIndex = 0;
}

void TopologyManager::printHistory(unsigned int pRunNumber)
{
    if (mHistoryOutput != "none")
    {
        std::pair<unsigned int, unsigned int> bestCostIndices;
        double overallBestCost = std::numeric_limits<double>::max();

        // find and mark best topology:
        for (unsigned int i = 0; i < mHistory.size(); i++)
        {
            std::vector<std::pair<boost::shared_ptr<SceneModel::Topology>, bool>> step = mHistory[i];

            for (unsigned int j = 0; j < step.size(); j++)
            {
                if (step[j].first->isCostValid() && step[j].first->getCost() < overallBestCost) // because of first check, this doesn't throw if there has not been set a cost.
                {
                    overallBestCost = step[j].first->getCost();
                    bestCostIndices.first = i;
                    bestCostIndices.second = j;
                }
            }
        }

        if (mHistoryOutput == "svg")
        {
            // Adapt history to ISM requirements:
            std::vector<std::vector<std::pair<ISM::TopologyPtr, unsigned int>>> history;
            std::string sceneId = mExamplesList[0]->mIdentifier; // Assuming all scene graphs are examples for the same scene.
            boost::shared_ptr<TopologyAdapter> topologyAdapter(new TopologyAdapter(mObjectTypes, sceneId));

            for (std::vector<std::pair<boost::shared_ptr<SceneModel::Topology>, bool>> psmstep: mHistory)
            {
                std::vector<std::pair<ISM::TopologyPtr, unsigned int>> ismstep;
                for (std::pair<boost::shared_ptr<SceneModel::Topology>, bool> psmtop: psmstep)
                {
                    ISM::TopologyPtr ismtop = topologyAdapter->psmToIsm(psmtop.first);
                    // ISM has no false negatives, so SVGHelper doesn't normally output them.
                    // Here, they are added to the false positives, so the output of them instead becomes the output of the number of recognition failures.
                    ismtop->evaluationResult.falsePositives += ismtop->evaluationResult.falseNegatives;
                    unsigned int selected;
                    if (psmtop.second) selected = 1;
                    else selected = 0;
                    ismstep.push_back(std::pair<ISM::TopologyPtr, unsigned int>(ismtop, selected));
                }
                history.push_back(ismstep);
            }

            // mark best topology:
            history[bestCostIndices.first][bestCostIndices.second].second = 4;

            ISM::CostFunctionPtr<ISM::TopologyPtr> globalCostFunction(new TopologyAdapter::CostFunction());
            mSVGHelper->processHistory(history, globalCostFunction, sceneId);
            // result gets written after all runs are completed.
        }
        else
        {
            std::stringstream documentation;
            std::string title = " step 000: ";
            std::string thirddivider = "";  // a third of a divider
            for (unsigned int i = 0; i < title.length(); i++) thirddivider += "=";
            std::stringstream divider;
            divider << thirddivider << thirddivider << thirddivider << std::endl;

            // Print History:
            documentation << divider.str() << "History of optimization run " << pRunNumber << ":" << std::endl << divider.str();

            unsigned int numberOfVisitedTopologies = 0;
            for (unsigned int i = 0; i < mHistory.size(); i++)
            {
                std::string padding = "";
                if (i < 10) padding += "0";
                if (i < 100) padding += "0";
                std::string title = " step " + padding + boost::lexical_cast<std::string>(i) + ": ";
                documentation << thirddivider << title << thirddivider << std::endl;

                std::vector<std::pair<boost::shared_ptr<SceneModel::Topology>, bool>> step = mHistory[i];

                for (unsigned int j = 0; j < step.size(); j++)
                {
                    std::pair<boost::shared_ptr<SceneModel::Topology>, bool> topologyPair = step[j];
                    boost::shared_ptr<SceneModel::Topology> topology = topologyPair.first;
                    documentation << topology->mIdentifier << ": ";
                    numberOfVisitedTopologies++;
                    if (topology->isEvaluated())
                    {
                        documentation << topology->getFalsePositives() << " false positives, " << topology->getFalseNegatives() << " false negatives, " <<  topology->getAverageRecognitionRuntime() << "s average recognition runtime.";
                        if (topology->isCostValid())
                        {
                            documentation << " cost: " << topology->getCost();
                            if (topologyPair.second) documentation << "[selected]";
                            if (i == bestCostIndices.first && j == bestCostIndices.second) documentation << "(best)";
                        }
                        else documentation << " <cost not calculated> ";
                    }
                    else documentation << " <not evaluated> ";
                    documentation << std::endl;
                }
            }
            documentation << divider.str();
            documentation << "Visited " << numberOfVisitedTopologies << " topologies  in " << mHistory.size() << " steps." << std::endl;
            documentation << "Best topology overall: " << std::endl;
            documentation << mHistory[bestCostIndices.first][bestCostIndices.second].first->mIdentifier << ": ";
            documentation << " cost: "  << overallBestCost << std::endl << divider.str();

            if (mHistoryOutput == "screen") // print to console:
            {
                mPrintHelper.printAsHeader("History:");
                std::cout << documentation.str();
            }
            else if (mHistoryOutput == "txt")
            {
                std::string filename = mHistoryFilePath + "history_of_run_" + boost::lexical_cast<std::string>(pRunNumber) + ".txt";
                std::ofstream file;
                file.open(filename);
                if (file.is_open())
                {
                    file << documentation.str();
                    file.close();
                }
                else throw std::runtime_error("Could not open file " + filename);
            }
            else throw std::runtime_error("Invalid history output type " + mHistoryOutput);
        }
    }
    // If mHistoryOutput == none: do nothing.
}

}