BlobRecognition.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    VisionX::Component
* @author     Kai Welke <welke at kit dot edu>
* @copyright  2013 Humanoids Group, HIS, KIT
* @license    http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "BlobRecognition.h"
 
#include <ArmarXCore/core/system/ArmarXDataPath.h>
 
#include <RobotAPI/interface/core/RobotStateObserverInterface.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/Pose.h>
 
 
#include <Image/ByteImage.h>
#include <Image/ImageProcessor.h>
 
 
#include <ObjectFinder/ObjectFinderStereo.h>
#include <ObjectFinder/CompactRegionFilter.h>
#include <SegmentableRecognition/SegmentableDatabase.h>
 
#include <MemoryX/libraries/helpers/ObjectRecognitionHelpers/ObjectRecognitionWrapper.h>
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
 
// This file must be included after all Ice-related headers when Ice 3.5.x is used (e.g. with Ubuntu 14.04 or Debian 7.0)!
// GLContext.h indirectly includes X.h which does "#define None", colliding with IceUtil::None.
#include <gui/GLContext.h>
 
using namespace armarx;
using namespace visionx;
using namespace memoryx::EntityWrappers;
 
 
BlobRecognition::BlobRecognition()
{
}
 
BlobRecognition::~BlobRecognition()
{
 
}
 
void BlobRecognition::onExitObjectLocalizerProcessor()
{
 
}
 
bool BlobRecognition::initRecognizer()
{
 
 
    Eigen::Vector3f minPoint = getProperty<Eigen::Vector3f>("MinPoint").getValue();
    Eigen::Vector3f maxPoint = getProperty<Eigen::Vector3f>("MaxPoint").getValue();
 
    Math3d::SetVec(validResultBoundingBoxMin, minPoint(0), minPoint(1), minPoint(2));
    Math3d::SetVec(validResultBoundingBoxMax, maxPoint(0), maxPoint(1), maxPoint(2));
 
    minPixelsPerRegion = getProperty<float>("MinPixelsPerRegion").getValue();
    maxEpipolarDistance = getProperty<float>("MaxEpipolarDistance").getValue();
    std::string colorParameterFilename = getProperty<std::string>("ColorParameterFile").getValue();
 
    if (!ArmarXDataPath::getAbsolutePath(colorParameterFilename, colorParameterFilename))
    {
        ARMARX_ERROR << "Could not find color parameter file in ArmarXDataPath: " << colorParameterFilename;
    }
 
    if (!objectFinderStereo)
    {
        colorParameters.reset(new CColorParameterSet());
 
        if (!colorParameters->LoadFromFile(colorParameterFilename.c_str()))
        {
            throw armarx::LocalException("Could not read color parameter file.");
        }
 
        objectFinderStereo.reset(new CObjectFinderStereo());
 
        ImageFormatInfo imageFormat = getImageFormat();
        contextGL.reset(new CGLContext());
        contextGL->CreateContext(imageFormat.dimension.width, imageFormat.dimension.height);
        contextGL->MakeCurrent();
 
        m_pOpenGLVisualizer.reset(new COpenGLVisualizer());
        m_pOpenGLVisualizer->InitByCalibration(getStereoCalibration()->GetRightCalibration());
 
        objectFinderStereo->SetColorParameterSet(colorParameters.get());
        objectFinderStereo->SetRegionFilter(new CCompactRegionFilter());
 
 
        ARMARX_VERBOSE << "BlobRecognition is initialized";
    }
 
 
    objectFinderStereo->Init(getStereoCalibration());
 
 
    return true;
}
 
bool BlobRecognition::addObjectClass(const memoryx::EntityPtr& objectClassEntity, const memoryx::GridFileManagerPtr& fileManager)
{
    ARMARX_VERBOSE << "Adding object class " << objectClassEntity->getName() << armarx::flush;
 
    SegmentableRecognitionWrapperPtr recognitionWrapper = objectClassEntity->addWrapper(new SegmentableRecognitionWrapper(fileManager));
    std::string dataPath = recognitionWrapper->getDataFiles();
    ObjectColor color = recognitionWrapper->getObjectColor();
 
    std::string colorString;
    CColorParameterSet::Translate(color, colorString);
    ARMARX_VERBOSE << "Color: " << (int)color << " (" << colorString << ")";
 
    if (dataPath != "")
    {
        // remember colors
        std::string className = objectClassEntity->getName();
        objectColors.insert(std::make_pair(className, color));
        colorToObjectClass.insert(std::make_pair(color, className));
 
        return true;
    }
    else
    {
        ARMARX_WARNING << "Datapath is empty for " << objectClassEntity->getName() << " object class - make sure you set the correct model file in PriorKnowledge!";
    }
 
    return false;
}
 
memoryx::ObjectLocalizationResultList BlobRecognition::localizeObjectClasses(const std::vector<std::string>& objectClassNames, CByteImage** cameraImages, armarx::MetaInfoSizeBasePtr imageMetaInfo, CByteImage** resultImages)
{
 
    RemoteRobot::synchronizeLocalCloneToTimestamp(localRobot, robotStateComponent, imageMetaInfo->timeProvided / 1000.0);
 
    objectFinderStereo->PrepareImages(cameraImages);
    objectFinderStereo->ClearObjectList();
 
    if (objectClassNames.size() < 1)
    {
        ARMARX_WARNING << "objectClassNames.size() = " << objectClassNames.size() << ", something is wrong here! ";
 
        memoryx::ObjectLocalizationResultList resultList;
        return resultList;
    }
 
    std::string allObjectNames;
    for (size_t i = 0; i < objectClassNames.size(); i++)
    {
        allObjectNames.append(" ");
        allObjectNames.append(objectClassNames.at(i));
    }
 
    contextGL->MakeCurrent();
 
 
    std::string color;
 
    for (size_t i = 0; i < objectClassNames.size(); i++)
    {
        CColorParameterSet::Translate(objectColors[objectClassNames.at(i)], color);
        ARMARX_VERBOSE << "Localizing " << objectClassNames.at(i) << ", color: " << color << flush;
 
        ObjectColor color = objectColors[objectClassNames.at(i)];
 
        objectFinderStereo->FindObjects(cameraImages, resultImages, color, minPixelsPerRegion, true);
 
    }
 
    objectFinderStereo->Finalize(validResultBoundingBoxMin.z, validResultBoundingBoxMax.z, true, eNone, maxEpipolarDistance, getImagesAreUndistorted());
    Object3DList objectList = objectFinderStereo->m_objectList;
 
    ARMARX_INFO << "Found " << objectList.size() << " objects";
 
 
    // help for problem analysis
    if (objectList.size() == 0)
    {
        ARMARX_INFO << "Looked unsuccessfully for:";
        std::string color;
 
        for (size_t i = 0; i < objectClassNames.size(); i++)
        {
            CColorParameterSet::Translate(objectColors[objectClassNames.at(i)], color);
            ARMARX_INFO << objectClassNames.at(i) << " color: " << color << flush;
        }
    }
 
    const auto agentName = getProperty<std::string>("AgentName").getValue();
    memoryx::ObjectLocalizationResultList resultList;
 
    for (Object3DEntry const& entry : objectList)
    {
        bool queriedClass = false;
 
        if (colorToObjectClass.count(entry.color))
        {
            queriedClass = (std::find(objectClassNames.begin(), objectClassNames.end(), colorToObjectClass[entry.color]) != objectClassNames.end());
        }
 
        if (queriedClass)
        {
 
            std::string objectName = colorToObjectClass[entry.color];
            float x = entry.pose.translation.x;
            float y = entry.pose.translation.y;
            float z = entry.pose.translation.z;
 
            if (seq.count(objectName))
            {
                seq[objectName]++;
            }
            else
            {
                seq[objectName] = 0;
            }
 
 
            StringVariantBaseMap mapValues;
            mapValues["x"] = new Variant(x);
            mapValues["y"] = new Variant(y);
            mapValues["z"] = new Variant(z);
            mapValues["name"] = new Variant(objectName);
            mapValues["sequence"] = new Variant(seq[objectName]);
            mapValues["timestamp"] =  new Variant(imageMetaInfo->timeProvided / 1000.0 / 1000.0);
            debugObserver->setDebugChannel("ObjectRecognition", mapValues);
 
            // only accept realistic positions
            if (x > validResultBoundingBoxMin.x && y > validResultBoundingBoxMin.y && z > validResultBoundingBoxMin.z &&
                x < validResultBoundingBoxMax.x && y < validResultBoundingBoxMax.y && z < validResultBoundingBoxMax.z)
            {
                // assemble result
                memoryx::ObjectLocalizationResult result;
 
                // position and orientation
                Eigen::Vector3f positionVec(entry.pose.translation.x, entry.pose.translation.y, entry.pose.translation.z);
 
                FramedPositionPtr position = new FramedPosition(positionVec, referenceFrameName, agentName);
                position->changeToGlobal(localRobot);
                float expectedHeight = 1020.0;
                if (std::fabs(position->z - expectedHeight) < 50.0)
                {
                    // manually update height
                    position->z = expectedHeight;
                    position->changeFrame(localRobot, referenceFrameName);
 
                    Eigen::Matrix3f m = Eigen::Matrix3f::Identity();
                    FramedOrientationPtr orientation = new FramedOrientation(m, armarx::GlobalFrame, agentName);
                    orientation->changeFrame(localRobot, referenceFrameName);
 
                    result.position = position;
                    result.orientation = orientation;
 
                    // calculate noise
                    result.positionNoise = calculateLocalizationUncertainty(entry.region_left.centroid, entry.region_right.centroid);
 
                    // calculate recognition certainty
                    result.recognitionCertainty = 0.5f + 0.5f * calculateRecognitionCertainty(objectName, entry);
                    result.objectClassName = objectName;
                    result.timeStamp = new TimestampVariant(imageMetaInfo->timeProvided);
 
                    resultList.push_back(result);
                }
                else
                {
                    ARMARX_INFO << deactivateSpam(1) << "Ignoring object localization result with height " << position->z;
                }
            }
            else
            {
                ARMARX_VERBOSE_S << "Refused unrealistic localization at position: " << x << " " << y << " " << z;
            }
        }
    }
 
    ARMARX_VERBOSE << "Finished localizing " << objectClassNames.at(0);
 
    return resultList;
}
 
 
 
float BlobRecognition::calculateRecognitionCertainty(const std::string& objectClassName, const Object3DEntry& entry)
{
    float foundProb = entry.quality * entry.quality2;
    float notFoundProb = (1 - entry.quality) * (1 - entry.quality2);
 
    if (foundProb <= 0)
    {
        return 0.0f;
    }
 
    return foundProb / (foundProb + notFoundProb);
}