SegmentableObjectRecognition.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 "SegmentableObjectRecognition.h"
 
// Core
#include <ArmarXCore/core/system/ArmarXDataPath.h>
 
// RobotState
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/Pose.h>
 
// IVT
 
#include <Image/ByteImage.h>
#include <Image/ImageProcessor.h>
#include <SegmentableRecognition/SegmentableDatabase.h>
 
// MemoryX
#include <ArmarXCore/observers/variant/TimestampVariant.h>
 
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/libraries/helpers/ObjectRecognitionHelpers/ObjectRecognitionWrapper.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;
 
SegmentableObjectRecognition::SegmentableObjectRecognition()
{
}
 
SegmentableObjectRecognition::~SegmentableObjectRecognition()
{
}
 
void
SegmentableObjectRecognition::onExitObjectLocalizerProcessor()
{
}
 
bool
SegmentableObjectRecognition::initRecognizer()
{
    ARMARX_VERBOSE << "Initializing SegmentableObjectRecognition";
 
    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 colorParemeterFilename = getProperty<std::string>("ColorParameterFile").getValue();
 
    if (!ArmarXDataPath::getAbsolutePath(colorParemeterFilename, colorParemeterFilename))
    {
        ARMARX_ERROR << "Could not find color parameter file in ArmarXDataPath: "
                     << colorParemeterFilename;
    }
 
 
    if (!segmentableRecognition)
    {
        colorParameters.reset(new CColorParameterSet());
 
        if (!colorParameters->LoadFromFile(colorParemeterFilename.c_str()))
        {
            throw armarx::LocalException("Could not read color parameter file.");
        }
 
        ARMARX_VERBOSE << "Startup step 1";
 
        segmentableRecognition.reset(new CSegmentableRecognition());
 
        ARMARX_VERBOSE << "Startup step 2";
 
        // create context
        ImageFormatInfo imageFormat = getImageFormat();
        contextGL.reset(new CGLContext());
 
        ARMARX_VERBOSE << "Startup step 3";
 
        contextGL->CreateContext(imageFormat.dimension.width, imageFormat.dimension.height);
 
        ARMARX_VERBOSE << "Startup step 4";
 
        contextGL->MakeCurrent();
 
        m_pOpenGLVisualizer.reset(new COpenGLVisualizer());
        m_pOpenGLVisualizer->InitByCalibration(getStereoCalibration()->GetRightCalibration());
 
        ARMARX_VERBOSE << "Startup step 5";
 
        // init segmentable recognition
        bool success = segmentableRecognition->InitWithoutDatabase(colorParameters.get(),
                                                                   getStereoCalibration());
 
        if (!success)
        {
            throw armarx::LocalException("Could not initialize segmentable object database.");
        }
 
        float sizeRatioThreshold = getProperty<float>("SizeRatioThreshold").getValue();
        float correlationThreshold = getProperty<float>("CorrelationThreshold").getValue();
        segmentableRecognition->GetObjectDatabase()->SetRecognitionThresholds(sizeRatioThreshold,
                                                                              correlationThreshold);
 
        ARMARX_VERBOSE << "SegmentableObjectRecognition is initialized";
 
        return success;
    }
 
    ARMARX_VERBOSE << "SegmentableObjectRecognition is initialized";
 
    return true;
}
 
bool
SegmentableObjectRecognition::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 != "")
    {
        if (!segmentableRecognition->GetObjectDatabase()->AddClass(
                dataPath, "", objectClassEntity->getName()))
        {
            ARMARX_INFO << "Skipped object class: " << objectClassEntity->getName();
            return false;
        }
 
        // remember colors
        std::string className = objectClassEntity->getName();
        objectColors.insert(std::make_pair(className, color));
 
        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
SegmentableObjectRecognition::localizeObjectClasses(
    const std::vector<std::string>& objectClassNames,
    CByteImage** cameraImages,
    armarx::MetaInfoSizeBasePtr imageMetaInfo,
    CByteImage** resultImages)
{
 
    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));
    }
 
    ARMARX_VERBOSE << "Localizing" << allObjectNames;
 
    contextGL->MakeCurrent();
 
    Object3DList objectList;
 
    if (objectClassNames.size() < 10)
    {
        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;
 
            segmentableRecognition->DoRecognitionSingleObject(cameraImages,
                                                              resultImages,
                                                              objectClassNames.at(i).c_str(),
                                                              minPixelsPerRegion,
                                                              maxEpipolarDistance,
                                                              objectColors[objectClassNames.at(i)],
                                                              false,
                                                              getImagesAreUndistorted());
 
            Object3DList result = segmentableRecognition->GetObject3DList();
 
            for (size_t j = 0; j < result.size(); j++)
            {
                objectList.push_back(result.at(j));
            }
 
            ARMARX_VERBOSE << "Found " << result.size() << " instances";
        }
    }
    else
    {
        ARMARX_VERBOSE << "Localizing everything at once";
 
        segmentableRecognition->DoRecognition(cameraImages,
                                              resultImages,
                                              minPixelsPerRegion,
                                              false,
                                              maxEpipolarDistance,
                                              eNone,
                                              false,
                                              getImagesAreUndistorted());
 
        objectList = segmentableRecognition->GetObject3DList();
    }
 
 
    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;
        }
    }
    else
    {
        visualizeResults(objectList, resultImages);
    }
 
    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;
 
            if (seq.count(iter->sName))
            {
                seq[iter->sName]++;
            }
            else
            {
                seq[iter->sName] = 0;
            }
 
 
            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[iter->sName]);
            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(
                    iter->pose.translation.x, iter->pose.translation.y, iter->pose.translation.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 noise
                result.positionNoise = calculateLocalizationUncertainty(
                    iter->region_left.centroid, iter->region_right.centroid);
 
                // calculate recognition certainty
                result.recognitionCertainty =
                    0.5f + 0.5f * calculateRecognitionCertainty(iter->sName, *iter);
                result.objectClassName = iter->sName;
                result.timeStamp = new TimestampVariant(imageMetaInfo->timeProvided);
 
                resultList.push_back(result);
            }
            else
            {
                ARMARX_VERBOSE_S << "Refused unrealistic localization at position: " << x << " "
                                 << y << " " << z;
            }
        }
    }
 
    ARMARX_VERBOSE << "Finished localizing " << objectClassNames.at(0);
 
    return resultList;
}
 
float
SegmentableObjectRecognition::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);
}
 
void
SegmentableObjectRecognition::visualizeResults(const Object3DList& objectList,
                                               CByteImage** resultImages)
{
    m_pOpenGLVisualizer->ActivateShading(true);
    glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
 
 
    m_pOpenGLVisualizer->SetProjectionMatrix(getStereoCalibration()->GetRightCalibration());
 
    m_pOpenGLVisualizer->Clear();
 
 
    for (int i = 0; i < (int)objectList.size(); i++)
    {
        const Object3DEntry& entry = objectList.at(i);
        CSegmentableDatabase::DrawObjectFromFile(
            m_pOpenGLVisualizer.get(), entry.sOivFilePath.c_str(), entry.pose);
    }
 
    const int nImageIndex = 1;
 
    if (resultImages && resultImages[nImageIndex])
    {
        CByteImage tempImage(resultImages[nImageIndex]);
        m_pOpenGLVisualizer->GetImage(&tempImage);
        ::ImageProcessor::FlipY(&tempImage, &tempImage);
        const int nBytes = 3 * tempImage.width * tempImage.height;
        const unsigned char* pixels = tempImage.pixels;
        unsigned char* output = resultImages[nImageIndex]->pixels;
 
        for (int i = 0; i < nBytes; i += 3)
        {
            if (pixels[i])
            {
                const unsigned char g = pixels[i];
                output[i] = g;
                output[i + 1] = g;
                output[i + 2] = g;
            }
        }
    }
}