AzureKinectIRImageProvider.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::AzureKinectIRImageProvider
 * @author     Mirko Wächter
 * @author     Christian R. G. Dreher <c.dreher@kit.edu>
 * @date       2019
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#include "AzureKinectIRImageProvider.h"
 
 
// IVT
#include <Calibration/Calibration.h>
#include <Image/ImageProcessor.h>
 
// ArmarX
#include <ArmarXCore/core/time/StopWatch.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/observers/variant/Variant.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
// VisionX
#include <VisionX/libraries/imrec/helper.h>
#include <VisionX/tools/ImageUtil.h>
#include <VisionX/tools/TypeMapping.h>
 
namespace
{
    /**
     * @brief Converts a k4a image to an IVT image.
     * @param color_image
     * @param result
     */
    void
    k4aToIvtImage(const k4a::image& color_image, ::CByteImage& result)
    {
        auto cw = static_cast<unsigned int>(color_image.get_width_pixels());
        auto ch = static_cast<unsigned int>(color_image.get_height_pixels());
        ARMARX_CHECK_EQUAL(static_cast<unsigned int>(result.width), cw);
        ARMARX_CHECK_EQUAL(static_cast<unsigned int>(result.height), ch);
        auto color_buffer = reinterpret_cast<const unsigned char*>(color_image.get_buffer());
        auto rgb_buffer_ivt = result.pixels;
 
        int index_ivt = 0;
        int index_k4a = 0;
        for (unsigned int y = 0; y < ch; ++y)
        {
            for (unsigned int x = 0; x < cw; ++x)
            {
                rgb_buffer_ivt[index_ivt] =
                    float(color_buffer[index_k4a] + color_buffer[index_k4a + 1]) / 2.f;
                index_ivt += 1;
                index_k4a += 2;
            }
        }
    }
} // namespace
 
namespace visionx
{
 
    AzureKinectIRImageProviderPropertyDefinitions::AzureKinectIRImageProviderPropertyDefinitions(
        std::string prefix) :
        visionx::CapturingPointCloudProviderPropertyDefinitions(prefix)
    {
    }
 
    armarx::PropertyDefinitionsPtr
    AzureKinectIRImageProvider::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr defs(
            new AzureKinectIRImageProviderPropertyDefinitions(getConfigIdentifier()));
        return defs;
    }
 
    void
    AzureKinectIRImageProvider::onStartCapture(float /* framesPerSecond */)
    {
        // Check for devices.
        const uint32_t DEVICE_COUNT = k4a::device::get_installed_count();
        if (DEVICE_COUNT == 0)
        {
            getArmarXManager()->asyncShutdown();
            throw armarx::LocalException("No Azure Kinect devices detected!");
        }
 
        device = k4a::device::open(K4A_DEVICE_DEFAULT);
        k4aCalibration = device.get_calibration(config.depth_mode, config.color_resolution);
 
        transformation = k4a::transformation(k4aCalibration);
 
        device.start_cameras(&config);
 
        setMetaInfo("serialNumber", new armarx::Variant(device.get_serialnum()));
        setMetaInfo("rgbVersion", new armarx::Variant(VersionToString(device.get_version().rgb)));
        setMetaInfo("depthVersion",
                    new armarx::Variant(VersionToString(device.get_version().depth)));
        setMetaInfo("depthSensorVersion",
                    new armarx::Variant(VersionToString(device.get_version().depth_sensor)));
        setMetaInfo("audioVersion",
                    new armarx::Variant(VersionToString(device.get_version().audio)));
    }
 
    bool
    AzureKinectIRImageProvider::capture(void** pp_image_buffers)
    {
        using armarx::core::time::StopWatch;
 
        k4a::capture capture;
        const std::chrono::milliseconds TIMEOUT{1000};
 
        bool status;
        try
        {
            status = device.get_capture(&capture, TIMEOUT);
        }
        catch (const std::exception&)
        {
            ARMARX_WARNING << "Failed to get capture from device.  Restarting camera.";
            StopWatch sw;
            device.stop_cameras();
            device.start_cameras(&config);
            ARMARX_INFO << "Restarting took " << sw.stop() << ".";
            return false;
        }
 
        if (status)
        {
            setMetaInfo("temperature", new armarx::Variant(capture.get_temperature_c()));
 
            const k4a::image irImage = capture.get_ir_image();
            CByteImage buffer(depthDim.first, depthDim.second, CByteImage::eGrayScale);
            k4aToIvtImage(irImage, buffer);
 
            std::memcpy(pp_image_buffers[0], buffer.pixels, 1024 * 1024);
            return true;
        }
        return false;
    }
 
    std::string
    AzureKinectIRImageProvider::getDefaultName() const
    {
        return "AzureKinectIRImageProvider";
    }
 
    void
    AzureKinectIRImageProvider::onInitCapturingImageProvider()
    {
        config = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
        auto fps = getProperty<float>("framerate").getValue();
        if (fps == 5.0f)
        {
            config.camera_fps = K4A_FRAMES_PER_SECOND_5;
        }
        else if (fps == 15.0f)
        {
            config.camera_fps = K4A_FRAMES_PER_SECOND_15;
        }
        else if (fps == 30.0f)
        {
            config.camera_fps = K4A_FRAMES_PER_SECOND_30;
        }
        else
        {
            throw armarx::LocalException("Invalid framerate: ")
                << fps << " - Only framerates 5, 15 and 30 are "
                << "supported by Azure Kinect.";
        }
        frameRate = fps;
        config.depth_mode = K4A_DEPTH_MODE_PASSIVE_IR;
        config.color_format = K4A_IMAGE_FORMAT_COLOR_BGRA32;
        config.color_resolution = K4A_COLOR_RESOLUTION_1080P;
 
        // This means that we'll only get captures that have both color and depth images, so we don't
        // need to check if the capture contains a particular type of image.
        config.synchronized_images_only = true;
 
        setNumberImages(1);
        depthDim = GetDepthDimensions(config.depth_mode);
        auto color_dim = GetColorDimensions(config.color_resolution);
        ARMARX_INFO << "Depth image size: " << depthDim.first << "x" << depthDim.second;
        ARMARX_INFO << "Color image size: " << color_dim.first << "x" << color_dim.second;
 
        setImageFormat(visionx::ImageDimension(depthDim.first, depthDim.second),
                       visionx::eGrayScale,
                       visionx::eBayerPatternGr);
        resultIRImage.reset(visionx::tools::createByteImage(getImageFormat(), visionx::eRgb));
    }
 
    void
    AzureKinectIRImageProvider::onExitCapturingImageProvider()
    {
    }
 
    void
    AzureKinectIRImageProvider::onStopCapture()
    {
    }
 
    std::string
    AzureKinectIRImageProvider::getReferenceFrame(const Ice::Current& current)
    {
        return std::string();
    }
 
    bool
    AzureKinectIRImageProvider::getImagesAreUndistorted(const Ice::Current& current)
    {
        return false;
    }
 
    StereoCalibration
    AzureKinectIRImageProvider::getStereoCalibration(const Ice::Current&)
    {
        using namespace Eigen;
        using Matrix3fRowMajor = Matrix<float, 3, 3, StorageOptions::RowMajor>;
        using visionx::tools::convertEigenMatToVisionX;
 
        const auto convert_calibration =
            [](const k4a_calibration_camera_t& k4a_calib, float scale = 1.)
        {
            MonocularCalibration calibration;
 
            const k4a_calibration_intrinsic_parameters_t& params = k4a_calib.intrinsics.parameters;
            calibration.cameraParam.principalPoint = {params.param.cx * scale,
                                                      params.param.cy * scale};
            calibration.cameraParam.focalLength = {params.param.fx * scale,
                                                   params.param.fy * scale};
            // TODO: Figure out convertions.  IVT (Calibration.h) expects 4 parameters:
            //  - The first radial lens distortion parameter.
            //  - The second radial lens distortion parameter.
            //  - The first tangential lens distortion parameter.
            //  - The second tangential lens distortion parameter.
            // However, the Kinect offers k1-k6 radial distortion coefficients and 2 p1-p2
            // tangential distortion parameters, which means that k3-k6 are unused.
            // It is even unclear whether this is correct now, as previously it was a vector of all
            // 6 k-params, which resulted in failed assertions in TypeMapping.cpp in the function
            // CStereoCalibration* visionx::tools::convert(const visionx::StereoCalibration& stereoCalibration)
            // lin 314 at time of this commit.
            // See: https://microsoft.github.io/Azure-Kinect-Sensor-SDK/master/structk4a__calibration__intrinsic__parameters__t_1_1__param.html
            calibration.cameraParam.distortion = {
                params.param.k1,
                params.param.k2,
                params.param.p1,
                params.param.p2,
            };
            calibration.cameraParam.width = k4a_calib.resolution_width;
            calibration.cameraParam.height = k4a_calib.resolution_height;
            const Matrix3fRowMajor ROTATION =
                Map<const Matrix3fRowMajor>{k4a_calib.extrinsics.rotation};
            calibration.cameraParam.rotation = convertEigenMatToVisionX(ROTATION);
            calibration.cameraParam.translation = {k4a_calib.extrinsics.translation,
                                                   k4a_calib.extrinsics.translation + 3};
 
            return calibration;
        };
 
        StereoCalibration calibration;
 
        calibration.calibrationLeft = calibration.calibrationRight =
            convert_calibration(k4aCalibration.color_camera_calibration);
 
        calibration.rectificationHomographyLeft = calibration.rectificationHomographyRight =
            convertEigenMatToVisionX(Matrix3f::Identity());
 
 
        return calibration;
    }
 
 
} // namespace visionx