NerianVisionPointCloudProvider.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::NerianVisionPointCloudProvider
 * @author     Julian Tusch <julian.tusch@student.kit.edu>
 * @date       2025
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "NerianVisionPointCloudProvider.h"
 
#include <algorithm>
#include <cstdint>
#include <cstring>
#include <exception>
#include <limits>
#include <memory>
#include <string>
#include <vector>
 
#include <pcl/common/impl/io.hpp>
#include <pcl/point_cloud.h>
 
#include <opencv2/opencv.hpp> // IWYU pragma: keep
 
#include <Ice/Config.h>
#include <Ice/Current.h>
#include <IceUtil/Time.h>
 
 
// Nerian Vision SDK
#include <visiontransfer/deviceenumeration.h>
#include <visiontransfer/deviceinfo.h>
#include <visiontransfer/deviceparameters.h>
#include <visiontransfer/imageset.h>
#include <visiontransfer/imagetransfer.h>
#include <visiontransfer/reconstruct3d.h>
 
// IVT
#include <Image/ByteImage.h>
#include <Image/ImageProcessor.h>
 
// ArmarX
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <ArmarXCore/observers/variant/Variant.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
// VisionX
#include "VisionX/components/pointcloud_core/PointCloudProvider.h"
#include "VisionX/core/ImageProvider.h"
#include <VisionX/interface/components/Calibration.h>
#include <VisionX/interface/core/DataTypes.h>
#include <VisionX/libraries/imrec/helper.h>
#include <VisionX/tools/ImageUtil.h>
#include <VisionX/tools/TypeMapping.h>
 
namespace visionx
{
    void
    NerianVisionPointCloudProvider::onInitComponent()
    {
        ImageProvider::onInitComponent();
        PointCloudProvider::onInitComponent();
    }
 
    void
    NerianVisionPointCloudProvider::onInitImageProvider()
    {
        using visiontransfer::DeviceEnumeration;
        using visiontransfer::DeviceInfo;
        using visiontransfer::DeviceParameters;
 
        DeviceEnumeration deviceEnum;
        DeviceEnumeration::DeviceList devices = deviceEnum.discoverDevices();
        if (devices.size() == 0)
        {
            ARMARX_INFO << "No Nerian Vision devices detected! Shutting down...";
            getArmarXManager()->asyncShutdown();
            throw armarx::LocalException("No Nerian Vision devices detected!");
        }
 
        device = devices[0];
        auto deviceModel = device.getModel();
        setMetaInfo("model", new armarx::Variant(getNameForDeviceModel(deviceModel)));
        setMetaInfo("serialNumber", new armarx::Variant(device.getSerialNumber()));
        setMetaInfo("firmwareVersion", new armarx::Variant(device.getFirmwareVersion()));
 
        ARMARX_INFO << "Using Nerian Vision device: " << device.toString();
 
        if (deviceModel != DeviceInfo::DeviceModel::RUBY)
        {
            ARMARX_INFO << "Device model " << getNameForDeviceModel(deviceModel)
                        << " is currently not supported for this component! Shutting down...";
            getArmarXManager()->asyncShutdown();
            throw armarx::LocalException("Unsupported device model!");
        }
 
        setNumberImages(2);
 
        std::unique_ptr<DeviceParameters> parameterPtr = nullptr;
        try
        {
            parameterPtr = std::make_unique<DeviceParameters>(device);
        }
        catch (const std::exception& e)
        {
            ARMARX_INFO << "Failed to open device parameters."
                        << " (" << e.what() << ")";
            return;
        }
 
        try
        {
            auto dim = parameterPtr->getParameter("RT_output_image_size").getTensorData();
            dimensions.width = static_cast<int>(dim[0]);
            dimensions.height = static_cast<int>(dim[1]);
        }
        catch (const std::exception& e)
        {
            ARMARX_INFO << "Failed to get dimension from device parameters. Using app property "
                           "`dimensions` as fallback."
                        << " (" << e.what() << ")";
            dimensions = getProperty<visionx::ImageDimension>("dimensions").getValue();
        }
 
        ARMARX_DEBUG << "Using image dimensions: " << dimensions.width << "x" << dimensions.height;
        setImageFormat(dimensions, visionx::eRgb, visionx::eBayerPatternRg);
 
        ARMARX_DEBUG << "Configuring device.";
 
        try
        {
            parameterPtr->setParameter("output_channel_left_enabled", false);
            parameterPtr->setParameter("output_channel_right_enabled", false);
            parameterPtr->setParameter("output_channel_color_enabled", true);
            parameterPtr->setParameter("output_channel_color_mode", 0);
            parameterPtr->setParameter("output_channel_disparity_enabled", true);
            parameterPtr->setOperationMode(DeviceParameters::OperationMode::STEREO_MATCHING);
        }
        catch (const std::exception& e)
        {
            ARMARX_INFO << "Failed to configure device." << " (" << e.what() << ")";
            ARMARX_WARNING
                << "Manual configuration of the device may be necessary. To do this, open the web "
                   "interface of the device at "
                << device.getIpAddress() << " and verify/set the following settings: "
                << "Output channels [color, disparity] are enabled and the channels [left, right] "
                   "are disabled. The Operation mode should be set to STEREO_MATCHING.";
        }
    }
 
    void
    NerianVisionPointCloudProvider::onInitPointCloudProvider()
    {
        pointcloud = std::make_unique<pcl::PointCloud<CloudPointType>>();
    }
 
    void
    NerianVisionPointCloudProvider::onConnectComponent()
    {
        ImageProvider::onConnectComponent();
        PointCloudProvider::onConnectComponent();
    }
 
    void
    NerianVisionPointCloudProvider::onConnectImageProvider()
    {
        ARMARX_DEBUG << "Connecting " << getName();
 
        processingTask = armarx::RunningTask<NerianVisionPointCloudProvider>::pointer_type(
            new armarx::RunningTask<NerianVisionPointCloudProvider>(
                this, &NerianVisionPointCloudProvider::runImageAndPointCloudPublishing));
        processingTask->start();
    }
 
    void
    NerianVisionPointCloudProvider::onConnectPointCloudProvider()
    {
        cloudFormat = getPointCloudFormat();
    }
 
    void
    NerianVisionPointCloudProvider::runImageAndPointCloudPublishing()
    {
        using visiontransfer::DeviceParameters;
        using visiontransfer::ImageSet;
        using visiontransfer::ImageTransfer;
 
        visiontransfer::Reconstruct3D reconstructor;
 
        ARMARX_DEBUG << "Started processing task.";
 
        std::unique_ptr<ImageTransfer> imageTransferPtr = nullptr;
        try
        {
            imageTransferPtr = std::make_unique<ImageTransfer>(device);
        }
        catch (const std::exception& e)
        {
            ARMARX_INFO << "Failed to open device."
                        << " (" << e.what() << ")";
            return;
        }
 
        if (imageTransferPtr == nullptr)
        {
            ARMARX_INFO << "Failed to connnect to device!";
            return;
        }
 
        while (not processingTask->isStopped())
        {
            ImageSet imageSet;
            bool status = false;
            try
            {
                // receiveImageSet() has an internal timout of 100ms
                status = imageTransferPtr->receiveImageSet(imageSet);
            }
            catch (const std::exception& e)
            {
                ARMARX_INFO << deactivateSpam(30) << "Error receiving images from device."
                            << " (" << e.what() << ")";
                continue;
            }
 
            if (!status)
            {
                ARMARX_DEBUG << deactivateSpam(30) << "Did not get frame until timeout of 100ms.";
                continue;
            }
 
            ARMARX_TRACE;
 
            int numImages = getNumberImages();
            if (numImages != imageSet.getNumberOfImages())
            {
                ARMARX_INFO << deactivateSpam(30) << "Expected " << numImages << " images, but got "
                            << imageSet.getNumberOfImages()
                            << " images. Please verify the device and component configurations.";
                continue;
            }
 
            if (!imageSet.hasImageType(ImageSet::ImageType::IMAGE_COLOR) ||
                !imageSet.hasImageType(ImageSet::ImageType::IMAGE_DISPARITY))
            {
                ARMARX_INFO << deactivateSpam(30)
                            << "Received image set does not contain the expected image types "
                               "(color, disparity). Please verify the device and component "
                               "configurations, then restart this component.";
                continue;
            }
 
            ARMARX_TRACE;
 
            int index_color = imageSet.getIndexOf(ImageSet::ImageType::IMAGE_COLOR);
            if (imageSet.getPixelFormat(index_color) != ImageSet::ImageFormat::FORMAT_8_BIT_RGB)
            {
                ARMARX_INFO
                    << deactivateSpam(30)
                    << "Received unexpected image format for color image. Expected 8-bit RGB.";
                continue;
            }
 
            int index_disparity = imageSet.getIndexOf(ImageSet::ImageType::IMAGE_DISPARITY);
            if (imageSet.getPixelFormat(index_disparity) !=
                ImageSet::ImageFormat::FORMAT_12_BIT_MONO)
            {
                ARMARX_INFO << deactivateSpam(30)
                            << "Received unexpected image format for disparity image. Expected "
                               "12-bit Mono.";
                continue;
            }
 
            std::vector<CByteImage*> imageBuffers(2);
 
            // Convert color image
            {
                cv::Mat bgr_image;
                imageSet.toOpenCVImage(index_color, bgr_image, true);
                imageBuffers[0] = new CByteImage();
                visionx::imrec::convert(bgr_image, *imageBuffers[0]);
            }
 
            ARMARX_TRACE;
 
            // Convert disparity image
            {
                const int dw = imageSet.getWidth();
                const int dh = imageSet.getHeight();
 
                imageBuffers[1] = new CByteImage(dw, dh, CByteImage::ImageType::eRGB24);
                CByteImage* depth_image = imageBuffers[1];
 
                auto* result_depth_buffer = depth_image->pixels;
                const auto* pixelData = imageSet.getPixelData(index_disparity);
                const auto* depth_buffer = reinterpret_cast<const uint16_t*>(pixelData);
 
                int index = 0;
                int index_2 = 0;
                for (int y = 0; y < dh; ++y)
                {
                    for (int x = 0; x < dw; ++x)
                    {
                        uint16_t depth_value = depth_buffer[index_2];
                        visionx::tools::depthValueToRGB(depth_value,
                                                        result_depth_buffer[index],
                                                        result_depth_buffer[index + 1],
                                                        result_depth_buffer[index + 2]);
                        index += 3;
                        index_2 += 1;
                    }
                }
            }
 
            ARMARX_TRACE;
 
            // Generate Pointcloud
            {
                ARMARX_CHECK_EXPRESSION(pointcloud);
 
                // the coordinates are in meters (!)
                auto temp = reconstructor.createXYZRGBCloud(imageSet, "");
 
                pointcloud->header.stamp = temp->header.stamp;
                pointcloud->is_dense = temp->is_dense;
                pointcloud->width = temp->width;
                pointcloud->height = temp->height;
                pointcloud->points.resize(pointcloud->width * pointcloud->height);
                pcl::detail::copyPointCloudMemcpy(*temp, *pointcloud);
 
                float max_depth = getProperty<float>("MaxDepth").getValue(); // [mm]
                for (auto& p : pointcloud->points)
                {
                    // for some reason, red and blue channels are swapped here
                    auto red = p.r;
                    p.r = p.b;
                    p.b = red;
 
                    // convert from [m] to [mm]
                    p.x *= 1000.0F;
                    p.y *= 1000.0F;
                    p.z *= 1000.0F;
 
                    // apply max depth filter
                    float z = p.z;
                    if (z <= max_depth and z != 0)
                    {
                        p.z = z;
                    }
                    else
                    {
                        p.z = std::numeric_limits<float>::quiet_NaN();
                    }
                }
            }
 
            ARMARX_TRACE;
 
            // provide results
            {
                int t_s = 0;
                int t_us = 0;
                imageSet.getTimestamp(t_s, t_us);
                const IceUtil::Time timestamp =
                    IceUtil::Time::seconds(t_s) + IceUtil::Time::microSeconds(t_us);
 
                provideImages(imageBuffers.data(), timestamp);
 
                ARMARX_TRACE;
 
                // FIXME:
                // the check with virtualRobotReaderPlugin->get().synchronizeRobot(robot, timestamp)
                // in PointCloudToArviz fails, and I suspect the timestamp might be the issue
                providePointCloud(pointcloud);
            }
        }
 
        ARMARX_DEBUG << "Stopped processing task.";
    }
 
    void
    NerianVisionPointCloudProvider::onDisconnectComponent()
    {
        ImageProvider::onDisconnectComponent();
    }
 
    void
    NerianVisionPointCloudProvider::onDisconnectImageProvider()
    {
        ARMARX_DEBUG << "Disconnecting " << getName();
 
        // Stop task
        {
            ARMARX_DEBUG << "Stopping processing thread...";
            processingTask->stop(false);
            ARMARX_DEBUG << "Waiting for processing thread to stop...";
            processingTask->waitForStop();
            ARMARX_DEBUG << "Processing thread stopped";
        }
    }
 
    void
    NerianVisionPointCloudProvider::onExitComponent()
    {
        ImageProvider::onExitComponent();
        PointCloudProvider::onExitComponent();
    }
 
    void
    NerianVisionPointCloudProvider::onExitImageProvider()
    {
    }
 
    void
    NerianVisionPointCloudProvider::onExitPointCloudProvider()
    {
    }
 
    visionx::StereoCalibration
    NerianVisionPointCloudProvider::getStereoCalibration(const Ice::Current& /*c*/)
    {
        StereoCalibration stereo_calibration;
 
        // TODO: find out how to retrieve the calibration data from nerian vision devices
        // stereo_calibration.calibrationLeft = stereo_calibration.calibrationRight = ...
 
        // the images are rectified, so the distortion params are 0
        auto& colorDistortionParams = stereo_calibration.calibrationLeft.cameraParam.distortion;
        std::fill(colorDistortionParams.begin(), colorDistortionParams.end(), 0);
        auto& depthDistortionParams = stereo_calibration.calibrationRight.cameraParam.distortion;
        std::fill(depthDistortionParams.begin(), depthDistortionParams.end(), 0);
 
        stereo_calibration.rectificationHomographyLeft =
            stereo_calibration.rectificationHomographyRight =
                visionx::tools::convertEigenMatToVisionX(Eigen::Matrix3f::Identity());
 
        return stereo_calibration;
    }
 
    std::string
    NerianVisionPointCloudProvider::getReferenceFrame(const Ice::Current& /*c*/)
    {
        return {};
    }
 
    bool
    NerianVisionPointCloudProvider::getImagesAreUndistorted(const Ice::Current& /*c*/)
    {
        // Ruby User Manual, Chapter 9.6 - Output Channels
        // "If the operation mode is set to stereo matching [...],
        // then the image data of all output channels are rectified."
        return true;
    }
 
    void
    NerianVisionPointCloudProvider::startCapture(const Ice::Current& /*c*/)
    {
    }
 
    void
    NerianVisionPointCloudProvider::stopCapture(const Ice::Current& /*c*/)
    {
    }
 
    void
    NerianVisionPointCloudProvider::changeFrameRate(Ice::Float /*framesPerSecond*/,
                                                    const Ice::Current& /*c*/)
    {
    }
 
    void
    NerianVisionPointCloudProvider::startCaptureForNumFrames(int /*numFrames*/,
                                                             const Ice::Current& /*c*/)
    {
    }
 
    bool
    NerianVisionPointCloudProvider::isCaptureEnabled(const Ice::Current& /*c*/)
    {
        return true;
    }
 
    armarx::PropertyDefinitionsPtr
    NerianVisionPointCloudProvider::createPropertyDefinitions()
    {
        return {new NerianVisionPointCloudProviderPropertyDefinitions(getConfigIdentifier())};
    }
} // namespace visionx