DepthImageProviderDynamicSimulation.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2013-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
 * @author     Raphael Grimm
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "DepthImageProviderDynamicSimulation.h"
 
#include <Eigen/Geometry>
 
#include <pcl/common/transforms.h>
#include <pcl/compression/organized_pointcloud_conversion.h>
 
#include <SimoxUtility/math/convert/deg_to_rad.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
 
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <VisionX/tools/ImageUtil.h>
#include <VisionX/tools/TypeMapping.h>
 
#include <Inventor/SoInteraction.h>
#include <Inventor/actions/SoGLRenderAction.h>
 
namespace armarx
{
 
    DepthImageProviderDynamicSimulationPropertyDefinitions::
        DepthImageProviderDynamicSimulationPropertyDefinitions(std::string prefix) :
        visionx::CapturingPointCloudProviderPropertyDefinitions(prefix)
    {
        defineOptionalProperty<bool>(
            "FloatImageMode", false, "Whether to provide a CFloatImage or the standard CByteImage");
        defineOptionalProperty<float>("Noise",
                                      0.0f,
                                      "Noise of the point cloud position results as standard "
                                      "deviation of the normal distribution (in mm)")
            .setMin(0);
        defineOptionalProperty<float>("DistanceZNear",
                                      20.0f,
                                      "Distance of the near clipping plain. (If set too small "
                                      "the agent's model's inside may be visible")
            .setMin(1e-8);
        defineOptionalProperty<float>(
            "DistanceZFar",
            5000.0f,
            "Distance of the far clipping plain. (DistanceZFar-DistanceZNear should be "
            "minimal, DistanceZFar > DistanceZNear)")
            .setMin(1e-8);
        defineOptionalProperty<float>(
            "FieldOfView", 90, "Vertical field of view (FOV) in degrees.");
        defineOptionalProperty<float>(
            "BaseLine", 0.075, "The value returned from getBaseline(). It has no other effect.");
        defineOptionalProperty<float>("NanValue",
                                      std::nan(""),
                                      "Value of points that are farther away than DistanceZFar. "
                                      "Most cameras return NaN here.");
 
        defineOptionalProperty<visionx::ImageDimension>(
            "ImageSize",
            visionx::ImageDimension(640, 480),
            "Target resolution of the images. Captured images will be converted to this size.")
            .setCaseInsensitive(true)
            .map("200x200", visionx::ImageDimension(200, 200))
            .map("320x240", visionx::ImageDimension(320, 240))
            .map("640x480", visionx::ImageDimension(640, 480))
            .map("800x600", visionx::ImageDimension(800, 600))
            .map("768x576", visionx::ImageDimension(768, 576))
            .map("1024x768", visionx::ImageDimension(1024, 768))
            .map("1280x960", visionx::ImageDimension(1280, 960))
            .map("1600x1200", visionx::ImageDimension(1600, 1200))
            .map("1920x1080", visionx::ImageDimension(1920, 1080))
            .map("none", visionx::ImageDimension(0, 0));
        defineOptionalProperty<std::string>("RobotName", "Armar3", "The robot's name.");
        defineOptionalProperty<std::string>(
            "RobotNodeCamera", "DepthCameraSim", "The coordinate system of the used camera");
 
        //used to draw the cloud to the simulator
        defineOptionalProperty<bool>(
            "DrawPointCloud",
            false,
            "Whether the point cloud is drawn to the given DebugDrawerTopic");
        defineOptionalProperty<std::string>("DrawPointCloud_DebugDrawerTopic",
                                            "DebugDrawerUpdates",
                                            "Name of the DebugDrawerTopic");
        defineOptionalProperty<unsigned int>(
            "DrawPointCloud_DrawDelay",
            1000,
            "The time between updates of the drawn point cloud (in ms)");
        defineOptionalProperty<float>("DrawPointCloud_PointSize", 4, "The size of a point.");
        defineOptionalProperty<std::size_t>(
            "DrawPointCloud_PointSkip",
            3,
            "Only draw every n'th point in x and y direction (n=DrawPointCloud_PointSkip). "
            "Increase this whenever the ice buffer size is to small to transmitt the cloud "
            "size. (>0)");
 
        defineOptionalProperty<bool>(
            "DrawPointCloud_ClipPoints",
            true,
            "Whether to clip the point cloud drawn to the given DebugDrawerTopic");
 
        defineOptionalProperty<float>(
            "DrawPointCloud_ClipXHi", 25000, "Skip points with x higher than this limit.");
        defineOptionalProperty<float>(
            "DrawPointCloud_ClipYHi", 25000, "Skip points with y higher than this limit.");
        defineOptionalProperty<float>(
            "DrawPointCloud_ClipZHi", 25000, "Skip points with z higher than this limit.");
 
        defineOptionalProperty<float>(
            "DrawPointCloud_ClipXLo", -25000, "Skip points with x lower than this limit.");
        defineOptionalProperty<float>(
            "DrawPointCloud_ClipYLo", -25000, "Skip points with y lower than this limit.");
        defineOptionalProperty<float>(
            "DrawPointCloud_ClipZLo", -25000, "Skip points with z lower than this limit.");
    }
 
    void
    DepthImageProviderDynamicSimulation::onInitComponent()
    {
        ARMARX_TRACE;
        ARMARX_VERBOSE_S << "DepthImageProviderDynamicSimulation::onInitComponent";
        baseline = getProperty<float>("BaseLine").getValue();
        robotName = getProperty<std::string>("RobotName").getValue();
        camNodeName = getProperty<std::string>("RobotNodeCamera").getValue();
 
        zNear = getProperty<float>("DistanceZNear").getValue();
        zFar = getProperty<float>("DistanceZFar").getValue();
        vertFov = simox::math::deg_to_rad(getProperty<float>("FieldOfView").getValue());
        nanValue = getProperty<float>("NanValue").getValue();
 
        floatImageMode = getProperty<bool>("FloatImageMode").getValue();
 
        drawPointCloud = getProperty<bool>("DrawPointCloud").getValue();
        if (drawPointCloud)
        {
            lastUpdate = std::chrono::high_resolution_clock::now() -
                         std::chrono::seconds{pointCloudDrawDelay};
            debugDrawerTopicName =
                getProperty<std::string>("DrawPointCloud_DebugDrawerTopic").getValue();
            offeringTopic(debugDrawerTopicName);
            pointCloudDrawDelay = getProperty<unsigned int>("DrawPointCloud_DrawDelay").getValue();
            pointCloudPointSize = getProperty<float>("DrawPointCloud_PointSize").getValue();
 
            pointSkip = getProperty<std::size_t>("DrawPointCloud_PointSkip").getValue();
 
            clipDrawnCloudXLo = getProperty<float>("DrawPointCloud_ClipXLo").getValue();
            clipDrawnCloudYLo = getProperty<float>("DrawPointCloud_ClipYLo").getValue();
            clipDrawnCloudZLo = getProperty<float>("DrawPointCloud_ClipZLo").getValue();
 
            clipDrawnCloudXHi = getProperty<float>("DrawPointCloud_ClipXHi").getValue();
            clipDrawnCloudYHi = getProperty<float>("DrawPointCloud_ClipYHi").getValue();
            clipDrawnCloudZHi = getProperty<float>("DrawPointCloud_ClipZHi").getValue();
 
            clipPointCloud = getProperty<bool>("DrawPointCloud_ClipPoints").getValue();
        }
        noise = getProperty<float>("Noise").getValue();
        if (noise > 0)
        {
            distribution = std::normal_distribution<double>(0, noise);
            randValues.resize(1000, 0);
            for (float& val : randValues)
            {
                val = distribution(generator);
            }
        }
 
        ImageProvider::onInitComponent();
        CapturingPointCloudProvider::onInitComponent();
        ARMARX_VERBOSE_S << "done DepthImageProviderDynamicSimulation::onInitComponent";
    }
 
    void
    DepthImageProviderDynamicSimulation::onConnectComponent()
    {
        ARMARX_TRACE;
        ARMARX_VERBOSE_S << "DepthImageProviderDynamicSimulation::onConnectComponent";
 
        if (drawPointCloud)
        {
            debugDrawer = getTopic<DebugDrawerInterfacePrx>(debugDrawerTopicName);
        }
 
        ImageProvider::onConnectComponent();
        CapturingPointCloudProvider::onConnectComponent();
        ARMARX_VERBOSE_S << "done DepthImageProviderDynamicSimulation::onConnectComponent";
    }
 
    void
    DepthImageProviderDynamicSimulation::onExitComponent()
    {
        ARMARX_VERBOSE_S << "DepthImageProviderDynamicSimulation::onExitComponent";
        CapturingPointCloudProvider::onExitComponent();
        ImageProvider::onExitComponent();
        ARMARX_VERBOSE_S << "done DepthImageProviderDynamicSimulation::onExitComponent";
    }
 
    void
    DepthImageProviderDynamicSimulation::onStartCapture(float frameRate)
    {
        ARMARX_TRACE;
        ARMARX_INFO << "DepthImageProviderDynamicSimulation::onStartCapture";
        // ensure that all data is loaded
        while (!simVisu || !simVisu->getRobot(robotName))
        {
            if (simVisu)
            {
                ARMARX_TRACE;
                simVisu->synchronizeVisualizationData();
            }
            usleep(100000);
            ARMARX_INFO << deactivateSpam(3) << "Waiting for visu";
        }
        syncVisu();
        ARMARX_INFO << "Got visu";
 
        ARMARX_TRACE;
        std::unique_lock lock(captureMutex);
        auto l = simVisu->getScopedLock();
 
        // some checks...
        ARMARX_TRACE;
        offscreenRenderer.reset(
            VirtualRobot::CoinVisualizationFactory::createOffscreenRenderer(width, height));
        ARMARX_INFO << "done DepthImageProviderDynamicSimulation::onStartCapture";
    }
 
    void
    DepthImageProviderDynamicSimulation::onStopCapture()
    {
    }
 
    DepthImageProviderDynamicSimulation::~DepthImageProviderDynamicSimulation()
    {
    }
 
    std::string
    DepthImageProviderDynamicSimulation::getDefaultName() const
    {
        return "DynamicSimulationDepthImageProvider";
    }
 
    bool
    DepthImageProviderDynamicSimulation::hasSharedMemorySupport(const Ice::Current& c)
    {
        return true;
    }
 
    visionx::StereoCalibration
    DepthImageProviderDynamicSimulation::getStereoCalibration(const Ice::Current& c)
    {
        visionx::StereoCalibration stereoCalibration;
 
        float r = static_cast<float>(width) / static_cast<float>(height);
 
        visionx::CameraParameters RGBCameraIntrinsics;
        RGBCameraIntrinsics.distortion = {0, 0, 0, 0};
        RGBCameraIntrinsics.focalLength = {
            static_cast<float>(width) / (2 * std::tan((vertFov * r) / 2)),
            static_cast<float>(height) / (2 * std::tan(vertFov / 2))};
        RGBCameraIntrinsics.height = height;
        RGBCameraIntrinsics.principalPoint = {width / 2.0f, height / 2.0f};
        RGBCameraIntrinsics.rotation =
            visionx::tools::convertEigenMatToVisionX(Eigen::Matrix3f::Identity());
        RGBCameraIntrinsics.translation =
            visionx::tools::convertEigenVecToVisionX(Eigen::Vector3f::Zero());
        RGBCameraIntrinsics.width = width;
 
        visionx::CameraParameters DepthCameraIntrinsics;
        DepthCameraIntrinsics.distortion = {0, 0, 0, 0};
        DepthCameraIntrinsics.focalLength = {
            static_cast<float>(width) / (2 * std::tan((vertFov * r) / 2)),
            static_cast<float>(height) / (2 * std::tan(vertFov / 2))};
        DepthCameraIntrinsics.height = height;
        DepthCameraIntrinsics.principalPoint = {width / 2.0f, height / 2.0f};
        DepthCameraIntrinsics.rotation =
            visionx::tools::convertEigenMatToVisionX(Eigen::Matrix3f::Identity());
        DepthCameraIntrinsics.translation = {0.075, 0, 0};
        DepthCameraIntrinsics.width = width;
 
 
        stereoCalibration.calibrationLeft = visionx::tools::createDefaultMonocularCalibration();
        stereoCalibration.calibrationRight = visionx::tools::createDefaultMonocularCalibration();
        stereoCalibration.calibrationLeft.cameraParam = RGBCameraIntrinsics;
        stereoCalibration.calibrationRight.cameraParam = DepthCameraIntrinsics;
        stereoCalibration.rectificationHomographyLeft =
            visionx::tools::convertEigenMatToVisionX(Eigen::Matrix3f::Zero());
        stereoCalibration.rectificationHomographyRight =
            visionx::tools::convertEigenMatToVisionX(Eigen::Matrix3f::Zero());
 
 
        return stereoCalibration;
    }
 
    bool
    DepthImageProviderDynamicSimulation::getImagesAreUndistorted(const Ice::Current& c)
    {
        return true;
    }
 
    std::string
    DepthImageProviderDynamicSimulation::getReferenceFrame(const Ice::Current& c)
    {
        return getProperty<std::string>("RobotNodeCamera");
    }
 
    void
    DepthImageProviderDynamicSimulation::onInitCapturingPointCloudProvider()
    {
    }
 
    void
    DepthImageProviderDynamicSimulation::onExitCapturingPointCloudProvider()
    {
        ARMARX_TRACE;
        rgbImage.reset();
        dImage.reset();
        dFloatImage.reset();
        imgPtr[0] = nullptr;
        imgPtr[1] = nullptr;
        imgPtrFlt[0] = nullptr;
 
        if (simVisu)
        {
            simVisu->releaseResources();
            getArmarXManager()->removeObjectBlocking(simVisu);
        }
        simVisu = nullptr;
        cameraNode = nullptr;
        visualization = nullptr;
 
        offscreenRenderer.reset();
 
        SoDB::finish();
    }
 
    void
    DepthImageProviderDynamicSimulation::onInitImageProvider()
    {
        ARMARX_TRACE;
        ARMARX_VERBOSE_S << "DepthImageProviderDynamicSimulation::onInitImageProvider";
 
        const auto imgSz = getProperty<visionx::ImageDimension>("ImageSize").getValue();
        height = imgSz.height;
        width = imgSz.width;
 
        if (!floatImageMode)
        {
            setImageFormat(visionx::ImageDimension(imgSz), visionx::eRgb);
            setNumberImages(2);
 
            rgbImage.reset(new CByteImage{
                static_cast<int>(width), static_cast<int>(height), CByteImage::eRGB24});
            dImage.reset(new CByteImage{
                static_cast<int>(width), static_cast<int>(height), CByteImage::eRGB24});
            imgPtr[0] = rgbImage.get();
            imgPtr[1] = dImage.get();
        }
        else
        {
            setImageFormat(visionx::ImageDimension(imgSz), visionx::eFloat1Channel);
            setNumberImages(1);
 
            dFloatImage.reset(
                new CFloatImage{static_cast<int>(width), static_cast<int>(height), 1});
            imgPtrFlt[0] = dFloatImage.get();
        }
 
 
        pointcloud.reset(new pcl::PointCloud<PointT>{static_cast<unsigned int>(width),
                                                     static_cast<unsigned int>(height)});
 
        rgbBuffer.resize(width * height);
        depthBuffer.resize(width * height);
        pointCloudBuffer.resize(width * height);
 
        // init SoDB / Coin3D
        SoDB::init();
 
        VirtualRobot::init(getName());
 
        // needed for SoSelection
        SoInteraction::init();
 
        std::stringstream svName;
        svName << getName() << "_PhysicsWorldVisualization";
        simVisu = Component::create<ArmarXPhysicsWorldVisualization>(
            getIceProperties(), getName() + "_PhysicsWorldVisualization");
        getArmarXManager()->addObject(simVisu);
        ARMARX_VERBOSE_S << "done DepthImageProviderDynamicSimulation::onInitImageProvider";
 
        renderTimesCount = 0;
        sumRenderTimes = 0;
        sumRenderTimesSquared = 0;
    }
 
    void
    DepthImageProviderDynamicSimulation::onDisconnectComponent()
    {
        ARMARX_TRACE;
        ImageProvider::onDisconnectComponent();
        CapturingPointCloudProvider::onDisconnectComponent();
    }
 
    void
    DepthImageProviderDynamicSimulation::onExitImageProvider()
    {
    }
 
    PropertyDefinitionsPtr
    DepthImageProviderDynamicSimulation::createPropertyDefinitions()
    {
        return new DepthImageProviderDynamicSimulationPropertyDefinitions(getConfigIdentifier());
    }
 
    bool
    DepthImageProviderDynamicSimulation::doCapture()
    {
        ARMARX_TRACE;
        bool succeeded = true;
        Ice::Long timestamp;
        try
        {
            timestamp = TimeUtil::HasTimeServer() ? simVisu->getSyncTimestamp()
                                                  : IceUtil::Time::now().toMicroSeconds();
            syncVisu();
            if (!visualization || !visualization)
            {
                return false;
            }
            succeeded = render();
        }
        catch (...)
        {
            handleExceptions();
            succeeded = false;
        }
 
        if (succeeded)
        {
            Eigen::Matrix4f rotate = Eigen::Matrix4f::Identity();
            // rotate to conform to ArmarX standards
            rotate.block(0, 0, 3, 3) =
                Eigen::AngleAxis<float>(M_PI_2, Eigen::Vector3f::UnitZ()).toRotationMatrix();
            pcl::PointCloud<PointT>::Ptr cloudTransformedPtr(new pcl::PointCloud<PointT>());
            pcl::transformPointCloud(*pointcloud, *cloudTransformedPtr, rotate);
            cloudTransformedPtr->header.stamp = timestamp;
            providePointCloud(cloudTransformedPtr);
 
            updateTimestamp(timestamp);
            if (!floatImageMode)
            {
                provideImages(imgPtr);
            }
            else
            {
                provideImages(imgPtrFlt);
            }
 
            if (drawPointCloud && (std::chrono::high_resolution_clock::now() - lastUpdate >
                                   std::chrono::milliseconds{pointCloudDrawDelay}))
            {
                lastUpdate = std::chrono::high_resolution_clock::now();
                DebugDrawer24BitColoredPointCloud dbgPcl;
                dbgPcl.points.reserve(cloudTransformedPtr->size());
 
                dbgPcl.pointSize = pointCloudPointSize;
 
                const auto pointCloudBaseNode =
                    simVisu->getRobot(robotName)->getRobotNode(camNodeName);
 
                if (!pointCloudBaseNode)
                {
                    ARMARX_ERROR_S << deactivateSpam() << robotName << " has no node "
                                   << camNodeName
                                   << "\nthe point cloud will be attached to (0,0,0)";
                }
 
                Eigen::Matrix4f rotate = Eigen::Matrix4f::Identity();
                //                rotate.block(0, 0, 3, 3) = Eigen::AngleAxis<float>(M_PI_2, Eigen::Vector3f::UnitZ()). toRotationMatrix();
                Eigen::Matrix4f transformCamToWorld =
                    (pointCloudBaseNode ? pointCloudBaseNode->getGlobalPose()
                                        : Eigen::Matrix4f::Identity()) *
                    rotate;
 
 
                assert(static_cast<std::size_t>(width) == pointcloud->width);
                assert(static_cast<std::size_t>(height) == pointcloud->height);
                for (std::size_t x = 0; x < static_cast<std::size_t>(width); x += pointSkip)
                {
                    for (std::size_t y = 0; y < static_cast<std::size_t>(height); y += pointSkip)
                    {
                        const auto& pclPoint = cloudTransformedPtr->at(x, y);
 
                        //transform to point in world coord
                        const Eigen::Vector4f pointFromPcl{pclPoint.x, pclPoint.y, pclPoint.z, 1};
 
                        const Eigen::Vector4f pointInWorld = transformCamToWorld * pointFromPcl;
                        const Eigen::Vector4f pointInWorldNormalizedW =
                            pointInWorld / pointInWorld(3);
 
                        DebugDrawer24BitColoredPointCloudElement dbgPoint;
 
                        dbgPoint.x = pointInWorldNormalizedW(0);
                        dbgPoint.y = pointInWorldNormalizedW(1);
                        dbgPoint.z = pointInWorldNormalizedW(2);
 
                        //clip to space
                        if (clipPointCloud &&
                            (dbgPoint.x < clipDrawnCloudXLo || dbgPoint.x > clipDrawnCloudXHi ||
                             dbgPoint.y < clipDrawnCloudYLo || dbgPoint.y > clipDrawnCloudYHi ||
                             dbgPoint.z < clipDrawnCloudZLo || dbgPoint.z > clipDrawnCloudZHi))
                        {
                            continue;
                        }
 
                        dbgPoint.color.r = pclPoint.r;
                        dbgPoint.color.g = pclPoint.g;
                        dbgPoint.color.b = pclPoint.b;
 
                        dbgPcl.points.emplace_back(dbgPoint);
                    }
                }
 
                debugDrawer->set24BitColoredPointCloudVisu(
                    "PointClouds", getName() + "::PointCloud", dbgPcl);
                //                debugDrawer->setPoseVisu("PointClouds", getName() + "::PointCloudBaseNode", new Pose { (pointCloudBaseNode ? pointCloudBaseNode->getGlobalPose() : Eigen::Matrix4f::Identity()) });
            }
        }
        return succeeded;
    }
 
    bool
    DepthImageProviderDynamicSimulation::render()
    {
        ARMARX_TRACE;
        //render images
        if (!cameraNode)
        {
            ARMARX_ERROR << deactivateSpam() << "No camera node:" << camNodeName;
            return false;
        }
 
        bool renderOK = false;
        {
            auto l = simVisu->getScopedLock();
            auto start = IceUtil::Time::now();
 
            ARMARX_TRACE;
            renderOK = VirtualRobot::CoinVisualizationFactory::renderOffscreenRgbDepthPointcloud(
                offscreenRenderer.get(),
                cameraNode,
                visualization,
                width,
                height,
                true,
                rgbBuffer,
                true,
                depthBuffer,
                true,
                pointCloudBuffer,
                zNear,
                zFar,
                vertFov,
                nanValue);
 
            ARMARX_DEBUG << deactivateSpam(1) << "Offscreen rendering took: "
                         << (IceUtil::Time::now() - start).toMilliSecondsDouble() << " ms";
        }
        if (renderOK)
        {
            ARMARX_TRACE;
            //get images + cloud
            std::unique_lock lock(captureMutex);
 
            auto start = std::chrono::high_resolution_clock::now();
 
            auto randValuesSize = randValues.size();
            int noiseOffset = noise > 0 ? rand() % randValuesSize : 0;
 
            float* depthBufferEndOfRow = depthBuffer.data() + width - 1;
            if (floatImageMode)
            {
                float* dFloatImageRow = dFloatImage->pixels;
                for (int y = 0; y < height; y++)
                {
                    for (int x = 0; x < width; x++)
                    {
                        float value = depthBufferEndOfRow[-x];
                        value = std::max(value, 0.0f);
                        value = std::min(value, static_cast<float>(0xffffff));
                        dFloatImageRow[x] = value;
                    }
                    dFloatImageRow += width;
                    depthBufferEndOfRow += width;
                }
            }
            else
            {
                ARMARX_TRACE;
                std::uint8_t* rgbImageRow = rgbImage->pixels;
                std::uint8_t* rgbBufferEndOfRow = rgbBuffer.data() + (width - 1) * 3;
                std::uint8_t* dImageRow = dImage->pixels;
                for (int y = 0; y < height; ++y)
                {
                    for (int x = 0; x < width; ++x)
                    {
                        memcpy(static_cast<void*>(&(rgbImageRow[x * 3])), //dest
                               static_cast<const void*>(&(rgbBufferEndOfRow[-x * 3])), //src
                               3 //count
                        );
                        float valueF = depthBufferEndOfRow[-x];
                        valueF = std::max(0.0f, valueF);
                        valueF = std::min(valueF, static_cast<float>(0xffffff));
                        std::uint32_t value = static_cast<std::uint32_t>(valueF);
                        memcpy(static_cast<void*>(&(dImageRow[x * 3])), //dest
                               static_cast<const void*>(&value), //src
                               3 //count
                        );
                    }
                    rgbImageRow += width * 3;
                    rgbBufferEndOfRow += width * 3;
                    dImageRow += width * 3;
                    depthBufferEndOfRow += width;
                }
            }
 
            ARMARX_TRACE;
            float* randValuesData = randValues.data();
            std::uint8_t* rgbBufferEntry = rgbBuffer.data();
            Eigen::Vector3f* inputRow = pointCloudBuffer.data();
            PointT* outputEndOfRow = pointcloud->points.data() + (width - 1);
            for (int y = 0; y < height; ++y)
            {
                ARMARX_TRACE;
                for (int x = 0; x < width; ++x)
                {
                    PointT& point = outputEndOfRow[-x];
                    Eigen::Vector3f pointCloudBufferPoint = inputRow[x];
 
                    int noiseIndex = (y * width + x) + noiseOffset;
                    float noiseX =
                        noise > 0.0f ? randValuesData[(noiseIndex) % randValuesSize] : 0.0f;
                    float noiseY =
                        noise > 0.0f ? randValuesData[(noiseIndex + 1) % randValuesSize] : 0.0f;
                    float noiseZ =
                        noise > 0.0f ? randValuesData[(noiseIndex + 2) % randValuesSize] : 0.0f;
 
                    point.x = pointCloudBufferPoint[0] + noiseX;
                    point.y = pointCloudBufferPoint[1] + noiseY;
                    point.z = pointCloudBufferPoint[2] + noiseZ;
                    point.r = rgbBufferEntry[0];
                    point.g = rgbBufferEntry[1];
                    point.b = rgbBufferEntry[2];
 
                    rgbBufferEntry += 3;
                }
                inputRow += width;
                outputEndOfRow += width;
            }
 
            auto end = std::chrono::high_resolution_clock::now();
            std::int64_t timeDiff =
                std::chrono::duration_cast<std::chrono::microseconds>(end - start).count();
            sumRenderTimes += timeDiff;
            sumRenderTimesSquared += timeDiff * timeDiff;
            renderTimesCount += 1;
 
            if (renderTimesCount > 256)
            {
                ARMARX_TRACE;
                double averageTime = sumRenderTimes * 1.0e-3 / renderTimesCount;
                double averageSquaredTime = sumRenderTimesSquared * 1.0e-6 / renderTimesCount;
                double stddevTime = std::sqrt(averageSquaredTime - averageTime * averageTime);
 
                ARMARX_DEBUG << "Copying depth buffer data took: " << averageTime << "ms +- "
                             << stddevTime << "ms";
 
                renderTimesCount = 0;
                sumRenderTimes = 0;
                sumRenderTimesSquared = 0;
                renderTimesCount = 0;
            }
        }
        return renderOK;
    }
 
    void
    DepthImageProviderDynamicSimulation::syncVisu()
    {
        ARMARX_TRACE;
        if (simVisu)
        {
            ARMARX_TRACE;
            simVisu->synchronizeVisualizationData();
            if (simVisu->getRobot(robotName))
            {
                ARMARX_TRACE;
                cameraNode = simVisu->getRobot(robotName)->getRobotNode(camNodeName);
            }
            visualization = simVisu->getVisualization();
            if (!cameraNode)
            {
                ARMARX_WARNING << "The camera node is not in the scene";
            }
            if (!visualization)
            {
                ARMARX_WARNING << "No physics visualization scene";
            }
        }
    }
 
} // namespace armarx