scene_configurator_data.cpp

Go to the documentation of this file.

// Local Include
#include "asr_ism/scene_configurator_data.hpp"

// ISM Inlcude
#include <ISM/utility/GeometryHelper.hpp>



void SceneConfiguratorData::addEstimation(ISM::ObjectPtr object)
{
    boost::lock_guard<boost::mutex> lock{estimations_mutex_}; //Thread-safety

    //Look whether object has already been observed and update or add the estimation for this object.
    bool found = false;
    for (ObjectEstimationPtrs::iterator it = estimations_.begin(); it != estimations_.end(); ++it)
    {
        if (((*it)->object->type == object->type) && ((*it)->object->observedId == object->observedId)
            && (!enable_neighborhood_evaluation_ || ISM::GeometryHelper::sharedNeighborhoodEvaluation((*it)->object->pose, object->pose, neighbour_distance_threshold_, neighbour_angle_threshold_)))
        {
            (*it)->object = object;
            (*it)->in_view = true;
            found = true;
            break;
        }
    }

    if (!found)
    {
        estimations_.push_back(ObjectEstimationPtr(new ObjectEstimation(object)));
    }
}

void SceneConfiguratorData::resetInViewForAllEstimations()
{
    boost::lock_guard<boost::mutex> lock{estimations_mutex_}; //Thread-safety

    for (ObjectEstimationPtrs::iterator it = estimations_.begin(); it != estimations_.end(); ++it)
    {
        (*it)->in_view = false;
    }
}

void SceneConfiguratorData::setMarkedForAllEstimationsInView(bool marked)
{
    boost::lock_guard<boost::mutex> lock{estimations_mutex_}; //Thread-safety

    for (ObjectEstimationPtrs::iterator it = estimations_.begin(); it != estimations_.end(); ++it)
    {
        if ((*it)->in_view)
        {
            (*it)->marked = marked;
        }
    }
}

void SceneConfiguratorData::unmarkAllEstimations()
{
    boost::lock_guard<boost::mutex> lock{estimations_mutex_}; //Thread-safety

    for (ObjectEstimationPtrs::iterator it = estimations_.begin(); it != estimations_.end(); ++it)
    {
        (*it)->marked = false;
    }
}

ISM::ObjectSetPtr SceneConfiguratorData::getObjectSetFromMarkedEstimations()
{
    ISM::ObjectSetPtr set(new ISM::ObjectSet());

    boost::lock_guard<boost::mutex> lock{estimations_mutex_}; //Thread-safety

    for (ObjectEstimationPtrs::iterator it = estimations_.begin(); it != estimations_.end(); ++it)
    {
        if ((*it)->marked)
        {
            set->insert(ISM::ObjectPtr(new ISM::Object(*(*it)->object)));
        }
    }
    return set;
}

ISM::ObjectSetPtr SceneConfiguratorData::getObjectSetFromAllEstimations()
{
    ISM::ObjectSetPtr set(new ISM::ObjectSet());

    boost::lock_guard<boost::mutex> lock{estimations_mutex_}; //Thread-safety

    for (ObjectEstimationPtrs::iterator it = estimations_.begin(); it != estimations_.end(); ++it)
    {
        set->insert(ISM::ObjectPtr(new ISM::Object(*(*it)->object)));
    }
    return set;
}

ObjectEstimationPtrs SceneConfiguratorData::getAllEstimations()
{
    ObjectEstimationPtrs estimations;

    boost::lock_guard<boost::mutex> lock{estimations_mutex_}; //Thread-safety

    for (const ObjectEstimationPtr estimation : estimations_)
    {
        estimations.push_back(ObjectEstimationPtr(new ObjectEstimation(*estimation)));
    }
    return estimations;
}

void SceneConfiguratorData::clearAllEstimations()
{
    boost::lock_guard<boost::mutex> lock{estimations_mutex_}; //Thread-safety
    estimations_.clear();
}

void SceneConfiguratorData::clearUnmarkedEstimations()
{
    boost::lock_guard<boost::mutex> lock{estimations_mutex_}; //Thread-safety

    for (ObjectEstimationPtrs::iterator it = estimations_.begin(); it != estimations_.end(); )
    {
        if (!((*it)->marked))
        {
            it = estimations_.erase(it);
        }
        else
        {
            ++it;
        }
    }
}