ObjectLocalizerProcessor.h

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
 */
 
#pragma once
 
#include "ObjectLocalizerProcessorJob.h"
 
// VisionX
#include <VisionX/core/ImageProcessor.h>
#include <VisionX/tools/TypeMapping.h>
 
// VisionXInterface
#include <VisionX/interface/components/ObjectLocalizerInterfaces.h>
#include <VisionX/interface/units/ObjectRecognitionUnit.h>
 
// MemoryX
#include <MemoryX/core/GridFileManager.h>
#include <MemoryX/core/entity/Entity.h>
#include <MemoryX/interface/components/PriorKnowledgeInterface.h>
 
// IVT
#include <string>
 
#include <boost/ptr_container/ptr_vector.hpp>
 
#include <Eigen/Core>
 
#include <Calibration/StereoCalibration.h>
#include <Structs/ObjectDefinitions.h>
 
 
// forward declarations
class CByteImage;
class CTexturedRecognition;
 
namespace memoryx
{
    class MultivariateNormalDistribution;
    using MultivariateNormalDistributionPtr = IceInternal::Handle<MultivariateNormalDistribution>;
} // namespace memoryx
 
namespace visionx
{
 
    static Eigen::Vector3i
    extractColorValue(std::string propertyValue)
    {
        int number = (int)strtol(&propertyValue.c_str()[1], NULL, 16);
 
        int r = number >> 16;
        int g = number >> 8 & 0xFF;
        int b = number & 0xFF;
 
        return Eigen::Vector3i(r, g, b);
    }
 
    class ObjectLocalizerProcessorPropertyDefinitions :
        public visionx::ImageProcessorPropertyDefinitions
    {
    public:
        ObjectLocalizerProcessorPropertyDefinitions(std::string prefix) :
            visionx::ImageProcessorPropertyDefinitions(prefix)
        {
            defineOptionalProperty<std::string>(
                "PriorKnowledgeProxyName", "PriorKnowledge", "name of prior memory proxy");
            defineOptionalProperty<std::string>(
                "DataBaseObjectCollectionName",
                "memdb.Prior_Objects",
                "name of collection from database to use for object classes");
            defineOptionalProperty<std::string>(
                "ImageProviderName", "ImageProvider", "name of the image provider to use");
            defineRequiredProperty<std::string>(
                "AgentName", "Name of the agent for which the sensor values are provided");
            defineOptionalProperty<std::string>("CalibrationUpdateTopicName",
                                                "StereoCalibrationInterface",
                                                "Topic name of the stereo calibration provider");
            defineOptionalProperty<float>("2DLocalizationNoise",
                                          4.0,
                                          "2D localization noise of object recognition. Used in "
                                          "order to calculate the 3D world localization noise.");
            defineOptionalProperty<bool>("EnableResultImages",
                                         true,
                                         "Enable the calculation of resultimages which are then "
                                         "provided using an ImageProvider");
            defineOptionalProperty<bool>("useResultImageMask",
                                         false,
                                         "Use a color mask for the result images if set. Otherwise "
                                         "the camera images are used");
            armarx::PropertyDefinition<Eigen::Vector3i>::PropertyFactoryFunction f =
                &extractColorValue;
            defineOptionalProperty<Eigen::Vector3i>(
                "colorMask",
                extractColorValue("#FFFFFF"),
                "image color that should be used as alpha channel")
                .setFactory(f);
 
            defineOptionalProperty<std::string>("DebugObserverName",
                                                "DebugObserver",
                                                "Name of the topic the DebugObserver listens on");
        }
    };
 
    /**
     * ObjectLocalizerProcessor
     *
     */
    class ObjectLocalizerProcessor :
        virtual public ImageProcessor,
        virtual public ObjectLocalizerImageInterface
    {
        friend class ObjectLocalizerProcessorJob;
 
    public:
        ObjectLocalizerProcessor();
 
        /**
         * @see PropertyUser::createPropertyDefinitions()
         */
        armarx::PropertyDefinitionsPtr
        createPropertyDefinitions() override
        {
            return armarx::PropertyDefinitionsPtr(
                new ObjectLocalizerProcessorPropertyDefinitions(getConfigIdentifier()));
        }
 
        /**
         * Called from framework. Sets up the required proxies and properties from config file.
         * Calls the onInitObjectLocalizerProcessor() hook.
         *
         * @see Component::onInitComponent()
         */
        void onInitImageProcessor() override;
 
        /**
         * Called from framework. Establishes the database connection, retrieves image provider settings and stereo calibration, initializes visualization
         * if enabled and reads the object classes from the MemoryX database.
         *
         * Calls the onConnectObjectLocalizerProcessor() hook.
         *
         * @see Component::onConnectComponent()
         */
        void onConnectImageProcessor() override;
 
        /**
         * Called from framework.
         *
         * Calls the onDisconnectObjectLocalizerProcessor() hook.
         *
         * @see Component::onExitComponent()
         */
        void
        onDisconnectComponent() override
        {
            // Prevent deadlock: Cancel job before stopping the other threads
            job.exit();
            ImageProcessor::onDisconnectComponent();
            onDisconnectObjectLocalizerProcessor();
        }
 
        /**
         * Called from framework.
         *
         * Calls the onExitObjectLocalizerProcessor() hook.
         *
         * @see Component::onExitComponent()
         */
        void
        onExitImageProcessor() override
        {
            onExitObjectLocalizerProcessor();
        }
 
        /**
         * The process method is inherited from ImageProcessor. The process method is called by the framework
         * in a separate thread and. The loading of the database and the triggering of recognition is performed
         * in this thread.
         *
         * @see ImageProcessor::process()
         */
        void process() override;
 
        /**
         * The process method is inherited from the ObjectLocalizationProcessorInterface and is called by the WorkingMemoryUpdater.
         * It should be considered final. Implement ObjectLocalizerProcessor::localizeObjectClass(const std::string& objecClassName, CByteImage** cameraImages, CByteIamge** resultImages)
         * in your subclass instead.
         */
        memoryx::ObjectLocalizationResultList
        localizeObjectClasses(const memoryx::ObjectClassNameList& objectClassNames,
                              const Ice::Current& c = Ice::emptyCurrent) override;
 
    protected:
        /**
         * ObjectLocalizerProcessor interface: The localizeObjectClass method needs to be implemented by any ObjectLocalizer.
         * Based on the object class name and the camera images it generates a list of localization results which correspond
         * to found instances of objects belonging to that class.
         *
         * This method is called by an @ObjectLocalizerProcessorJob.
         *
         * @param objectClassNames names of the class to localize
         * @param cameraImages the two input images
         * @param resultImages the two result images. are provided if result images are enabled.
         *
         * @return list of object instances
         */
        virtual memoryx::ObjectLocalizationResultList
        localizeObjectClasses(const std::vector<std::string>& objectClassNames,
                              CByteImage** cameraImages,
                              armarx::MetaInfoSizeBasePtr imageMetaInfo,
                              CByteImage** resultImages) = 0;
 
        /**
         * ObjectLocalizerProcessor interface: The initRecognizer method needs to be implemented by any ObjectLocalizer.
         * the method is called in the same thread as addObjectClass and localizeObjectClass. Initialize your
         * recognition method here.
         *
         * @return success of adding this entity to the recognition method
         */
        virtual bool initRecognizer() = 0;
 
        /**
         * ObjectLocalizerProcessor interface: The addObjectClass method needs to be implemented by any ObjectLocalizer.
         * Add object class to localizer. Called by the ObjectLocalizerProcessor for each object class in prior knowledge, that
         * has the RecognitionMethod attribute corresponding to the default name of this component (see Component::getDefaultName())
         *
         * @param objectClassEntity entity containing all information available for the object class
         * @param fileManager GridFileManager required to read files associated to prior knowledgge from the database. Usually accessed via an AbstractEntityWrapper.
         *
         * @return success of adding this entity to the recognition method
         */
        virtual bool addObjectClass(const memoryx::EntityPtr& objectClassEntity,
                                    const memoryx::GridFileManagerPtr& fileManager) = 0;
 
        /**
         * ObjectLocalizerProcessor interface: subclass hook
         *
         * @see ObjectLocalizerProcessor::onInitImageProcessor()
         */
        virtual void
        onInitObjectLocalizerProcessor()
        {
        }
 
        /**
         * ObjectLocalizerProcessor interface: subclass hook
         *
         * @see ObjectLocalizerProcessor::onConnectImageProcessor()
         */
        virtual void
        onConnectObjectLocalizerProcessor()
        {
        }
 
        /**
         * ObjectLocalizerProcessor interface: subclass hook
         *
         * @see ObjectLocalizerProcessor::onDisconnectImageProcessor()
         */
        virtual void
        onDisconnectObjectLocalizerProcessor()
        {
        }
 
        /**
         * ObjectLocalizerProcessor interface: subclass hook
         *
         * @see ObjectLocalizerProcessor::onExitImageProcessor()
         */
        virtual void
        onExitObjectLocalizerProcessor()
        {
        }
 
        /**
         * Calculate 3D uncertainty from two 2d points in left and right camera. Uses the
         * property "2DLocalizationNoise" for estimating the 3D noise. Uses unscented
         * transform through epipolar geometry to derive an approximation
         * The reference frame is always the left camera.
         *
         * @param left_point point in left camera image
         * @param right_point point in right camera image
         * @return 3D localization uncertainty
         */
        memoryx::MultivariateNormalDistributionPtr
        calculateLocalizationUncertainty(Vec2d left_point, Vec2d right_point);
 
        /**
         * Calculate localization uncertainty for a 3d point. First projects the point
         * to the camera images and then applies calculateLocalizationUncertainty(Vec2d left_point, Vec2d right_point).
         * The reference frame is always the left camera.
         *
         * @param position 3d position of the point
         * @return 3D localization uncertainty
         */
        memoryx::MultivariateNormalDistributionPtr
        calculateLocalizationUncertainty(const Eigen::Vector3f& position);
 
        void
        reportStereoCalibrationChanged(const StereoCalibration& stereoCalibration,
                                       bool imagesAreUndistorted,
                                       const std::string& referenceFrame,
                                       const Ice::Current& c = Ice::emptyCurrent) override
        {
            this->stereoCalibration.reset(visionx::tools::convert(stereoCalibration));
            this->imagesAreUndistorted = imagesAreUndistorted;
            this->referenceFrameName = referenceFrame;
        }
 
        /**
         * Retrieve whether result images are enabled
         *
         * @return result images enables
         */
        bool
        getResultImagesEnabled() const
        {
            return resultImagesEnabled;
        }
 
        /**
         * Retrieve whether result images are enabled
         *
         * @return result images enables
         */
        std::string
        getReferenceFrameName() const
        {
            return referenceFrameName;
        }
 
        /**
         * Retrieve format of input images
         *
         * @return iamge format
         */
        ImageFormatInfo
        getImageFormat() const
        {
            return imageFormat;
        }
 
        /**
         * Retrieve whether images are undistorted
         *
         * @return whether images are undistorted
         */
        bool
        getImagesAreUndistorted() const
        {
            return imagesAreUndistorted;
        }
 
        bool
        isResultImageMaskEnabled() const
        {
            return useResultImageMask;
        }
 
        Eigen::Vector3i
        getColorMask() const
        {
            return colorMask;
        }
 
        /**
         * Retrieve stereo calibration corresponding to image provider
         *
         * @return stereo calibration
         */
        CStereoCalibration*
        getStereoCalibration() const
        {
            return stereoCalibration.get();
        }
 
    private:
        // read object classes from prior memory
        void initObjectClasses();
        // setup image objects for a specific size
        void setupImages(int width, int height);
 
 
        Eigen::Vector3i colorMask;
        bool useResultImageMask;
        bool resultImagesEnabled;
 
        // proxies
        ImageProviderInterfacePrx imageProviderPrx;
        memoryx::PriorKnowledgeInterfacePrx priorKnowledgePrx;
        memoryx::PersistentObjectClassSegmentBasePrx classesSegmentPrx;
        memoryx::CommonStorageInterfacePrx databasePrx;
 
        memoryx::GridFileManagerPtr fileManager;
 
 
        // stereo information
        std::unique_ptr<CStereoCalibration> stereoCalibration;
        bool imagesAreUndistorted;
        ImageFormatInfo imageFormat;
 
    protected:
        // settings
        std::string imageProviderName;
        std::string priorKnowledgeProxyName;
        std::string referenceFrameName;
 
        // images
        CByteImage cameraImagesData[2];
        CByteImage* cameraImages[2];
        boost::ptr_vector<boost::nullable<CByteImage>> resultImagesData;
        CByteImage** resultImages;
        armarx::MetaInfoSizeBasePtr imageMetaInfo;
        int numberOfResultImages;
 
 
    private:
        // noise
        Eigen::Vector2d imageNoiseLeft;
        Eigen::Vector2d imageNoiseRight;
 
        // threading
        ObjectLocalizerProcessorJob job;
 
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };
 
} // namespace visionx