FakeWorkingMemoryObjectLocalizer.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    ArmarXSimulation::ArmarXObjects::FakeWorkingMemoryObjectLocalizer
 * @author     Mirko Waechter ( mirko dot waechter at kit dot edu )
 * @date       2016
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "FakeWorkingMemoryObjectLocalizer.h"
 
#include <ArmarXCore/observers/variant/TimestampVariant.h>
 
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
 
using namespace armarx;
 
std::string
FakeWorkingMemoryObjectLocalizer::getDefaultName() const
{
    return "FakeWorkingMemoryObjectLocalizer";
}
 
void
FakeWorkingMemoryObjectLocalizer::onInitComponent()
{
    usingProxy(getProperty<std::string>("WorkingMemoryName").getValue());
}
 
void
FakeWorkingMemoryObjectLocalizer::onConnectComponent()
{
    getProxyFromProperty(wm, "WorkingMemoryName");
}
 
void
FakeWorkingMemoryObjectLocalizer::onDisconnectComponent()
{
}
 
void
FakeWorkingMemoryObjectLocalizer::onExitComponent()
{
}
 
armarx::PropertyDefinitionsPtr
FakeWorkingMemoryObjectLocalizer::createPropertyDefinitions()
{
    return armarx::PropertyDefinitionsPtr(
        new FakeWorkingMemoryObjectLocalizerPropertyDefinitions(getConfigIdentifier()));
}
 
::memoryx::MultivariateNormalDistributionPtr
computePositionNoise(const Eigen::Vector3f& pos)
{
    memoryx::FloatVector mean;
    mean.push_back(pos(0));
    mean.push_back(pos(1));
    mean.push_back(pos(2));
 
    // assuming 2 mm noise
    memoryx::MultivariateNormalDistributionPtr posDist =
        new memoryx::MultivariateNormalDistribution(3);
    posDist->setMean(mean);
    posDist->setCovariance(0, 0, 2.0f);
    posDist->setCovariance(1, 1, 2.0f);
    posDist->setCovariance(2, 2, 2.0f);
    return posDist;
}
 
memoryx::ObjectLocalizationResultList
FakeWorkingMemoryObjectLocalizer::localizeObjectClasses(
    const memoryx::ObjectClassNameList& objectClassNames,
    const Ice::Current&)
{
    memoryx::ObjectLocalizationResultList results;
    auto objInstSeg = wm->getObjectInstancesSegment();
    for (auto& className : objectClassNames)
    {
        ARMARX_DEBUG << deactivateSpam(5, className);
 
        auto objInstances = objInstSeg->getObjectInstancesByClass(className);
        for (memoryx::ObjectInstanceBasePtr obj : objInstances)
        {
            if (obj)
            {
                memoryx::ObjectLocalizationResult result;
                result.objectClassName = className;
                result.orientation = obj->getOrientationBase();
                result.position = obj->getPositionBase();
                result.positionNoise = computePositionNoise(
                    armarx::FramedPositionPtr::dynamicCast(result.position)->toEigen());
                result.recognitionCertainty = 0.95;
                result.timeStamp = TimestampVariant::nowPtr();
                ARMARX_INFO << "Returning instance at pose " << obj->getPositionBase()->output();
                results.push_back(result);
            }
        }
    }
    return results;
}