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 <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <VisionX/interface/core/PointCloudProviderInterface.h>
#include <VisionX/libraries/AffordanceKitArmarX/PrimitiveSetArmarX.h>
#include <AffordanceKit/primitives/Plane.h>
 
#include <string>
 
#include <QDoubleSpinBox>
#include <QCheckBox>
#include <QComboBox>
 
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
 
#include <memory>
 
#include <filesystem>
 
#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";
        }
    }
}