ObjectLocalizerProcessor.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 "ObjectLocalizerProcessor.h"
 
// VisionXInterface
#include <VisionX/interface/core/DataTypes.h>
#include <VisionX/interface/components/PointCloudAndImageAndCalibrationProviderInterface.h>
 
// VisionXTools
#include <VisionX/tools/ImageUtil.h>
#include <RobotAPI/libraries/core/Pose.h>
 
// MemoryX
#include <MemoryX/core/entity/Entity.h>
#include <MemoryX/core/entity/ProbabilityMeasures.h>
#include <MemoryX/libraries/helpers/ObjectRecognitionHelpers/ObjectRecognitionWrapper.h>
#include <MemoryX/libraries/helpers/EarlyVisionHelpers/EarlyVisionConverters.h>
#include <MemoryX/libraries/helpers/EarlyVisionHelpers/UnscentedTransform.h>
 
// ArmarXCore
#include <ArmarXCore/core/exceptions/Exception.h>
 
// IVT
#include "Calibration/Calibration.h"
 
namespace visionx
{
 
    ObjectLocalizerProcessor::ObjectLocalizerProcessor()
        : resultImagesEnabled(true)
        , imagesAreUndistorted(false)
        , numberOfResultImages(2)
        , job(this)
    {
        cameraImages[0] = &cameraImagesData[0];
        cameraImages[1] = &cameraImagesData[1];
    }
 
    void ObjectLocalizerProcessor::onInitImageProcessor()
    {
        ARMARX_VERBOSE << "Number of result images: " << numberOfResultImages;
        resultImagesData.clear();
        resultImagesData.resize(numberOfResultImages, nullptr);
        // HACK: boost::ptr_vector::c_array() is broken if the value type is nullable (check in new boost version)
        void* firstEntry = &resultImagesData.begin().base()[0];
        resultImages = static_cast<CByteImage**>(firstEntry);
 
        // get properties
        priorKnowledgeProxyName = getProperty<std::string>("PriorKnowledgeProxyName").getValue();
        imageProviderName = getProperty<std::string>("ImageProviderName").getValue();
 
        resultImagesEnabled = getProperty<bool>("EnableResultImages").getValue();
 
        useResultImageMask = getProperty<bool>("useResultImageMask").getValue();
        colorMask = getProperty<Eigen::Vector3i>("colorMask").getValue();
 
        // setup proxy
        usingProxy(priorKnowledgeProxyName);
        usingImageProvider(imageProviderName);
        usingTopic(getProperty<std::string>("CalibrationUpdateTopicName").getValue());
 
        // setup noise
        imageNoiseLeft(0) = imageNoiseLeft(1) = getProperty<float>("2DLocalizationNoise").getValue();
        imageNoiseRight(0) = imageNoiseRight(1) = getProperty<float>("2DLocalizationNoise").getValue();
 
        // subclass initialization
        onInitObjectLocalizerProcessor();
    }
 
    void ObjectLocalizerProcessor::onConnectImageProcessor()
    {
        // retrieve proxies and required parameters
        ImageProviderInfo imageProviderInfo = getImageProvider(imageProviderName, visionx::eRgb);
        imageFormat = imageProviderInfo.imageFormat;
 
        imageProviderPrx = getProxy<ImageProviderInterfacePrx>(imageProviderName);
 
        // retrieve stereo information
 
        StereoCalibrationInterfacePrx calibrationProvider = StereoCalibrationInterfacePrx::checkedCast(imageProviderPrx);
 
        if (calibrationProvider)
        {
            stereoCalibration.reset(visionx::tools::convert(calibrationProvider->getStereoCalibration()));
            imagesAreUndistorted = calibrationProvider->getImagesAreUndistorted();
            referenceFrameName = calibrationProvider->getReferenceFrame();
        }
 
        else
        {
            ARMARX_ERROR << "Unable to get calibration data. The image provider is not a StereoCalibrationProvider";
        }
 
 
        if (resultImagesEnabled)
        {
            ARMARX_INFO << "Enabeling Visualization with " << numberOfResultImages << " images." << std::endl;
            enableResultImages(numberOfResultImages, ImageDimension(imageFormat.dimension.width, imageFormat.dimension.height), visionx::eRgb);
        }
        job.reset();
        setupImages(imageFormat.dimension.width, imageFormat.dimension.height);
 
        onConnectObjectLocalizerProcessor();
    }
 
    void ObjectLocalizerProcessor::initObjectClasses()
    {
        ARMARX_VERBOSE_S << "Initializing object classes";
 
        // init proxies
        priorKnowledgePrx = getProxy<memoryx::PriorKnowledgeInterfacePrx>(priorKnowledgeProxyName);
        classesSegmentPrx = priorKnowledgePrx->getObjectClassesSegment();
        databasePrx = priorKnowledgePrx->getCommonStorage();
 
        fileManager.reset(new memoryx::GridFileManager(databasePrx));
 
        // retrieve object classes suitable for this recognizer type
        memoryx::CollectionInterfacePrx coll = databasePrx->requestCollection(getProperty<std::string>("DataBaseObjectCollectionName").getValue());
        classesSegmentPrx->addReadCollection(coll);
        //    classesSegmentPrx->setWriteCollection(coll);
 
        memoryx::EntityIdList idList = classesSegmentPrx->getAllEntityIds();
 
        ARMARX_INFO << "Found " << idList.size() << " object classes in the class segments: " << classesSegmentPrx->getReadCollectionsNS();
 
        int classesUsed = 0;
 
        for (memoryx::EntityIdList::iterator iter = idList.begin(); iter != idList.end(); iter++)
        {
            memoryx::EntityPtr entity = memoryx::EntityPtr::dynamicCast(classesSegmentPrx->getEntityById(*iter));
 
            if (!entity)
            {
                ARMARX_IMPORTANT << "RECEIVED NULL Entity";
                continue;
            }
 
            memoryx::EntityWrappers::ObjectRecognitionWrapperPtr recognitionWrapper = entity->addWrapper(new memoryx::EntityWrappers::ObjectRecognitionWrapper());
 
            if (recognitionWrapper->getRecognitionMethod() == getName())
            {
                if (addObjectClass(entity, fileManager))
                {
                    std::string className = entity->getName();
                    ARMARX_INFO << "Adding class " << className << " to " << getDefaultName();
                    classesUsed++;
                }
            }
        }
 
        ARMARX_INFO << getDefaultName() << " is available using " << classesUsed << " object classes";
    }
 
    memoryx::ObjectLocalizationResultList ObjectLocalizerProcessor::localizeObjectClasses(const memoryx::ObjectClassNameList& objectClassNames, const Ice::Current& c)
    {
        // assure processor thread is started and ManagedIceObject is fully connected
        //    getObjectScheduler()->waitForObjectState(eManagedIceObjectStarted);
        memoryx::ObjectLocalizationResultList result;
 
        // start localization job in a separate thread
        try
        {
            job.start(objectClassNames);
            result = job.waitForResult();
        }
        catch (std::exception& e)
        {
            armarx::handleExceptions();
        }
 
        return result;
    }
 
    void ObjectLocalizerProcessor::process()
    {
        job.process();
    }
 
    memoryx::MultivariateNormalDistributionPtr ObjectLocalizerProcessor::calculateLocalizationUncertainty(Vec2d left_point, Vec2d right_point)
    {
        // initialize noise
        UnscentedTransform ut;
 
        Eigen::MatrixXd combinedNoise(4, 4);
        combinedNoise.setZero();
        combinedNoise(0, 0) = imageNoiseLeft(0);
        combinedNoise(1, 1) = imageNoiseLeft(1);
        combinedNoise(2, 2) = imageNoiseRight(0);
        combinedNoise(3, 3) = imageNoiseRight(1);
 
        Eigen::VectorXd mean(4);
        mean << left_point.x, left_point.y, right_point.x, right_point.y;
 
        Gaussian imageSpaceNoise(4);
        imageSpaceNoise.setCovariance(combinedNoise);
        imageSpaceNoise.setMean(mean);
 
        // calculate sigma points
        Eigen::MatrixXd sigmapoints = ut.getSigmaPoints(imageSpaceNoise);
 
        // pass sigma points through system
        Vec2d l, r;
        Vec3d w;
        Eigen::VectorXd base(4);
        Eigen::VectorXd world(4);
 
        Eigen::MatrixXd processedpoints(3, sigmapoints.cols());
 
        for (int n = 0 ; n < sigmapoints.cols() ; n++)
        {
 
            Math2d::SetVec(l, sigmapoints(0, n), sigmapoints(1, n));
            Math2d::SetVec(r, sigmapoints(2, n), sigmapoints(3, n));
 
            // calc 3d point (2D points are rectified but not distorted)
            stereoCalibration->Calculate3DPoint(l, r, w, true, true);
            world << w.x, w.y, w.z, 1.0f;
 
            processedpoints(0, n) = world(0);
            processedpoints(1, n) = world(1);
            processedpoints(2, n) = world(2);
        }
 
        // recover covariance
        Gaussian worldSpaceNoise = ut.extractGaussian(processedpoints);
 
        // manual hack: avoid underestimation of noise
        worldSpaceNoise.setCovariance(4.0f * worldSpaceNoise.getCovariance());
 
        return memoryx::EarlyVisionConverters::convertToMemoryX_MULTI(worldSpaceNoise);
    }
 
    memoryx::MultivariateNormalDistributionPtr ObjectLocalizerProcessor::calculateLocalizationUncertainty(const Eigen::Vector3f& position)
    {
        // calculate 2d points for localization uncerainty
        Vec2d left2d, right2d;
        Vec3d pos;
        Math3d::SetVec(pos, position(0), position(1), position(2));
 
        getStereoCalibration()->GetLeftCalibration()->WorldToImageCoordinates(pos, left2d);
        getStereoCalibration()->GetRightCalibration()->WorldToImageCoordinates(pos, right2d);
 
        // transform to rectified coordinates
        Mat3d inverseHLeft, inverseHRight;
        Math3d::Invert(getStereoCalibration()->rectificationHomographyLeft, inverseHLeft);
        Math3d::Invert(getStereoCalibration()->rectificationHomographyRight, inverseHRight);
        Math2d::ApplyHomography(inverseHLeft, left2d, left2d);
        Math2d::ApplyHomography(inverseHRight, right2d, right2d);
 
        return calculateLocalizationUncertainty(left2d, right2d);
    }
 
 
    void ObjectLocalizerProcessor::setupImages(int width, int height)
    {
        ARMARX_IMPORTANT << "Setting up images";
 
        for (CByteImage& cameraImage : cameraImagesData)
        {
            cameraImage.Set(width, height, CByteImage::eRGB24);
        }
 
        for (int i = 0; i < numberOfResultImages; i++)
        {
            resultImagesData.replace(i, new CByteImage(width, height, CByteImage::eRGB24));
        }
    }
}