RCPointCloudProvider.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::RCPointCloudProvider
 * @author     Markus Grotz ( markus dot grotz at kit dot edu )
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "RCPointCloudProvider.h"
 
#include <pcl/common/transforms.h>
 
#include <rc_genicam_api/system.h>
#include <rc_genicam_api/interface.h>
#include <rc_genicam_api/buffer.h>
#include <rc_genicam_api/image.h>
#include <rc_genicam_api/config.h>
#include <rc_genicam_api/pixel_formats.h>
 
#include <VirtualRobot/MathTools.h>
 
#include <VisionX/tools/TypeMapping.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXGui/libraries/RemoteGui/WidgetBuilder.h>
 
 
#include <Calibration/Calibration.h>
#include <Calibration/StereoCalibration.h>
#include <Image/ImageProcessor.h>
#include <stdlib.h>
 
#include <VisionX/libraries/RoboceptionUtility/roboception_constants.h>
 
 
using namespace visionx;
namespace visionx
{
 
    using Point = pcl::PointXYZRGBA;
    using PointCloud = pcl::PointCloud<Point>;
 
    // Our installed version is missing the getColor method
    // From: https://github.com/roboception/rc_genicam_api/blob/f7f1621fc3ac5d638292e3d5d1ea9393959ecd9d/rc_genicam_api/image.cc
    static void rcg_getColor(uint8_t rgb[3], const std::shared_ptr<const rcg::Image>& img,
                             uint32_t ds, uint32_t i, uint32_t k)
    {
        i *= ds;
        k *= ds;
 
        if (img->getPixelFormat() == Mono8) // convert from monochrome
        {
            size_t lstep = img->getWidth() + img->getXPadding();
            const uint8_t* p = img->getPixels() + k * lstep + i;
 
            uint32_t g = 0, n = 0;
 
            for (uint32_t kk = 0; kk < ds; kk++)
            {
                for (uint32_t ii = 0; ii < ds; ii++)
                {
                    g += p[ii];
                    n++;
                }
 
                p += lstep;
            }
 
            rgb[2] = rgb[1] = rgb[0] = static_cast<uint8_t>(g / n);
        }
        else if (img->getPixelFormat() == YCbCr411_8) // convert from YUV
        {
            size_t lstep = (img->getWidth() >> 2) * 6 + img->getXPadding();
            const uint8_t* p = img->getPixels() + k * lstep;
 
            uint32_t r = 0;
            uint32_t g = 0;
            uint32_t b = 0;
            uint32_t n = 0;
 
            for (uint32_t kk = 0; kk < ds; kk++)
            {
                for (uint32_t ii = 0; ii < ds; ii++)
                {
                    uint8_t v[3];
                    rcg::convYCbCr411toRGB(v, p, static_cast<int>(i + ii));
 
                    r += v[0];
                    g += v[1];
                    b += v[2];
                    n++;
                }
 
                p += lstep;
            }
 
            rgb[0] = static_cast<uint8_t>(r / n);
            rgb[1] = static_cast<uint8_t>(g / n);
            rgb[2] = static_cast<uint8_t>(b / n);
        }
    }
 
    // Adapted from: https://github.com/roboception/rc_genicam_api/blob/master/tools/gc_pointcloud.cc
    static void storePointCloud(double f, double t, double scale,
                                std::shared_ptr<const rcg::Image> intensityCombined,
                                std::shared_ptr<const rcg::Image> disp,
                                PointCloud* outPoints)
    {
        // intensityCombined: Top half is the left image, bottom half the right image
        // get size and scale factor between left image and disparity image
 
        size_t width = disp->getWidth();
        size_t height = disp->getHeight();
        bool bigendian = disp->isBigEndian();
        size_t leftWidth = intensityCombined->getWidth();
        size_t ds = (leftWidth + disp->getWidth() - 1) / disp->getWidth();
 
        // convert focal length factor into focal length in (disparity) pixels
 
        f *= width;
 
        // get pointer to disparity data and size of row in bytes
 
        const uint8_t* dps = disp->getPixels();
        size_t dstep = disp->getWidth() * sizeof(uint16_t) + disp->getXPadding();
 
        // count number of valid disparities
 
        int n = 0;
        for (size_t k = 0; k < height; k++)
        {
            int j = 0;
            for (size_t i = 0; i < width; i++)
            {
                if ((dps[j] | dps[j + 1]) != 0)
                {
                    n++;
                }
                j += 2;
            }
 
            dps += dstep;
        }
 
        dps = disp->getPixels();
 
        outPoints->clear();
        outPoints->reserve(height * width);
 
        // create point cloud
 
        for (size_t k = 0; k < height; k++)
        {
            for (size_t i = 0; i < width; i++)
            {
                // convert disparity from fixed comma 16 bit integer into float value
 
                double d;
                if (bigendian)
                {
                    size_t j = i << 1;
                    d = scale * ((dps[j] << 8) | dps[j + 1]);
                }
                else
                {
                    size_t j = i << 1;
                    d = scale * ((dps[j + 1] << 8) | dps[j]);
                }
 
                // if disparity is valid and color can be obtained
 
                if (d > 0)
                {
                    // reconstruct 3D point from disparity value
 
                    double x = (i + 0.5 - 0.5 * width) * t / d;
                    double y = (k + 0.5 - 0.5 * height) * t / d;
                    double z = f * t / d;
 
                    // compute size of reconstructed point
 
                    // double x2=(i-0.5*width)*t/d;
                    // double size=2*1.4*std::abs(x2-x); // unused
 
                    // get corresponding color value
 
                    uint8_t rgb[3] = {0, 0, 0 };
                    // FIXME: Replace with rcg::getColor once available
                    ::rcg_getColor(rgb, intensityCombined, static_cast<uint32_t>(ds), static_cast<uint32_t>(i),
                                   static_cast<uint32_t>(k));
 
                    // store colored point
                    Point point;
                    const double meterToMillimeter = 1000.0f;
                    point.x = x * meterToMillimeter;
                    point.y = y * meterToMillimeter;
                    point.z = z * meterToMillimeter;
                    point.r = rgb[0];
                    point.g = rgb[1];
                    point.b = rgb[2];
                    point.a = 255;
 
                    outPoints->push_back(point);
                }
            }
 
            dps += dstep;
        }
    }
 
    RCPointCloudProvider::RCPointCloudProvider()
        : intensityBuffer(50)
        , disparityBuffer(25)
    {
 
    }
 
    void RCPointCloudProvider::onInitComponent()
    {
        ImageProvider::onInitComponent();
        CapturingPointCloudProvider::onInitComponent();
 
        updateFinalCloudTransform(
            getProperty<float>("PointCloud_Scale_X").getValue(),
            getProperty<float>("PointCloud_Scale_Y").getValue(),
            getProperty<float>("PointCloud_Scale_Z").getValue(),
            getProperty<float>("PointCloud_Rotate_Roll").getValue(),
            getProperty<float>("PointCloud_Rotate_Pitch").getValue(),
            getProperty<float>("PointCloud_Rotate_Yaw").getValue(),
            getProperty<float>("PointCloud_Translate_X").getValue(),
            getProperty<float>("PointCloud_Translate_Y").getValue(),
            getProperty<float>("PointCloud_Translate_Z").getValue());
    }
 
    void RCPointCloudProvider::onConnectComponent()
    {
        ImageProvider::onConnectComponent();
        CapturingPointCloudProvider::onConnectComponent();
        createOrUpdateRemoteGuiTab(buildGui(), [this](armarx::RemoteGui::TabProxy & prx)
        {
            processGui(prx);
        });
    }
 
    void RCPointCloudProvider::onDisconnectComponent()
    {
        ImageProvider::onDisconnectComponent();
        CapturingPointCloudProvider::onDisconnectComponent();
    }
 
    void RCPointCloudProvider::onExitComponent()
    {
        CapturingPointCloudProvider::onExitComponent();
        ImageProvider::onExitComponent();
    }
 
    void RCPointCloudProvider::onInitCapturingPointCloudProvider()
    {
        using namespace visionx::roboception;
 
        if (!initDevice(getProperty<std::string>("DeviceId")))
        {
            getArmarXManager()->asyncShutdown();
            return;
        }
        enableIntensity(false);
        enableIntensityCombined(true);
        enableDisparity(true);
        enableConfidence(false);
        enableError(false);
 
        rcg::setString(nodemap, GC_COMPONENT_SELECTOR, "IntensityCombined", true);
 
        // Color format
        if (rcg::setEnum(nodemap, "PixelFormat", "YCbCr411_8", false))
        {
            ARMARX_INFO << "Enabling color mode";
        }
        else
        {
            ARMARX_INFO << "Falling back to monochrome mode";
        }
 
        setupStreamAndCalibration(
            getProperty<float>("ScaleFactor"),
            "/tmp/armar6_rc_point_cloud_calibration.txt"
        );
 
        std::string depthQuality = getProperty<std::string>("DepthImageResolution").getValue();
        try
        {
            rcg::setEnum(nodemap, GC_DEPTH_QUALITY, depthQuality.c_str(), true);
        }
        catch (std::invalid_argument& e)
        {
            ARMARX_INFO << e.what();
            ARMARX_WARNING << "Selected DepthImageResolution '" << depthQuality.c_str() << "' is currently not available for this device. "
                           << "Falling back to '" << GC_DEPTH_QUALITY_HIGH << "' (640x480@3FPS).";
            rcg::setEnum(nodemap, GC_DEPTH_QUALITY, GC_DEPTH_QUALITY_HIGH, true);
        }
        ARMARX_INFO << "Setting depth quality to " << rcg::getEnum(nodemap, GC_DEPTH_QUALITY) << " (out of Low, Medium, High, Full)";
        frameRate = DepthQualityToFPS.at(rcg::getEnum(nodemap, GC_DEPTH_QUALITY));
 
        frameRate = std::min(frameRate, getProperty<float>("framerate").getValue());
        rcg::setFloat(nodemap, "AcquisitionFrameRate", frameRate);
        ARMARX_INFO << VAROUT(frameRate);
 
        scan3dCoordinateScale = rcg::getFloat(nodemap, "Scan3dCoordinateScale", nullptr, nullptr, true);
 
        setNumberImages(numImages);
        setImageFormat(dimensions, visionx::eRgb, visionx::eBayerPatternRg);
        setPointCloudSyncMode(visionx::eCaptureSynchronization);
    }
 
    void RCPointCloudProvider::onExitCapturingPointCloudProvider()
    {
        cleanupRC();
    }
 
    void RCPointCloudProvider::onStartCapture(float framesPerSecond)
    {
        startRC();
    }
 
    void RCPointCloudProvider::onStopCapture()
    {
        stopRC();
    }
 
    bool RCPointCloudProvider::doCapture()
    {
        const rcg::Buffer* buffer = stream->grab(10000);
        if (!buffer)
        {
            ARMARX_WARNING << deactivateSpam(10) <<  "Unable to get image";
            return false;
        }
 
        // check for a complete image in the buffer
 
        if (buffer->getIsIncomplete() || !buffer->getImagePresent(0))
        {
            ARMARX_INFO << deactivateSpam(1) << "Waiting for image";
            return false;
        }
 
        // store image in the corresponding list
 
        uint64_t pixelformat = buffer->getPixelFormat(0);
        if (pixelformat == Mono8 || pixelformat == YCbCr411_8)
        {
            intensityBuffer.add(buffer, 0);
        }
        else if (pixelformat == Coord3D_C16)
        {
            disparityBuffer.add(buffer, 0);
        }
        else
        {
            ARMARX_WARNING << "Unexpected pixel format: " << pixelformat;
            return false;
        }
        // check if both lists contain an image with the current timestamp
 
        uint64_t timestamp = buffer->getTimestampNS();
 
        std::shared_ptr<const rcg::Image> intensityCombined = intensityBuffer.find(timestamp);
        std::shared_ptr<const rcg::Image> disparity = disparityBuffer.find(timestamp);
 
        if (!intensityCombined || !disparity)
        {
            // Waiting for second synchronized image
            return false;
        }
        updateTimestamp(buffer->getTimestamp());
 
        // compute and store point cloud from synchronized image pair
 
        PointCloud::Ptr pointCloud(new PointCloud());
        storePointCloud(focalLengthFactor, baseline, scan3dCoordinateScale, intensityCombined, disparity, pointCloud.get());
 
 
        // Provide images
        {
            visionx::ImageFormatInfo imageFormat = getImageFormat();
 
            // ARMARX_LOG << "grabbing image " << imageFormat.dimension.width << "x" << imageFormat.dimension.height << " vs " <<  buffer->getWidth() << "x" << buffer->getHeight();
 
            int width = imageFormat.dimension.width;
            int height = imageFormat.dimension.height;
            if (scaleFactor > 1.0)
            {
                width *= scaleFactor;
                height *= scaleFactor;
            }
 
            //const int imageSize = imageFormat.dimension.width * imageFormat.dimension.height * imageFormat.bytesPerPixel;
            const uint8_t* p = intensityCombined->getPixels();
 
            size_t px = intensityCombined->getXPadding();
            uint64_t format = intensityCombined->getPixelFormat();
 
            for (size_t n = 0; n < numImages; n++)
            {
                //fillCameraImageRGB(cameraImages[n]->pixels, n, *intensityCombined);
                fillCameraImageRGB(cameraImages[n]->pixels, n, p, width, height, format, px);
            }
 
 
            if (scaleFactor > 1.0)
            {
                ::ImageProcessor::Resize(cameraImages[0], smallImages[0]);
                ::ImageProcessor::Resize(cameraImages[1], smallImages[1]);
                provideImages(smallImages);
 
                //            armarx::SharedMemoryScopedWriteLockPtr lock(this->ImageProvider::sharedMemoryProvider->getScopedWriteLock());
 
                //            for (size_t i = 0; i < numImages; i++)
                //            {
                //                memcpy(ppImageBuffers[i], smallImages[i]->pixels, imageSize);
                //            }
            }
            else
            {
                provideImages(cameraImages);
                //            for (size_t i = 0; i < numImages; i++)
                //            {
                //                memcpy(ppImageBuffers[i], cameraImages[i]->pixels, imageSize);
                //            }
            }
        }
 
        PointCloud::Ptr transformedPointCloud(new PointCloud());
        const auto& tr = getFinalCloudTransform();
        ARMARX_DEBUG << deactivateSpam(0.5) << "final transform =\n" << tr;
        pcl::transformPointCloud(*pointCloud, *transformedPointCloud, tr);
 
        providePointCloud(transformedPointCloud);
 
        // remove all images from the buffer with the current or an older time stamp
        intensityBuffer.removeOld(timestamp);
        disparityBuffer.removeOld(timestamp);
 
        return true;
    }
 
 
    armarx::PropertyDefinitionsPtr RCPointCloudProvider::createPropertyDefinitions()
    {
        return armarx::PropertyDefinitionsPtr(new RCPointCloudProviderPropertyDefinitions(
                getConfigIdentifier()));
    }
 
 
 
    void visionx::RCPointCloudProvider::startCaptureForNumFrames(Ice::Int, const Ice::Current&)
    {
        throw std::runtime_error("Not yet implemented");
    }
 
 
    void visionx::RCPointCloudProvider::onInitImageProvider()
    {
    }
 
    void visionx::RCPointCloudProvider::onExitImageProvider()
    {
    }
 
    RCPointCloudProviderPropertyDefinitions::RCPointCloudProviderPropertyDefinitions(std::string prefix):
        armarx::ComponentPropertyDefinitions(prefix)
    {
        using namespace visionx::roboception;
 
        defineOptionalProperty<std::string>("DeviceId", "00_14_2d_2c_6e_ed", "");
        defineOptionalProperty<float>("ScaleFactor", 2.0, "Image down scale factor");
        defineOptionalProperty<float>("framerate", 25.0f, "Frames per second, actual framerate is equal to min(this framerate, framerate provided by the device)")
        .setMatchRegex("\\d+(.\\d*)?")
        .setMin(0.0f)
        .setMax(25.0f);
        defineOptionalProperty<bool>("isEnabled", true, "enable the capturing process immediately");
        defineOptionalProperty<std::string>("DepthImageResolution", "640x480@3Fps", "Resolution of the depth image. "
                                            "The higher the resolution, the lower the framerate provided by the device")
        .setCaseInsensitive(true)
        .map("214x160@25Fps",   GC_DEPTH_QUALITY_LOW)
        .map("320x240@15Fps",   GC_DEPTH_QUALITY_MEDIUM)
        .map("640x480@3Fps",    GC_DEPTH_QUALITY_HIGH)
        .map("1280x960@0.8Fps", GC_DEPTH_QUALITY_FULL);
 
        defineOptionalProperty<std::string>("ReferenceFrameName", "Roboception_LeftCamera", "Optional reference frame name.");
 
        defineOptionalProperty<float>("PointCloud_Scale_X", 1.0, "");
        defineOptionalProperty<float>("PointCloud_Scale_Y", 1.0, "");
        defineOptionalProperty<float>("PointCloud_Scale_Z", 1.0, "");
        defineOptionalProperty<float>("PointCloud_Translate_X", 0, "");
        defineOptionalProperty<float>("PointCloud_Translate_Y", 0, "");
        defineOptionalProperty<float>("PointCloud_Translate_Z", 0, "");
        defineOptionalProperty<float>("PointCloud_Rotate_Roll", 0, "");
        defineOptionalProperty<float>("PointCloud_Rotate_Pitch", 0, "");
        defineOptionalProperty<float>("PointCloud_Rotate_Yaw", 0, "");
    }
 
    armarx::RemoteGui::WidgetPtr RCPointCloudProvider::buildGui()
    {
        const auto& srt = finalCloudTransform.getWriteBuffer();;
        return armarx::RemoteGui::makeSimpleGridLayout().cols(5)
               .addTextLabel("Scale x/y/z")
               .addChild(armarx::RemoteGui::makeFloatSpinBox("sx").decimals(4).min(0).max(1000).value(srt(0)))
               .addChild(armarx::RemoteGui::makeFloatSpinBox("sy").decimals(4).min(0).max(1000).value(srt(1)))
               .addChild(armarx::RemoteGui::makeFloatSpinBox("sz").decimals(4).min(0).max(1000).value(srt(2)))
               .addChild(new armarx::RemoteGui::HSpacer)
 
               .addTextLabel("Roll / Pitch / Yaw")
               .addChild(armarx::RemoteGui::makeFloatSpinBox("rx").value(srt(3)).min(-M_PI).max(M_PI))
               .addChild(armarx::RemoteGui::makeFloatSpinBox("ry").value(srt(4)).min(-M_PI).max(M_PI))
               .addChild(armarx::RemoteGui::makeFloatSpinBox("rz").value(srt(5)).min(-M_PI).max(M_PI))
               .addChild(new armarx::RemoteGui::HSpacer)
 
               .addTextLabel("Translate x/y/z")
               .addChild(armarx::RemoteGui::makeFloatSpinBox("tx").value(srt(6)).min(-1000).max(1000))
               .addChild(armarx::RemoteGui::makeFloatSpinBox("ty").value(srt(7)).min(-1000).max(1000))
               .addChild(armarx::RemoteGui::makeFloatSpinBox("tz").value(srt(8)).min(-1000).max(1000))
               .addChild(new armarx::RemoteGui::HSpacer);
    }
    void RCPointCloudProvider::processGui(armarx::RemoteGui::TabProxy& prx)
    {
        prx.receiveUpdates();
        updateFinalCloudTransform(
            prx.getValue<float>("sx").get(),
            prx.getValue<float>("sy").get(),
            prx.getValue<float>("sz").get(),
            prx.getValue<float>("rx").get(),
            prx.getValue<float>("ry").get(),
            prx.getValue<float>("rz").get(),
            prx.getValue<float>("tx").get(),
            prx.getValue<float>("ty").get(),
            prx.getValue<float>("tz").get());
    }
 
    void RCPointCloudProvider::updateFinalCloudTransform(
        float sx, float sy, float sz,
        float rx, float ry, float rz,
        float tx, float ty, float tz)
    {
        finalCloudTransform.getWriteBuffer()(0) = sx;
        finalCloudTransform.getWriteBuffer()(1) = sy;
        finalCloudTransform.getWriteBuffer()(2) = sz;
 
        finalCloudTransform.getWriteBuffer()(3) = rx;
        finalCloudTransform.getWriteBuffer()(4) = ry;
        finalCloudTransform.getWriteBuffer()(5) = rz;
 
        finalCloudTransform.getWriteBuffer()(6) = tx;
        finalCloudTransform.getWriteBuffer()(7) = ty;
        finalCloudTransform.getWriteBuffer()(8) = tz;
 
        finalCloudTransform.commitWrite();
    }
 
    Eigen::Matrix4f RCPointCloudProvider::getFinalCloudTransform()
    {
        const auto& srt = finalCloudTransform.getUpToDateReadBuffer();
        return VirtualRobot::MathTools::posrpy2eigen4f(srt(6), srt(7), srt(8), srt(4), srt(5), srt(6)) *
               Eigen::Vector4f{srt(0), srt(1), srt(2), 1.f}.asDiagonal();
    }
}