CoFusionProcessor.h

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::ArmarXObjects::CoFusionProcessor
 * @author     Rainer Kartmann ( rainer dot kartmann at student dot kit dot edu )
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#ifndef _ARMARX_COMPONENT_VisionX_CoFusionProcessor_H
#define _ARMARX_COMPONENT_VisionX_CoFusionProcessor_H
 
 
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
 
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
 
#include <VisionX/components/pointcloud_core/CapturingPointCloudProvider.h>
#include <VisionX/core/ImageProcessor.h>
#include <VisionX/interface/components/CoFusionProcessor.h>
 
// from CoFusion
#include <RobotAPI/interface/core/RobotState.h>
 
#include "CoFusionParams.h"
#include "Stopwatch.h"
#include <Core/Callbacks.h>
#include <Core/PoseMatch.h>
#include <Core/Utils/Intrinsics.h>
 
class Model;
 
namespace armarx
{
    /**
     * @class CoFusionProcessorPropertyDefinitions
     * @brief
     */
    class CoFusionProcessorPropertyDefinitions : public visionx::ImageProcessorPropertyDefinitions
    {
    public:
        CoFusionProcessorPropertyDefinitions(std::string prefix) :
            visionx::ImageProcessorPropertyDefinitions(prefix)
        {
            //defineRequiredProperty<std::string>("PropertyName", "Description");
            //defineOptionalProperty<std::string>("PropertyName", "DefaultValue", "Description");
 
            defineOptionalProperty<std::string>("RobotStateComponentName",
                                                "RobotStateComponent",
                                                "Name of the RobotState of the Robot");
 
            // Point Cloud Provider properties
            defineOptionalProperty<float>("framerate", 30.0f, "framerate for the point clouds")
                .setMin(0.0f)
                .setMax(60.0f);
            defineOptionalProperty<bool>(
                "isEnabled", true, "enable the capturing process immediately");
 
            defineOptionalProperty<std::string>(
                "ImageProviderName", "ImageProvider", "Name of the input image provider.");
 
 
            defineOptionalProperty<int>(
                "FramesBetweenUpdates", 1, "Number of frames between two point cloud updates.");
 
            defineOptionalProperty<visionx::ImageSyncMode>(
                "PointCloudSyncMode",
                visionx::eCaptureSynchronization,
                "The point cloud sync mode ('capture' (default) or 'fps').")
                .map("capture", visionx::eCaptureSynchronization)
                .map("fps", visionx::eFpsSynchronization);
 
            defineOptionalProperty<float>(
                "DownsamplingLeafSize",
                5.0f,
                "If > 0, the result point cloud will be downsampled with the given leaf size.");
 
            defineOptionalProperty<bool>(
                "ShowTimeStats",
                false,
                "If true, an execution time report is logged periodically.");
 
            defineOptionalProperty<std::string>(
                "ReferenceFrameName", "DepthCamera", "reference frame name.");
 
            // COFUSION PARAMS
            defineOptionalProperty<int>("cofusion.TimeDelta", 200, "The CoFusion time delta.");
            defineOptionalProperty<int>(
                "cofusion.CountThresh", 35000, "The CoFusion count thresh.");
            defineOptionalProperty<float>("cofusion.ErrThresh", 5e-05, "The CoFusion err thresh.");
            defineOptionalProperty<float>("cofusion.CovThresh", 1e-05, "The CoFusion cov thresh.");
            defineOptionalProperty<bool>("cofusion.CloseLoops", false, "The CoFusion close loops.");
            defineOptionalProperty<bool>("cofusion.Iclnuim", false, "The CoFusion iclnuim.");
            defineOptionalProperty<bool>("cofusion.Reloc", false, "The CoFusion reloc.");
            defineOptionalProperty<float>(
                "cofusion.PhotoThresh", 115, "The CoFusion photo thresh.");
            defineOptionalProperty<float>(
                "cofusion.InitConfidenceGlobal", 4, "The CoFusion init confidence global.");
            defineOptionalProperty<float>(
                "cofusion.InitConfidenceObject", 2, "The CoFusion init confidence object.");
            defineOptionalProperty<float>("cofusion.DepthCut", 3, "The CoFusion depth cut.");
            defineOptionalProperty<float>("cofusion.IcpThresh", 10, "The CoFusion icp thresh.");
            defineOptionalProperty<bool>("cofusion.FastOdom", false, "The CoFusion fast odom.");
            defineOptionalProperty<float>(
                "cofusion.FernThresh", 0.3095, "The CoFusion fern thresh.");
            defineOptionalProperty<bool>("cofusion.So3", true, "The CoFusion so3.");
            defineOptionalProperty<bool>(
                "cofusion.FrameToFrameRGB", false, "The CoFusion frame to frame r g b.");
            defineOptionalProperty<unsigned>(
                "cofusion.ModelSpawnOffset", 20, "The CoFusion model spawn offset.");
            defineOptionalProperty<Model::MatchingType>(
                "cofusion.MatchingType", Model::MatchingType::Drost, "The CoFusion matching type.");
            defineOptionalProperty<std::string>(
                "cofusion.ExportDirectory", "", "The CoFusion export directory.");
            defineOptionalProperty<bool>("cofusion.ExportSegmentationResults",
                                         false,
                                         "The CoFusion export segmentation results.");
 
#if USE_MASKFUSION == 1
            defineOptionalProperty<bool>("cofusion.UsePrecomputedMasksOnly", false, "");
            defineOptionalProperty<int>(
                "cofusion.FrameQueueSize", 5, "Set size of frame-queue manually");
            defineOptionalProperty<int>(
                "cofusion.DeviceMaskRCNN", -1, "Select GPU (device ID) which is running MaskRCNN.");
 
            defineOptionalProperty<int>("cofusion.PreallocatedModelsCount",
                                        1,
                                        " Preallocate memory for a number of models, which can "
                                        "increase performance (default: 0)");
#endif
        }
    };
 
    /**
     * @defgroup Component-CoFusionProcessor CoFusionProcessor
     * @ingroup VisionX-Components
     * A description of the component CoFusionProcessor.
     *
     * @class CoFusionProcessor
     * @ingroup Component-CoFusionProcessor
     * @brief Brief description of class CoFusionProcessor.
     *
     * Detailed description of class CoFusionProcessor.
     */
    class CoFusionProcessor :
        virtual public visionx::ImageProcessor,
        virtual public visionx::CapturingPointCloudProvider,
        virtual public visionx::CoFusionProcessorInterface
    {
    public:
        /**
         * @see armarx::ManagedIceObject::getDefaultName()
         */
        std::string
        getDefaultName() const override
        {
            return "CoFusionProcessor";
        }
 
        // CapturingPointCloudProviderInterface interface
        virtual void startCaptureForNumFrames(Ice::Int numFrames,
                                              const Ice::Current& c = Ice::emptyCurrent) override;
 
    protected:
        // ImageProcessor interface
        virtual void onInitImageProcessor() override;
        /// Allocates imageRgb and imageDepth and sets images[].
        virtual void onConnectImageProcessor() override;
        /// Frees imageRgb and imageDepth.
        virtual void onDisconnectImageProcessor() override;
        virtual void onExitImageProcessor() override;
 
        /**
         * @brief Performs the image processing.
         *
         * Processes input images, constructs FrameData and calls CoFusion::processFrame().
         *
         * This is the (only) thread that directly accesses cofusion.
         * If cofusion is not allocated, it is initialized via initializeCoFusion().
         *
         * Every <framesBetweenUpdates> frames, updateSurfelMaps() is called.
         */
        virtual void process() override;
 
 
        // CapturingPointCloudProvider interface
        virtual void onInitCapturingPointCloudProvider() override;
        virtual void onExitCapturingPointCloudProvider() override;
        virtual void onStartCapture(float framesPerSecond) override;
        virtual void onStopCapture() override;
 
        virtual bool doCapture() override;
 
        virtual visionx::MetaPointCloudFormatPtr getDefaultPointCloudFormat() override;
 
 
        // ManagedIceObject interface (multiple inheritance)
        virtual void onInitComponent() override;
        virtual void onConnectComponent() override;
        virtual void onDisconnectComponent() override;
        virtual void onExitComponent() override;
 
 
        void onNewModel(std::shared_ptr<Model> model);
        void onInactiveModel(std::shared_ptr<Model> model);
 
 
        /**
         * @see PropertyUser::createPropertyDefinitions()
         */
        armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
 
    private:
        using OutPointT = pcl::PointXYZRGBL;
 
        /**
         * @brief Initializes CoFusion.
         * Initializes Pangolin, glew and creates a gl program.
         * Allocates and sets cofusion.
         */
        void initializeCoFusion();
 
        /**
         * @brief Constructs frame data from current imageRgb and imageDepth.
         * @return the frame data
         */
        FrameData constructFrameData();
 
        /** @brief Updates surfelMaps with current CoFusion models. */
        void updateSurfelMaps();
 
        /**
         * \brief Constructs result point cloud from current surfelMaps.
         * Allocates surfelMapMutex internally.
         */
        void reconstructResultPointCloud();
 
        /// Called periodically by stopwatchReportTask.
        void reportStopwatchStats();
 
 
        std::string imageProviderName;
        visionx::ImageProviderInterfacePrx imageProvider;
 
 
        // == CoFusion ==
        CoFusionParams cofusionParams;
 
#if USE_MASKFUSION == 1
        std::unique_ptr<MaskFusion> cofusion;
#else
        std::unique_ptr<CoFusion> cofusion;
#endif
 
 
        // == input image processing ==
        /// to be acquired when accessing images
        std::mutex imagesMutex;
 
        /// input RGB image
        visionx::CByteImageUPtr imageRgb;
        /// input depth image
        visionx::CByteImageUPtr imageDepth;
        /// [ imageRgb, imageDepth ]
        CByteImage* images[2];
 
 
        /// Number of frames from one point cloud update to the next.
        std::size_t framesBetweenUpdates;
        /// Current number of frames since the latest point cloud update.
        std::size_t framesSinceLastUpdate = 0;
 
 
        // == output surfel maps for point cloud construction ==
        /// to be acquired when accessing surfelMaps
        std::mutex surfelMapMutex;
 
        /// The SurfelMaps of CoFusion models (up to framesBetweenUpdates frames old)
        std::map<unsigned int, Model::SurfelMap> surfelMaps;
        /// If not empty, contains the output point cloud constructed from the surfel maps.
        pcl::PointCloud<OutPointT>::Ptr resultPointCloud;
 
        /// If > 0, this will be used for downsampling.
        float downsamplingLeafSize = 0.f;
 
        /// Used for time stats
        stopwatch::Stopwatch stopwatch;
 
        PeriodicTask<CoFusionProcessor>::pointer_type stopwatchReportTask;
 
        RobotStateComponentInterfacePrx robotStateComponent;
 
        std::string referenceFrameName;
 
        std::atomic<bool> newFrameAvailable = false;
    };
} // namespace armarx
 
#endif