FeatureLearningWidgetController.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::gui-plugins::FeatureLearningWidgetController
 * \author     Julian Zimmer ( urdbu at student dot kit dot edu )
 * \date       2018
 * \copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "FeatureLearningWidgetController.h"
 
#include <mutex>
#include <string>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include <ArmarXGui/libraries/ArmarXGuiBase/widgets/IceProxyFinder.h>
 
#include <VisionX/interface/components/Calibration.h>
#include <VisionX/tools/ImageUtil.h>
#include <VisionX/tools/TypeMapping.h>
 
#include <Calibration/StereoCalibration.h>
#include <DataStructures/DynamicArray.h>
#include <Image/ImageProcessor.h>
#include <Image/PrimitivesDrawer.h>
 
using namespace armarx;
using namespace visionx;
 
FeatureLearningWidgetController::FeatureLearningWidgetController()
{
    qRegisterMetaType<CByteImage**>("CByteImage**");
 
    QPointer<QWidget> widgetPointer = getWidget();
    this->widgetPointer = widgetPointer.data();
 
    widget.setupUi(widgetPointer);
 
 
    proxyFinder = new armarx::IceProxyFinder<ImageProviderInterfacePrx>(widgetPointer);
    proxyFinder->setSearchMask("*Provider|*Result");
    //ui.horizontalLayout->addWidget(proxyFinder);
    widget.chooseProviderLayout->addWidget(proxyFinder);
 
    imageViewer = new SelectableImageViewer();
    QSizePolicy sizePoli(QSizePolicy::Expanding, QSizePolicy::Expanding);
    sizePoli.setVerticalStretch(15);
    imageViewer->setSizePolicy(sizePoli);
 
    widget.imageViewerLayout_2->addWidget(imageViewer);
    imageViewer->show();
}
 
FeatureLearningWidgetController::~FeatureLearningWidgetController()
{
}
 
void
FeatureLearningWidgetController::loadSettings(QSettings* settings)
{
}
 
void
FeatureLearningWidgetController::saveSettings(QSettings* settings)
{
}
 
void
FeatureLearningWidgetController::onInitImageProcessor()
{
    proxyFinder->setIceManager(this->getIceManager(),
                               proxyFinder->getSelectedProxyName().isEmpty());
 
    points = Eigen::ArrayX2f::Zero(4, 2);
    numSelectedPoints = 0;
 
 
    connect(imageViewer,
            SIGNAL(selected(int, float, float)),
            this,
            SLOT(addPolygonPoint(int, float, float)));
    connect(widget.connectButton, SIGNAL(clicked(bool)), this, SLOT(connectButtonClicked(bool)));
    connect(widget.loadFeaturesButton,
            SIGNAL(clicked(bool)),
            this,
            SLOT(loadFeaturesButtonClicked(bool)));
    connect(widget.saveFeaturesButton,
            SIGNAL(clicked(bool)),
            this,
            SLOT(saveFeaturesButtonClicked(bool)));
    connect(
        widget.pausePlayButton, SIGNAL(clicked(bool)), this, SLOT(pausePlayButtonClicked(bool)));
    connect(
        widget.disconnectButton, SIGNAL(clicked(bool)), this, SLOT(disconnectButtonClicked(bool)));
    connect(widget.addFeaturesButton,
            SIGNAL(clicked(bool)),
            this,
            SLOT(addFeaturesButtonClicked(bool)));
    connect(widget.clearFeaturesButton,
            SIGNAL(clicked(bool)),
            this,
            SLOT(clearFeaturesButtonClicked(bool)));
    connect(widget.thresholdSpinBox,
            SIGNAL(valueChanged(double)),
            this,
            SLOT(thresholdChanged(double)));
    connect(
        widget.maxFeatureSpinBox, SIGNAL(valueChanged(int)), this, SLOT(maxFeaturesChanged(int)));
    connect(
        widget.levelSpinBox, SIGNAL(valueChanged(int)), this, SLOT(levelChanged(int)));
    connect(widget.minDistanceSpinBox,
            SIGNAL(valueChanged(double)),
            this,
            SLOT(minDistanceChanged(double)));
 
 
    connected = false;
    isPaused = true;
}
 
void
FeatureLearningWidgetController::onConnectImageProcessor()
{
    objectFeatureSet = new CTexturedFeatureSet("objectFeatureSet");
    viewFeatureSet = new CTexturedFeatureSet("viewFeatureSet");
 
    featureCalculator = new CHarrisSIFTFeatureCalculator(0.001, 1, 3500);
    //    featureCalculator->SetNumberOfLevels(1);
}
 
//add the features from the current region to the selection
int
FeatureLearningWidgetController::addFeatures()
{
    int numAdded = 0;
    for (int i = 0; i < viewFeatureSet->GetSize(); i++)
    {
        CFeatureEntry* copiedEntry = viewFeatureSet->GetFeature(i)->Clone();
        Math2d::ApplyHomography(homography, copiedEntry->point, copiedEntry->point);
        bool addedSuccessfully = objectFeatureSet->AddFeature(copiedEntry, false);
        if (addedSuccessfully)
        {
            numAdded++;
        }
    }
    return numAdded;
}
 
void
FeatureLearningWidgetController::clearFeatures()
{
    objectFeatureSet->Clear();
}
 
//load a feature set from a .dat file
void
FeatureLearningWidgetController::loadFeatureSet(QString filePath)
{
    std::string filePathStr = filePath.toStdString();
    const char* filePathCStr = filePathStr.c_str();
    if (!objectFeatureSet->LoadFromFile(filePathCStr))
    {
        ARMARX_ERROR << " could not load feature set from file " << filePath;
        widget.lastActionInfoLabel->setText("Loading features failed!");
    }
    else
    {
        ARMARX_INFO << "loaded feature set with " << objectFeatureSet->GetSize() << " features";
 
        QDir currentDir;
        mySettings.setValue("default_load_dir", currentDir.absoluteFilePath(filePath));
 
        widget.lastActionInfoLabel->setText("Loaded feature set with " +
                                            QString::number(objectFeatureSet->GetSize()) +
                                            " features");
 
        float x = fabsf(2.f * objectFeatureSet->m_3dPoint2.x);
        float y = fabsf(2.f * objectFeatureSet->m_3dPoint2.y);
        float z = fabsf(2.f * objectFeatureSet->m_3dPoint2.z);
 
        int xInt = (int)x;
        int yInt = (int)y;
        int zInt = (int)z;
 
        widget.widthLineEdit->setText(QString::number(xInt));
        widget.heightLineEdit->setText(QString::number(yInt));
        widget.depthLineEdit->setText(QString::number(zInt));
    }
 
    points = Eigen::ArrayX2f::Zero(4, 2);
    numSelectedPoints = 0;
}
 
//save a feature set to a .dat file
void
FeatureLearningWidgetController::saveFeatureSet(QString filePath, float w, float h, float d)
{
    ARMARX_CHECK_EXPRESSION(objectFeatureSet->GetSize() > 0);
    ARMARX_CHECK_EXPRESSION(w > 0.f);
    ARMARX_CHECK_EXPRESSION(h > 0.f);
    ARMARX_CHECK_EXPRESSION(d > 0.f);
 
    Math3d::SetVec(objectFeatureSet->m_3dPoint1, -w / 2.0f, -h / 2.0f, -d / 2.0f);
    Math3d::SetVec(objectFeatureSet->m_3dPoint2, w / 2.0f, -h / 2.0f, -d / 2.0f);
    Math3d::SetVec(objectFeatureSet->m_3dPoint3, w / 2.0f, h / 2.0f, -d / 2.0f);
    Math3d::SetVec(objectFeatureSet->m_3dPoint4, -w / 2.0f, h / 2.0f, -d / 2.0f);
 
    std::string filePathStr = filePath.toStdString();
    const char* filePathCStr = filePathStr.c_str();
 
 
    if (!objectFeatureSet->SaveToFile(filePathCStr))
    {
        ARMARX_ERROR << " could not save feature set to file " << filePath;
        widget.lastActionInfoLabel->setText("Saving features failed!");
    }
    else
    {
        ARMARX_INFO << " saved feature set with " << objectFeatureSet->GetSize()
                    << " features to file " << filePathCStr;
 
 
        std::string matPath = filePath.toStdString();
        const unsigned int n = matPath.find_last_of(".dat");
        matPath.resize(n - 2);
        matPath += "mat";
 
        CFloatMatrix* object = createCuboid(w, h, d);
        object->SaveToFile(matPath.c_str());
        delete object;
 
        QDir currentDir;
        mySettings.setValue("default_save_dir", currentDir.absoluteFilePath(filePath));
        widget.lastActionInfoLabel->setText(
            "Saved " + QString::number(objectFeatureSet->GetSize()) + " features into file");
    }
}
 
void
FeatureLearningWidgetController::onDisconnectImageProcessor()
{
}
 
void
FeatureLearningWidgetController::onExitImageProcessor()
{
    points = Eigen::ArrayX2f::Zero(4, 2);
 
    if (connected)
    {
        for (int i = 0; i < numImages; i++)
        {
            delete cameraImages[i];
        }
 
        delete[] cameraImages;
 
        delete visualizationImage;
        delete grayImage;
    }
 
    delete featureCalculator;
 
    delete objectFeatureSet;
    delete viewFeatureSet;
 
    //    releaseImageProvider(imageProviderName);
}
 
static bool
SameSide(const Vec2d& p1, const Vec2d& p2, const Vec2d& a, const Vec2d& b)
{
    double ba0 = b.x - a.x;
    double ba1 = b.y - a.y;
 
    double p1a0 = p1.x - a.x;
    double p1a1 = p1.y - a.y;
 
    double p2a0 = p2.x - a.x;
    double p2a1 = p2.y - a.y;
 
    double cp1 = ba0 * p1a1 - ba1 * p1a0;
    double cp2 = ba0 * p2a1 - ba1 * p2a0;
 
    return cp1 * cp2 >= 0;
}
 
static bool
PointInPoly(const Vec2d& p, const Vec2d& a, const Vec2d& b, const Vec2d& c)
{
    return SameSide(p, a, b, c) && SameSide(p, b, a, c) && SameSide(p, c, a, b);
}
 
static double
DistancePointStraightLine(const Vec2d& a, const Vec2d& b, const Vec2d p)
{
    Vec2d n = {b.y - a.y, a.x - b.x};
    Math2d::NormalizeVec(n);
 
    return std::fabs(Math2d::ScalarProduct(n, p) - Math2d::ScalarProduct(n, a));
}
 
//calculate features for the current region
void
FeatureLearningWidgetController::updateFeatures()
{
    if (numSelectedPoints == 0)
    {
        viewFeatureSet->Clear();
        return;
    }
 
    // wait until region is selected
    if (numSelectedPoints < 4)
    {
        return;
    }
 
    Vec2d sourcePoints[4];
 
    for (int i = 0; i < numSelectedPoints; i++)
    {
        sourcePoints[i].x = points(i, 0) * grayImage->width;
        sourcePoints[i].y = points(i, 1) * grayImage->height;
    }
 
    const float w = widget.widthLineEdit->text().toFloat();
    const float h = widget.heightLineEdit->text().toFloat();
 
    Vec2d targetPoints[4];
    Math2d::SetVec(targetPoints[0], 0, 0);
    Math2d::SetVec(targetPoints[1], w, 0);
    Math2d::SetVec(targetPoints[2], w, h);
    Math2d::SetVec(targetPoints[3], 0, h);
 
    LinearAlgebra::DetermineHomography(sourcePoints, targetPoints, 4, homography, true);
 
    objectFeatureSet->SetCornerPoints(
        targetPoints[0], targetPoints[1], targetPoints[2], targetPoints[3]);
 
    featureCalculatorMutex.lock();
    CDynamicArray dynamic_array(featureCalculator->GetMaxInterestPoints());
    const int nFeatures = featureCalculator->CalculateFeatures(grayImage, &dynamic_array);
    featureCalculatorMutex.unlock();
    viewFeatureSet->Clear();
 
    for (int i = 0; i < nFeatures; i++)
    {
        CFeatureEntry* pFeatureEntry = (CFeatureEntry*)dynamic_array.GetElement(i);
 
        if (PointInPoly(pFeatureEntry->point, sourcePoints[0], sourcePoints[1], sourcePoints[2]) ||
            PointInPoly(pFeatureEntry->point, sourcePoints[2], sourcePoints[3], sourcePoints[0]))
        {
            const double dThreshold = 10;
 
            if (DistancePointStraightLine(sourcePoints[0], sourcePoints[1], pFeatureEntry->point) >
                    dThreshold &&
                DistancePointStraightLine(sourcePoints[1], sourcePoints[2], pFeatureEntry->point) >
                    dThreshold &&
                DistancePointStraightLine(sourcePoints[2], sourcePoints[3], pFeatureEntry->point) >
                    dThreshold &&
                DistancePointStraightLine(sourcePoints[3], sourcePoints[0], pFeatureEntry->point) >
                    dThreshold)
            {
                viewFeatureSet->AddFeature(pFeatureEntry->Clone(), false);
            }
        }
    }
 
    ARMARX_LOG << deactivateSpam(2.f) << "Features in polygon " << viewFeatureSet->GetSize();
}
 
void
FeatureLearningWidgetController::updateSelection()
{
    Vec2d sourcePoints[4];
 
    //Depending on the size of the widget, the image gets scaled up or down
    //The dimensions of the visualization image have to be regarded, to find the position of the mouse click on the image
    //Vec2d scaledImageDimensions = imageViewer->getScaledImageDimensions();
    for (int i = 0; i < numSelectedPoints; i++)
    {
        float relativeX = points(i, 0);
        float relativeY = points(i, 1);
 
        sourcePoints[i].x = relativeX * visualizationImage->width;
        sourcePoints[i].y = relativeY * visualizationImage->height;
        //sourcePoints[i].x = points(i, 0);
        //sourcePoints[i].y = points(i, 1);
    }
 
    if (numSelectedPoints >= 1)
    {
        PrimitivesDrawer::DrawCircle(visualizationImage, sourcePoints[0], 3, 255, 0, 0, -1);
        //ARMARX_INFO << deactivateSpam(2.f) << "DRAWING POINT 1 AT (" << sourcePoints[0].x << ", " << sourcePoints[0].y << ")";
    }
    if (numSelectedPoints >= 2)
    {
        PrimitivesDrawer::DrawCircle(visualizationImage, sourcePoints[1], 3, 255, 0, 0, -1);
        PrimitivesDrawer::DrawLine(visualizationImage, sourcePoints[0], sourcePoints[1], 0, 0, 255);
        //ARMARX_INFO << deactivateSpam(2.f) << "DRAWING POINT 2 AT (" << sourcePoints[1].x << ", " << sourcePoints[1].y << ")";
    }
    if (numSelectedPoints >= 3)
    {
        PrimitivesDrawer::DrawCircle(visualizationImage, sourcePoints[2], 3, 255, 0, 0, -1);
        PrimitivesDrawer::DrawLine(visualizationImage, sourcePoints[1], sourcePoints[2], 0, 0, 255);
        //ARMARX_INFO << deactivateSpam(2.f) << "DRAWING POINT 3 AT (" << sourcePoints[2].x << ", " << sourcePoints[2].y << ")";
    }
    if (numSelectedPoints >= 4)
    {
        PrimitivesDrawer::DrawCircle(visualizationImage, sourcePoints[3], 3, 255, 0, 0, -1);
        PrimitivesDrawer::DrawLine(visualizationImage, sourcePoints[2], sourcePoints[3], 0, 0, 255);
        PrimitivesDrawer::DrawLine(visualizationImage, sourcePoints[3], sourcePoints[0], 0, 0, 255);
        //ARMARX_INFO << deactivateSpam(2.f) << "DRAWING POINT 4 AT (" << sourcePoints[3].x << ", " << sourcePoints[3].y << ")";
    }
 
 
    // draw features
    if (numSelectedPoints >= 4)
    {
        for (int i = 0; i < viewFeatureSet->GetSize(); i++)
        {
            const CFeatureEntry* pFeatureEntry = viewFeatureSet->GetFeature(i);
            PrimitivesDrawer::DrawCircle(
                visualizationImage, pFeatureEntry->point, 2, 0, 255, 0, -1);
        }
    }
}
 
void
FeatureLearningWidgetController::process()
{
 
    //std::unique_lock<std::mutex> lock(imageMutex, std::try_to_lock);
    std::unique_lock lock(imageMutex);
 
    if (!lock.owns_lock())
    {
        ARMARX_INFO << deactivateSpam(3) << "unable to process next image(" << lock.owns_lock()
                    << ")";
        return;
    }
 
    if (!connected)
    {
        ARMARX_INFO << deactivateSpam(3) << "Not connected to any image provider";
        return;
    }
 
    armarx::MetaInfoSizeBasePtr info;
    IceUtil::Time timeReceived;
    if (!isPaused)
    {
        //only update current image, if unpaused
        if (!waitForImages(imageProviderName))
        {
            ARMARX_WARNING << "Timeout while waiting for images";
            return;
        }
        if (getImages(imageProviderName, cameraImages, info) != numImages)
        {
            ARMARX_WARNING << "Unable to transfer or read images";
            return;
        }
        timeReceived = TimeUtil::GetTime();
    }
 
    //create grayscale image for feature calculation and color image for the visualization
    ::ImageProcessor::ConvertImage(cameraImages[0], grayImage);
    ::ImageProcessor::CopyImage(cameraImages[0], visualizationImage);
 
    updateFeatures();
    updateSelection();
 
    //update the availability of the buttons etc.
    updateFeaturesUI();
 
 
    CByteImage** images = new CByteImage* {visualizationImage};
    imageViewer->setImages(
        1, images, IceUtil::Time::microSeconds(info->timeProvided), timeReceived);
}
 
void
FeatureLearningWidgetController::addPolygonPoint(int, float x, float y)
{
    if (!connected)
    {
        return;
    }
 
    if (numSelectedPoints >= 4)
    {
        numSelectedPoints = 0;
    }
 
    points(numSelectedPoints, 0) = x;
    points(numSelectedPoints, 1) = y;
 
    numSelectedPoints++;
}
 
void
FeatureLearningWidgetController::connectButtonClicked(bool toggled)
{
    reconnect();
}
 
//opens a QDialog with the last used load location and calls the feature loading function
void
FeatureLearningWidgetController::loadFeaturesButtonClicked(bool toggled)
{
    QString previousLoadDirectory = mySettings.value("default_load_dir").toString();
    if (previousLoadDirectory == "")
    {
        QString previousSaveDirectory = mySettings.value("default_save_dir").toString();
        if (previousSaveDirectory == "")
        {
            previousLoadDirectory = "~";
        }
        else
        {
            previousLoadDirectory = previousSaveDirectory;
        }
    }
    QString filePath = QFileDialog::getOpenFileName(
        widgetPointer, "Load Features", previousLoadDirectory, "(*.dat)");
    loadFeatureSet(filePath);
}
 
//checks if features are selected and opens a QDialog to choose the save location
void
FeatureLearningWidgetController::saveFeaturesButtonClicked(bool toggled)
{
    if (objectFeatureSet->GetSize() < 1)
    {
        QMessageBox emptyFeatureSetBox;
        emptyFeatureSetBox.setText("Cannot save empty feature set!");
        emptyFeatureSetBox.setWindowTitle("Feature set empty");
        emptyFeatureSetBox.exec();
        return;
    }
    float width = widget.widthLineEdit->text().toFloat();
    float height = widget.heightLineEdit->text().toFloat();
    float depth = widget.depthLineEdit->text().toFloat();
 
    if (width <= 0.f || height <= 0.f || depth <= 0.f)
    {
        QMessageBox invalidDimensionsBox;
        invalidDimensionsBox.setText("Cannot save feature set with object dimensions <= 0!");
        invalidDimensionsBox.setWindowTitle("Invalid object dimensions");
        invalidDimensionsBox.exec();
        return;
    }
 
    QString previousSaveDirectory = mySettings.value("default_save_dir").toString();
    if (previousSaveDirectory == "")
    {
        QString previousLoadDirectory = mySettings.value("default_load_dir").toString();
        if (previousLoadDirectory == "")
        {
            previousSaveDirectory = "~";
        }
        else
        {
            previousSaveDirectory = previousLoadDirectory;
        }
    }
    QString filePath = QFileDialog::getSaveFileName(
        widgetPointer, "Save Features", previousSaveDirectory, "(*.dat)");
    if (filePath.right(4) != ".dat")
    {
        filePath += ".dat";
    }
    saveFeatureSet(filePath, width, height, depth);
}
 
void
FeatureLearningWidgetController::pausePlayButtonClicked(bool toggled)
{
    if (!connected)
    {
        isPaused = false;
    }
    else
    {
        isPaused = !isPaused;
    }
    updateImageMonitorUI();
}
 
//disconncts and connects again
void
FeatureLearningWidgetController::reconnect()
{
    std::unique_lock lock(imageMutex);
 
    disconnectFromProvider();
 
    // connect to provider
    try
    {
        connectToProvider();
    }
    catch (...)
    {
        armarx::handleExceptions();
    }
}
 
//trying to connect to the image provider
void
FeatureLearningWidgetController::connectToProvider()
{
    if (connected)
    {
        return;
    }
 
 
    imageProviderName = proxyFinder->getSelectedProxyName().toStdString();
    ARMARX_INFO << "Trying to connect to provider " << imageProviderName;
 
    try
    {
        usingImageProvider(imageProviderName);
    }
    catch (Ice::NotRegisteredException& e)
    {
        ARMARX_ERROR << "Cannot connect to the given image provider called " << imageProviderName;
 
        QMessageBox cannotConnectToProviderBox;
        QString cannotConnectToProviderString =
            "Cannot connect to the given image provider called " +
            QString::fromStdString(imageProviderName);
        cannotConnectToProviderBox.setText(cannotConnectToProviderString);
        cannotConnectToProviderBox.setWindowTitle("Cannot connect to Image Provider");
        cannotConnectToProviderBox.exec();
        return;
    }
    catch (armarx::LocalException& e)
    {
        ARMARX_ERROR << "Image Provider field is empty, cannot connect!" << imageProviderName;
 
        QMessageBox noProviderGivenBox;
        QString cannotConnectToProviderString =
            "Cannot connect to an image provider since the field is empty.";
        noProviderGivenBox.setText(cannotConnectToProviderString);
        noProviderGivenBox.setWindowTitle("No Provider given");
        noProviderGivenBox.exec();
        return;
    }
 
    ARMARX_INFO << getName() << " connecting to " << imageProviderName;
 
    // connect to proxy
    imageProviderInfo =
        getImageProvider(imageProviderName, visionx::tools::typeNameToImageType("rgb"));
 
    // update members
    imageProviderPrx = imageProviderInfo.proxy;
    numImages = imageProviderInfo.numberImages;
 
    StereoCalibrationProviderInterfacePrx calibrationProvider =
        StereoCalibrationProviderInterfacePrx::checkedCast(imageProviderInfo.proxy);
 
    if (!calibrationProvider)
    {
        ARMARX_WARNING << "image provider does not have a stereo calibration interface";
    }
    else
    {
        undistortImages = calibrationProvider->getImagesAreUndistorted();
        if (!undistortImages)
        {
            ARMARX_ERROR << "Images expected to be undistorted, but the calibration returns that "
                            "the images are distorted";
        }
        // visionx::StereoCalibration stereoCalibration = calibrationProvider->getStereoCalibration();
        // CStereoCalibration* ivtStereoCalibration = visionx::tools::convert(stereoCalibration);
    }
 
 
    // create images
    {
        std::unique_lock lock(imageMutex);
        cameraImages = new CByteImage*[numImages];
 
        for (int i = 0; i < numImages; i++)
        {
            cameraImages[i] = visionx::tools::createByteImage(imageProviderInfo);
        }
    }
 
    visualizationImage = visionx::tools::createByteImage(imageProviderInfo);
    grayImage = new CByteImage(imageProviderInfo.imageFormat.dimension.width,
                               imageProviderInfo.imageFormat.dimension.height,
                               CByteImage::eGrayScale);
 
 
    timeProvided = IceUtil::Time::seconds(0);
 
    isPaused = false;
    connected = true;
    updateImageMonitorUI();
    updateFeaturesUI();
 
 
    emit imageProviderConnected(true);
}
 
//disconnecting the image provider
void
FeatureLearningWidgetController::disconnectFromProvider()
{
    if (!connected)
    {
        return;
    }
    ARMARX_INFO << "Disconnecting from provider";
    {
        std::unique_lock lock(imageMutex);
        // clear images
        if (cameraImages)
        {
            for (int i = 0; i < numImages; i++)
            {
                delete cameraImages[i];
            }
 
            delete[] cameraImages;
        }
        delete visualizationImage;
        delete grayImage;
    }
 
    releaseImageProvider(imageProviderName);
 
    imageProviderPrx = 0;
 
    connected = false;
    isPaused = true;
    numSelectedPoints = 0;
 
    QString emptyFeaturesInfoText = "";
    widget.lastActionInfoLabel->setText(emptyFeaturesInfoText);
 
    updateImageMonitorUI();
    updateFeaturesUI();
 
    emit imageProviderConnected(false);
}
 
void
FeatureLearningWidgetController::updatePausePlayButtonText()
{
    if (isPaused)
    {
        widget.pausePlayButton->setText("Play");
        return;
    }
    widget.pausePlayButton->setText("Pause");
}
 
void
FeatureLearningWidgetController::updateImageMonitorUI()
{
    updatePausePlayButtonText();
 
    widget.disconnectButton->setEnabled(connected);
    widget.pausePlayButton->setEnabled(connected);
 
    if (connected)
    {
        QString providerName = imageProviderName.c_str();
        QString statusString = "Status: connected to image provider " + providerName;
        if (isPaused)
        {
            statusString = statusString + " - PAUSED";
        }
        widget.imageMonitorStatusLabel->setText(statusString);
    }
    else
    {
        widget.imageMonitorStatusLabel->setText("Status: not connected");
    }
}
 
void
FeatureLearningWidgetController::disconnectButtonClicked(bool toggled)
{
    disconnectFromProvider();
}
 
void
FeatureLearningWidgetController::addFeaturesButtonClicked(bool toggled)
{
    int numFeaturesAdded = addFeatures();
    QString featuresString = " features";
    if (numFeaturesAdded == 1)
    {
        featuresString = " feature";
    }
    QString addedFeaturesText =
        "Added " + QString::number(numFeaturesAdded) + featuresString + " to the selection";
    widget.lastActionInfoLabel->setText(addedFeaturesText);
    updateFeaturesUI();
}
 
void
FeatureLearningWidgetController::clearFeaturesButtonClicked(bool toggled)
{
    clearFeatures();
    QString clearedFeaturesText = "Cleared the selected features";
    widget.lastActionInfoLabel->setText(clearedFeaturesText);
    updateFeaturesUI();
}
 
//updates the information about the features on the right part of the ui
void
FeatureLearningWidgetController::updateFeaturesUI()
{
    if (!connected)
    {
        widget.addFeaturesButton->setEnabled(false);
        widget.clearFeaturesButton->setEnabled(false);
        widget.featuresInRegionLabel->setText("");
        widget.featuresSelectedLabel->setText("");
        return;
    }
    widget.addFeaturesButton->setEnabled(true);
    widget.clearFeaturesButton->setEnabled(true);
    QString featuresInRegionString = "Features in Region: ";
    if (numSelectedPoints == 4)
    {
        featuresInRegionString += QString::number(viewFeatureSet->GetSize());
    }
    widget.featuresInRegionLabel->setText(featuresInRegionString);
 
    QString featuresSelectedString =
        "Features selected: " + QString::number(objectFeatureSet->GetSize());
    widget.featuresSelectedLabel->setText(featuresSelectedString);
}
 
CFloatMatrix*
FeatureLearningWidgetController::createCuboid(float w, float h, float d)
{
    CFloatMatrix* object = new CFloatMatrix(6, 36);
 
    int offset = 0;
    addRectangle(object, offset, 0, 0, 0, w, 0, 0, w, h, 0, 0, h, 0, 0, 0, -1);
    addRectangle(object, offset, 0, 0, d, w, 0, d, w, h, d, 0, h, d, 0, 0, 1);
    addRectangle(object, offset, 0, 0, 0, 0, 0, d, 0, h, d, 0, h, 0, -1, 0, 0);
    addRectangle(object, offset, w, 0, 0, w, 0, d, w, h, d, w, h, 0, 1, 0, 0);
    addRectangle(object, offset, 0, 0, 0, 0, 0, d, w, 0, d, w, 0, 0, 0, -1, 0);
    addRectangle(object, offset, 0, h, 0, 0, h, d, w, h, d, w, h, 0, 0, 1, 0);
 
    for (int i = 0; i < 36; i++)
    {
        const int offset = 6 * i;
        object->data[offset + 3] -= w / 2.0;
        object->data[offset + 4] -= h / 2.0;
        object->data[offset + 5] -= d / 2.0;
    }
 
    return object;
}
 
void
FeatureLearningWidgetController::addRectangle(CFloatMatrix* object,
                                              int& offset,
                                              float x1,
                                              float y1,
                                              float z1,
                                              float x2,
                                              float y2,
                                              float z2,
                                              float x3,
                                              float y3,
                                              float z3,
                                              float x4,
                                              float y4,
                                              float z4,
                                              float n1,
                                              float n2,
                                              float n3)
{
    addTriangle(object, offset, x1, y1, z1, x2, y2, z2, x3, y3, z3, n1, n2, n3);
    addTriangle(object, offset, x4, y4, z4, x1, y1, z1, x3, y3, z3, n1, n2, n3);
}
 
void
FeatureLearningWidgetController::addTriangle(CFloatMatrix* object,
                                             int& offset,
                                             float x1,
                                             float y1,
                                             float z1,
                                             float x2,
                                             float y2,
                                             float z2,
                                             float x3,
                                             float y3,
                                             float z3,
                                             float n1,
                                             float n2,
                                             float n3)
{
    float* data = object->data;
 
    data[offset + 0] = n1;
    data[offset + 1] = n2;
    data[offset + 2] = n3;
    data[offset + 3] = x1;
    data[offset + 4] = y1;
    data[offset + 5] = z1;
    offset += 6;
 
    data[offset + 0] = n1;
    data[offset + 1] = n2;
    data[offset + 2] = n3;
    data[offset + 3] = x2;
    data[offset + 4] = y2;
    data[offset + 5] = z2;
    offset += 6;
 
    data[offset + 0] = n1;
    data[offset + 1] = n2;
    data[offset + 2] = n3;
    data[offset + 3] = x3;
    data[offset + 4] = y3;
    data[offset + 5] = z3;
    offset += 6;
}
 
void
FeatureLearningWidgetController::thresholdChanged(double threshold)
{
    std::lock_guard g{featureCalculatorMutex};
    featureCalculator->SetThreshold(threshold);
}
 
void
FeatureLearningWidgetController::maxFeaturesChanged(int maxFeatures)
{
    std::lock_guard g{featureCalculatorMutex};
    featureCalculator->SetMaxInterestPoints(maxFeatures);
}
 
void
FeatureLearningWidgetController::minDistanceChanged(double minDistance)
{
    std::lock_guard g{featureCalculatorMutex};
    featureCalculator->SetMinDistance(minDistance);
}
 
void
FeatureLearningWidgetController::levelChanged(int level)
{
    std::lock_guard g{featureCalculatorMutex};
    featureCalculator->SetNumberOfLevels(level);
}