ImagePassThrough.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     David Schiebener (schiebener at kit dot edu)
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ImagePassThrough.h"
 
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
#include <Eigen/Core>
 
#include <IceUtil/Time.h>
 
#include <opencv2/calib3d.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/highgui.hpp>
 
#include <SimoxUtility/algorithm/string.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/ice_conversions.h>
#include <ArmarXCore/interface/core/SharedMemory.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <VisionX/tools/ImageUtil.h>
#include <VisionX/tools/OpenCVUtil.h>
#include <VisionX/tools/TypeMapping.h>
 
#include <Calibration/Calibration.h>
#include <Calibration/StereoCalibration.h>
#include <Image/IplImageAdaptor.h>
 
namespace visionx
{
 
    ImagePassThroughPropertyDefinitions::ImagePassThroughPropertyDefinitions(std::string prefix) :
        ImageProcessorPropertyDefinitions(prefix)
    {
    }
 
    armarx::PropertyDefinitionsPtr
    ImagePassThrough::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr defs(
            new ImagePassThroughPropertyDefinitions(getConfigIdentifier()));
 
        defs->optional(
            p.imageProviderName, "img.ImageProviderName", "Name of the image provider to use.");
        defs->optional(
            p.referenceFrame, "img.ReferenceFrameName", "Name of the ReferenceFrameName");
        defs->optional(p.agentName, "img.AgentName", "Name of the agent");
        defs->optional(p.cameraIndex, "img.cameraIndex", "Camera index. Left = 0, Right = 1, ...");
 
        std::string pattern = "[ k3 [, k4, k5, k6 [, s1, s2, s3, s4 ] ] ]";
        // defs->optional(p.extraDistortionCoeffs, "cam.ExtraDistortionCoeffs",
        defs->defineOptionalProperty<std::vector<std::string>>(
                "cam.ExtraDistortionCoeffs",
                {},
                "Optional extra distortion coefficients (which cannot be retrieved from the image "
                "provider)."
                "\nThe expected parameters are: " +
                    pattern +
                    ""
                    "\nThat is, you must specifiy 1 (k3), 4 (k3-k6), or 8 (k3-k6, s1-s4) elements."
                    "\nNote: k1, k2, p1, p2 are retrieved from the image provider.")
            .map(pattern, {});
 
 
        // defs->defineOptionalProperty<bool>(
        //     "visu.Enabled", p.visuEnabled.load(), "If true, visualize marker poses.");
 
        return defs;
    }
 
    ImagePassThrough::ImagePassThrough()
    {
        // addPlugin(virtualRobotReaderPlugin);
    }
 
    std::string
    ImagePassThrough::GetDefaultName()
    {
        return "ImagePassThrough";
    }
 
    std::string
    ImagePassThrough::getDefaultName() const
    {
        return GetDefaultName();
    }
 
    void
    ImagePassThrough::onInitImageProcessor()
    {
 
        {
            std::string propName = "cam.ExtraDistortionCoeffs";
            std::vector<std::string> coeffsStr;
            // getProperty(coeffsStr, "cam.ExtraDistortionCoeffs");
 
            // p.extraDistortionCoeffs.clear();
            // for (const auto& coeffStr : coeffsStr)
            // {
            //     try
            //     {
            //         p.extraDistortionCoeffs.push_back(std::stof(coeffStr));
            //     }
            //     catch (const std::invalid_argument&)
            //     {
            //         ARMARX_WARNING << "Could not parse '" << coeffStr << "' as float in property "
            //                        << propName << "."
            //                        << "\nIgnoring extra parameters.";
            //         p.extraDistortionCoeffs.clear();
            //         break;
            //     }
            // }
        }
 
        ARMARX_INFO << "Will use image provider " << QUOTED(p.imageProviderName);
        usingProxy(p.imageProviderName);
    }
 
    void
    ImagePassThrough::onConnectImageProcessor()
    {
        // ARMARX_CHECK_NOT_NULL(virtualRobotReaderPlugin);
 
        // robot = virtualRobotReaderPlugin->get().getRobot(p.agentName);
        // ARMARX_CHECK_NOT_NULL(robot) << p.agentName;
 
        // referenceNode = robot->getRobotNode(p.referenceFrame);
        // ARMARX_CHECK_NOT_NULL(referenceNode) << p.referenceFrame;
 
        ARMARX_INFO << "Connecting to image provider '" << p.imageProviderName << "'...";
 
        visionx::ImageProviderInfo imageProviderInfo =
            ImageProcessor::getImageProvider(p.imageProviderName);
        StereoCalibrationInterfacePrx calibrationProvider =
            StereoCalibrationInterfacePrx::checkedCast(imageProviderInfo.proxy);
 
        std::unique_ptr<CStereoCalibration> stereoCalibration(
            visionx::tools::convert(calibrationProvider->getStereoCalibration()));
        // CCalibration::CCameraParameters ivtCameraParameters =
        //     stereoCalibration->GetLeftCalibration()->GetCameraParameters();
 
        // this->cameraMatrix = visionx::makeCameraMatrix(ivtCameraParameters);
        // ARMARX_VERBOSE << "Camera matrix: \n" << this->cameraMatrix;
 
        // {
        //     // "Vector of distortion coefficients (k1,k2,p1,p2 [,k3 [,k4,k5,k6], [s1,s2,s3,s4]]) of 4, 5, 8 or 12 elements"
        //     // Source: https://docs.opencv.org/3.2.0/d9/d6a/group__aruco.html#ga7da45d2e8139504f3a532d884b4fb4ac
 
        //     std::set<int> allowedSizes = {4, 5, 8, 12};
        //     ARMARX_VERBOSE << "Got " << p.extraDistortionCoeffs.size()
        //                    << " extra distortion coefficients.";
 
        //     int num = int(4 + p.extraDistortionCoeffs.size());
        //     ARMARX_CHECK_POSITIVE(allowedSizes.count(num))
        //         << "Allowed sizes: "
        //         << simox::alg::join(
        //                simox::alg::multi_to_string(allowedSizes.begin(), allowedSizes.end()), " ")
        //         << "\n num = " << num << " = 4 + " << p.extraDistortionCoeffs.size()
        //         << " = 4 + #extra coeffs";
 
        //     cv::Mat distortionParameters(num, 1, cv::DataType<float>::type);
 
        //     if (!calibrationProvider->getImagesAreUndistorted())
        //     {
        //         // k1, k2, p1, p2
        //         for (int i = 0; i < 4; ++i)
        //         {
        //             distortionParameters.at<float>(i, 0) = ivtCameraParameters.distortion[i];
        //         }
        //         // extra
        //         for (int i = 0; size_t(i) < p.extraDistortionCoeffs.size(); ++i)
        //         {
        //             distortionParameters.at<float>(4 + i, 0) =
        //                 p.extraDistortionCoeffs.at(size_t(i));
        //         }
        //     }
        //     else
        //     {
        //         for (int i = 0; i < distortionParameters.rows; ++i)
        //         {
        //             distortionParameters.at<float>(i, 0) = 0;
        //         }
        //     }
 
        //     this->distortionCoeffs = distortionParameters;
        //     ARMARX_CHECK_POSITIVE(allowedSizes.count(this->distortionCoeffs.rows));
        // }
        // ARMARX_VERBOSE << "Distortion coefficients: \n" << this->distortionCoeffs;
 
 
        cameraImages = new CByteImage*[2];
 
        cameraImages[0] = tools::createByteImage(imageProviderInfo);
        cameraImages[1] = tools::createByteImage(imageProviderInfo);
 
        // createRemoteGuiTab();
        // RemoteGui_startRunningTask();
 
        // set the image format and number of images for the ImageProvider interface
        ARMARX_INFO << "Number of images: " << imageProviderInfo.numberImages;
        setNumberImages(imageProviderInfo.numberImages);
        setImageFormat(imageProviderInfo.imageFormat.dimension,
                       imageProviderInfo.imageFormat.type,
                       imageProviderInfo.imageFormat.bpType);
    }
 
    void
    ImagePassThrough::process()
    {
        ARMARX_VERBOSE << "Waiting for images from provider '" << p.imageProviderName << "'...";
        if (ImageProcessor::waitForImages(500))
        {
            armarx::MetaInfoSizeBasePtr info;
            ImageProcessor::getImages(p.imageProviderName, cameraImages, info);
            m_timestamp_last_image =
                armarx::DateTime(armarx::Duration::MicroSeconds(info->timeProvided));
 
            // CByteImage* images[2] = {resultColorImage.get(), resultDepthImage.get()};
 
            ARMARX_VERBOSE << "Got images from provider '" << p.imageProviderName
                           << "'. Providing to working memory...";
 
            // provide the images through the shared memory
            ImageProvider::provideImages(cameraImages,
                                         IceUtil::Time::microSeconds(info->timeProvided));
        }
        else
        {
            ARMARX_VERBOSE << "Timeout or error in wait for images.";
            return;
        }
    }
 
    void
    ImagePassThrough::createRemoteGuiTab()
    {
        // using namespace armarx::RemoteGui::Client;
 
        // GridLayout grid;
        // int row = 0;
 
        // tab.markerSize.setRange(1.0, 1000.0);
        // tab.markerSize.setSteps(1000);
        // tab.markerSize.setDecimals(1);
        // tab.markerSize.setValue(p.markerSize);
 
        // tab.visuEnabled.setValue(p.visuEnabled);
 
        // grid.add(Label("Marker size:"), {row, 0}).add(tab.markerSize, {row, 1});
        // row++;
        // grid.add(Label("Enable visu:"), {row, 0}).add(tab.visuEnabled, {row, 1});
        // row++;
 
        // VBoxLayout root = {grid, VSpacer()};
        // RemoteGui_createTab(getName(), root, &tab);
    }
 
    void
    ImagePassThrough::RemoteGui_update()
    {
        // if (tab.markerSize.hasValueChanged())
        // {
        //     p.markerSize = tab.markerSize.getValue();
        // }
        // if (tab.visuEnabled.hasValueChanged())
        // {
        //     p.visuEnabled = tab.visuEnabled.getValue();
        // }
    }
 
    std::string
    ImagePassThrough::getReferenceFrame(const ::Ice::Current&)
    {
        return getStereoCalibrationProvider()->getReferenceFrame();
    }
 
    visionx::StereoCalibration
    ImagePassThrough::getStereoCalibration(const ::Ice::Current&)
    {
        return getStereoCalibrationProvider()->getStereoCalibration();
    }
 
    bool
    ImagePassThrough::getImagesAreUndistorted(const ::Ice::Current&)
    {
        return getStereoCalibrationProvider()->getImagesAreUndistorted();
    }
 
    StereoCalibrationInterfacePrx
    ImagePassThrough::getStereoCalibrationProvider()
    {
        visionx::ImageProviderInfo imageProviderInfo =
            ImageProcessor::getImageProvider(p.imageProviderName);
        auto calibProvider = StereoCalibrationInterfacePrx::checkedCast(imageProviderInfo.proxy);
 
 
        ARMARX_CHECK(calibProvider) << "Image provider '" << p.imageProviderName
                                    << "' does not provide stereo calibration.";
        return calibProvider;
    }
 
    void
    ImagePassThrough::startCapture(::Ice::Float framesPerSecond, const ::Ice::Current& current)
    {
    }
 
    void
    ImagePassThrough::stopCapture(const ::Ice::Current& current)
    {
    }
 
    ARMARX_REGISTER_COMPONENT_EXECUTABLE(::visionx::ImagePassThrough, ::visionx::ImagePassThrough::GetDefaultName());
} // namespace visionx