KLGImageProvider.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::KLGImageProvider
 * @author     Markus Grotz ( markus dot grotz at kit dot edu )
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "KLGImageProvider.h"
 
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
#include <stdio.h>
 
#include <filesystem>
 
#include <ArmarXCore/core/ArmarXManager.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
 
#include <Image/ImageProcessor.h>
#include <zlib.h>
 
 
using namespace armarx;
 
void
KLGImageProvider::onInitCapturingImageProvider()
{
    offeringTopic(getProperty<std::string>("DebugObserverName").getValue());
    offeringTopic(getProperty<std::string>("DebugDrawerTopicName").getValue());
 
    doRewind = getProperty<bool>("Rewind").getValue();
 
    Eigen::Vector2i dimensions = getProperty<Eigen::Vector2i>("Dimensions").getValue();
    setImageFormat(visionx::ImageDimension(dimensions(0), dimensions(1)), visionx::eRgb);
    setNumberImages(2);
 
    rgbImages = new CByteImage*[2];
    rgbImages[0] = new CByteImage(dimensions(0), dimensions(1), CByteImage::eRGB24);
    rgbImages[1] = new CByteImage(dimensions(0), dimensions(1), CByteImage::eRGB24);
    depthBuffer = cv::Mat(dimensions(1), dimensions(0), CV_16UC1, 0.0);
 
    std::string fileName = getProperty<std::string>("FileName").getValue();
 
    if (!ArmarXDataPath::getAbsolutePath(fileName, fileName))
    {
        ARMARX_ERROR << "Could not find point cloud file in ArmarXDataPath: " << fileName;
    }
 
 
    if (!std::filesystem::exists(fileName.c_str()))
    {
        ARMARX_FATAL << " files does not exists " << fileName;
        return;
    }
 
    fp = fopen(fileName.c_str(), "rb");
 
    currentFrame = 0;
 
    if (fread(&totalFrames, sizeof(int32_t), 1, fp) != sizeof(int32_t))
    {
        ARMARX_FATAL << "invalid file";
        return;
    }
 
    ARMARX_INFO << "total number of frames " << totalFrames;
}
 
void
KLGImageProvider::onStartCapture(float framesPerSecond)
{
    debugObserver = getTopic<DebugObserverInterfacePrx>(
        getProperty<std::string>("DebugObserverName").getValue());
    debugDrawer = getTopic<DebugDrawerInterfacePrx>(
        getProperty<std::string>("DebugDrawerTopicName").getValue());
 
    /*
    StringVariantBaseMap debugValues;
    debugValues["debug_value"] = new Variant(1.0);
    debugObserver->setDebugChannel(getName(), debugValues);
    */
}
 
void
KLGImageProvider::onStopCapture()
{
}
 
bool
KLGImageProvider::capture(void** ppImageBuffers)
{
 
    if (currentFrame > totalFrames)
    {
        if (doRewind)
        {
            currentFrame = 0;
            rewind(fp);
 
            [[maybe_unused]] auto r = fread(&totalFrames, sizeof(int32_t), 1, fp);
        }
        else
        {
            getArmarXManager()->asyncShutdown();
            stopCapture();
            return false;
        }
    }
 
    ::ImageProcessor::Zero(rgbImages[0]);
    ::ImageProcessor::Zero(rgbImages[1]);
 
    int64_t timestamp;
    int depthSize;
    int32_t imageSize;
 
    [[maybe_unused]] auto r = fread(&timestamp, sizeof(int64_t), 1, fp);
    r = fread(&depthSize, sizeof(int32_t), 1, fp);
    r = fread(&imageSize, sizeof(int32_t), 1, fp);
 
    if (depthSize == rgbImages[1]->height * rgbImages[1]->width * 2)
    {
        r = fread(depthBuffer.data, depthSize, 1, fp);
        //fread(rgbImages[1]->pixels, imageSize, 1, fp);
    }
    else if (depthSize > 0)
    {
        uint8_t* tmpImage = new uint8_t[depthSize];
 
        unsigned long decompLength = rgbImages[1]->height * rgbImages[1]->width * 2;
 
        r = fread(tmpImage, depthSize, 1, fp);
        uncompress(depthBuffer.data, &decompLength, tmpImage, depthSize);
    }
 
    if (imageSize == rgbImages[0]->height * rgbImages[0]->width * 3)
    {
        r = fread(rgbImages[0]->pixels, imageSize, 1, fp);
    }
    else if (imageSize > 0)
    {
        std::vector<uint8_t> tmpImage(imageSize, 0);
 
        r = fread(&tmpImage[0], imageSize, 1, fp);
        cv::Mat decImg = cv::imdecode(tmpImage, 1);
        imageSize = decImg.step * decImg.rows;
        memcpy(rgbImages[0]->pixels, decImg.data, decImg.step * decImg.rows);
    }
 
    currentFrame++;
 
    for (int i = 0; i < rgbImages[1]->height; i++)
    {
        for (int j = 0; j < rgbImages[1]->width; j++)
        {
            unsigned short value = depthBuffer.at<unsigned short>(i, j);
 
            rgbImages[1]->pixels[3 * (i * rgbImages[1]->width + j) + 0] = value & 0xFF;
            rgbImages[1]->pixels[3 * (i * rgbImages[1]->width + j) + 1] = (value >> 8) & 0xFF;
        }
    }
 
    // TODO set the timestamp
 
    armarx::SharedMemoryScopedWriteLockPtr lock(sharedMemoryProvider->getScopedWriteLock());
 
    for (size_t i = 0; i < 2; i++)
    {
        memcpy(ppImageBuffers[i], rgbImages[i]->pixels, imageSize);
    }
 
    return true;
}
 
void
KLGImageProvider::onExitCapturingImageProvider()
{
    fclose(fp);
 
    for (size_t i = 0; i < 2; i++)
    {
        delete rgbImages[i];
    }
 
    delete[] rgbImages;
}
 
armarx::PropertyDefinitionsPtr
KLGImageProvider::createPropertyDefinitions()
{
    return armarx::PropertyDefinitionsPtr(
        new KLGImageProviderPropertyDefinitions(getConfigIdentifier()));
}
 
std::string
KLGImageProvider::GetDefaultName()
{
    return "KLGImageProvider";
}
 
std::string
KLGImageProvider::getDefaultName() const
{
    return GetDefaultName();
}
 
ARMARX_REGISTER_COMPONENT_EXECUTABLE(::armarx::KLGImageProvider, ::armarx::KLGImageProvider::GetDefaultName());