CropRobotFromImage.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::CropRobotFromImage
 * @author     Julian Zimmer ( urdbu at student dot kit dot edu )
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "CropRobotFromImage.h"
 
 
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
 
#include <VirtualRobot/CollisionDetection/CollisionModel.h>
 
// IVT
#include <Image/ByteImage.h>
#include <Image/ImageProcessor.h>
#include <Calibration/StereoCalibration.h>
#include <Calibration/Calibration.h>
#include <Math/LinearAlgebra.h>
#include <Math/FloatMatrix.h>
 
 
namespace armarx
{
 
    void CropRobotFromImage::onInitImageProcessor()
    {
        providerName = getProperty<std::string>("providerName").getValue();
        usingImageProvider(providerName);
 
 
        usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
        cameraFrameName = getProperty<std::string>("cameraFrameName").getValue();
 
        backgroundR = getProperty<float>("filterColorR").getValue();
        backgroundG = getProperty<float>("filterColorG").getValue();
        backgroundB = getProperty<float>("filterColorB").getValue();
 
        dilationStrength = getProperty<int>("dilationStrength").getValue();
 
        flipImages = getProperty<bool>("flipImages").getValue();
        useFullModel = getProperty<bool>("useFullModel").getValue();
        collisionModelInflationMargin = getProperty<float>("collisionModelInflationMargin").getValue();
 
        numResultImages = getProperty<int>("numResultImages").getValue();
    }
 
 
    void CropRobotFromImage::onConnectImageProcessor()
    {
 
        if (getProperty<std::string>("RobotStateComponentName").getValue() != "")
        {
            robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateComponentName").getValue());
        }
        else
        {
            ARMARX_ERROR << "Specification of RobotStateComponentName missing";
        }
 
 
        //ImageType imageDisplayType = visionx::tools::typeNameToImageType("float-1-channel");
        visionx::ImageType imageDisplayType = visionx::tools::typeNameToImageType("rgb");
        //ImageType imageDisplayType = visionx::tools::typeNameToImageType("gray-scale");
 
 
        imageProviderInfo = getImageProvider(providerName, imageDisplayType);
 
 
        numImages = imageProviderInfo.numberImages;
 
        images = new CByteImage*[numImages];
        for (int i = 0 ; i < numImages; i++)
        {
            images[i] = visionx::tools::createByteImage(imageProviderInfo);
        }
 
 
        //calibrationPrx = getTopic<StereoCalibrationInterfacePrx>("StereoCalibrationInterface");
 
        visionx::StereoCalibrationProviderInterfacePrx calibrationProvider = visionx::StereoCalibrationProviderInterfacePrx::checkedCast(imageProviderInfo.proxy);
 
        if (calibrationProvider)
        {
 
            visionx::StereoCalibration stereoCalibration = calibrationProvider->getStereoCalibration();
 
            ::visionx::MonocularCalibration calibrationLeft = stereoCalibration.calibrationLeft;
            width = calibrationLeft.cameraParam.width;
            height = calibrationLeft.cameraParam.height;
            fov = 2.0 * std::atan(width / (2.0 * calibrationLeft.cameraParam.focalLength[1]));
            //fov = M_PI / 4;
 
            ARMARX_INFO << "Using calibration from image provider " << providerName << ". Field of view is: " << fov ;
 
            ARMARX_INFO << "Iamge size: (" << width << ", " << height << ").";
 
            /*
            StereoResultImageProviderPtr stereoResultImageProvider = StereoResultImageProviderPtr::dynamicCast(resultImageProvider);
            stereoResultImageProvider->setStereoCalibration(stereoCalibration, calibrationProvider->getImagesAreUndistorted(), calibrationProvider->getReferenceFrame());
            */
        }
        else
        {
            visionx::StereoCalibrationInterfacePrx pointCloudAndStereoCalibrationProvider = visionx::StereoCalibrationInterfacePrx::checkedCast(imageProviderInfo.proxy);
            if (pointCloudAndStereoCalibrationProvider)
            {
                visionx::StereoCalibration stereoCalibration = pointCloudAndStereoCalibrationProvider->getStereoCalibration();
 
                ::visionx::MonocularCalibration calibrationLeft = stereoCalibration.calibrationLeft;
                width = calibrationLeft.cameraParam.width;
                height = calibrationLeft.cameraParam.height;
                fov = 2.0 * std::atan(width / (2.0 * calibrationLeft.cameraParam.focalLength[0]));
                //fov = M_PI / 4;
 
                ARMARX_INFO << "Using calibration from image provider " << providerName << ".";
            }
            else
            {
                ARMARX_IMPORTANT << "image provider " << providerName << " does not have a stereo calibration interface";
                ARMARX_IMPORTANT << "using default calibration instead";
 
                depthCameraCalibration = visionx::tools::createDefaultMonocularCalibration();
 
                width = depthCameraCalibration.cameraParam.width;
                height = depthCameraCalibration.cameraParam.height;
 
                //appears to be the correct FOV for the stereo camera (e.g. built into Armar3)
                //fov = 2.0 * std::atan(width / (2.0 * depthCameraCalibration.cameraParam.focalLength[0]));
 
                //appears to be the correct FOV for the depth camera (e.g. mounted on Armar6)
                fov = M_PI / 4;
            }
        }
 
        visionx::ImageDimension dimension(images[0]->width, images[0]->height);
        //enableResultImages(numResultImages, dimension, visionx::tools::typeNameToImageType("float-1-channel"));
        enableResultImages(numResultImages, dimension, visionx::tools::typeNameToImageType("rgb"));
 
 
 
        maskRobot = new MaskRobotInImage(robotStateComponent, cameraFrameName, imageProviderInfo.imageFormat, fov, SbColor(backgroundR / 255.f, backgroundG / 255.f, backgroundB / 255.f), flipImages, useFullModel, collisionModelInflationMargin);
    }
 
 
    void CropRobotFromImage::onDisconnectImageProcessor()
    {
 
    }
 
 
    void CropRobotFromImage::onExitImageProcessor()
    {
        for (int i = 0; i < numImages; i++)
        {
            delete images[i];
        }
        delete [] images;
    }
 
    void CropRobotFromImage::process()
    {
        ARMARX_VERBOSE << "CropRobotFromImage is processing data!";
 
        std::lock_guard<std::mutex> lock(mutex);
 
        ARMARX_VERBOSE << "waiting for images";
        if (!waitForImages(providerName))
        {
            ARMARX_WARNING << "Timeout while waiting for images";
            return;
        }
 
 
        //get images including the metaInfo which contains the timestamp of the image to synchronize the robot clone
        ARMARX_VERBOSE << "getting images";
        armarx::MetaInfoSizeBasePtr metaInfo;
        if (this->ImageProcessor::getImages(providerName, images, metaInfo) != numImages)
        {
            ARMARX_WARNING << "Unable to transfer or read images";
            return;
        }
 
        CByteImage* maskImage = maskRobot->getMaskImage(metaInfo->timeProvided);
 
 
        //apply dilation
        if (dilationStrength >= 2)
        {
            ::ImageProcessor::Dilate(maskImage, maskImage, dilationStrength);
        }
 
 
        for (int i = 0; i < numResultImages; i++)
        {
            //apply maskImage to the provided image
            std::uint8_t* pixelsRow = images[i]->pixels;
            std::uint8_t* maskPixelsRow = maskImage->pixels;
 
            for (int y = 0; y < height; y++)
            {
                for (int x = 0; x < width; x++)
                {
                    if (maskPixelsRow[x] == 255)
                    {
                        pixelsRow[x * 3 + 0] = 0;
                        pixelsRow[x * 3 + 1] = 0;
                        pixelsRow[x * 3 + 2] = 0;
                    }
                }
 
                pixelsRow += width * 3;
                maskPixelsRow += width;
            }
        }
 
 
 
        int imageSize = imageProviderInfo.imageFormat.dimension.width *  imageProviderInfo.imageFormat.dimension.height * imageProviderInfo.imageFormat.bytesPerPixel;
        MetaInfoSizeBasePtr myMetaInfo = new MetaInfoSizeBase(numResultImages * imageSize, numResultImages * imageSize, metaInfo->timeProvided);
        provideResultImages(images, myMetaInfo);
 
        ARMARX_VERBOSE << "CropRobotFromImage process finished";
    }
 
    armarx::PropertyDefinitionsPtr CropRobotFromImage::createPropertyDefinitions()
    {
        return armarx::PropertyDefinitionsPtr(new CropRobotFromImagePropertyDefinitions(
                getConfigIdentifier()));
    }
}