PrimitiveExtractionWidgetController.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::PrimitiveExtractionWidgetController
 * \author     Peter Kaiser ( peter dot kaiser at kit dot edu )
 * \date       2016
 * \copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "PrimitiveExtractionWidgetController.h"
 
#include <filesystem>
#include <memory>
#include <string>
 
#include <QCheckBox>
#include <QComboBox>
#include <QDoubleSpinBox>
 
#include <pcl/common/transforms.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
#include <VisionX/interface/core/PointCloudProviderInterface.h>
#include <VisionX/libraries/AffordanceKitArmarX/PrimitiveSetArmarX.h>
 
#include <AffordanceKit/primitives/Plane.h>
 
#define PI 3.141592654
 
namespace armarx
{
    PrimitiveExtractionWidgetController::PrimitiveExtractionWidgetController()
    {
        widget.setupUi(getWidget());
    }
 
    PrimitiveExtractionWidgetController::~PrimitiveExtractionWidgetController()
    {
    }
 
    void
    PrimitiveExtractionWidgetController::loadSettings(QSettings* settings)
    {
        pointCloudProviderName =
            settings->value("pointCloudProviderName", "").toString().toStdString();
        pointCloudSegmenterName =
            settings->value("pointCloudSegmenterName", "").toString().toStdString();
        primitiveExtractorName =
            settings->value("primitiveExtractorName", "").toString().toStdString();
        pipelineVisualizationName =
            settings->value("pipelineVisualizationName", "").toString().toStdString();
        workingMemoryName = settings->value("workingMemoryName", "").toString().toStdString();
    }
 
    void
    PrimitiveExtractionWidgetController::saveSettings(QSettings* settings)
    {
        settings->setValue("pointCloudProviderName",
                           QString::fromStdString(pointCloudProviderName));
        settings->setValue("pointCloudSegmenterName",
                           QString::fromStdString(pointCloudSegmenterName));
        settings->setValue("primitiveExtractorName",
                           QString::fromStdString(primitiveExtractorName));
        settings->setValue("pipelineVisualizationName",
                           QString::fromStdString(pipelineVisualizationName));
        settings->setValue("workingMemoryName", QString::fromStdString(workingMemoryName));
    }
 
    QPointer<QDialog>
    PrimitiveExtractionWidgetController::getConfigDialog(QWidget* parent)
    {
        if (!configDialog)
        {
            configDialog = new PrimitiveExtractionConfigDialog(parent);
        }
 
        return qobject_cast<PrimitiveExtractionConfigDialog*>(configDialog);
    }
 
    void
    PrimitiveExtractionWidgetController::configured()
    {
        pointCloudProviderName =
            configDialog->pointCloudProviderProxyFinder->getSelectedProxyName().toStdString();
        pointCloudSegmenterName =
            configDialog->pointCloudSegmenterProxyFinder->getSelectedProxyName().toStdString();
        primitiveExtractorName =
            configDialog->primitiveExtractorProxyFinder->getSelectedProxyName().toStdString();
        pipelineVisualizationName =
            configDialog->pipelineVisualizationProxyFinder->getSelectedProxyName().toStdString();
        workingMemoryName =
            configDialog->pipelineVisualizationProxyFinder->getSelectedProxyName().toStdString();
    }
 
    void
    PrimitiveExtractionWidgetController::onInitComponent()
    {
    }
 
    void
    PrimitiveExtractionWidgetController::onConnectComponent()
    {
        pointCloudProvider =
            getProxy<visionx::PointCloudProviderInterfacePrx>(pointCloudProviderName);
        if (!pointCloudProvider)
        {
            ARMARX_ERROR << "Could not obtain point cloud provider proxy";
            return;
        }
 
        visionx::FakePointCloudProviderInterfacePrx _fakePointCloudProvider =
            visionx::FakePointCloudProviderInterfacePrx::checkedCast(pointCloudProvider);
        if (_fakePointCloudProvider)
        {
            widget.pushButtonLoadPointCloud->setEnabled(true);
        }
        else
        {
            widget.pushButtonLoadPointCloud->setEnabled(false);
        }
 
        // The segmenter is optional (it does not exist when processing ground truth segmentations)
        if (pointCloudSegmenterName != "")
        {
            try
            {
                pointCloudSegmenter =
                    getProxy<visionx::PointCloudSegmenterInterfacePrx>(pointCloudSegmenterName);
            }
            catch (const armarx::UserException& e)
            {
                ARMARX_INFO << "No segmenter found, assuming ground truth segmentation";
            }
        }
 
        primitiveExtractor = getProxy<visionx::PrimitiveMapperInterfacePrx>(primitiveExtractorName);
        if (!primitiveExtractor)
        {
            ARMARX_ERROR << "Could not obtain primitive extractor proxy";
            return;
        }
 
        pipelineVisualization =
            getProxy<AffordancePipelineVisualizationInterfacePrx>(pipelineVisualizationName);
        if (!pipelineVisualization)
        {
            ARMARX_ERROR << "Could not obtain affordance pipeline visualization proxy";
            return;
        }
 
        workingMemory = getProxy<memoryx::WorkingMemoryInterfacePrx>(workingMemoryName);
        if (!workingMemory)
        {
            ARMARX_ERROR << "Could not obtain point working memory proxy";
            return;
        }
 
        debugDrawer = getTopic<DebugDrawerInterfacePrx>("DebugDrawerUpdates");
        if (!debugDrawer)
        {
            ARMARX_ERROR << "Failed to obtain debug drawer proxy";
            return;
        }
 
        connect(widget.pushButtonLoadPointCloud, SIGNAL(clicked()), this, SLOT(loadPointCloud()));
        connect(widget.pushButtonRun, SIGNAL(clicked()), this, SLOT(run()));
        connect(widget.pushButtonSetParam, SIGNAL(clicked()), this, SLOT(setParameters()));
        connect(widget.pushButtonApplyTransformation,
                SIGNAL(clicked()),
                this,
                SLOT(applyPointCloudTransformation()));
        connect(
            widget.pushButtonApplyAutoRotate, SIGNAL(clicked()), this, SLOT(applyAutoRotation()));
        connect(widget.checkBoxShowPrimitives,
                SIGNAL(toggled(bool)),
                this,
                SLOT(visualizationOptionsChanged()));
        connect(widget.checkBoxShowRootPose,
                SIGNAL(toggled(bool)),
                this,
                SLOT(visualizationOptionsChanged()));
        connect(widget.pushButtonImportConfig, SIGNAL(clicked()), this, SLOT(importConfig()));
        connect(widget.pushButtonExportConfig, SIGNAL(clicked()), this, SLOT(exportConfig()));
 
        if (pointCloudSegmenter)
        {
            visionx::LccpParameters lccpParameters = pointCloudSegmenter->getLccpParameters();
            widget.doubleSpinBoxLccpMinSegmentSize->setValue(lccpParameters.minSegmentSize);
            widget.doubleSpinBoxLccpVoxelResolution->setValue(lccpParameters.voxelResolution);
            widget.doubleSpinBoxLccpSeedResolution->setValue(lccpParameters.seedResolution);
            widget.doubleSpinBoxLccpColorImportance->setValue(lccpParameters.colorImportance);
            widget.doubleSpinBoxLccpSpatialImportance->setValue(lccpParameters.spatialImportance);
            widget.doubleSpinBoxLccpNormalImportance->setValue(lccpParameters.normalImportance);
            widget.doubleSpinBoxLccpConcavityThreshold->setValue(lccpParameters.concavityThreshold);
            widget.doubleSpinBoxLccpSmoothnessThreshold->setValue(
                lccpParameters.smoothnessThreshold);
        }
        else
        {
            widget.doubleSpinBoxLccpMinSegmentSize->setEnabled(false);
            widget.doubleSpinBoxLccpVoxelResolution->setEnabled(false);
            widget.doubleSpinBoxLccpSeedResolution->setEnabled(false);
            widget.doubleSpinBoxLccpColorImportance->setEnabled(false);
            widget.doubleSpinBoxLccpSpatialImportance->setEnabled(false);
            widget.doubleSpinBoxLccpNormalImportance->setEnabled(false);
            widget.doubleSpinBoxLccpConcavityThreshold->setEnabled(false);
            widget.doubleSpinBoxLccpSmoothnessThreshold->setEnabled(false);
        }
 
        visionx::PrimitiveExtractorParameters primtiveParameters =
            primitiveExtractor->getParameters();
        ARMARX_LOG << "parameters " << primtiveParameters.minSegmentSize;
        widget.doubleSpinBoxMinPrimitiveSize->setValue(primtiveParameters.minSegmentSize);
        widget.doubleSpinBoxMaxPrimitiveSize->setValue(primtiveParameters.maxSegmentSize);
        widget.doubleSpinBoxEuclideanClusteringTolerance->setValue(
            primtiveParameters.euclideanClusteringTolerance);
        widget.doubleSpinBoxOutlierDistanceThreshold->setValue(primtiveParameters.outlierThreshold);
 
        widget.doubleSpinBoxPlaneMaxIterations->setValue(primtiveParameters.planeMaxIterations);
        widget.doubleSpinBoxPlaneDistanceThreshold->setValue(
            primtiveParameters.planeDistanceThreshold);
        widget.doubleSpinBoxPlaneNormalDistance->setValue(primtiveParameters.planeNormalDistance);
        widget.doubleSpinBoxPlaneCircularDistanceThreshold->setValue(
            primtiveParameters.circularDistanceThreshold);
 
        widget.doubleSpinBoxCylinderMaxIterations->setValue(
            primtiveParameters.cylinderMaxIterations);
        widget.doubleSpinBoxCylinderDistanceThreshold->setValue(
            primtiveParameters.cylinderDistanceThreshold);
        widget.doubleSpinBoxCylinderRadiusLimit->setValue(primtiveParameters.cylinderRadiusLimit);
 
        widget.doubleSpinBoxSphereMaxIterations->setValue(primtiveParameters.sphereMaxIterations);
        widget.doubleSpinBoxSphereDistanceThreshold->setValue(
            primtiveParameters.sphereDistanceThreshold);
        widget.doubleSpinBoxSphereNormalDistance->setValue(primtiveParameters.sphereNormalDistance);
    }
 
    void
    PrimitiveExtractionWidgetController::onDisconnectComponent()
    {
        QObject::disconnect(this, SLOT(loadPointCloud()));
        QObject::disconnect(this, SLOT(run()));
        QObject::disconnect(this, SLOT(setParameters()));
        QObject::disconnect(this, SLOT(visualizationOptionsChanged()));
        QObject::disconnect(this, SLOT(importConfig()));
        QObject::disconnect(this, SLOT(exportConfig()));
    }
 
    void
    PrimitiveExtractionWidgetController::loadPointCloud()
    {
        QString filename =
            QFileDialog::getOpenFileName(NULL, "Open Point Cloud", "", "Point Cloud Files (*.pcd)");
        if (filename != "")
        {
            widget.lineEditPointCloud->setText(filename);
        }
    }
 
    void
    PrimitiveExtractionWidgetController::setParameters()
    {
        visionx::FakePointCloudProviderInterfacePrx _fakePointCloudProvider =
            visionx::FakePointCloudProviderInterfacePrx::checkedCast(pointCloudProvider);
        if (_fakePointCloudProvider)
        {
            std::string path = widget.lineEditPointCloud->text().toStdString();
            _fakePointCloudProvider->setPointCloudFilename(path);
        }
 
        if (pointCloudSegmenter)
        {
            visionx::LccpParameters segmenterPrm;
            segmenterPrm.minSegmentSize = widget.doubleSpinBoxLccpMinSegmentSize->value();
            segmenterPrm.voxelResolution = widget.doubleSpinBoxLccpVoxelResolution->value();
            segmenterPrm.seedResolution = widget.doubleSpinBoxLccpSeedResolution->value();
            segmenterPrm.colorImportance = widget.doubleSpinBoxLccpColorImportance->value();
            segmenterPrm.spatialImportance = widget.doubleSpinBoxLccpSpatialImportance->value();
            segmenterPrm.normalImportance = widget.doubleSpinBoxLccpNormalImportance->value();
            segmenterPrm.concavityThreshold = widget.doubleSpinBoxLccpConcavityThreshold->value();
            segmenterPrm.smoothnessThreshold = widget.doubleSpinBoxLccpSmoothnessThreshold->value();
            pointCloudSegmenter->setLccpParameters(segmenterPrm);
        }
 
        visionx::PrimitiveExtractorParameters primitiveExtractionPrm;
        primitiveExtractionPrm.minSegmentSize = widget.doubleSpinBoxMinPrimitiveSize->value();
        primitiveExtractionPrm.maxSegmentSize = widget.doubleSpinBoxMaxPrimitiveSize->value();
        primitiveExtractionPrm.planeMaxIterations = widget.doubleSpinBoxPlaneMaxIterations->value();
        primitiveExtractionPrm.planeDistanceThreshold =
            widget.doubleSpinBoxPlaneDistanceThreshold->value();
        primitiveExtractionPrm.planeNormalDistance =
            widget.doubleSpinBoxPlaneNormalDistance->value();
        primitiveExtractionPrm.cylinderMaxIterations =
            widget.doubleSpinBoxCylinderMaxIterations->value();
        primitiveExtractionPrm.cylinderDistanceThreshold =
            widget.doubleSpinBoxCylinderDistanceThreshold->value();
        primitiveExtractionPrm.cylinderRadiusLimit =
            widget.doubleSpinBoxCylinderRadiusLimit->value();
        primitiveExtractionPrm.sphereMaxIterations =
            widget.doubleSpinBoxSphereMaxIterations->value();
        primitiveExtractionPrm.sphereDistanceThreshold =
            widget.doubleSpinBoxSphereDistanceThreshold->value();
        primitiveExtractionPrm.sphereNormalDistance =
            widget.doubleSpinBoxSphereNormalDistance->value();
        primitiveExtractionPrm.euclideanClusteringTolerance =
            widget.doubleSpinBoxEuclideanClusteringTolerance->value();
        primitiveExtractionPrm.outlierThreshold =
            widget.doubleSpinBoxOutlierDistanceThreshold->value();
        primitiveExtractionPrm.circularDistanceThreshold =
            widget.doubleSpinBoxPlaneCircularDistanceThreshold->value();
        primitiveExtractor->setParameters(primitiveExtractionPrm);
    }
 
    void
    PrimitiveExtractionWidgetController::run()
    {
        setParameters();
 
        visionx::CapturingPointCloudProviderInterfacePrx cx =
            visionx::CapturingPointCloudProviderInterfacePrx::checkedCast(pointCloudProvider);
        if (cx)
        {
            cx->begin_startCaptureForNumFrames(1);
        }
    }
 
    void
    PrimitiveExtractionWidgetController::visualizationOptionsChanged()
    {
        pipelineVisualization->begin_enableVisualization(
            widget.checkBoxShowPrimitives->isChecked(), false, false);
 
        if (widget.checkBoxShowRootPose->isChecked())
        {
            debugDrawer->setPoseVisu(
                "PrimitiveExtractorGuiLayer", "RootPose", new Pose(Eigen::Matrix4f::Identity()));
        }
        else
        {
            debugDrawer->removePoseVisu("PrimitiveExtractorGuiLayer", "RootPose");
        }
    }
 
    void
    PrimitiveExtractionWidgetController::applyPointCloudTransformation()
    {
        std::string filename = widget.lineEditPointCloud->text().toStdString();
 
        if (!boost::algorithm::ends_with(filename, ".pcd") ||
            !std::filesystem::is_regular_file(filename))
        {
            ARMARX_WARNING << "Unable to load point cloud " << filename;
            return;
        }
 
        pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pc(new pcl::PointCloud<pcl::PointXYZRGBA>());
        if (pcl::io::loadPCDFile<pcl::PointXYZRGBA>(filename.c_str(), *pc) == -1)
        {
            ARMARX_WARNING << "Unable to load point cloud " << filename;
            return;
        }
 
        ARMARX_INFO << "Transforming point cloud: " << filename;
        Eigen::Affine3f scaling(Eigen::Scaling((float)widget.doubleSpinBoxScaling->value()));
        pcl::transformPointCloud(*pc, *pc, scaling);
 
        std::string out_filename =
            QFileDialog::getSaveFileName(
                NULL, "Save transformed PCD", filename.c_str(), "Point Cloud Files (*.pcd)")
                .toStdString();
        if (filename != "")
        {
            pcl::io::savePCDFileASCII(out_filename, *pc);
            ARMARX_INFO << "Point cloud saved";
        }
    }
 
    void
    PrimitiveExtractionWidgetController::importConfig()
    {
        QString filename = QFileDialog::getOpenFileName(
            NULL, "Open Pipeline Configuration file", "", "Config file (*.cfg)");
        if (filename == "")
        {
            return;
        }
 
        QSettings config(filename, QSettings::IniFormat);
 
        config.beginGroup("LCCP");
        widget.doubleSpinBoxLccpMinSegmentSize->setValue(
            config.value("MinSegmentSize", widget.doubleSpinBoxLccpMinSegmentSize->value())
                .toDouble());
        widget.doubleSpinBoxLccpVoxelResolution->setValue(
            config.value("VoxelResolution", widget.doubleSpinBoxLccpVoxelResolution->value())
                .toDouble());
        widget.doubleSpinBoxLccpSeedResolution->setValue(
            config.value("SeedResolution", widget.doubleSpinBoxLccpSeedResolution->value())
                .toDouble());
        widget.doubleSpinBoxLccpColorImportance->setValue(
            config.value("ColorImportance", widget.doubleSpinBoxLccpColorImportance->value())
                .toDouble());
        widget.doubleSpinBoxLccpSpatialImportance->setValue(
            config.value("SpatialImportance", widget.doubleSpinBoxLccpSpatialImportance->value())
                .toDouble());
        widget.doubleSpinBoxLccpNormalImportance->setValue(
            config.value("NormalImportance", widget.doubleSpinBoxLccpNormalImportance->value())
                .toDouble());
        widget.doubleSpinBoxLccpConcavityThreshold->setValue(
            config.value("ConcavityThreshold", widget.doubleSpinBoxLccpConcavityThreshold->value())
                .toDouble());
        widget.doubleSpinBoxLccpSmoothnessThreshold->setValue(
            config
                .value("SmoothnessThreshold", widget.doubleSpinBoxLccpSmoothnessThreshold->value())
                .toDouble());
        config.endGroup();
 
        config.beginGroup("PrimitiveExtraction");
        widget.doubleSpinBoxMinPrimitiveSize->setValue(
            config.value("MinPrimitiveSize", widget.doubleSpinBoxMinPrimitiveSize->value())
                .toDouble());
        widget.doubleSpinBoxMaxPrimitiveSize->setValue(
            config.value("MaxPrimitiveSize", widget.doubleSpinBoxMaxPrimitiveSize->value())
                .toDouble());
        widget.doubleSpinBoxPlaneMaxIterations->setValue(
            config.value("PlaneMaxIterations", widget.doubleSpinBoxPlaneMaxIterations->value())
                .toDouble());
        widget.doubleSpinBoxPlaneDistanceThreshold->setValue(
            config
                .value("PlaneDistanceThreshold",
                       widget.doubleSpinBoxPlaneDistanceThreshold->value())
                .toDouble());
        widget.doubleSpinBoxPlaneNormalDistance->setValue(
            config.value("PlaneNormalDistance", widget.doubleSpinBoxPlaneNormalDistance->value())
                .toDouble());
        widget.doubleSpinBoxPlaneCircularDistanceThreshold->setValue(
            config
                .value("PlaneCircularDistanceThreshold",
                       widget.doubleSpinBoxPlaneCircularDistanceThreshold->value())
                .toDouble());
        widget.doubleSpinBoxCylinderMaxIterations->setValue(
            config
                .value("CylinderMaxIterations", widget.doubleSpinBoxCylinderMaxIterations->value())
                .toDouble());
        widget.doubleSpinBoxCylinderDistanceThreshold->setValue(
            config
                .value("CylinderDistanceThreshold",
                       widget.doubleSpinBoxCylinderDistanceThreshold->value())
                .toDouble());
        widget.doubleSpinBoxCylinderRadiusLimit->setValue(
            config.value("CylinderRadiusLimit", widget.doubleSpinBoxSphereMaxIterations->value())
                .toDouble());
        widget.doubleSpinBoxSphereMaxIterations->setValue(
            config.value("SphereMaxIterations", widget.doubleSpinBoxSphereMaxIterations->value())
                .toDouble());
        widget.doubleSpinBoxSphereDistanceThreshold->setValue(
            config
                .value("SphereDistanceThreshold",
                       widget.doubleSpinBoxSphereDistanceThreshold->value())
                .toDouble());
        widget.doubleSpinBoxSphereNormalDistance->setValue(
            config.value("SphereNormalDistance", widget.doubleSpinBoxSphereNormalDistance->value())
                .toDouble());
        widget.doubleSpinBoxEuclideanClusteringTolerance->setValue(
            config
                .value("EuclideanClusteringTolerance",
                       widget.doubleSpinBoxEuclideanClusteringTolerance->value())
                .toDouble());
        widget.doubleSpinBoxOutlierDistanceThreshold->setValue(
            config
                .value("OutlierDistanceThreshold",
                       widget.doubleSpinBoxOutlierDistanceThreshold->value())
                .toDouble());
        config.endGroup();
 
        ARMARX_INFO << "Current settings read from '" << filename << "'";
    }
 
    void
    PrimitiveExtractionWidgetController::exportConfig()
    {
        QString filename = QFileDialog::getSaveFileName(
            NULL, "Save Pipeline Configuration file", "", "Config file (*.cfg)");
        if (filename == "")
        {
            return;
        }
 
        QSettings config(filename, QSettings::IniFormat);
 
        config.beginGroup("LCCP");
        config.setValue("MinSegmentSize", widget.doubleSpinBoxLccpMinSegmentSize->value());
        config.setValue("VoxelResolution", widget.doubleSpinBoxLccpVoxelResolution->value());
        config.setValue("SeedResolution", widget.doubleSpinBoxLccpSeedResolution->value());
        config.setValue("ColorImportance", widget.doubleSpinBoxLccpColorImportance->value());
        config.setValue("SpatialImportance", widget.doubleSpinBoxLccpSpatialImportance->value());
        config.setValue("NormalImportance", widget.doubleSpinBoxLccpNormalImportance->value());
        config.setValue("ConcavityThreshold", widget.doubleSpinBoxLccpConcavityThreshold->value());
        config.setValue("SmoothnessThreshold",
                        widget.doubleSpinBoxLccpSmoothnessThreshold->value());
        config.endGroup();
 
        config.beginGroup("PrimitiveExtraction");
        config.setValue("MinPrimitiveSize", widget.doubleSpinBoxMinPrimitiveSize->value());
        config.setValue("MaxPrimitiveSize", widget.doubleSpinBoxMaxPrimitiveSize->value());
        config.setValue("PlaneMaxIterations", widget.doubleSpinBoxPlaneMaxIterations->value());
        config.setValue("PlaneDistanceThreshold",
                        widget.doubleSpinBoxPlaneDistanceThreshold->value());
        config.setValue("PlaneNormalDistance", widget.doubleSpinBoxPlaneNormalDistance->value());
        config.setValue("PlaneCircularDistanceThreshold",
                        widget.doubleSpinBoxPlaneCircularDistanceThreshold->value());
        config.setValue("CylinderMaxIterations",
                        widget.doubleSpinBoxCylinderMaxIterations->value());
        config.setValue("CylinderDistanceThreshold",
                        widget.doubleSpinBoxCylinderDistanceThreshold->value());
        config.setValue("CylinderRadiusLimit", widget.doubleSpinBoxSphereMaxIterations->value());
        config.setValue("SphereMaxIterations", widget.doubleSpinBoxSphereMaxIterations->value());
        config.setValue("SphereDistanceThreshold",
                        widget.doubleSpinBoxSphereDistanceThreshold->value());
        config.setValue("SphereNormalDistance", widget.doubleSpinBoxSphereNormalDistance->value());
        config.setValue("EuclideanClusteringTolerance",
                        widget.doubleSpinBoxEuclideanClusteringTolerance->value());
        config.setValue("OutlierDistanceThreshold",
                        widget.doubleSpinBoxOutlierDistanceThreshold->value());
        config.endGroup();
 
        config.sync();
 
        ARMARX_INFO << "Current settings saved to '" << filename << "'";
    }
 
    // Apply a rotation to the pointCloud corresponding to the inverse of the rotational component of the biggest plane primitive.
    // The should result in a point cloud in which the ground is fitted exactly into the x/y-plane.
    void
    PrimitiveExtractionWidgetController::applyAutoRotation()
    {
        AffordanceKitArmarX::PrimitiveSetArmarXPtr primitives;
        primitives.reset(new AffordanceKitArmarX::PrimitiveSetArmarX(
            workingMemory->getEnvironmentalPrimitiveSegment()));
 
        bool found = false;
        unsigned int indexOfBiggestPrimitive;
        unsigned int maxSampleSize = 0;
 
        using boost::dynamic_pointer_cast;
        using std::dynamic_pointer_cast;
 
        // Find biggest plane primitive.
        for (unsigned int i = 0; i < primitives->size(); i++)
        {
            AffordanceKit::PrimitivePtr primitive = primitives->at(i);
            if (!dynamic_pointer_cast<AffordanceKit::Plane>(primitive))
            {
                continue;
            }
            AffordanceKit::PlanePtr plane = dynamic_pointer_cast<AffordanceKit::Plane>(primitive);
            plane->sample(20, 1);
            unsigned int curPlaneSize = plane->getSamplingSize();
            if (curPlaneSize > maxSampleSize)
            {
                found = true;
                maxSampleSize = curPlaneSize;
                indexOfBiggestPrimitive = i;
            }
        }
 
        if (!found)
        {
            ARMARX_WARNING << "Did not find any plane-shaped primitives to rotate upon!";
            return;
        }
 
        // If found: get rotation of biggest plane primitive.
        AffordanceKit::PrimitivePtr primitive = primitives->at(indexOfBiggestPrimitive);
        if (!dynamic_pointer_cast<AffordanceKit::Plane>(primitive))
        {
            ARMARX_ERROR << "Could not convert to planar primitive";
            return;
        }
        AffordanceKit::PlanePtr plane = dynamic_pointer_cast<AffordanceKit::Plane>(primitive);
        Eigen::Matrix4f gp = plane->getPose();
 
        debugDrawer->setPoseVisu(
            "PrimitiveExtractorGuiLayer", "BiggestPlaneRotation", new Pose(gp));
 
        Eigen::Matrix4f gpi = gp.inverse();
 
        // Rotate the point cloud around the inverse of the ground's rotation matrix.
        std::string filename = widget.lineEditPointCloud->text().toStdString();
        pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pc(new pcl::PointCloud<pcl::PointXYZRGBA>());
        if (pcl::io::loadPCDFile<pcl::PointXYZRGBA>(filename.c_str(), *pc) == -1)
        {
            ARMARX_WARNING << "Unable to load point cloud " << filename << " for auto-rotation.";
            return;
        }
 
        // Apply transformation
        pcl::transformPointCloud(*pc, *pc, gpi);
 
        // Use a simple heuristic to check if z points in the right direction (upwards)
        int balance = 0;
        for (unsigned int i = 0; i < pc->points.size(); i++)
        {
            balance += (pc->points[i].z >= 0) ? 1 : -1;
        }
 
        ARMARX_INFO << "Z-axis balance after transformation: " << balance;
        if (balance < 0)
        {
            // z seems to point in the wrong direction (most point have negative z-value)
            Eigen::Affine3f rotation(Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitX()));
            pcl::transformPointCloud(*pc, *pc, rotation);
 
            ARMARX_INFO << "Flipping Z axis";
 
            // Update visualization
            Eigen::Matrix4f T = rotation.matrix();
            debugDrawer->setPoseVisu(
                "PrimitiveExtractorGuiLayer", "BiggestPlaneRotation", new Pose(T * gp));
        }
 
        std::string out_filename =
            QFileDialog::getSaveFileName(
                NULL, "Save transformed PCD", filename.c_str(), "Point Cloud Files (*.pcd)")
                .toStdString();
        if (filename != "")
        {
            pcl::io::savePCDFileASCII(out_filename, *pc);
            ARMARX_INFO << "Point cloud saved";
        }
    }
} // namespace armarx