StereoImagePointCloudProvider.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::ArmarXObjects::StereoImagePointCloudProvider
 * @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 "StereoImagePointCloudProvider.h"
#include "DepthFromStereo.h"
 
#include <VisionX/tools/TypeMapping.h>
#include <VisionX/tools/ImageUtil.h>
 
#include <Image/ImageProcessor.h>
#include <Calibration/StereoCalibration.h>
 
 
using namespace armarx;
using namespace pcl;
 
namespace visionx
{
 
 
    void StereoImagePointCloudProvider::onInitComponent()
    {
        ImageProcessor::onInitComponent();
        CapturingPointCloudProvider::onInitComponent();
        ImageProvider::onInitComponent();
        downsamplingRate = getProperty<int>("DownsamplingRate").getValue();
        smoothDisparity = getProperty<bool>("SmoothDisparity").getValue();
    }
 
    void StereoImagePointCloudProvider::onConnectComponent()
    {
        ImageProcessor::onConnectComponent();
        CapturingPointCloudProvider::onConnectComponent();
        ImageProvider::onConnectComponent();
    }
 
    void StereoImagePointCloudProvider::onDisconnectComponent()
    {
        ImageProcessor::onDisconnectComponent();
        CapturingPointCloudProvider::onDisconnectComponent();
        ImageProvider::onDisconnectComponent();
    }
 
 
    void StereoImagePointCloudProvider::onExitComponent()
    {
        ImageProvider::onExitComponent();
        CapturingPointCloudProvider::onExitComponent();
        ImageProcessor::onExitComponent();
    }
 
    void StereoImagePointCloudProvider::onInitCapturingPointCloudProvider()
    {
    }
 
 
    void StereoImagePointCloudProvider::onStartCapture(float frameRate)
    {
    }
 
 
    void StereoImagePointCloudProvider::onStopCapture()
    {
    }
 
 
    void StereoImagePointCloudProvider::onExitCapturingPointCloudProvider()
    {
    }
 
 
    MetaPointCloudFormatPtr StereoImagePointCloudProvider::getDefaultPointCloudFormat()
    {
        MetaPointCloudFormatPtr info = new MetaPointCloudFormat();
        //info->frameId = getProperty<std::string>("frameId").getValue();
        info->type = PointContentType::eColoredPoints;
        info->capacity = 1600 * 1200 * sizeof(ColoredPoint3D);// + info->frameId.size();
        info->size = info->capacity;
        return info;
    }
 
 
    void StereoImagePointCloudProvider::onInitImageProcessor()
    {
        // set desired image provider
        providerName = getProperty<std::string>("ImageProviderAdapterName").getValue();
        usingImageProvider(providerName);
    }
 
 
    void StereoImagePointCloudProvider::onConnectImageProcessor()
    {
        // connect to image provider
        ARMARX_INFO << getName() << " connecting to " << providerName;
        visionx::ImageProviderInfo imageProviderInfo = getImageProvider(providerName);
        imageProviderPrx = getProxy<ImageProviderInterfacePrx>(providerName);
        setImageFormat(imageProviderInfo.imageFormat.dimension, imageProviderInfo.imageFormat.type, imageProviderInfo.imageFormat.bpType);
 
        StereoCalibrationProviderInterfacePrx calibrationProviderPrx1 = StereoCalibrationProviderInterfacePrx::checkedCast(imageProviderPrx);
        CapturingPointCloudAndImageAndStereoCalibrationProviderInterfacePrx calibrationProviderPrx2 = CapturingPointCloudAndImageAndStereoCalibrationProviderInterfacePrx::checkedCast(imageProviderPrx);
 
        if (calibrationProviderPrx1)
        {
            stereoCalibration = visionx::tools::convert(calibrationProviderPrx1->getStereoCalibration());
            imagesAreUndistorted = calibrationProviderPrx1->getImagesAreUndistorted();
        }
        else if (calibrationProviderPrx2)
        {
            stereoCalibration = visionx::tools::convert(calibrationProviderPrx2->getStereoCalibration());
            imagesAreUndistorted = calibrationProviderPrx2->getImagesAreUndistorted();
        }
        else
        {
            ARMARX_WARNING << "Image provider with name " << providerName << " is not a StereoCalibrationProvider";
            imagesAreUndistorted = true;
        }
 
        cameraImages = new CByteImage*[2];
        cameraImages[0] = tools::createByteImage(imageProviderInfo);
        cameraImages[1] = tools::createByteImage(imageProviderInfo);
        width = imageProviderInfo.imageFormat.dimension.width;
        height = imageProviderInfo.imageFormat.dimension.height;
        cameraImagesGrey = new CByteImage*[2];
        cameraImagesGrey[0] = new CByteImage(width, height, CByteImage::eGrayScale);
        cameraImagesGrey[1] = new CByteImage(width, height, CByteImage::eGrayScale);
        disparityImage = new CByteImage(width, height, CByteImage::eGrayScale);
        disparityImageRGB = new CByteImage(cameraImages[0]);
        resultImages = new CByteImage*[2];
        resultImages[0] = cameraImages[0];
        resultImages[1] = disparityImageRGB;
    }
 
 
    void StereoImagePointCloudProvider::onExitImageProcessor()
    {
        delete cameraImages[0];
        delete cameraImages[1];
        delete[] cameraImages;
        delete cameraImagesGrey[0];
        delete cameraImagesGrey[1];
        delete[] cameraImagesGrey;
        delete disparityImage;
        delete disparityImageRGB;
        delete[] resultImages;
    }
 
 
 
    void StereoImagePointCloudProvider::onInitImageProvider()
    {
        setImageFormat(visionx::ImageDimension(width, height), visionx::eRgb);
        setNumberImages(2);
    }
 
 
 
 
    MonocularCalibration StereoImagePointCloudProvider::getMonocularCalibration(const Ice::Current& c)
    {
        return visionx::tools::convert(*stereoCalibration->GetLeftCalibration());
    }
 
    bool StereoImagePointCloudProvider::doCapture()
    {
        std::unique_lock lock(captureLock);
 
        if (!waitForImages(8000))
        {
            ARMARX_IMPORTANT << "Timeout or error in wait for images";
            return false;
        }
        else
        {
            // get images
            int nNumberImages = ImageProcessor::getImages(cameraImages);
            ARMARX_DEBUG << getName() << " got " << nNumberImages << " images";
 
            pcl::PointCloud<PointXYZRGBA>::Ptr pointcloud(new pcl::PointCloud<PointXYZRGBA>());
            ::ImageProcessor::ConvertImage(cameraImages[0], cameraImagesGrey[0]);
            ::ImageProcessor::ConvertImage(cameraImages[1], cameraImagesGrey[1]);
 
            ARMARX_DEBUG << getName() << "depthfromstereo::GetPointsFromDisparity";
            depthfromstereo::GetPointsFromDisparity(cameraImages[0], cameraImages[1], cameraImagesGrey[0], cameraImagesGrey[1], downsamplingRate,
                                                    *pointcloud, disparityImage, width, height, stereoCalibration, imagesAreUndistorted, smoothDisparity);
 
            // provide point cloud
            ARMARX_DEBUG << "providing pointcloud";
            providePointCloud(pointcloud);
 
            for (size_t j = 0; j < pointcloud->height; j++)
            {
                for (size_t i = 0; i < pointcloud->width; i++)
                {
                    int idx = (j * pointcloud->width + i);
                    int value = pointcloud->points[idx].z;
                    // if(value <= max ...)
 
                    disparityImageRGB->pixels[3 * idx + 0] = value & 0xFF;
                    disparityImageRGB->pixels[3 * idx + 1] = (value >> 8) & 0xFF;
                }
            }
 
            provideImages(resultImages);
 
            ARMARX_DEBUG << "done";
 
            return true;
        }
    }
 
 
    PropertyDefinitionsPtr StereoImagePointCloudProvider::createPropertyDefinitions()
    {
        return PropertyDefinitionsPtr(new StereoImagePointCloudProviderPropertyDefinitions(getConfigIdentifier()));
    }
 
}