RoboceptionUser.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::RoboceptionUser
 * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * @date       2019
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "RoboceptionUser.h"
 
#include <stdlib.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
 
#include <VisionX/tools/ImageUtil.h>
#include <VisionX/tools/TypeMapping.h>
 
#include <Calibration/Calibration.h>
#include <Calibration/StereoCalibration.h>
#include <Image/ImageProcessor.h>
#include <rc_genicam_api/buffer.h>
#include <rc_genicam_api/config.h>
#include <rc_genicam_api/image.h>
#include <rc_genicam_api/interface.h>
#include <rc_genicam_api/pixel_formats.h>
#include <rc_genicam_api/system.h>
 
 
using namespace armarx;
 
namespace
{
 
    const char* const GC_COMPONENT_ENABLE = "ComponentEnable";
    const char* const GC_COMPONENT_SELECTOR = "ComponentSelector";
 
} // namespace
 
bool
visionx::RoboceptionUser::initDevice(const std::string& dname)
{
    if (!getenv("GENICAM_GENTL64_PATH"))
    {
        setenv("GENICAM_GENTL64_PATH", "/usr/lib/rc_genicam_api/", 0);
    }
    dev = rcg::getDevice(dname.c_str());
    if (!dev)
    {
        //this randomly gets added by the rc driver
        const auto dname2 = "devicemodul" + dname;
        dev = rcg::getDevice(dname2.c_str());
    }
 
    if (dev)
    {
        ARMARX_INFO << "Connecting to device: " << dname;
        dev->open(rcg::Device::EXCLUSIVE);
        nodemap = dev->getRemoteNodeMap();
        return true;
    }
    ARMARX_WARNING << "Unable to connect to device: " << dname;
 
    std::vector<std::shared_ptr<rcg::Device>> devices = rcg::getDevices();
    ARMARX_INFO << "found " << devices.size() << " devices";
 
    for (const rcg::DevicePtr& d : devices)
    {
        if (dname == d->getID())
        {
            ARMARX_ERROR << "Device: " << d->getID() << " !!!(THIS IS THE DEVICE YOU REQUESTED)!!!";
        }
        else
        {
            ARMARX_INFO << "Device: " << d->getID();
        }
        ARMARX_INFO << "Vendor: " << d->getVendor();
        ARMARX_INFO << "Model: " << d->getModel();
        ARMARX_INFO << "TL type: " << d->getTLType();
        ARMARX_INFO << "Display name: " << d->getDisplayName();
        ARMARX_INFO << "Access status:" << d->getAccessStatus();
        ARMARX_INFO << "User name: " << d->getUserDefinedName();
        ARMARX_INFO << "Serial number: " << d->getSerialNumber();
        ARMARX_INFO << "Version: " << d->getVersion();
        ARMARX_INFO << "TS Frequency: " << d->getTimestampFrequency();
    }
 
    ARMARX_ERROR << "Please specify the device id. Aborting";
 
    return false;
}
 
void
visionx::RoboceptionUser::enableIntensity(bool enabled)
{
    rcg::setString(nodemap, GC_COMPONENT_SELECTOR, "Intensity", true);
    rcg::setString(nodemap, GC_COMPONENT_ENABLE, enabled ? "1" : "0", true);
}
 
void
visionx::RoboceptionUser::enableIntensityCombined(bool enabled)
{
    rcg::setString(nodemap, GC_COMPONENT_SELECTOR, "IntensityCombined", true);
    rcg::setString(nodemap, GC_COMPONENT_ENABLE, enabled ? "1" : "0", true);
}
 
void
visionx::RoboceptionUser::enableDisparity(bool enabled)
{
    rcg::setString(nodemap, GC_COMPONENT_SELECTOR, "Disparity", true);
    rcg::setString(nodemap, GC_COMPONENT_ENABLE, enabled ? "1" : "0", true);
}
 
void
visionx::RoboceptionUser::enableConfidence(bool enabled)
{
    rcg::setString(nodemap, GC_COMPONENT_SELECTOR, "Confidence", true);
    rcg::setString(nodemap, GC_COMPONENT_ENABLE, enabled ? "1" : "0", true);
}
 
void
visionx::RoboceptionUser::enableError(bool enabled)
{
    rcg::setString(nodemap, GC_COMPONENT_SELECTOR, "Error", true);
    rcg::setString(nodemap, GC_COMPONENT_ENABLE, enabled ? "1" : "0", true);
}
 
void
visionx::RoboceptionUser::setupStreamAndCalibration(float scalefactor,
                                                    const std::string& calibSavePath)
{
    numImages = 2;
 
    if (!rcg::setBoolean(nodemap, "GevIEEE1588", true))
    {
        ARMARX_WARNING << "Could not activate Precision Time Protocol (PTP) client!";
    }
 
    dimensions.height = Ice::Int(rcg::getInteger(nodemap, "Height") / 2.0);
    dimensions.width = Ice::Int(rcg::getInteger(nodemap, "Width"));
    ARMARX_INFO << VAROUT(dimensions.height) << VAROUT(dimensions.width);
 
    baseline = rcg::getFloat(nodemap, "Baseline", nullptr, nullptr, true);
    focalLengthFactor = rcg::getFloat(nodemap, "FocalLengthFactor", nullptr, nullptr, false);
    float focalLength = float(focalLengthFactor * dimensions.width);
    ARMARX_INFO << VAROUT(focalLengthFactor) << "\t" << VAROUT(focalLength);
 
 
    ARMARX_INFO << "Get stream";
    {
        std::vector<rcg::StreamPtr> availableStreams = dev->getStreams();
 
        for (const rcg::StreamPtr& s : availableStreams)
        {
            ARMARX_INFO << "Stream: " << s->getID();
        }
 
        if (!availableStreams.size())
        {
            ARMARX_FATAL << "Unable to open stream";
            return;
        }
        else
        {
            this->stream = availableStreams[0];
        }
    }
 
 
    cameraImages = new CByteImage*[numImages];
    for (size_t i = 0; i < numImages; i++)
    {
        cameraImages[i] = new CByteImage(dimensions.width, dimensions.height, CByteImage::eRGB24);
    }
 
    scaleFactor = scalefactor;
    if (scaleFactor > 1.0)
    {
        dimensions.height /= scaleFactor;
        dimensions.width /= scaleFactor;
        focalLength /= scaleFactor;
    }
 
    smallImages = new CByteImage*[numImages];
    for (size_t i = 0; i < numImages; i++)
    {
        smallImages[i] = new CByteImage(dimensions.width, dimensions.height, CByteImage::eRGB24);
    }
 
    CCalibration c;
    c.SetCameraParameters(focalLength,
                          focalLength,
                          dimensions.width / 2.0,
                          dimensions.height / 2.0,
                          0.0,
                          0.0,
                          0.0,
                          0.0,
                          Math3d::unit_mat,
                          Math3d::zero_vec,
                          dimensions.width,
                          dimensions.height);
 
    CStereoCalibration ivtStereoCalibration;
    ivtStereoCalibration.SetSingleCalibrations(c, c);
    ivtStereoCalibration.rectificationHomographyRight = Math3d::unit_mat;
    ivtStereoCalibration.rectificationHomographyLeft = Math3d::unit_mat;
 
    Vec3d right_translation;
    right_translation.x = baseline * -1000.0;
    right_translation.y = 0.0;
    right_translation.z = 0.0;
    ivtStereoCalibration.SetExtrinsicParameters(
        Math3d::unit_mat, Math3d::zero_vec, Math3d::unit_mat, right_translation, false);
 
    ARMARX_INFO << "Saving calibration to: " << calibSavePath;
    ivtStereoCalibration.SaveCameraParameters(calibSavePath.c_str());
    stereoCalibration = visionx::tools::convert(ivtStereoCalibration);
}
 
void
visionx::RoboceptionUser::startRC()
{
    ARMARX_INFO << "Start streaming Roboception Images";
    // rcg::setString(nodemap, "AcquisitionFrameRate", std::to_string(framesPerSecond), true);
    stream->open();
    stream->startStreaming();
}
 
void
visionx::RoboceptionUser::stopRC()
{
    ARMARX_INFO << "Stop streaming Roboception Images";
    stream->stopStreaming();
    stream->close();
}
 
void
visionx::RoboceptionUser::cleanupRC()
{
    if (stream)
    {
        stream->stopStreaming();
        stream->close();
    }
 
    if (dev)
    {
        dev->close();
    }
 
    for (size_t i = 0; i < numImages; i++)
    {
        delete cameraImages[i];
        delete smallImages[i];
    }
 
    delete[] cameraImages;
    delete[] smallImages;
}
 
void
visionx::RoboceptionUser::printNodeMap() const
{
    GenApi::NodeList_t nodeList;
    nodemap->_GetNodes(nodeList);
    ARMARX_INFO << "RC config nodes";
    for (const auto& node : nodeList)
    {
        ARMARX_INFO << "---- properties of node '" << node->GetName(true) << '\'';
        GENICAM_NAMESPACE::gcstring_vector properties;
        node->GetPropertyNames(properties);
        for (const auto& string : properties)
        {
            ARMARX_INFO << "-------- '" << string << '\'';
        }
    }
}
 
void
visionx::RoboceptionUser::fillCameraImageRGB(unsigned char* outputPixels,
                                             size_t nImage,
                                             const uint8_t* inputPixels,
                                             size_t width,
                                             size_t height,
                                             uint64_t pixelFormat,
                                             size_t xpadding)
{
    const unsigned char* p = inputPixels;
 
    switch (pixelFormat)
    {
        case PfncFormat::Mono8:
        case PfncFormat::Confidence8:
        case Error8:
        {
            unsigned char const* row = p + (nImage * height) * (width + xpadding);
            unsigned char* outRow = outputPixels;
            for (size_t j = 0; j < height; j++)
            {
                for (size_t i = 0; i < width; i++)
                {
                    const unsigned char c = row[i];
                    outRow[3 * i + 0] = c;
                    outRow[3 * i + 1] = c;
                    outRow[3 * i + 2] = c;
                }
 
                p += xpadding;
                outRow += width * 3;
                row += width + xpadding;
            }
        }
        break;
        case PfncFormat::YCbCr411_8:
        {
            // Compression: 4 pixel are encoded in 6 bytes
            size_t inStride = width * 6 / 4 + xpadding;
            unsigned char const* row = p + (nImage * height) * inStride;
            unsigned char* outRow = outputPixels;
            uint8_t rgb[3];
            for (size_t j = 0; j < height; j++)
            {
                for (size_t i = 0; i < width; i++)
                {
                    rcg::convYCbCr411toRGB(rgb, row, int(i));
                    outRow[i * 3 + 0] = rgb[0];
                    outRow[i * 3 + 1] = rgb[1];
                    outRow[i * 3 + 2] = rgb[2];
                }
 
                row += inStride;
                outRow += width * 3;
            }
        }
        break;
        default:
            ARMARX_INFO << "Unexpected pixel format: " << int(pixelFormat);
            break;
    }
}
 
void
visionx::RoboceptionUser::fillCameraImageRGB(unsigned char* outputPixels,
                                             size_t nImage,
                                             const rcg::Image& inputRgb)
{
    fillCameraImageRGB(outputPixels,
                       nImage,
                       inputRgb.getPixels(),
                       inputRgb.getWidth(),
                       inputRgb.getHeight(),
                       inputRgb.getPixelFormat(),
                       inputRgb.getXPadding());
}
 
void
visionx::RoboceptionUser::fillCameraImageDepth(unsigned char* outputPixels,
                                               double f,
                                               double t,
                                               double scale,
                                               const rcg::Image& disparity,
                                               size_t upscale)
{
    ARMARX_CHECK(upscale == 1 || upscale == 2)
        << "Upscaling by factors other than 1 or 2 is not implemented.";
 
    // 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 = disparity.getWidth();
    size_t height = disparity.getHeight();
    bool bigendian = disparity.isBigEndian();
 
    // 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 = disparity.getPixels();
 
    ARMARX_CHECK_EQUAL(disparity.getPixelFormat(), Coord3D_C16);
    size_t dstep = disparity.getWidth() * sizeof(uint16_t) + disparity.getXPadding();
 
 
    // Fill depth image.
 
    disparity.getXPadding();
    unsigned char* outRow = outputPixels;
 
    for (size_t j = 0; j < height; j++)
    {
        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]);
            }
 
            unsigned char* pixel = &outRow[3 * i * upscale];
 
            // If disparity is valid
            if (d > 0)
            {
                // Reconstruct depth from disparity value
                double z = f * t / d;
 
                // Store depth
                const double meterToMillimeter = 1000.0;
                unsigned int depthMM = static_cast<unsigned int>(std::round(z * meterToMillimeter));
 
                visionx::tools::depthValueToRGB(depthMM, pixel[0], pixel[1], pixel[2]);
            }
            else
            {
                pixel[0] = 0;
                pixel[1] = 0;
                pixel[2] = 0;
            }
 
            if (upscale == 2)
            {
                // Copy the pixel inside this row.
                unsigned char* nextPixel = pixel + 3;
                std::memcpy(nextPixel, pixel, 3);
            }
        }
 
        if (upscale == 2)
        {
            // Copy the entire row.
            unsigned char* nextRow = outRow + width * 3 * upscale;
            std::memcpy(nextRow, outRow, width * 3 * upscale);
            outRow = nextRow;
        }
 
        dps += dstep;
        outRow += width * 3 * upscale;
    }
}