OpenCVUtil.cpp

Go to the documentation of this file.

#include "OpenCVUtil.h"
 
#include <cstring>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include <VisionX/interface/components/Calibration.h>
 
#include <Image/ByteImage.h>
 
void
visionx::copyCvMatToIVT(cv::Mat const& input, CByteImage* output)
{
    ARMARX_CHECK_NOT_NULL(input.data);
    ARMARX_CHECK_NOT_NULL(output);
    ARMARX_CHECK_EXPRESSION(input.isContinuous());
    ARMARX_CHECK_EQUAL(int(input.elemSize()), output->bytesPerPixel);
 
    int imageSize = output->width * output->height * output->bytesPerPixel;
    ARMARX_CHECK_EQUAL(int(input.total() * input.elemSize()), imageSize);
 
    std::memcpy(output->pixels, input.data, imageSize);
}
 
void
visionx::copyCvDepthToGrayscaleIVT(cv::Mat const& inputInMeters, CByteImage* output)
{
    ARMARX_CHECK_NOT_NULL(inputInMeters.data);
    ARMARX_CHECK_NOT_NULL(output);
    ARMARX_CHECK_EQUAL(inputInMeters.type(), CV_32FC1);
    ARMARX_CHECK_EQUAL(output->type, CByteImage::eRGB24);
    ARMARX_CHECK_EQUAL(inputInMeters.rows, output->height);
    ARMARX_CHECK_EQUAL(inputInMeters.cols, output->width);
 
    uchar* outRow = output->pixels;
    int outStride = 3 * output->width;
 
    double minD, maxD;
    cv::minMaxLoc(inputInMeters, &minD, &maxD);
 
    float invRangeD = 1.0 / (maxD - minD);
 
    for (int y = 0; y < inputInMeters.rows; ++y)
    {
        float const* depthRow = inputInMeters.ptr<float>(y);
        for (int x = 0; x < inputInMeters.cols; ++x)
        {
            float depth = depthRow[x];
            float normalizedDepth = (depth - minD) * invRangeD;
            int pixelValue = std::floor(255.0f - 255.0f * normalizedDepth);
 
            outRow[3 * x + 0] = pixelValue;
            outRow[3 * x + 1] = pixelValue;
            outRow[3 * x + 2] = pixelValue;
        }
 
        outRow += outStride;
    }
}
 
cv::Mat
visionx::convertWeirdArmarXToDepthInMeters(CByteImage const* input)
{
    ARMARX_CHECK_NOT_NULL(input);
    ARMARX_CHECK_EQUAL(input->type, CByteImage::eRGB24);
 
    int width = input->width;
    int height = input->height;
    cv::Mat result(height, width, CV_32FC1);
 
    convertWeirdArmarXToDepthInMeters(input->pixels, result);
 
    return result;
}
 
void
visionx::convertWeirdArmarXToDepthInMeters(const uint8_t* input, cv::Mat& output)
{
    ARMARX_CHECK_EQUAL(output.type(), CV_32FC1);
 
    const unsigned char* inputRow = input;
    for (int row = 0; row < output.rows; ++row)
    {
        float* outputRow = output.ptr<float>(row);
        for (int col = 0; col < output.cols; ++col)
        {
            // Depth is stored in mm (lower 8 bits in r, higher 8 bits g)
            int r = inputRow[col * 3 + 0];
            int g = inputRow[col * 3 + 1];
            int depthInMM = (g << 8) + r;
            outputRow[col] = static_cast<float>(depthInMM) / 1000.0f;
        }
 
        inputRow += output.cols * 3;
    }
}
 
void
visionx::convertDepthInMetersToWeirdArmarX(const cv::Mat& input, CByteImage* output)
{
    ARMARX_CHECK_NOT_NULL(output);
    ARMARX_CHECK_EQUAL(output->type, CByteImage::eRGB24);
    ARMARX_CHECK_EQUAL(output->width, input.cols);
    ARMARX_CHECK_EQUAL(output->height, input.rows);
    ARMARX_CHECK_EQUAL(CV_32FC1, input.type());
 
    const auto height = output->height;
    const auto width = output->width;
 
    unsigned char* outputRow = output->pixels;
    for (int y = 0; y < height; ++y)
    {
        const float* inputRow = input.ptr<float>(y);
        for (int x = 0; x < width; ++x)
        {
            const auto depthInMM = inputRow[x] * 1000;
            // Depth is stored in mm (lower 8 bits in r, higher 8 bits g)
            outputRow[x * 3 + 0] = static_cast<unsigned char>(std::fmod(depthInMM, 256));
            outputRow[x * 3 + 1] = static_cast<unsigned char>(std::fmod(depthInMM / 256, 256));
            outputRow[x * 3 + 2] =
                static_cast<unsigned char>(std::fmod(depthInMM / 256 / 256, 256));
        }
 
        inputRow += width * 3;
    }
}
 
namespace detail
{
    template <typename Float>
    void
    fillEmptyValues(cv::Mat& output)
    {
        output.at<Float>(0, 1) = 0;
        output.at<Float>(1, 0) = 0;
        output.at<Float>(2, 0) = 0;
        output.at<Float>(2, 1) = 0;
        output.at<Float>(2, 2) = 1;
    }
 
    void
    checkFloatMatrix(const cv::Mat& matrix, const std::string& semanticName)
    {
        if (!(matrix.depth() == CV_32F || matrix.depth() == CV_64F))
        {
            std::stringstream ss;
            ss << "Passed " << semanticName << " must be floating point type, "
               << "i.e. CV_32F (" << CV_32F << ") or CV_64F (" << CV_64F << "), "
               << "but its type was " << matrix.type() << ".";
            throw std::invalid_argument(ss.str());
        }
    }
 
    void
    checkCameraMatrix(const cv::Mat& matrix)
    {
        checkFloatMatrix(matrix, "camera matrix");
        ARMARX_CHECK_GREATER_EQUAL(matrix.rows, 3);
        ARMARX_CHECK_GREATER_EQUAL(matrix.cols, 3);
    }
 
    template <typename Float>
    void
    fillCameraMatrix(const CCalibration::CCameraParameters& ivtCameraParameters, cv::Mat& output)
    {
        checkCameraMatrix(output);
 
        fillEmptyValues<Float>(output);
 
        output.at<Float>(0, 0) = Float(ivtCameraParameters.focalLength.x);
        output.at<Float>(0, 2) = Float(ivtCameraParameters.principalPoint.x);
        output.at<Float>(1, 1) = Float(ivtCameraParameters.focalLength.y);
        output.at<Float>(1, 2) = Float(ivtCameraParameters.principalPoint.y);
    }
 
    template <typename Float>
    void
    fillCameraMatrix(const visionx::CameraParameters& cameraParams, cv::Mat& output)
    {
        checkCameraMatrix(output);
 
        ARMARX_CHECK_EQUAL(cameraParams.focalLength.size(), 2);
        ARMARX_CHECK_EQUAL(cameraParams.principalPoint.size(), 2);
 
        const float nodalPointX = cameraParams.principalPoint.at(0);
        const float nodalPointY = cameraParams.principalPoint.at(1);
        const float focalLengthX = cameraParams.focalLength.at(0);
        const float focalLengthY = cameraParams.focalLength.at(1);
 
        fillEmptyValues<Float>(output);
 
        output.at<Float>(0, 0) = Float(focalLengthX);
        output.at<Float>(0, 2) = Float(nodalPointX);
        output.at<Float>(1, 1) = Float(focalLengthY);
        output.at<Float>(1, 2) = Float(nodalPointY);
    }
 
} // namespace detail
 
void
visionx::fillCameraMatrix(const visionx::CameraParameters& cameraParams, cv::Mat& output)
{
    detail::checkCameraMatrix(output);
    if (output.depth() == CV_32F)
    {
        detail::fillCameraMatrix<float>(cameraParams, output);
    }
    else if (output.depth() == CV_64F)
    {
        detail::fillCameraMatrix<double>(cameraParams, output);
    }
}
 
void
visionx::fillCameraMatrix(const CCalibration::CCameraParameters& ivtCameraParameters,
                          cv::Mat& output)
{
    detail::checkCameraMatrix(output);
    if (output.depth() == CV_32F)
    {
        detail::fillCameraMatrix<float>(ivtCameraParameters, output);
    }
    else if (output.depth() == CV_64F)
    {
        detail::fillCameraMatrix<double>(ivtCameraParameters, output);
    }
}