KinectV1PointCloudProvider.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::KinectV1PointCloudProvider
 * @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 "KinectV1PointCloudProvider.h"
#include <VisionX/tools/TypeMapping.h>
#include <VisionX/tools/ImageUtil.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <Image/ImageProcessor.h>
#include <Calibration/Calibration.h>
 
#include <pcl/io/pcd_io.h>
 
 
using namespace armarx;
using namespace pcl;
using namespace cv;
 
namespace visionx
{
    void KinectV1PointCloudProvider::onInitComponent()
    {
        ARMARX_TRACE;
        ARMARX_VERBOSE << "onInitComponent()";
 
        ImageProvider::onInitComponent();
        CapturingPointCloudProvider::onInitComponent();
    }
 
    void KinectV1PointCloudProvider::onConnectComponent()
    {
        ARMARX_TRACE;
        ARMARX_VERBOSE << "onConnectComponent()";
 
        ImageProvider::onConnectComponent();
        CapturingPointCloudProvider::onConnectComponent();
    }
 
    void KinectV1PointCloudProvider::onDisconnectComponent()
    {
        ARMARX_TRACE;
        // hack to prevent deadlock
        captureEnabled = false;
        ImageProvider::onDisconnectComponent();
        CapturingPointCloudProvider::onDisconnectComponent();
    }
 
    void KinectV1PointCloudProvider::onExitComponent()
    {
        ARMARX_TRACE;
        ARMARX_VERBOSE << "onExitComponent()";
 
        ImageProvider::onExitComponent();
        CapturingPointCloudProvider::onExitComponent();
        ARMARX_INFO << "Resetting grabber interface";
    }
 
    void KinectV1PointCloudProvider::onInitCapturingPointCloudProvider()
    {
        ARMARX_TRACE;
 
 
        ARMARX_VERBOSE << "onInitCapturingPointCloudProvider()";
        // from https://jasonchu1313.github.io/2017/10/01/kinect-calibration/
        cx_ = 343.65;
        cy_ = 229.81;
        fx_ = 458.46;
        fy_ = 458.20;
        device_ = &freenect_.createDevice<KinectV1Device>(0);
    }
 
 
    void KinectV1PointCloudProvider::onStartCapture(float frameRate)
    {
        device_->startVideo();
        device_->startDepth();
    }
 
 
    void KinectV1PointCloudProvider::onStopCapture()
    {
        ARMARX_TRACE;
        ARMARX_INFO << "Stopping KinectV1 Grabber Interface";
        device_->stopVideo();
        device_->stopDepth();
        ARMARX_INFO << "Stopped KinectV1 Grabber Interface";
    }
 
 
    void KinectV1PointCloudProvider::onExitCapturingPointCloudProvider()
    {
        ARMARX_TRACE;
        ARMARX_VERBOSE << "onExitCapturingPointCloudProvider()";
 
        delete rgbImages_[0];
        delete rgbImages_[1];
 
        delete[] rgbImages_;
    }
 
 
    void KinectV1PointCloudProvider::onInitImageProvider()
    {
        ARMARX_TRACE;
        // init images
        width_ = 640;
        height_ = 480;
        rgbImages_ = new CByteImage *[2];
        size_ = width_ * height_;
        rgbImages_[0] = new CByteImage(width_, height_, CByteImage::eRGB24);
        rgbImages_[1] = new CByteImage(width_, height_, CByteImage::eRGB24);
        ::ImageProcessor::Zero(rgbImages_[0]);
        ::ImageProcessor::Zero(rgbImages_[1]);
        Eigen::Vector2i dimensions = Eigen::Vector2i(width_, height_);
        setImageFormat(visionx::ImageDimension(dimensions(0), dimensions(1)), visionx::eRgb);
        setNumberImages(2);
    }
 
 
    bool KinectV1PointCloudProvider::doCapture()
    {
        static std::vector<uint8_t> rgb(size_ * 3);
        static std::vector<uint16_t> depth(size_);
        //        float f = 595.f;
        if (!device_->getRGB(rgb) || device_->getDepth(depth))
        {
            return false;
        };
        pcl::PointCloud<PointT>::Ptr outputCloud = pcl::PointCloud<PointT>::Ptr(new pcl::PointCloud<PointT>());
        outputCloud->resize(size_);
        for (int i = 0; i < size_; i++)
        {
            // color
            int index = 3 * i;
            for (int j = 0; j < 3; j++)
            {
                rgbImages_[0]->pixels[index + j] = rgb[index + j];
            }
            visionx::tools::depthValueToRGB((int)depth[i], rgbImages_[1]->pixels[index + 0], rgbImages_[1]->pixels[index + 1],
                                            rgbImages_[1]->pixels[index + 2], false);
            // X = (x - cx) * d / fx
            // Y = (y - cy) * d / fy
            // Z = d
            // TODO: precalculate some of this stuff for efficiency
            //            outputCloud->points[i].x = (i % width_ - (width_ - 1) / 2.f) * depth[i] / f;
            //            outputCloud->points[i].y = (i / width_ - (height_ - 1) / 2.f) * depth[i] / f;
            outputCloud->points[i].x = (i % width_ - cx_ + 0.5) * depth[i] / fx_;
            outputCloud->points[i].y = (i / width_ - cy_ + 0.5) * depth[i] / fy_;
            outputCloud->points[i].z = depth[i];
            outputCloud->points[i].r = rgb[index];
            outputCloud->points[i].g = rgb[index + 1];
            outputCloud->points[i].b = rgb[index + 2];
        }
        IceUtil::Time ts = IceUtil::Time::now();
        outputCloud->header.stamp = ts.toMicroSeconds();
        providePointCloud<PointT>(outputCloud);
        provideImages(rgbImages_, ts);
        return true;
    }
 
 
    PropertyDefinitionsPtr KinectV1PointCloudProvider::createPropertyDefinitions()
    {
        return PropertyDefinitionsPtr(new KinectV1PointCloudProviderPropertyDefinitions(getConfigIdentifier()));
    }
 
    StereoCalibration KinectV1PointCloudProvider::getStereoCalibration(const Ice::Current& c)
    {
        return calibration;
    }
 
}