ImageSourceSelection.cpp

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::ImageSourceSelection
 * @author     Markus Grotz ( markus dot grotz at kit dot edu )
 * @date       2016
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ImageSourceSelection.h"
 
#include <ArmarXCore/core/ArmarXManager.h>
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include <VisionX/tools/ImageUtil.h>
#include <VisionX/tools/TypeMapping.h>
 
#include <Image/ImageProcessor.h>
 
namespace armarx
{
 
    void
    ImageSourceSelection::onInitImageProcessor()
    {
        defaultProvider = getProperty<std::string>("defaultProviderName").getValue();
        usingImageProvider(defaultProvider);
 
        usingTopic(getName());
    }
 
    void
    ImageSourceSelection::onConnectImageProcessor()
    {
        imageDisplayType = visionx::tools::typeNameToImageType("rgb");
 
        imageProviderInfo = getImageProvider(defaultProvider, imageDisplayType);
        numImages = getProperty<int>("NumberOfImages").getValue() <= 0
                        ? imageProviderInfo.numberImages
                        : getProperty<int>("NumberOfImages").getValue();
        ARMARX_INFO << defaultProvider << " " << imageProviderInfo.imageFormat.dimension.width
                    << "x" << imageProviderInfo.imageFormat.dimension.height;
 
        cameraImages[defaultProvider].resize(imageProviderInfo.numberImages);
        for (int i = 0; i < imageProviderInfo.numberImages; i++)
        {
            cameraImages[defaultProvider].at(i).reset(
                visionx::tools::createByteImage(imageProviderInfo));
        }
 
 
        visionx::ImageDimension targetDimension =
            getProperty<visionx::ImageDimension>("TargetDimension").getValue();
        visionx::ImageFormatInfo info;
        info.bytesPerPixel = 3;
        info.dimension = targetDimension;
        info.type = imageDisplayType;
        for (int i = 0; i < numImages; ++i)
        {
            resultCameraImages.push_back(
                visionx::CByteImageUPtr(visionx::tools::createByteImage(info, imageDisplayType)));
        }
 
        if (numImages == 2)
        {
            enableStereoResultImages(targetDimension, imageDisplayType);
        }
        else
        {
            enableResultImages(numImages,
                               targetDimension,
                               imageDisplayType,
                               getProperty<std::string>("resultProviderName"));
        }
 
        setImageSource(defaultProvider);
    }
 
    void
    ImageSourceSelection::onExitImageProcessor()
    {
        //    for (int i = 0; i < numImages; i++)
        //    {
        //        delete cameraImages[i];
        //    }
 
        //    delete [] cameraImages;
 
        //    if (scaleFactorX > 0.0)
        //    {
        //        for (int i = 0; i < numImages; i++)
        //        {
        //            delete scaledCameraImages[i];
        //        }
        //    }
        //    delete [] scaledCameraImages;
    }
 
    void
    ImageSourceSelection::deleteTimedOutProviders()
    {
        std::unique_lock lock(imageSourceMutex);
 
        std::map<std::string, int> deletionMap;
        for (auto it = providerTimeouts.begin(); it != providerTimeouts.end(); it++)
        {
            deletionMap[it->first] = 0;
        }
        // delete providers that are timed out
        for (auto it = providerTimeouts.begin(); it != providerTimeouts.end(); it++)
        {
            if (TimeUtil::GetTime() > it->second)
            {
 
                if (it->first != defaultProvider)
                {
                    deletionMap[it->first]++;
                }
                it = providerTimeouts.erase(it);
            }
            else
            {
                deletionMap[it->first]--;
            }
        }
        for (auto& pair : deletionMap)
        {
            if (pair.second > 0)
            {
                ARMARX_INFO << "Deleting image source " << pair.first;
                std::unique_lock lock(mutex);
                releaseImageProvider(pair.first);
            }
        }
    }
 
    void
    ImageSourceSelection::process()
    {
 
        deleteTimedOutProviders();
 
        auto activeProvider = getCurrentImageSource();
        std::unique_lock lock(mutex);
        if (!usedImageProviders.count(activeProvider))
        {
            setImageSource(activeProvider);
        }
        if (!waitForImages(activeProvider))
        {
            ARMARX_INFO << deactivateSpam(30, activeProvider)
                        << "Timeout while waiting for images from " << activeProvider;
            return;
        }
        armarx::MetaInfoSizeBasePtr info;
        ARMARX_DEBUG << deactivateSpam(5, activeProvider) << "Getting image from " << activeProvider
                     << " width: " << cameraImages.at(activeProvider).at(0)->width;
        if (getImages(activeProvider, cameraImages.at(activeProvider), info) <
            imageProviderInfoMap.at(activeProvider).numberImages)
        {
            ARMARX_WARNING << "Unable to transfer or read images";
            return;
        }
 
        auto currentNumberOfImages =
            std::min(numImages, imageProviderInfoMap.at(activeProvider).numberImages);
        ARMARX_DEBUG << deactivateSpam(5, activeProvider) << VAROUT(currentNumberOfImages);
 
        visionx::ImageDimension targetDimension =
            getProperty<visionx::ImageDimension>("TargetDimension").getValue();
        auto providerDimension = imageProviderInfoMap.at(activeProvider).imageFormat.dimension;
        if (targetDimension != providerDimension &&
            imageProviderInfo.imageFormat.type != imageDisplayType)
        {
            info->capacity = info->size = numImages * resultCameraImages[0]->bytesPerPixel *
                                          resultCameraImages[0]->width *
                                          resultCameraImages[0]->height;
            ARMARX_DEBUG << deactivateSpam(30, activeProvider) << "Resizing and converting"
                         << activeProvider;
            for (int i = 0; i < currentNumberOfImages; i++)
            {
                auto otherTypeResizedImage = visionx::CByteImageUPtr(
                    visionx::tools::createByteImage(imageProviderInfoMap.at(activeProvider)));
                ::ImageProcessor::Resize(cameraImages.at(activeProvider).at(i).get(),
                                         otherTypeResizedImage.get());
                ::ImageProcessor::ConvertImage(otherTypeResizedImage.get(),
                                               resultCameraImages.at(i).get());
            }
        }
        else if (targetDimension != providerDimension)
        {
            info->capacity = info->size = numImages * resultCameraImages[0]->bytesPerPixel *
                                          resultCameraImages[0]->width *
                                          resultCameraImages[0]->height;
            for (int i = 0; i < currentNumberOfImages; i++)
            {
                ::ImageProcessor::Resize(cameraImages.at(activeProvider).at(i).get(),
                                         resultCameraImages.at(i).get());
            }
            provideResultImages(resultCameraImages, info);
        }
        else
        {
            ARMARX_DEBUG << deactivateSpam(5, activeProvider) << "No resizing";
            // convert greyscale to rgb
            if (imageProviderInfo.imageFormat.type != imageDisplayType)
            {
                for (int i = 0; i < currentNumberOfImages; i++)
                {
                    ::ImageProcessor::ConvertImage(cameraImages.at(activeProvider).at(i).get(),
                                                   resultCameraImages.at(i).get());
                }
                provideResultImages(resultCameraImages, info);
            }
            else
            {
                ARMARX_DEBUG << deactivateSpam(5, activeProvider) << "Same image type";
                // copy since number of images might be different
                for (int i = 0; i < currentNumberOfImages; i++)
                {
                    ::ImageProcessor::CopyImage(cameraImages.at(activeProvider).at(i).get(),
                                                resultCameraImages.at(i).get());
                }
                provideResultImages(resultCameraImages, info);
            }
        }
    }
 
    void
    ImageSourceSelection::setImageSource(const std::string& imageSource,
                                         int relativeTimeoutMs,
                                         const Ice::Current& c)
    {
        ARMARX_CHECK_EXPRESSION(!imageSource.empty());
        ARMARX_CHECK_GREATER(relativeTimeoutMs, 0);
        std::unique_lock lock(imageSourceMutex);
 
        auto newTimeout = TimeUtil::GetTime() + IceUtil::Time::milliSeconds(relativeTimeoutMs);
 
 
        if (!providerTimeouts.empty() && providerTimeouts.front().first == imageSource)
        {
            ARMARX_VERBOSE << "Refreshing timeout: " << newTimeout.toDateTime();
            providerTimeouts.front().second = newTimeout;
        }
        else
        {
            ARMARX_VERBOSE << "inserting new imageprovider " << imageSource
                           << " at front with timeout " << newTimeout.toDateTime();
            providerTimeouts.insert(providerTimeouts.begin(),
                                    std::make_pair(imageSource, newTimeout));
        }
        //    setImageSource(imageSource);
    }
 
    void
    ImageSourceSelection::setImageSource(const std::string& imageSource)
    {
        std::unique_lock lock(mutex);
        if (!usedImageProviders.count(imageSource))
        {
            addImageProvider(imageSource);
        }
        auto activeProvider = getCurrentImageSource();
        if (imageSource != activeProvider)
        {
            ARMARX_INFO << "getting calibration for image provider " << imageSource;
            visionx::StereoCalibrationInterfacePrx calibrationProvider =
                visionx::StereoCalibrationInterfacePrx::checkedCast(
                    imageProviderInfoMap[imageSource].proxy);
 
            //    activeProvider = imageSource.empty() ? defaultProvider : imageSource;
 
            if (calibrationProvider)
            {
 
                visionx::StereoCalibration stereoCalibration =
                    calibrationProvider->getStereoCalibration();
 
 
                if (scaleFactorX)
                {
 
                    stereoCalibration.calibrationLeft.cameraParam.focalLength[0] /= scaleFactorX;
                    stereoCalibration.calibrationLeft.cameraParam.focalLength[1] /= scaleFactorX;
 
                    stereoCalibration.calibrationLeft.cameraParam.principalPoint[0] /= scaleFactorX;
                    stereoCalibration.calibrationLeft.cameraParam.principalPoint[1] /= scaleFactorX;
 
                    stereoCalibration.calibrationLeft.cameraParam.width /= scaleFactorX;
                    stereoCalibration.calibrationLeft.cameraParam.height /= scaleFactorX;
 
                    stereoCalibration.calibrationRight.cameraParam.focalLength[0] /= scaleFactorX;
                    stereoCalibration.calibrationRight.cameraParam.focalLength[1] /= scaleFactorX;
 
 
                    stereoCalibration.calibrationRight.cameraParam.principalPoint[0] /=
                        scaleFactorX;
                    stereoCalibration.calibrationRight.cameraParam.principalPoint[1] /=
                        scaleFactorX;
 
 
                    stereoCalibration.calibrationRight.cameraParam.width =
                        ((float)stereoCalibration.calibrationRight.cameraParam.width) /
                        scaleFactorX;
                    stereoCalibration.calibrationRight.cameraParam.height =
                        ((float)stereoCalibration.calibrationRight.cameraParam.height) /
                        scaleFactorX;
                }
 
 
                StereoResultImageProviderPtr stereoResultImageProvider =
                    StereoResultImageProviderPtr::dynamicCast(resultImageProvider);
                if (stereoResultImageProvider)
                {
                    stereoResultImageProvider->setStereoCalibration(
                        stereoCalibration,
                        calibrationProvider->getImagesAreUndistorted(),
                        calibrationProvider->getReferenceFrame());
                }
            }
        }
    }
 
    void
    ImageSourceSelection::removeImageSource(const std::string& imageSource)
    {
        for (auto it = providerTimeouts.begin(); it != providerTimeouts.end(); it++)
        {
            if (it->first == imageSource)
            {
                providerTimeouts.erase(it);
                break;
            }
        }
    }
 
    void
    ImageSourceSelection::enableStereoResultImages(visionx::ImageDimension imageDimension,
                                                   visionx::ImageType imageType)
    {
        if (!resultImageProvider)
        {
            ARMARX_INFO << "Enabling StereoResultImageProvider";
 
            StereoResultImageProviderPtr stereoResultImageProvider =
                Component::create<StereoResultImageProvider>();
            stereoResultImageProvider->setName(getProperty<std::string>("resultProviderName"));
            resultImageProvider = IceInternal::Handle<visionx::ResultImageProvider>::dynamicCast(
                stereoResultImageProvider);
 
            getArmarXManager()->addObject(resultImageProvider);
 
            resultImageProvider->setNumberResultImages(2);
            resultImageProvider->setResultImageFormat(imageDimension, imageType);
 
            // wait for resultImageProvider
            resultImageProvider->getObjectScheduler()->waitForObjectState(eManagedIceObjectStarted);
        }
    }
 
    armarx::PropertyDefinitionsPtr
    ImageSourceSelection::createPropertyDefinitions()
    {
        return armarx::PropertyDefinitionsPtr(
            new ImageSourceSelectionPropertyDefinitions(getConfigIdentifier()));
    }
 
    bool
    ImageSourceSelection::addImageProvider(const std::string& providerName,
                                           visionx::ImageType imageDisplayType,
                                           visionx::ImageProviderInfo& imageProviderInfo)
    {
        bool result = true;
 
        visionx::ImageProviderInfo otherImageProviderInfo =
            getImageProvider(providerName, imageDisplayType);
        ARMARX_INFO << providerName << " " << otherImageProviderInfo.imageFormat.dimension.width
                    << "x" << otherImageProviderInfo.imageFormat.dimension.height;
        cameraImages[providerName].resize(otherImageProviderInfo.numberImages);
        for (int i = 0; i < otherImageProviderInfo.numberImages; i++)
        {
            cameraImages[providerName].at(i).reset(
                visionx::tools::createByteImage(otherImageProviderInfo));
        }
        visionx::StereoCalibrationInterfacePrx calibrationProvider =
            visionx::StereoCalibrationInterfacePrx::checkedCast(imageProviderInfo.proxy);
        if (!calibrationProvider)
        {
            ARMARX_WARNING << "image provider does not have a stereo calibration interface";
        }
 
 
        if (!result)
        {
            releaseImageProvider(providerName);
        }
        return result;
    }
} // namespace armarx