OpenCVUtil.cpp

Go to the documentation of this file.

#include "OpenCVUtil.h"
 
#include <cstring>
 
#include <Image/ByteImage.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include <VisionX/interface/components/Calibration.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);
    }
 
}
 
 
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);
    }
}