ImageToPointCloud.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    ActiveVision::ArmarXObjects::ImageToPointCloud
 * @author     Markus Grotz ( markus dot grotz at kit dot edu )
 * @author     Mirko Waechter ( waechter at kit dot edu )
 * @date       2019
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ImageToPointCloud.h"
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <pcl/common/transforms.h>
 
#include <ArmarXCore/observers/variant/Variant.h>
 
#include <VisionX/components/pointcloud_core/PCLUtilities.h>
#include <VisionX/interface/components/Calibration.h>
#include <VisionX/tools/ImageUtil.h>
 
 
using namespace armarx;
 
void
ImageToPointCloud::onInitImageProcessor()
{
    offeringTopicFromProperty("DebugObserverName");
 
    providerName = getProperty<std::string>("ProviderName").getValue();
    usingImageProvider(providerName);
 
    fovH = getProperty<float>("HorizontalViewAngle").getValue();
    fovV = getProperty<float>("VerticalViewAngle").getValue();
    if (!getProperty<std::string>("CalibrationProviderName").getValue().empty())
    {
        usingProxy(getProperty<std::string>("CalibrationProviderName").getValue());
    }
    nanValue = getProperty<float>("NaNValue").getValue();
    maxDist = getProperty<float>("MaxDist").getValue();
    minDist = getProperty<float>("MinDist").getValue();
}
 
void
ImageToPointCloud::onConnectImageProcessor()
{
    ARMARX_VERBOSE << "onConnectImageProcessor()";
    imageProviderInfo = getImageProvider(providerName);
    ARMARX_VERBOSE << "after getImageProvider('" << providerName << "')";
 
    ARMARX_CHECK_GREATER_EQUAL(imageProviderInfo.numberImages, 2);
    numImages = std::min(imageProviderInfo.numberImages, 3);
 
    images = new CByteImage*[numImages];
    for (size_t i = 0; i < numImages; i++)
    {
        images[i] = visionx::tools::createByteImage(imageProviderInfo);
    }
 
    getTopicFromProperty(debugObserver, "DebugObserverName");
 
 
    auto applyCalibration = [&](visionx::StereoCalibrationInterfacePrx prx)
    {
        if (getProperty<bool>("ComputeViewAngleFromCalibration").getValue())
        {
            visionx::StereoCalibration stereoCalibration = prx->getStereoCalibration();
            float fx = stereoCalibration.calibrationRight.cameraParam.focalLength[0];
            float fy = stereoCalibration.calibrationRight.cameraParam.focalLength[1];
            fovV = 180.0 / M_PI * 2.0 * std::atan(images[0]->height / (2.0 * fy));
            fovH = 180.0 / M_PI * 2.0 * std::atan(images[0]->width / (2.0 * fx));
            ARMARX_INFO << " Using provided HorizontalViewAngle/VerticalViewAngle parameters from "
                           "calibration";
        }
    };
    if (!getProperty<std::string>("CalibrationProviderName").getValue().empty())
    {
        calibrationPrx = getProxy<visionx::StereoCalibrationInterfacePrx>(
            getProperty<std::string>("CalibrationProviderName").getValue());
 
        if (calibrationPrx)
        {
            applyCalibration(calibrationPrx);
        }
        else
        {
            ARMARX_WARNING << "Property CalibrationProviderName was given, but the proxy is not a "
                              "StereoCalibrationInterfacePrx. Calibration not available!";
        }
    }
    else
    {
        visionx::StereoCalibrationInterfacePrx calibrationInterface =
            visionx::StereoCalibrationInterfacePrx::checkedCast(imageProviderInfo.proxy);
        if (calibrationInterface)
        {
            applyCalibration(calibrationInterface);
        }
    }
 
    ARMARX_INFO << "HorizontalViewAngle: " << fovH << " VerticalViewAngle: " << fovV;
    cloud.reset(new pcl::PointCloud<PointL>());
    transformedCloud.reset(new pcl::PointCloud<PointL>());
 
    debugObserver->setDebugDatafield(getName(), "processCount", new Variant(processCounter));
}
 
void
ImageToPointCloud::onDisconnectComponent()
{
    visionx::PointCloudProvider::onDisconnectComponent();
    visionx::ImageProcessor::onDisconnectComponent();
    for (size_t i = 0; i < numImages; i++)
    {
        delete images[i];
    }
    delete[] images;
}
 
void
ImageToPointCloud::onExitImageProcessor()
{
}
 
void
ImageToPointCloud::process()
{
    std::unique_lock lock(mutex);
 
    if (!waitForImages(1000))
    {
        ARMARX_INFO << deactivateSpam(300) << "Timeout or error while waiting for image data";
        return;
    }
 
    if (getImages(providerName, images, imageMetaInfo) != static_cast<int>(numImages))
    {
        ARMARX_WARNING << "Unable to transfer or read images";
        return;
    }
 
 
    cloud->width = images[0]->width;
    cloud->height = images[0]->height;
    cloud->header.stamp = imageMetaInfo->timeProvided;
    ARMARX_CHECK_EQUAL(getProperty<int>("PointCloudWidth").getValue(), images[0]->width);
    ARMARX_CHECK_EQUAL(getProperty<int>("PointCloudHeight").getValue(), images[0]->height);
    cloud->points.resize(images[0]->width * images[0]->height);
    cloud->is_dense = true;
 
 
    const float scaleX = std::tan(fovH * M_PI / 180.0 / 2.0) * 2.0;
    const float scaleY = std::tan(fovV * M_PI / 180.0 / 2.0) * 2.0;
 
    const size_t width = static_cast<size_t>(images[0]->width);
    const size_t height = static_cast<size_t>(images[0]->height);
    const float halfWidth = (width / 2.0);
    const float halfHeight = (height / 2.0);
    const auto& image0Data = images[0]->pixels;
    const auto& image1Data = images[1]->pixels;
 
    for (size_t j = 0; j < height; j++)
    {
        for (size_t i = 0; i < width; i++)
        {
            auto coord = j * width + i;
            //            unsigned short value = images[1]->pixels[3 * coord + 0]
            //                                   + (images[1]->pixels[3 * coord + 1] << 8);
            auto value = visionx::tools::rgbToDepthValue(
                image1Data[3 * coord + 0], image1Data[3 * coord + 1], image1Data[3 * coord + 2]);
 
            if (maxDist > 0 && value > maxDist)
            {
                value = -1.0f;
            }
            else if (value < minDist)
            {
                value = -1.0f;
            }
 
 
            PointL& p = cloud->points.at(coord);
            auto index = 3 * (coord);
            p.r = image0Data[index + 0];
            p.g = image0Data[index + 1];
            p.b = image0Data[index + 2];
 
            if (value < 0)
            {
                if (nanValue >= 0)
                {
                    p.z = static_cast<float>(nanValue);
                    //p.z /= 1000.0;
                    p.x = -1.0 * (i - halfWidth) / width * p.z * scaleX;
                    p.y = (halfHeight - j) / height * p.z * scaleY;
                }
                else
                {
                    p.x = p.y = p.z = std::numeric_limits<float>::quiet_NaN();
                }
            }
            else
            {
                p.z = static_cast<float>(value);
                p.x = -1.0 * (i - halfWidth) / width * p.z * scaleX;
                p.y = (halfHeight - j) / height * p.z * scaleY;
            }
 
            if (numImages > 2)
            {
                auto& image2Data = images[2]->pixels;
 
                if (images[2]->bytesPerPixel == 3)
                {
                    p.label = static_cast<unsigned int>(image2Data[index + 0] +
                                                        (image2Data[index + 1] << 8) +
                                                        (image2Data[index + 2] << 16));
                }
                else
                {
                    p.label = static_cast<unsigned int>(image2Data[coord]);
                }
            }
            else
            {
                p.label = 0;
            }
        }
    }
 
    processCounter++;
    debugObserver->setDebugDatafield(getName(), "processCount", new Variant(processCounter));
 
    float angle = getProperty<float>("PointCloudRotationZ").getValue() / 180.f * M_PI;
    if (angle != 0.0f)
    {
        Eigen::Matrix4f transform2 = Eigen::Matrix4f::Identity();
        Eigen::Matrix3f m;
        m = Eigen::AngleAxisf(angle, Eigen::Vector3f::UnitZ());
        transform2.block<3, 3>(0, 0) *= m;
        pcl::transformPointCloud(*cloud, *transformedCloud, transform2);
        transformedCloud->header.stamp = imageMetaInfo->timeProvided;
        providePointCloud(transformedCloud);
    }
    else
    {
        providePointCloud(cloud);
    }
    //    pcl::PointCloud<PointL>::Ptr temp(new pcl::PointCloud<PointL>());
}
 
visionx::MetaPointCloudFormatPtr
ImageToPointCloud::getDefaultPointCloudFormat()
{
    visionx::MetaPointCloudFormatPtr info = new visionx::MetaPointCloudFormat();
    info->size = getProperty<int>("PointCloudWidth").getValue() *
                 getProperty<int>("PointCloudHeight").getValue() * sizeof(PointL);
    info->capacity = info->size;
    info->type = visionx::PointContentType::eColoredLabeledPoints;
    return info;
}
 
armarx::PropertyDefinitionsPtr
ImageToPointCloud::createPropertyDefinitions()
{
    return armarx::PropertyDefinitionsPtr(
        new ImageToPointCloudPropertyDefinitions(getConfigIdentifier()));
}