ImageMonitorWidgetController.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    ObserverTest::
* @author     ( at kit dot edu)
* @date
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
 
#include <VisionX/gui-plugins/ImageMonitor/ImageMonitorWidgetController.h>
 
 
// STD/STL
#include <cmath>
#include <filesystem>
#include <iomanip>
 
// Qt
#include <QApplication>
 
// IVT
#include <Image/ImageProcessor.h>
 
// Somox
#include <SimoxUtility/algorithm/string.h>
 
// ArmarXCore
#include <ArmarXCore/core/exceptions/Exception.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
 
// RobotAPI
#include <RobotAPI/libraries/core/math/ColorUtils.h>
 
// VisionX
#include <VisionX/tools/ImageUtil.h>
#include <VisionX/tools/TypeMapping.h>
 
#include "ImageViewerArea.h"
using namespace armarx;
 
namespace
{
    /**
     * @brief Helper function to determine the correct camera ID
     *
     * For example, if there are two image sources and the depth index is not set, then the first
     * camera is "left" and the second "right". If there are more than two cameras, they are just
     * called "camX" where X is the ID. If the depth index is set, then that camera will be called
     * "depth". For two image sources, the other one will then be called "rgb". Only one image
     * source will yield in no ID at all.
     *
     * @param index Index of the camera
     * @param index_count Number of cameras (or indices) in total
     * @param depth_index Index of the depth camera
     * @return Human readable string representation of the configuration
     */
    std::string
    getHumanReadableCameraID(unsigned int index, unsigned int index_count, int depth_index);
 
    /**
     * @brief Helper function to create a date string
     * @return
     */
    std::string getDateString();
} // namespace
 
namespace visionx
{
    ImageMonitorProperties::ImageMonitorProperties()
    {
        providerName = "";
        frameRate = -1.0f;
 
        // Qt Moc does not seem to accept outputDirectory being a std::filesystem::path().
        {
            if (const char* home = std::getenv("HOME"))
            {
                // ToDo: Replace with XDG_PICTURES_DIR standards when available.
                outputDirectory = std::filesystem::path(home) / "Pictures";
            }
            else
            {
                outputDirectory = std::filesystem::absolute(".");
            }
        }
 
        imageBufferSize = 100;
        bufferFps = 5;
        controlsHidden = false;
        depthImageIndex = -1;
        maxDepthmm = 5000;
        compressionType = eNoCompression;
        compressionQuality = 9;
    }
 
    ImageMonitorWidgetController::ImageMonitorWidgetController() :
        //        displayDelayFilter(20)
        displayDelayFilter(IceUtil::Time::seconds(3), 100)
    {
    }
 
    // *******************************************************
    // Inherited from ArmarXWidgetController
    // *******************************************************
    QPointer<QWidget>
    ImageMonitorWidgetController::getWidget()
    {
        if (!widget)
        {
            widget = new ImageMonitorWidget(this);
        }
 
        return qobject_cast<QWidget*>(widget);
    }
 
    // *******************************************************
    // Inherited from ImageProcessorBase
    // *******************************************************
    void
    ImageMonitorWidgetController::onInitImageProcessor()
    {
        getWidget()->show();
        std::unique_lock lock(imageMutex);
        images = nullptr;
        connected = false;
        playing = false;
        writeImageBuffer = false;
        lastBufferTime = IceUtil::Time::now();
        imageBuffer.resize(10);
 
        // signals / slots
        connect(this, SIGNAL(imageProviderConnected(bool)), widget, SLOT(setConnected(bool)));
        connect(this,
                SIGNAL(statisticsUpdated(const QString&)),
                widget,
                SLOT(updateStatistics(const QString&)));
        connect(this, SIGNAL(recordingBufferEmpty(bool)), widget, SLOT(updateRecordButton(bool)));
    }
 
    void
    ImageMonitorWidgetController::onConnectImageProcessor()
    {
        if (properties.providerName != "" /*&& !connected*/)
        {
            applyProperties(properties);
        }
    }
 
    void
    ImageMonitorWidgetController::onExitImageProcessor()
    {
        disconnectFromProvider();
    }
 
    void
    ImageMonitorWidgetController::process()
    {
        if (!connected)
        {
            usleep(10000);
            return;
        }
 
        updateStatistics();
 
        if (!playing)
        {
            usleep(10000);
            return;
        }
 
        std::unique_lock lock(imageMutex);
        unsigned int retrievedImages = 0;
        const bool sourceFps = this->properties.frameRate < 0.0f;
 
        // Fetch images and visualise
        {
            if (sourceFps && !this->waitForImages(this->properties.providerName))
            {
                ARMARX_VERBOSE << "Timeout or error in wait for images" << armarx::flush;
                return;
            }
 
            armarx::MetaInfoSizeBasePtr info;
            retrievedImages = static_cast<unsigned int>(
                this->getImages(this->properties.providerName, this->images, info));
            timeReceived = TimeUtil::GetTime();
            if (sourceFps)
            {
                this->timeProvided = IceUtil::Time::microSeconds(info->timeProvided);
            }
 
            std::vector<CByteImage*> deleteList;
            std::vector<CByteImage*> selectedImages;
 
            for (unsigned int i = 0;
                 i < static_cast<unsigned int>(retrievedImages) && i < this->numberImages;
                 ++i)
            {
                CByteImage* currentImage;
 
                if (i == static_cast<unsigned int>(this->properties.depthImageIndex) &&
                    this->properties.recordDepthRaw)
                {
                    currentImage = new CByteImage(this->images[i]);
                    ::ImageProcessor::CopyImage(this->images[i], currentImage);
                    deleteList.push_back(currentImage);
                }
                else
                {
                    currentImage = this->images[i];
                }
 
                if (this->properties.imagesToShow.size() == 0 or
                    (this->properties.imagesToShow.size() > 0 and
                     this->properties.imagesToShow.count(i) > 0))
                {
                    if (i == static_cast<unsigned int>(this->properties.depthImageIndex))
                    {
                        this->convertToHeatMapDepthImage(currentImage);
                    }
 
                    selectedImages.push_back(currentImage);
                }
            }
 
            if (selectedImages.size() > 0)
            {
                this->widget->drawImages(static_cast<int>(selectedImages.size()),
                                         &selectedImages[0],
                                         this->timeProvided,
                                         timeReceived);
            }
 
            for (CByteImage* image : deleteList)
            {
                delete image;
            }
 
            if (!sourceFps)
            {
                this->fpsCounter.assureFPS(this->properties.frameRate);
            }
        }
 
        if (writeImageBuffer && retrievedImages > 0)
        {
            if ((imageBuffer.size()) != static_cast<unsigned int>(properties.imageBufferSize) &&
                properties.imageBufferSize > 0)
            {
                imageBuffer.resize(static_cast<unsigned int>(properties.imageBufferSize));
            }
 
            IceUtil::Time timePassed = IceUtil::Time::now() - lastBufferTime;
 
            if (timePassed.toMilliSeconds() > 1000.f / properties.bufferFps)
            {
                ImageContainer newImages;
 
                for (unsigned int i = 0; i < retrievedImages; i++)
                {
                    newImages.push_back(CByteImagePtr(new CByteImage(images[i])));
                    ::ImageProcessor::CopyImage(images[i], newImages[i].get());
                }
 
                imageBuffer.push_front(newImages);
                lastBufferTime = IceUtil::Time::now();
            }
        }
 
        if (this->recordingTask && this->writeRecordingBuffer)
        {
            std::vector<CByteImage*> tmpImages(this->numberImages);
            for (unsigned int i = 0; i < this->numberImages; ++i)
            {
                tmpImages.at(i) = new CByteImage(this->images[i]);
                ::ImageProcessor::CopyImage(this->images[i], tmpImages[i]);
            }
            lock.unlock();
            std::unique_lock lock2(this->recordingBufferMutex);
            this->recordingBuffer.push(tmpImages);
        }
    }
 
    void
    ImageMonitorWidgetController::loadSettings(QSettings* settings)
    {
        properties.providerName = settings->value("providerName", "").toString().toStdString();
        properties.frameRate = settings->value("frameRate", -1).toInt();
        properties.outputDirectory = settings->value("outputPath", ".").toString().toStdString();
        properties.imageBufferSize = settings->value("imageBufferSize", 99).toInt();
        properties.bufferFps = settings->value("bufferFps", 5.0f).toFloat();
        auto imagesToShowStringList = settings->value("imagesToShow", 0).toStringList();
        properties.imagesToShow.clear();
        for (QString& s : imagesToShowStringList)
        {
            properties.imagesToShow.insert(s.toULong());
        }
        properties.controlsHidden = settings->value("controlsHidden", false).toBool();
        properties.depthImageIndex = settings->value("depthImageIndex", -1).toInt();
        properties.maxDepthmm = settings->value("maxDepthmm", 5000).toInt();
        properties.recordDepthRaw = settings->value("recordDepthRaw", true).toBool();
        properties.recordingMethods = settings->value("recordingMethods", 0).toStringList();
        properties.compressionType =
            static_cast<CompressionType>(settings->value("compressionType", 0).toInt());
        properties.compressionQuality = settings->value("compressionQuality", 0).toInt();
 
        ARMARX_INFO << "bufferFps: " << properties.bufferFps;
    }
 
    void
    ImageMonitorWidgetController::saveSettings(QSettings* settings)
    {
        settings->setValue("providerName", QString(properties.providerName.c_str()));
        settings->setValue("frameRate", properties.frameRate);
        settings->setValue("outputPath", QString(properties.outputDirectory.c_str()));
        settings->setValue("imageBufferSize", properties.imageBufferSize);
        settings->setValue("bufferFps", QString::number(static_cast<double>(properties.bufferFps)));
        QStringList l;
        for (auto number : properties.imagesToShow)
        {
            l << QString::number(number);
        }
        settings->setValue("imagesToShow", l);
        settings->setValue("controlsHidden", properties.controlsHidden);
        settings->setValue("depthImageIndex", properties.depthImageIndex);
        settings->setValue("maxDepthmm", properties.maxDepthmm);
        settings->setValue("recordDepthRaw", properties.recordDepthRaw);
        settings->setValue("recordingMethods", properties.recordingMethods);
 
        settings->setValue("compressionType", (int)properties.compressionType);
        settings->setValue("compressionQuality", (int)properties.compressionQuality);
    }
 
    // *******************************************************
    // properties handling
    // *******************************************************
    void
    ImageMonitorWidgetController::applyProperties(ImageMonitorProperties properties)
    {
        ARMARX_INFO << "Applying properties";
        std::unique_lock lock(imageMutex);
 
        disconnectFromProvider();
 
        // update properties
        this->properties = properties;
 
        // connect to provider
        try
        {
            connectToProvider();
        }
        catch (...)
        {
            armarx::handleExceptions();
        }
    }
 
    ImageMonitorProperties
    ImageMonitorWidgetController::getProperties()
    {
        return properties;
    }
 
    ImageTransferStats
    ImageMonitorWidgetController::getStatistics()
    {
        return getImageTransferStats(properties.providerName);
    }
 
    const ImageContainer&
    ImageMonitorWidgetController::getBufferedImage(unsigned int position,
                                                   unsigned int& realPosition)
    {
        int posInBuffer = static_cast<int>(imageBuffer.size()) - 1 - static_cast<int>(position);
 
        if (posInBuffer < 0)
        {
            posInBuffer = 0;
        }
 
        if (static_cast<unsigned int>(posInBuffer) >= imageBuffer.size())
        {
            posInBuffer = static_cast<int>(imageBuffer.size()) - 1;
        }
 
        if (imageBuffer.size() == 0)
        {
            throw LocalException("ImageBuffer size is 0");
        }
 
        realPosition = static_cast<unsigned int>(posInBuffer);
        return imageBuffer[static_cast<unsigned int>(posInBuffer)];
    }
 
    // *******************************************************
    // private methods
    // *******************************************************
    void
    ImageMonitorWidgetController::connectToProvider()
    {
        if (connected)
        {
            return;
        }
 
        // TODO: using image provider can be done only here???? Why???
        usingImageProvider(properties.providerName);
 
        ARMARX_INFO << getName() << " connecting to " << properties.providerName;
 
        // connect to proxy
        imageProviderInfo =
            getImageProvider(properties.providerName, visionx::tools::typeNameToImageType("rgb"));
 
        // update members
        imageProviderPrx = imageProviderInfo.proxy;
        numberImages = static_cast<unsigned int>(imageProviderInfo.numberImages);
 
        // create images
        {
            std::unique_lock lock(imageMutex);
            images = new CByteImage*[numberImages];
 
            for (unsigned int i = 0; i < numberImages; i++)
            {
                images[i] = tools::createByteImage(imageProviderInfo);
            }
        }
 
        timeProvided = IceUtil::Time::seconds(0);
        ImageProcessor::setCompressionType(properties.compressionType,
                                           properties.compressionQuality);
        connected = true;
 
 
        emit imageProviderConnected(true);
    }
 
    void
    ImageMonitorWidgetController::disconnectFromProvider()
    {
        if (!connected)
        {
            return;
        }
        {
            std::unique_lock lock(imageMutex);
            // clear images
            if (images)
            {
                for (unsigned int i = 0; i < numberImages; i++)
                {
                    delete images[i];
                }
 
                delete[] images;
            }
        }
        releaseImageProvider(properties.providerName);
        // release provider
        imageProviderPrx = nullptr;
 
        connected = false;
 
        emit imageProviderConnected(false);
    }
 
    std::string
    ImageMonitorWidgetController::getAbsoluteOutputPath()
    {
        namespace fs = std::filesystem;
        fs::path absPath = fs::absolute(fs::path(properties.outputDirectory));
        return absPath.string();
    }
 
    void
    ImageMonitorWidgetController::createSnapshot()
    {
        std::unique_lock lock(this->imageMutex);
 
        std::filesystem::path path = this->getAbsoluteOutputPath();
        std::string baseFilename = "snapshot_" + getDateString();
 
        for (unsigned int i = 0; i < this->numberImages; ++i)
        {
            std::filesystem::path filename =
                baseFilename +
                getHumanReadableCameraID(i, this->numberImages, this->properties.depthImageIndex);
            visionx::imrec::takeSnapshot(*this->images[i], path / filename);
        }
    }
 
    void
    ImageMonitorWidgetController::startRecording()
    {
        if (this->recordingTask && this->recordingTask->isRunning())
        {
            return;
        }
 
        // Initialise needed values
        const std::string baseFilename = "recording_" + getDateString();
        const std::filesystem::path path = this->getAbsoluteOutputPath();
        const double fps = [this]() -> double
        {
            if (this->properties.frameRate <= 0)
            {
                visionx::ImageTransferStats stats =
                    this->getImageTransferStats(this->properties.providerName);
                const float source_fps = std::round(stats.imageProviderFPS.getFPS());
                return static_cast<unsigned int>(source_fps);
            }
 
            return static_cast<double>(this->properties.frameRate);
        }();
 
        for (unsigned int i = 0; i < this->numberImages; ++i)
        {
            // Determine full filename and extension
            const std::string ext =
                this->properties.recordingMethods.at(static_cast<int>(i)).toStdString();
            const std::filesystem::path filename =
                baseFilename +
                getHumanReadableCameraID(i, this->numberImages, this->properties.depthImageIndex) +
                ext;
            const std::filesystem::path fullFilePath = path / filename;
 
            // Create recorder and add to list
            visionx::imrec::Recording rec =
                ext != "" ? visionx::imrec::newRecording(fullFilePath, fps) : nullptr;
            this->recorders.push_back(rec);
        }
 
        ARMARX_IMPORTANT << "Starting recording with " << fps
                         << " fps to path: " << getAbsoluteOutputPath();
 
        this->recordingTask = new PeriodicTask<ImageMonitorWidgetController>(
            this, &ImageMonitorWidgetController::recordFrame, 1, true, "ImageRecorderTask");
        this->writeRecordingBuffer = true;
        this->recordingTask->start();
    }
 
    void
    ImageMonitorWidgetController::stopRecording()
    {
        emit this->recordingBufferEmpty(/* isEmpty = */ false);
        // Disallow writing recording buffer and wait until it is empty
        this->writeRecordingBuffer = false;
        if (!this->recordingBuffer.empty())
        {
            ARMARX_IMPORTANT << "Stopping recording... Still " << this->recordingBuffer.size()
                             << " images in the queue to write";
            return;
        }
 
        emit this->recordingBufferEmpty(/* isEmpty = */ true);
 
        if (this->recordingTask)
        {
            this->recordingTask->stop();
        }
 
        ARMARX_INFO << "Closing recordings";
 
        for (const visionx::imrec::Recording& rec : this->recorders)
        {
            if (rec)
            {
                rec->stopRecording();
            }
        }
        this->recorders.clear();
    }
 
    void
    ImageMonitorWidgetController::recordFrame()
    {
        std::vector<CByteImage*> images;
        {
            std::unique_lock lock(this->recordingBufferMutex);
 
            // If the recording was stopped, keep running until the buffer was emptied completely
            if (!this->writeRecordingBuffer)
            {
                if (this->recordingBuffer.empty())
                {
                    ARMARX_IMPORTANT << "All images in queue were written";
                    this->stopRecording();
                    return;
                }
                else if (this->recordingBuffer.size() % 5 == 0)
                {
                    ARMARX_INFO << "Stopping recording... Still " << this->recordingBuffer.size()
                                << " images in the queue to write";
                }
            }
            else if (this->recordingBuffer.size() > 0 and this->recordingBuffer.size() % 5 == 0)
            {
                ARMARX_INFO << deactivateSpam(1) << "Recording buffer queue size currently at "
                            << this->recordingBuffer.size() << " images";
            }
 
            // Early return (nop) if the buffer is initially empty etc.
            if (!this->connected or this->recordingBuffer.empty())
            {
                return;
            }
 
            images = this->recordingBuffer.front();
            this->recordingBuffer.pop();
        }
 
        TIMING_START(imageWriting);
 
        for (unsigned int i = 0; i < this->numberImages; ++i)
        {
            if (this->recorders[i])
            {
                this->recorders[i]->recordFrame(*images[i], std::chrono::microseconds{0});
            }
            delete images[i];
        }
 
        TIMING_END(imageWriting);
    }
 
    void
    ImageMonitorWidgetController::updateStatistics()
    {
        std::unique_lock lock(imageMutex);
        visionx::ImageTransferStats stats = getImageTransferStats(properties.providerName);
 
        std::stringstream ss;
 
        ss << std::fixed << std::setprecision(1)
           << "source fps: " << stats.imageProviderFPS.getFPS();
        ss << " - ";
 
        IceUtil::Time displayDelay = widget->getImageViewer()->getDisplayDelay();
        displayDelayFilter.update(timeReceived, displayDelay.toMilliSecondsDouble());
 
        if (playing)
        {
            ss << "display fps: " << stats.pollingFPS.getFPS();
            if (timeProvided.toMilliSeconds())
            {
                ss << " - image age: " << (timeReceived - timeProvided).toMilliSeconds()
                   << " ms - display delay: "
                   << IceUtil::Time::milliSecondsDouble(displayDelayFilter.getCurrentValue())
                          .toMilliSecondsDouble()
                   << " ms";
            }
        }
        else
        {
            ss << "display paused";
        }
 
        std::string statisticsStr = ss.str();
        emit statisticsUpdated(QString(statisticsStr.c_str()));
    }
 
    void
    ImageMonitorWidgetController::hideControls(bool hide)
    {
        widget->hideControlWidgets(hide);
        properties.controlsHidden = hide;
    }
 
    QPointer<QWidget>
    ImageMonitorWidgetController::getCustomTitlebarWidget(QWidget* parent)
    {
        if (customToolbar)
        {
            if (parent != customToolbar->parent())
            {
                customToolbar->setParent(parent);
            }
 
            return customToolbar.data();
        }
 
        customToolbar = new QToolBar(parent);
        customToolbar->setIconSize(QSize(16, 16));
 
        viewingModeAction =
            customToolbar->addAction(QIcon(":icons/object-locked-2.ico"), "Hide controls");
        viewingModeAction->setCheckable(true);
        connect(viewingModeAction, SIGNAL(toggled(bool)), this, SLOT(hideControls(bool)));
        viewingModeAction->setChecked(properties.controlsHidden);
 
        return customToolbar.data();
    }
 
    void
    ImageMonitorWidgetController::convertToHeatMapDepthImage(CByteImage* depthImage)
    {
        float maxDistance = properties.maxDepthmm;
        int pixelCount = depthImage->width * depthImage->height;
        auto& depthToRgbLookUpTable = depthToRgbLookUpTables[maxDistance];
        size_t depthValueCount = 256 * 256; // 16 bit depth images
        if (depthToRgbLookUpTable.size() != depthValueCount)
        {
            ARMARX_VERBOSE << "Resizing lookup table for max distance " << maxDistance;
            depthToRgbLookUpTable.resize(depthValueCount);
        }
        size_t pixelPos = 0;
        for (int i = 0; i < pixelCount; ++i)
        {
            int z_value = visionx::tools::rgbToDepthValue(depthImage->pixels[pixelPos + 0],
                                                          depthImage->pixels[pixelPos + 1],
                                                          depthImage->pixels[pixelPos + 2],
                                                          false);
            if (z_value > 0)
            {
                DrawColor24Bit rgb;
                if (z_value <
                    static_cast<signed int>(
                        depthToRgbLookUpTable.size())) // lookup only for 16 bit depth images
                {
                    std::optional<armarx::DrawColor24Bit>& rgbOptional =
                        depthToRgbLookUpTable.at(z_value);
                    if (rgbOptional)
                    {
                        rgb = *rgbOptional;
                    }
                    else
                    {
                        auto hsv = colorutils::HeatMapColor(1.0f - z_value / maxDistance);
                        rgb = colorutils::HsvToRgb(hsv);
                        rgbOptional = rgb;
                    }
                }
                else // fallback for 24 bit depth images
                {
                    auto hsv = colorutils::HeatMapColor(1.0f - z_value / maxDistance);
                    rgb = colorutils::HsvToRgb(hsv);
                }
                depthImage->pixels[pixelPos] = rgb.r;
                depthImage->pixels[pixelPos + 1] = rgb.g;
                depthImage->pixels[pixelPos + 2] = rgb.b;
            }
            else
            {
                depthImage->pixels[pixelPos] = 0;
                depthImage->pixels[pixelPos + 1] = 0;
                depthImage->pixels[pixelPos + 2] = 0;
            }
            pixelPos += 3;
        }
    }
 
 
} // namespace visionx
 
namespace
{
    std::string
    getHumanReadableCameraID(unsigned int index, unsigned int index_count, int depth_index)
    {
        if (index_count >= 2)
        {
            if (depth_index >= 0)
            {
                if (index == static_cast<unsigned int>(depth_index))
                {
                    return "_depth";
                }
                if (index_count == 2 and index != static_cast<unsigned int>(depth_index))
                {
                    return "_rgb";
                }
            }
            else if (index_count == 2)
            {
                if (index == 0)
                {
                    return "_left";
                }
                if (index == 1)
                {
                    return "_right";
                }
            }
 
            return "_cam" + std::to_string(index);
        }
 
        return "";
    }
 
    std::string
    getDateString()
    {
        IceUtil::Time recordStartTime = IceUtil::Time::now();
        std::string date_str = recordStartTime.toDateTime();
        // I checked that this is equal to using boost::replace_all()
        date_str = simox::alg::replace_all(date_str, "/", "-");
        date_str = simox::alg::replace_all(date_str, " ", "_");
        date_str = simox::alg::replace_all(date_str, ":", "-");
        return date_str;
    }
} // namespace