OpticalFlow.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::OpticalFlow
 * @author     David Sippel ( uddoe at student dot kit dot edu )
 * @date       2016
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
 
#include <ArmarXCore/core/Component.h>
 
#include <ArmarXCore/observers/DebugObserver.h>
 
#include <VisionX/core/ImageProcessor.h>
#include <VisionX/tools/ImageUtil.h>
#include <VisionX/tools/FPSCounter.h>
 
#include <VisionX/interface/components/Calibration.h>
#include <VisionX/interface/components/OpticalFlowInterface.h>
 
#include <opencv2/opencv.hpp>
 
#include <Image/IplImageAdaptor.h>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <mutex>
#include <queue>
 
 
namespace armarx
{
 
    enum OPTICAL_FLOW_METHOD
    {
        Chessboard, Dense, Feature
    };
 
    /**
     * @class OpticalFlowPropertyDefinitions
     * @brief
     */
    class OpticalFlowPropertyDefinitions:
        public visionx::ImageProcessorPropertyDefinitions
    {
    public:
        OpticalFlowPropertyDefinitions(std::string prefix):
            visionx::ImageProcessorPropertyDefinitions(prefix)
        {
            //defineRequiredProperty<std::string>("PropertyName", "Description");
            //defineOptionalProperty<std::string>("PropertyName", "DefaultValue", "Description");
            defineOptionalProperty<float>("Framerate", 0.0, "the framerate");
            defineOptionalProperty<std::string>("providerName", "Armar3ImageProvider", "ImageProvider name");
            defineOptionalProperty<OPTICAL_FLOW_METHOD>("Method",  Chessboard, "use chessboard for feature tracking")
            .map("Chessboard", Chessboard)
            .map("Dense", Dense)
            .map("Feature", Feature);
 
            defineOptionalProperty<int>("Chessboard.Width", 7, "Chessboard width");
            defineOptionalProperty<int>("Chessboard.Height", 5, "Chessboard height");
            defineOptionalProperty<std::string>("OpticalFlowTopicName", "OpticalFlowTopic", "OpticalFlowTopicName name");
            defineOptionalProperty<std::string>("DebugObserverName", "DebugObserver", "Name of the topic the DebugObserver listens on");
            defineOptionalProperty<std::string>("RobotNodeSetName", "Robot", "Name of the RobotNodeSet");
 
            defineOptionalProperty<std::string>("CalibrationUpdateTopicName", "StereoCalibrationInterface", "Topic name of the stereo calibration provider");
        }
    };
 
    /**
     * @defgroup Component-OpticalFlow OpticalFlow
     * @ingroup VisionX-Components
     * A description of the component OpticalFlow.
     *
     * @class OpticalFlow
     * @ingroup Component-OpticalFlow
     * @brief Brief description of class OpticalFlow.
     *
     * Detailed description of class OpticalFlow.
     */
    class OpticalFlow :
        virtual public visionx::ImageProcessor,
        virtual public OpticalFlowInterface
    {
    public:
        /**
         * @see armarx::ManagedIceObject::getDefaultName()
         */
        std::string getDefaultName() const override
        {
            return "OpticalFlow";
        }
 
        void reportStereoCalibrationChanged(const visionx::StereoCalibration& stereoCalibration, bool, const std::string&, const Ice::Current& c = Ice::emptyCurrent) override
        {
 
            std::unique_lock lock(imageMutex);
 
            float f_x = stereoCalibration.calibrationRight.cameraParam.focalLength[0];
            float f_y = stereoCalibration.calibrationRight.cameraParam.focalLength[1];
 
            unitFactorX = 2.0 * std::atan(imageDimension.width / (2.0 * f_x)) / imageDimension.width;
            unitFactorY = 2.0 * std::atan(imageDimension.height / (2.0 * f_y)) / imageDimension.height;
 
            previousTime = 0;
        }
 
 
    protected:
        /**
         * @see PropertyUser::createPropertyDefinitions()
         */
        armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
 
        // ImageProcessor interface
    protected:
        void onInitImageProcessor() override;
        void onConnectImageProcessor() override;
        void onExitImageProcessor() override;
        void process() override;
 
    private:
 
        std::vector<float> removeHighestAndLowestMember(std::vector<float>& input);
        void drawDenseFlowMap(const cv::Mat& flow, cv::Mat& cflowmap, int step, const cv::Scalar& color);
        void computeAverageDenseFlow(const cv::Mat& flow, double& mean, double& rmse);
        void ComputeOpticalFlow(cv::Mat resultImage, cv::Mat grayImage);
 
        CByteImage** cameraImages;
        std::string providerName;
        std::mutex imageMutex;
        cv::Mat previousImage, old2;
        std::vector< cv::Point2f > previousFeatures;
        visionx::ImageProviderInterfacePrx imageProviderPrx;
        OpticalFlowListenerPrx prx;
        visionx::ImageDimension imageDimension;
 
        float resultX, resultY, timeDiff;
 
        Ice::Long previousTime;
 
        int chessboardWidth, chessboardHeight;
 
        visionx::FPSCounter fpsCounter;
 
 
        DebugObserverInterfacePrx debugObserver;
 
        float frameRate;
        float unitFactorX, unitFactorY;
 
        OPTICAL_FLOW_METHOD method;
    };
}