KinectV2PointCloudProvider.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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::KinectV2PointCloudProvider
 * @author     Christoph Pohl (christoph dot pohl at kit dot edu)
 * @date       2019
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#include "KinectV2PointCloudProvider.h"
 
#include <pcl/io/pcd_io.h>
 
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <VisionX/tools/ImageUtil.h>
#include <VisionX/tools/TypeMapping.h>
 
#include <Calibration/Calibration.h>
#include <Image/ImageProcessor.h>
 
 
using namespace armarx;
using namespace pcl;
using namespace cv;
 
namespace visionx
{
    void
    KinectV2PointCloudProvider::onInitComponent()
    {
        ARMARX_TRACE;
        ARMARX_VERBOSE << "onInitComponent()";
 
        ImageProvider::onInitComponent();
        CapturingPointCloudProvider::onInitComponent();
    }
 
    void
    KinectV2PointCloudProvider::onConnectComponent()
    {
        ARMARX_TRACE;
        ARMARX_VERBOSE << "onConnectComponent()";
 
        ImageProvider::onConnectComponent();
        CapturingPointCloudProvider::onConnectComponent();
    }
 
    void
    KinectV2PointCloudProvider::onDisconnectComponent()
    {
        ARMARX_TRACE;
        // hack to prevent deadlock
        captureEnabled = false;
        ImageProvider::onDisconnectComponent();
        CapturingPointCloudProvider::onDisconnectComponent();
    }
 
    void
    KinectV2PointCloudProvider::onExitComponent()
    {
        ARMARX_TRACE;
        ARMARX_VERBOSE << "onExitComponent()";
 
        ImageProvider::onExitComponent();
        CapturingPointCloudProvider::onExitComponent();
        ARMARX_INFO << "Resetting grabber interface";
    }
 
    void
    KinectV2PointCloudProvider::onInitCapturingPointCloudProvider()
    {
        ARMARX_TRACE;
        enable_rgb_ = getProperty<bool>("EnableRGB").getValue();
        enable_depth_ = getProperty<bool>("EnableDepth").getValue();
        mirror_ = getProperty<bool>("Mirror").getValue();
        serial_ = getProperty<std::string>("Serial").getValue();
        pipeline_type_ = getProperty<KinectProcessorType>("Pipeline").getValue();
        deviceId_ = -1;
        dev_ = nullptr;
        pipeline_ = nullptr;
        rgb_height_ = 1080;
        rgb_width_ = 1920;
        d_height_ = 424;
        d_width_ = 512;
 
        ARMARX_VERBOSE << "onInitCapturingPointCloudProvider()";
        if (!pipeline_)
        {
            ARMARX_TRACE;
            switch (pipeline_type_)
            {
                case CPU:
                    pipeline_ = new libfreenect2::CpuPacketPipeline();
                    break;
                case OPENGL:
                    pipeline_ = new libfreenect2::OpenGLPacketPipeline();
                    break;
                case OPENCL:
                    pipeline_ = new libfreenect2::OpenCLPacketPipeline(deviceId_);
                    break;
                case OPENCLKDE:
                    pipeline_ = new libfreenect2::OpenCLKdePacketPipeline(deviceId_);
                    break;
                case CUDA:
                    pipeline_ = new libfreenect2::CudaPacketPipeline(deviceId_);
                    break;
                case CUDAKDE:
                    pipeline_ = new libfreenect2::CudaKdePacketPipeline(deviceId_);
                    break;
                default:
                    pipeline_ = new libfreenect2::CpuPacketPipeline();
                    break;
            }
        }
 
        if (!enable_rgb_ && !enable_depth_)
        {
            ARMARX_TRACE;
            ARMARX_ERROR << "Either depth or rgb needs to be enabled";
        }
        if (freenect2_.enumerateDevices() == 0)
        {
            ARMARX_ERROR << "no device connected!";
            //            return -1;
        }
        if (serial_.empty())
        {
            ARMARX_TRACE;
            serial_ = freenect2_.getDefaultDeviceSerialNumber();
        }
        if (pipeline_)
        {
            ARMARX_TRACE;
            dev_ = freenect2_.openDevice(serial_, pipeline_);
        }
        else
        {
            ARMARX_TRACE;
            dev_ = freenect2_.openDevice(serial_);
        }
 
        if (dev_ == nullptr)
        {
            ARMARX_ERROR << "failure opening device!";
        }
 
        // init images
        rgbImages_ = new CByteImage*[2];
 
        rgbImages_[0] = new CByteImage(d_width_, d_height_, CByteImage::eRGB24);
        rgbImages_[1] = new CByteImage(d_width_, d_height_, CByteImage::eRGB24);
        ::ImageProcessor::Zero(rgbImages_[0]);
        ::ImageProcessor::Zero(rgbImages_[1]);
    }
 
    void
    KinectV2PointCloudProvider::onStartCapture(float frameRate)
    {
        if (enable_rgb_ && enable_depth_)
        {
            ARMARX_TRACE;
            if (!dev_->start())
            {
                ARMARX_ERROR << "Could not Start dev stream";
            }
        }
        else
        {
            ARMARX_TRACE;
            if (!dev_->startStreams(enable_rgb_, enable_depth_))
            {
                ARMARX_ERROR << "Could not Start dev stream";
            }
        }
        libfreenect2::Freenect2Device::IrCameraParams depth_p =
            dev_->getIrCameraParams(); ///< Depth camera parameters.
        libfreenect2::Freenect2Device::ColorCameraParams rgb_p = dev_->getColorCameraParams();
        registration_ = new libfreenect2::Registration(depth_p, rgb_p);
        dev_->setColorFrameListener(&listener_);
        dev_->setIrAndDepthFrameListener(&listener_);
        pclHelper = new KinectToPCLHelper<PointT>(getCameraIrParameters(), mirror_);
        ivtHelper = new KinectToIVTHelper(mirror_);
        initializeCameraCalibration();
    }
 
    void
    KinectV2PointCloudProvider::onStopCapture()
    {
        ARMARX_TRACE;
        ARMARX_INFO << "Stopping KinectV2 Grabber Interface";
        if (dev_)
        {
            dev_->stop();
            dev_->close();
        }
        ARMARX_INFO << "Stopped KinectV2 Grabber Interface";
    }
 
    void
    KinectV2PointCloudProvider::onExitCapturingPointCloudProvider()
    {
        ARMARX_TRACE;
        ARMARX_VERBOSE << "onExitCapturingPointCloudProvider()";
 
        delete rgbImages_[0];
        delete rgbImages_[1];
 
        delete[] rgbImages_;
    }
 
    void
    KinectV2PointCloudProvider::onInitImageProvider()
    {
        ARMARX_TRACE;
        Eigen::Vector2i dimensions = Eigen::Vector2i(512, 424);
        setImageFormat(visionx::ImageDimension(dimensions(0), dimensions(1)), visionx::eRgb);
        setNumberImages(2);
    }
 
    bool
    KinectV2PointCloudProvider::doCapture()
    {
        if (!listener_.waitForNewFrame(frames_, 1 * 1000)) // 100 msconds
        {
            ARMARX_ERROR << "TIMEOUT";
            return false;
        }
        IceUtil::Time ts = TimeUtil::GetTime();
        pcl::PointCloud<PointT>::Ptr outputCloud =
            pcl::PointCloud<PointT>::Ptr(new pcl::PointCloud<PointT>());
 
 
        libfreenect2::Frame* rgb = frames_[libfreenect2::Frame::Color];
        //        libfreenect2::Frame *ir = frames_[libfreenect2::Frame::Ir];
        libfreenect2::Frame* depth = frames_[libfreenect2::Frame::Depth];
        if (enable_rgb_ && enable_depth_)
        {
            registration_->apply(rgb, depth, &undistorted_, &registered_, true);
        }
        ivtHelper->calculateImages(undistorted_, registered_, rgbImages_);
        pclHelper->calculatePointCloud(undistorted_, registered_, outputCloud);
        //        outputCloud->header.stamp = depth->timestamp;
        outputCloud->header.stamp = ts.toMicroSeconds();
        outputCloud->header.seq = depth->sequence;
        providePointCloud<PointT>(outputCloud);
        //        provideImages(rgbImages_, IceUtil::Time::microSeconds(rgb->timestamp * 0.125 * 1000));
        provideImages(rgbImages_, ts);
        listener_.release(frames_);
        return true;
    }
 
    PropertyDefinitionsPtr
    KinectV2PointCloudProvider::createPropertyDefinitions()
    {
        return PropertyDefinitionsPtr(
            new KinectV2PointCloudProviderPropertyDefinitions(getConfigIdentifier()));
    }
 
    libfreenect2::Freenect2Device::IrCameraParams
    KinectV2PointCloudProvider::getCameraIrParameters()
    {
        libfreenect2::Freenect2Device::IrCameraParams ir = dev_->getIrCameraParams();
        return ir;
    }
 
    libfreenect2::Freenect2Device::ColorCameraParams
    KinectV2PointCloudProvider::getCameraRGBParameters()
    {
        libfreenect2::Freenect2Device::ColorCameraParams rgb = dev_->getColorCameraParams();
        return rgb;
    }
 
    StereoCalibration
    KinectV2PointCloudProvider::getStereoCalibration(const Ice::Current& c)
    {
        return calibration;
    }
 
    void
    KinectV2PointCloudProvider::initializeCameraCalibration()
    {
        visionx::CameraParameters RGBCameraIntrinsics;
        RGBCameraIntrinsics.focalLength.resize(2, 0.0f);
        RGBCameraIntrinsics.principalPoint.resize(2, 0.0f);
        RGBCameraIntrinsics.distortion.resize(4, 0.0f);
 
        //Eigen::Matrix3f id = Eigen::Matrix3f::Identity();
        RGBCameraIntrinsics.rotation =
            visionx::tools::convertEigenMatToVisionX(Eigen::Matrix3f::Identity());
        RGBCameraIntrinsics.translation =
            visionx::tools::convertEigenVecToVisionX(Eigen::Vector3f::Zero());
        auto rgb_params = getCameraRGBParameters();
        ARMARX_TRACE;
 
 
        RGBCameraIntrinsics.principalPoint[0] = rgb_params.cx;
        RGBCameraIntrinsics.principalPoint[1] = rgb_params.cy;
        RGBCameraIntrinsics.focalLength[0] = rgb_params.fx;
        RGBCameraIntrinsics.focalLength[1] = rgb_params.fy;
        RGBCameraIntrinsics.width = rgb_width_;
        RGBCameraIntrinsics.height = rgb_height_;
 
 
        visionx::CameraParameters depthCameraIntrinsics;
        depthCameraIntrinsics.focalLength.resize(2, 0.0f);
        depthCameraIntrinsics.principalPoint.resize(2, 0.0f);
        depthCameraIntrinsics.distortion.resize(4, 0.0f);
        depthCameraIntrinsics.rotation =
            visionx::tools::convertEigenMatToVisionX(Eigen::Matrix3f::Identity());
        depthCameraIntrinsics.translation =
            visionx::tools::convertEigenVecToVisionX(Eigen::Vector3f::Zero());
 
        auto d_params = getCameraIrParameters();
        depthCameraIntrinsics.principalPoint[0] = d_params.cx;
        depthCameraIntrinsics.principalPoint[1] = d_params.cy;
        depthCameraIntrinsics.focalLength[0] = d_params.fx;
        depthCameraIntrinsics.focalLength[1] = d_params.fy;
        depthCameraIntrinsics.width = d_width_;
        depthCameraIntrinsics.height = d_height_;
 
 
        ARMARX_TRACE;
 
        calibration.calibrationLeft = visionx::tools::createDefaultMonocularCalibration();
        calibration.calibrationRight = visionx::tools::createDefaultMonocularCalibration();
        calibration.calibrationLeft.cameraParam = RGBCameraIntrinsics;
        calibration.calibrationRight.cameraParam = depthCameraIntrinsics;
        calibration.rectificationHomographyLeft =
            visionx::tools::convertEigenMatToVisionX(Eigen::Matrix3f::Zero());
        calibration.rectificationHomographyRight =
            visionx::tools::convertEigenMatToVisionX(Eigen::Matrix3f::Zero());
    }
 
} // namespace visionx