PrimitiveExtractor.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
 * @author     Eren Aksoy ( eren dot aksoy at kit dot edu )
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <stdarg.h>
 
#include <cmath>
#include <functional>
#include <limits>
#include <map>
#include <mutex>
#include <tuple>
 
#include <Eigen/Core>
 
#include <pcl/common/angles.h>
#include <pcl/common/geometry.h>
#include <pcl/common/intersections.h>
#include <pcl/features/moment_of_inertia_estimation.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/kdtree/kdtree_flann.h>
 
#include <IceUtil/IceUtil.h>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
 
#include <ArmarXCore/core/Component.h>
 
#include <RobotAPI/interface/core/PoseBase.h>
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <VisionX/components/pointcloud_core/PointCloudProcessor.h>
#include <VisionX/components/pointcloud_core/PointCloudProvider.h>
#include <VisionX/interface/components/PointCloudSegmenter.h>
#include <VisionX/interface/components/PrimitiveExtractor.h>
#include <VisionX/interface/components/PrimitiveMapper.h>
 
#include "PrimitiveFilter.h"
#include "PrimitiveFusion.h"
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/interface/components/WorkingMemoryInterface.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/libraries/memorytypes/segment/EnvironmentalPrimitiveSegment.h>
 
namespace pcl::console
{
    // Replace PCL's print function in order to pass the messages to the ArmarX logging system.
    // TODO: This should be done automatically if PCL is linked into a component
    void
    print(pcl::console::VERBOSITY_LEVEL level, const char* format, ...)
    {
        std::string message;
 
        va_list args, args_copy;
        va_start(args, format);
        va_copy(args_copy, args);
 
        int size = std::vsnprintf(nullptr, 0, format, args) + 1;
        message = std::string(size, ' ');
        std::vsnprintf(&message.front(), size, format, args_copy);
 
        va_end(args_copy);
        va_end(args);
 
        simox::alg::trim(message);
 
        switch (level)
        {
            case L_ALWAYS:
            case L_ERROR:
            case L_WARN:
                ARMARX_INFO_S << message;
                break;
 
            case L_INFO:
            case L_DEBUG:
            case L_VERBOSE:
                // Ignore
                break;
        }
    }
} // namespace pcl::console
 
namespace AffordanceKit
{
    class PrimitiveExtractor;
    using PrimitiveExtractorPtr = std::shared_ptr<PrimitiveExtractor>;
} // namespace AffordanceKit
 
namespace armarx
{
    /**
     * @class PrimitiveExtractorPropertyDefinitions
     * @brief
     */
    class PrimitiveExtractorPropertyDefinitions : public armarx::ComponentPropertyDefinitions
    {
    public:
        PrimitiveExtractorPropertyDefinitions(std::string prefix) :
            armarx::ComponentPropertyDefinitions(prefix)
        {
            defineOptionalProperty<std::string>("segmenterName",
                                                "PointCloudSegmenterResult",
                                                "name of the segmented point cloud data processor");
 
            // required primitive parameters
            defineOptionalProperty<double>(
                "minimumSegmentSize",
                100,
                "Minimum segment size to be considered in the primitive extraction");
            defineOptionalProperty<double>(
                "maximumSegmentSize",
                75000,
                "Maximum segment size to be considered in the primitive extraction");
            defineOptionalProperty<double>(
                "planeMaximumIteration", 100, "Maximum ransac iteration for plane estimation");
            defineOptionalProperty<float>(
                "planeDistanceThreshold", 10.0, "Distance threshold for plane estimation");
            defineOptionalProperty<float>(
                "planeNormalDistance", 0.10, "Normal distance for plane estimation");
            defineOptionalProperty<double>("cylinderMaximumIteration",
                                           100,
                                           "Maximum ransac iteration for cylinder estimation");
            defineOptionalProperty<float>(
                "cylinderDistanceThreshold", 0.1, "Distance threshold for cylinder estimation");
            defineOptionalProperty<float>(
                "cylinderRadiousLimit", 0.1, "Radious limit for cylinder estimation");
            defineOptionalProperty<double>(
                "sphereMaximumIteration", 100, "Maximum ransac iteration for sphere estimation");
            defineOptionalProperty<float>(
                "sphereDistanceThreshold", 0.9, "Distance threshold for sphere estimation");
            defineOptionalProperty<float>(
                "sphereNormalDistance", 0.1, "Normal distance for sphere estimation");
            defineOptionalProperty<bool>(
                "vebose", false, "Required for debugging the primitive engine");
            defineOptionalProperty<float>("euclideanClusteringTolerance",
                                          50.0,
                                          "Euclidean distance threshold for fine clustering");
            defineOptionalProperty<float>(
                "outlierThreshold", 50.0, "Outlier threshold for computing model outliers");
            defineOptionalProperty<float>(
                "circularDistanceThreshold", 0.01, "Distance threshold for circle estimation");
            defineOptionalProperty<bool>(
                "enablePrimitiveFusion", false, "Fuse new primitives with existing ones");
            defineOptionalProperty<bool>("UseAffordanceExtractionLibExport",
                                         false,
                                         "Use export functionality of affordance extraction lib");
            defineOptionalProperty<float>(
                "SpatialSamplingDistance", 50, "Distance between two spatial samplings in mm");
            defineOptionalProperty<int>(
                "NumOrientationalSamplings", 4, "Number of orientational samplings");
            defineOptionalProperty<bool>(
                "EnableBoxPrimitives", false, "Toggle detection of box primitives");
 
            defineOptionalProperty<std::string>(
                "sourceFrameName",
                "DepthCamera",
                "the robot node to use as source coordinate system for the captured point clouds");
            defineOptionalProperty<std::string>("SegmentedPointCloudTopicName",
                                                "SegmentedPointCloud",
                                                "name of SegmentedPointCloud topic");
            defineOptionalProperty<std::string>(
                "RobotStateComponentName",
                "RobotStateComponent",
                "Name of the robot state component that should be used");
 
            defineOptionalProperty<std::string>(
                "PriorKnowledgeProxyName", "PriorKnowledge", "name of prior memory proxy");
            defineOptionalProperty<std::string>(
                "WorkingMemoryName", "WorkingMemory", "name of WorkingMemory component");
            defineOptionalProperty<std::string>(
                "DebugDrawerTopicName", "DebugDrawerUpdates", "name of DebugDrawer topic");
            defineOptionalProperty<std::string>("DebugObserverName",
                                                "DebugObserver",
                                                "Name of the topic the DebugObserver listens on");
        }
    };
 
    /**
     * @class PrimitiveExtractor
     *
     * @ingroup VisionX-Components
     * @brief A brief description
     *
     *
     * Detailed Description
     */
    class PrimitiveExtractor :
        virtual public visionx::PrimitiveMapperInterface,
        virtual public visionx::PointCloudProcessor
    {
    public:
        /**
         * @see armarx::ManagedIceObject::getDefaultName()
         */
        std::string
        getDefaultName() const override
        {
            return "PrimitiveExtractor";
        }
 
    protected:
        /**
         * @see visionx::PointCloudProcessor::onInitPointCloudProcessor()
         */
        void onInitPointCloudProcessor() override;
 
        /**
         * @see visionx::PointCloudProcessor::onConnectPointCloudProcessor()
         */
        void onConnectPointCloudProcessor() override;
 
        /**
         * @see visionx::PointCloudProcessor::onDisconnectPointCloudProcessor()
         */
        void onDisconnectPointCloudProcessor() override;
 
        /**
         * @see visionx::PointCloudProcessor::onExitPointCloudProcessor()
         */
        void onExitPointCloudProcessor() override;
 
        /**
         * @see visionx::PointCloudProcessor::process()
         */
        void process() override;
 
        void enablePipelineStep(const Ice::Current& c = Ice::emptyCurrent) override;
        void disablePipelineStep(const Ice::Current& c = Ice::emptyCurrent) override;
        bool isPipelineStepEnabled(const Ice::Current& c = Ice::emptyCurrent) override;
        armarx::TimestampBasePtr
        getLastProcessedTimestamp(const Ice::Current& c = Ice::emptyCurrent) override;
 
        void setParameters(const visionx::PrimitiveExtractorParameters& parameters,
                           const Ice::Current& c = Ice::emptyCurrent) override;
        void loadParametersFromFile(const std::string& filename,
                                    const Ice::Current& c = Ice::emptyCurrent) override;
 
        visionx::PrimitiveExtractorParameters
        getParameters(const Ice::Current& c = Ice::emptyCurrent) override;
 
        /**
         * @see PropertyUser::createPropertyDefinitions()
         */
        armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
 
    private:
        void fusePrimitives(std::vector<memoryx::EntityBasePtr>& newPrimitives, long timestamp);
 
        void extractPrimitives();
        float getGraspPointInlierRatio(memoryx::EnvironmentalPrimitiveBasePtr leftPrimitive,
                                       memoryx::EnvironmentalPrimitiveBasePtr rightPrimitive);
 
        void pushPrimitivesToMemory(IceUtil::Int64 originalTimestamp);
        void pushPrimitivesToMemoryAffordanceKit(IceUtil::Int64 originalTimestamp);
 
    private:
        AffordanceKit::PrimitiveExtractorPtr primitiveExtractor;
 
        bool isConnectedtoExtractor;
        double minSegmentSize;
        double maxSegmentSize;
        double pMaxIter;
        float pDistThres;
        float pNorDist;
        double cMaxIter;
        float cDistThres;
        float cRadLim;
        double sMaxIter;
        float sDistThres;
        float sNorDist;
        bool verbose;
        float euclideanClusteringTolerance;
        float outlierThreshold;
        float circleDistThres;
 
        // init point clouds
        pcl::PointCloud<pcl::PointXYZL>::Ptr labeledCloudPtr;
        pcl::PointCloud<pcl::PointXYZL>::Ptr inliersCloudPtr;
        pcl::PointCloud<pcl::PointXYZL>::Ptr graspCloudPtr;
 
        std::mutex mutexPushing;
        std::mutex pointCloudMutex;
        std::mutex enableMutex;
 
        std::mutex timestampMutex;
        long lastProcessedTimestamp;
 
        bool isConnectedtoSegmenter;
 
        std::string providerName;
 
        memoryx::WorkingMemoryInterfacePrx workingMemory;
        memoryx::EnvironmentalPrimitiveSegmentBasePrx environmentalPrimitiveSegment;
        visionx::PointCloudSegmentationListenerPrx pointCloudSegmentationPrx;
 
        std::string sourceFrameName;
        std::string agentName;
 
        DebugDrawerInterfacePrx debugDrawerTopicPrx;
 
        int lastUsedLabel;
 
        bool mappingEnabled;
        bool enablePrimitiveFusion;
 
        armarx::RobotStateComponentInterfacePrx robotStateComponent;
 
        PrimitiveFusion fusionStrategy;
        PrimitiveFilter primitiveFilter;
 
        size_t numViews;
 
        float spatialSamplingDistance;
        int numOrientationalSamplings;
 
        DebugObserverInterfacePrx debugObserver;
    };
} // namespace armarx