TexturedObjectRecognition.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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)
 * @date       2013
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "TexturedObjectRecognition.h"
 
// RobotState
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
// MemoryX
#include <ArmarXCore/observers/variant/TimestampVariant.h>
 
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/libraries/helpers/ObjectRecognitionHelpers/ObjectRecognitionWrapper.h>
 
// IVT
#include <Calibration/Calibration.h>
 
// IVTRecognition
#include <TexturedRecognition/TexturedFeatureSet.h>
#include <TexturedRecognition/TexturedObjectDatabase.h>
#include <TexturedRecognition/TexturedRecognition.h>
 
using namespace armarx;
using namespace visionx;
using namespace memoryx;
using namespace memoryx::EntityWrappers;
 
bool
TexturedObjectRecognition::initRecognizer()
{
    float SIFTThreshold = getProperty<float>("SIFTThreshold").getValue();
 
    texturedRecognition = new CTexturedRecognition(
        getImageFormat().dimension.width, getImageFormat().dimension.height, SIFTThreshold);
    texturedRecognition->SetVerbose(true);
    texturedRecognition->SetStereo(true);
    texturedRecognition->SetQualityThreshold(getProperty<float>("QualityThreshold").getValue());
    texturedRecognition->SetRecognitionThresholds(getProperty<int>("nMinValidFeatures").getValue(),
                                                  getProperty<float>("MaxError").getValue());
 
 
    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));
 
 
    correlationWindowSize = getProperty<int>("StereoCorrelationWindowSize").getValue();
    correlationThreshold = getProperty<float>("StereoCorrelationThreshold").getValue();
 
    texturedRecognition->GetObjectDatabase()->InitCameraParameters(getStereoCalibration(), true);
    texturedRecognition->GetObjectDatabase()->SetCorrelationParameters(
        correlationWindowSize, minPoint(2), maxPoint(2), correlationThreshold);
 
    return true;
}
 
bool
TexturedObjectRecognition::addObjectClass(const memoryx::EntityPtr& objectClassEntity,
                                          const memoryx::GridFileManagerPtr& fileManager)
{
    TexturedRecognitionWrapperPtr recognitionWrapper =
        objectClassEntity->addWrapper(new TexturedRecognitionWrapper(fileManager));
    std::string fileName = recognitionWrapper->getFeatureFile();
 
    if (fileName != "")
    {
        std::string className = objectClassEntity->getName();
 
        if (texturedRecognition->GetObjectDatabase()->AddClass(className, fileName))
        {
            return true;
        }
    }
    else
    {
        ARMARX_WARNING_S << "No descriptor file defined for object "
                         << objectClassEntity->getName();
    }
 
    return false;
}
 
memoryx::ObjectLocalizationResultList
TexturedObjectRecognition::localizeObjectClasses(const std::vector<std::string>& objectClassNames,
                                                 CByteImage** cameraImages,
                                                 armarx::MetaInfoSizeBasePtr imageMetaInfo,
                                                 CByteImage** resultImages)
{
    std::string allObjectNames;
 
    for (size_t i = 0; i < objectClassNames.size(); i++)
    {
        allObjectNames.append(objectClassNames.at(i));
        allObjectNames.append(" ");
    }
 
    ARMARX_DEBUG_S << "Localizing " << allObjectNames;
 
    // result images
    CByteImage** tmpResultImages = getResultImagesEnabled() ? resultImages : NULL;
 
    if (objectClassNames.size() == 1)
    {
        texturedRecognition->DoRecognitionSingleObject(cameraImages,
                                                       tmpResultImages,
                                                       objectClassNames.at(0).c_str(),
                                                       true,
                                                       50,
                                                       getImagesAreUndistorted());
    }
    else
    {
        // TODO: multiple is not implemented so searching for all classes
        texturedRecognition->DoRecognition(
            cameraImages, tmpResultImages, true, 50, getImagesAreUndistorted());
    }
 
    Object3DList objectList = texturedRecognition->GetObject3DList();
 
    ARMARX_DEBUG_S << "Localized " << objectList.size() << " objects";
    const auto agentName = getProperty<std::string>("AgentName").getValue();
 
    memoryx::ObjectLocalizationResultList resultList;
 
    for (Object3DList::iterator iter = objectList.begin(); iter < objectList.end(); iter++)
    {
        // assure instance belongs to queried class
        bool queriedClass =
            (std::find(objectClassNames.begin(), objectClassNames.end(), iter->sName) !=
             objectClassNames.end());
 
        if (iter->localizationValid && queriedClass)
        {
            float x = iter->pose.translation.x;
            float y = iter->pose.translation.y;
            float z = iter->pose.translation.z;
 
            StringVariantBaseMap mapValues;
            mapValues["x"] = new Variant(x);
            mapValues["y"] = new Variant(y);
            mapValues["z"] = new Variant(z);
            mapValues["name"] = new Variant(iter->sName);
            mapValues["sequence"] = new Variant(seq++);
            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 position(x, y, z);
                Eigen::Matrix3f orientation;
                orientation << iter->pose.rotation.r1, iter->pose.rotation.r2,
                    iter->pose.rotation.r3, iter->pose.rotation.r4, iter->pose.rotation.r5,
                    iter->pose.rotation.r6, iter->pose.rotation.r7, iter->pose.rotation.r8,
                    iter->pose.rotation.r9;
 
                result.position =
                    new armarx::FramedPosition(position, referenceFrameName, agentName);
                result.orientation =
                    new armarx::FramedOrientation(orientation, referenceFrameName, agentName);
 
                // calculate recognition certainty
                result.recognitionCertainty =
                    0.5f + 0.5f * calculateRecognitionCertainty(iter->sName, *iter);
 
                // calculate position uncertainty
                memoryx::MultivariateNormalDistributionPtr posUncertainty =
                    calculateLocalizationUncertainty(position);
                Eigen::MatrixXf cov = posUncertainty->toEigenCovariance();
                cov *= 4.0f / pow(result.recognitionCertainty, 4);
                result.positionNoise = memoryx::MultivariateNormalDistributionPtr(
                    new memoryx::MultivariateNormalDistribution(posUncertainty->toEigenMean(),
                                                                cov));
 
                result.objectClassName = iter->sName;
                result.timeStamp = new TimestampVariant(imageMetaInfo->timeProvided);
 
                resultList.push_back(result);
 
                FramedPose objectPose(orientation, position, referenceFrameName, agentName);
                ARMARX_DEBUG_S << "Localized object at pose: " << objectPose;
            }
            else
            {
                ARMARX_DEBUG_S << "Refused unrealistic localization at position: " << x << " " << y
                               << " " << z;
            }
        }
    }
 
    return resultList;
}
 
float
TexturedObjectRecognition::calculateRecognitionCertainty(const std::string& objectClassName,
                                                         const Object3DEntry& entry)
{
    //**************************************************************************************************
    // calculate recognition certainty (ask David Schiebener about that part !!!!)
    // quality: mean of pixel error
    // quality2: found number of feature correspondences
    //**************************************************************************************************
    // retrieve number of known features for this object
    int objectIndex = -1;
    int n = texturedRecognition->GetObjectDatabase()->GetSize();
 
    for (int i = 0; i < n; i++)
    {
        if (objectClassName.compare(
                texturedRecognition->GetObjectDatabase()->GetFeatureSet(i)->GetName()) == 0)
        {
            objectIndex = i;
            break;
        }
    }
 
    if (objectIndex == -1)
    {
        return 0.0f;
    }
 
    int numberObjectFeatures =
        texturedRecognition->GetObjectDatabase()->GetFeatureSet(objectIndex)->GetSize();
 
    // rate with 1 if pixel error <= 0.7, rate with 1/e if error = 2.2
    float temp = (entry.quality > 0.7f) ? entry.quality - 0.7f : 0;
    float ratingPixelError = expf(-(4.0f / 9.0f) * temp * temp);
    ratingPixelError = 0.15f + 0.7f * ratingPixelError;
 
    // rate with 0.15 if 10 matched features, with 0.85 if 30 matched features
    temp = 0.1f * (entry.quality2 - 20.0f);
    float ratingMatchedFeaturesAbsolute = 0.5f + 0.5f * (temp / sqrtf(1.0f + temp * temp));
    ratingMatchedFeaturesAbsolute = 0.15f + 0.7f * ratingMatchedFeaturesAbsolute;
 
    // rate with 1 if number of matched features >= 0.15 * total number of features
    if (numberObjectFeatures > 300)
    {
        numberObjectFeatures = 300;
    }
 
    temp = entry.quality2;
 
    if (temp > (float)numberObjectFeatures)
    {
        temp = (float)numberObjectFeatures;
    }
 
    float ratingMatchedFeaturesRelative = temp / (0.15f * (float)numberObjectFeatures);
    ratingMatchedFeaturesRelative =
        (ratingMatchedFeaturesRelative > 1.0f) ? 1.0f : ratingMatchedFeaturesRelative;
    ratingMatchedFeaturesRelative = 0.15f + 0.7f * ratingMatchedFeaturesRelative;
 
    // division is no problem because fRatingPixelError*fRatingMatchedFeaturesAbsolute*fRatingMatchedFeaturesRelative > 0
    float recognitionCertainty =
        (ratingPixelError * ratingMatchedFeaturesAbsolute * ratingMatchedFeaturesRelative) /
        ((ratingPixelError * ratingMatchedFeaturesAbsolute * ratingMatchedFeaturesRelative) +
         ((1 - ratingPixelError) * (1 - ratingMatchedFeaturesAbsolute) *
          (1 - ratingMatchedFeaturesRelative)));
 
    return recognitionCertainty;
}