HandEyeCalibrationWidgetController.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    RobotComponents::gui-plugins::HandEyeCalibrationWidgetController
 * \author     Stefan Reither ( stef dot reither at web dot de )
 * \date       2018
 * \copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "HandEyeCalibrationWidgetController.h"
 
#include <cctype>
#include <iomanip>
 
#include <QClipboard>
#include <QFileDialog>
#include <QFileInfo>
#include <QMessageBox>
 
#include <pcl/common/transforms.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/passthrough.h>
#include <VirtualRobot/MathTools.h>
 
#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/util/json/JSONObject.h>
 
#include <RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h>
#include <RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLoggerEntry.h>
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
#include <VisionX/interface/components/Calibration.h>
 
const QString defaultFileName("HandEyeCalib_Data.json");
 
using namespace armarx;
 
HandEyeCalibrationWidgetController::HandEyeCalibrationWidgetController() = default;
 
 
HandEyeCalibrationWidgetController::~HandEyeCalibrationWidgetController() = default;
 
void
HandEyeCalibrationWidgetController::loadSettings(QSettings* settings)
{
}
 
void
HandEyeCalibrationWidgetController::saveSettings(QSettings* settings)
{
}
 
QPointer<QDialog>
HandEyeCalibrationWidgetController::getConfigDialog(QWidget* parent)
{
    if (!dialog)
    {
        dialog = new SimpleConfigDialog(parent);
        dialog->addProxyFinder<visionx::PointCloudProviderInterfacePrx>(
            {"PointCloudProvider", "PointCloudProvider", "*"});
        dialog->addProxyFinder<RobotStateComponentInterfacePrx>(
            {"RobotStateComponent", "RobotStateComponent", "RobotState*"});
    }
    return qobject_cast<SimpleConfigDialog*>(dialog);
}
 
void
HandEyeCalibrationWidgetController::configured()
{
    providerName = dialog->getProxyName("PointCloudProvider");
    robotStateComponentName = dialog->getProxyName("RobotStateComponent");
}
 
void
HandEyeCalibrationWidgetController::onInitPointCloudProcessor()
{
    ARMARX_INFO << "onInitPointCloudProcessor";
 
    QMetaObject::invokeMethod(this, "initUI");
 
    usingProxy(robotStateComponentName);
}
 
void
HandEyeCalibrationWidgetController::initUI()
{
    widget.setupUi(getWidget());
    widget.lineEdit_file->setText(QDir::currentPath() + "/" + defaultFileName);
    getWidget()->setEnabled(false);
}
 
void
HandEyeCalibrationWidgetController::saveDatapoint()
{
    if (activeEndEffector)
    {
        SimpleJsonLoggerEntry entry;
        entry.Add("EndEffector", activeEndEffector->getName());
 
        std::map<std::string, float> config = localRobot->getConfig()->getRobotNodeJointValueMap();
        entry.Add("RobotConfig", config);
 
        Eigen::Matrix4f offset = getOffsetMatrixForEEF(activeEndEffector);
        entry.AddMatrix("Offset", offset);
 
        std::string fileName = widget.lineEdit_file->text().toStdString();
        SimpleJsonLogger logger(fileName, true);
        logger.log(entry);
        logger.close();
 
        QMessageBox::information(
            getWidget(), tr("HandEyeCalibration"), tr("Datapoint successful written to file."));
    }
}
 
void
HandEyeCalibrationWidgetController::selectFile()
{
    QString fileName = QFileDialog::getOpenFileName(getWidget(),
                                                    tr("Select file for saving data points"),
                                                    QDir::currentPath(),
                                                    tr("JSON-File (*.json);; All Files (*)"));
    if (!fileName.isEmpty())
    {
        widget.lineEdit_file->setText(fileName);
    }
}
 
VirtualRobot::RobotPtr
HandEyeCalibrationWidgetController::getEEFRobot(VirtualRobot::EndEffectorPtr eef) const
{
    if (eef && robotEEFMap.count(eef->getName()) == 1)
    {
        return robotEEFMap.at(eef->getName()).first;
    }
    return VirtualRobot::RobotPtr();
}
 
Eigen::Matrix4f
HandEyeCalibrationWidgetController::getOffsetMatrixForEEF(
    const VirtualRobot::EndEffectorPtr eef) const
{
    if (eef && robotEEFMap.count(eef->getName()) == 1)
    {
        return robotEEFMap.at(eef->getName()).second;
    }
    return Eigen::Matrix4f::Identity();
}
 
void
HandEyeCalibrationWidgetController::setOffsetMatrixForEEF(const VirtualRobot::EndEffectorPtr eef,
                                                          const Eigen::Matrix4f& offset)
{
    if (eef && robotEEFMap.count(eef->getName()) == 1)
    {
        robotEEFMap.at(eef->getName()).second = offset;
    }
}
 
void
HandEyeCalibrationWidgetController::onConnectPointCloudProcessor()
{
    ARMARX_INFO << "onConnectPointCloudProcessor";
 
    robotStateComponentPrx =
        getProxy<RobotStateComponentInterfacePrx>(robotStateComponentName, true);
 
    usingPointCloudProvider(providerName);
    providerInfo = getPointCloudProvider(providerName, true);
    referenceFrame = getProviderFrame();
    providerBuffer.reset(new pcl::PointCloud<PointT>());
 
    localRobot = loadRobotFromFile();
    try
    {
        pointcloudRobot = RemoteRobot::createLocalClone(robotStateComponentPrx);
    }
    catch (...)
    {
        ARMARX_ERROR
            << "Exception while creating local clone of robot in onConnectPointCLoudProcessor()";
        handleExceptions();
    }
 
    // Setup periodic tasks
    taskEEFManipulation = new PeriodicTask<HandEyeCalibrationWidgetController>(
        this, &HandEyeCalibrationWidgetController::taskEEFManipulationCB, 30.0f);
    taskLocalRobotUpdate = new PeriodicTask<HandEyeCalibrationWidgetController>(
        this, &HandEyeCalibrationWidgetController::taskLocalRobotUpdateCB, 50.0f);
 
    // Setup visualizations
    QMetaObject::invokeMethod(this, "connectQt", Qt::BlockingQueuedConnection);
 
    // Finally connecting the signals and slots
    connect(this, SIGNAL(pointCloudUpdated()), this, SLOT(processPointCloud()));
    connect(widget.comboBox_endEffectorSelection,
            SIGNAL(currentIndexChanged(QString)),
            this,
            SLOT(activeEndEffectorChanged(QString)));
    connect(widget.horizontalSlider_croppingBox,
            SIGNAL(valueChanged(int)),
            this,
            SLOT(slider_croppingRange_changed(int)));
    connect(widget.checkBox_croppingActive,
            SIGNAL(stateChanged(int)),
            this,
            SLOT(cb_croppingActive_changed(int)));
 
    connect(widget.checkBox_offsetInverted,
            SIGNAL(toggled(bool)),
            this,
            SLOT(cb_inverted_changed(bool)));
    connect(widget.pushButton_copyToClipboard,
            SIGNAL(clicked()),
            this,
            SLOT(btn_copyToClipboard_pressed()));
    connect(widget.pushButton_resetEndeffector,
            SIGNAL(clicked()),
            this,
            SLOT(btn_resetToModel_pressed()));
 
    connect(widget.doubleSpinBox_camera_x,
            SIGNAL(valueChanged(double)),
            this,
            SLOT(cameraNodeTransformationChanged(double)));
    connect(widget.doubleSpinBox_camera_y,
            SIGNAL(valueChanged(double)),
            this,
            SLOT(cameraNodeTransformationChanged(double)));
    connect(widget.doubleSpinBox_camera_z,
            SIGNAL(valueChanged(double)),
            this,
            SLOT(cameraNodeTransformationChanged(double)));
    connect(widget.doubleSpinBox_camera_roll,
            SIGNAL(valueChanged(double)),
            this,
            SLOT(cameraNodeTransformationChanged(double)));
    connect(widget.doubleSpinBox_camera_pitch,
            SIGNAL(valueChanged(double)),
            this,
            SLOT(cameraNodeTransformationChanged(double)));
    connect(widget.doubleSpinBox_camera_yaw,
            SIGNAL(valueChanged(double)),
            this,
            SLOT(cameraNodeTransformationChanged(double)));
 
    connect(widget.pushButton_backgroundColor,
            SIGNAL(clicked()),
            this,
            SLOT(btn_backgroundColor_pressed()));
    connect(widget.pushButton_pointcloudColor,
            SIGNAL(clicked()),
            this,
            SLOT(btn_pointCloudColor_pressed()));
    connect(
        widget.spinBox_pointSize, SIGNAL(valueChanged(int)), this, SLOT(sB_pointSize_changed(int)));
 
    connect(widget.pushButton_changeFile, SIGNAL(clicked()), this, SLOT(selectFile()));
    connect(widget.pushButton_saveDatapoint, SIGNAL(clicked()), this, SLOT(saveDatapoint()));
 
    taskEEFManipulation->start();
    taskLocalRobotUpdate->start();
    enableMainWidgetAsync(true);
}
 
void
HandEyeCalibrationWidgetController::connectQt()
{
    pointCloudVisu = new PointCloudVisualization();
    widget.displayWidget->getDisplay()->getRootNode()->addChild(pointCloudVisu);
 
    manipulatorVisu = new ManipulatorVisualization();
    widget.displayWidget->getDisplay()->getRootNode()->addChild((SoNode*)manipulatorVisu);
 
    widget.displayWidget->getDisplay()->cameraViewAll();
 
    if (manipulatorVisu)
    {
        setupEndEffectorSelection(localRobot);
    }
    else
    {
        ARMARX_WARNING << "ManipulatorVisu not present";
    }
    setupCameraNodeSlider(pointcloudRobot);
    disableGuiElements();
}
 
void
HandEyeCalibrationWidgetController::onDisconnectPointCloudProcessor()
{
    ARMARX_INFO << "onDisconnectPointCloudProcessor";
 
    taskEEFManipulation->stop();
    taskLocalRobotUpdate->stop();
 
    releasePointCloudProvider(providerName);
    QMetaObject::invokeMethod(this, "disconnectQt");
 
    localRobot.reset();
    pointcloudRobot.reset();
    activeEndEffector.reset();
    providerBuffer.reset();
}
 
void
HandEyeCalibrationWidgetController::disconnectQt()
{
    disableGuiElements();
 
    widget.comboBox_endEffectorSelection->clear();
    manipulatorVisu->removeVisualization();
    pointCloudVisu->removeAllChildren();
    widget.displayWidget->getDisplay()->getRootNode()->removeAllChildren();
 
    // disconnect signals that would interfere with the gui setup
    QObject::disconnect(widget.comboBox_endEffectorSelection, 0, 0, 0);
}
 
void
HandEyeCalibrationWidgetController::onExitPointCloudProcessor()
{
    ARMARX_INFO << "onExitPointCloudProcessor";
}
 
void
HandEyeCalibrationWidgetController::setupEndEffectorSelection(VirtualRobot::RobotPtr robot)
{
    try
    {
        armarx::RemoteRobot::synchronizeLocalClone(robot, robotStateComponentPrx);
    }
    catch (...)
    {
        handleExceptions();
    }
 
    std::vector<VirtualRobot::EndEffectorPtr> endEffectors;
    robot->getEndEffectors(endEffectors);
    for (VirtualRobot::EndEffectorPtr e : endEffectors)
    {
        widget.comboBox_endEffectorSelection->addItem(QString::fromStdString(e->getName()));
        VirtualRobot::RobotPtr r = e->createEefRobot(e->getName(), e->getName());
        r->setGlobalPoseForRobotNode(r->getRobotNode(e->getTcp()->getName()),
                                     e->getTcp()->getGlobalPose());
        robotEEFMap.insert(
            std::make_pair(e->getName(), std::make_pair(r, Eigen::Matrix4f::Identity())));
    }
    widget.comboBox_endEffectorSelection->setCurrentIndex(-1);
}
 
bool
HandEyeCalibrationWidgetController::findStringIC(const std::string& strHaystack,
                                                 const std::string& strNeedle) const
{
    auto it =
        std::search(strHaystack.begin(),
                    strHaystack.end(),
                    strNeedle.begin(),
                    strNeedle.end(),
                    [](char ch1, char ch2) { return std::toupper(ch1) == std::toupper(ch2); });
    return (it != strHaystack.end());
}
 
void
HandEyeCalibrationWidgetController::setupCameraNodeSlider(VirtualRobot::RobotPtr robot)
{
    if (robot)
    {
        ARMARX_CHECK_EXPRESSION(robot->hasRobotNode(referenceFrame))
            << "Robot " + robot->getName() + " does not have node with name " + referenceFrame;
        widget.label_cameraNodeName->setText(QString::fromStdString((referenceFrame)));
 
        cameraNode = robot->getRobotNode(referenceFrame);
        auto trans = VirtualRobot::MathTools::eigen4f2posrpy(cameraNode->getLocalTransformation());
 
        widget.doubleSpinBox_camera_x->setValue(trans(0));
        widget.doubleSpinBox_camera_y->setValue(trans(1));
        widget.doubleSpinBox_camera_z->setValue(trans(2));
        widget.doubleSpinBox_camera_roll->setValue(VirtualRobot::MathTools::rad2deg(trans(3)));
        widget.doubleSpinBox_camera_pitch->setValue(VirtualRobot::MathTools::rad2deg(trans(4)));
        widget.doubleSpinBox_camera_yaw->setValue(VirtualRobot::MathTools::rad2deg(trans(5)));
    }
}
 
VirtualRobot::RobotPtr
HandEyeCalibrationWidgetController::loadRobotFromFile() const
{
    Ice::StringSeq includePaths;
 
    try
    {
        Ice::StringSeq packages = robotStateComponentPrx->getArmarXPackages();
        packages.push_back(Application::GetProjectName());
 
        for (const std::string& projectName : packages)
        {
            if (projectName.empty())
            {
                continue;
            }
 
            CMakePackageFinder project(projectName);
            auto pathsString = project.getDataDir();
            Ice::StringSeq projectIncludePaths = armarx::Split(pathsString, ";,", true, true);
            includePaths.insert(
                includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
        }
    }
    catch (...)
    {
        ARMARX_ERROR << "Unable to retrieve robot filename." << std::endl;
        return VirtualRobot::RobotPtr();
    }
 
    try
    {
        std::string rfile = robotStateComponentPrx->getRobotFilename();
        ArmarXDataPath::getAbsolutePath(rfile, rfile, includePaths);
        return VirtualRobot::RobotIO::loadRobot(rfile);
    }
    catch (...)
    {
        ARMARX_ERROR << "Unable to load robot from file" << std::endl;
        return VirtualRobot::RobotPtr();
    }
}
 
std::string
HandEyeCalibrationWidgetController::getProviderFrame() const
{
    std::string frame = "Global";
    try
    {
        visionx::ReferenceFrameInterfacePrx refFramePrx =
            visionx::ReferenceFrameInterfacePrx::checkedCast(providerInfo.proxy);
        if (refFramePrx)
        {
            frame = refFramePrx->getReferenceFrame();
        }
    }
    catch (...)
    {
        handleExceptions();
    }
    return frame;
}
 
std::string
HandEyeCalibrationWidgetController::formatTransformationMatrix(Eigen::Matrix4f mat,
                                                               int decimalPlacesOrientation,
                                                               int decimalPlacesPosition) const
{
    std::stringstream ss;
    ss.imbue(std::locale::classic());
    ss << std::fixed;
 
    // The first three rows behave the same
    for (int i = 0; i < 3; i++)
    {
        float x0 = mat(i, 0);
        float x1 = mat(i, 1);
        float x2 = mat(i, 2);
        ss << std::setprecision(decimalPlacesOrientation);
        if (!std::signbit(x0))
        {
            ss << " ";
        }
        ss << x0 << " ";
        if (!std::signbit(x1))
        {
            ss << " ";
        }
        ss << x1 << " ";
        if (!std::signbit(x2))
        {
            ss << " ";
        }
        ss << x2 << " ";
 
        ss << std::setprecision(decimalPlacesPosition);
        if (!std::signbit(mat(i, 3)))
        {
            ss << " ";
        }
        ss << mat(i, 3);
        ss << std::endl;
    }
    return ss.str();
}
 
void
HandEyeCalibrationWidgetController::enableGuiElements()
{
    widget.horizontalSlider_croppingBox->setEnabled(true);
    widget.horizontalSlider_x->setEnabled(true);
    widget.horizontalSlider_y->setEnabled(true);
    widget.horizontalSlider_z->setEnabled(true);
    widget.checkBox_croppingActive->setEnabled(true);
    widget.checkBox_offsetInverted->setEnabled(true);
    widget.pushButton_copyToClipboard->setEnabled(true);
    widget.pushButton_resetEndeffector->setEnabled(true);
    widget.pushButton_pointcloudColor->setEnabled(true);
    widget.pushButton_backgroundColor->setEnabled(true);
    widget.spinBox_pointSize->setEnabled(true);
    widget.pushButton_changeFile->setEnabled(true);
    widget.pushButton_saveDatapoint->setEnabled(true);
    widget.lineEdit_file->setEnabled(true);
 
    widget.doubleSpinBox_camera_x->setEnabled(true);
    widget.doubleSpinBox_camera_y->setEnabled(true);
    widget.doubleSpinBox_camera_z->setEnabled(true);
    widget.doubleSpinBox_camera_roll->setEnabled(true);
    widget.doubleSpinBox_camera_pitch->setEnabled(true);
    widget.doubleSpinBox_camera_yaw->setEnabled(true);
 
    widget.label_offsetMatrix->setText(QString("No Endeffector selected"));
    widget.label_offsetMatrix->setStyleSheet("QLabel { color : black; }");
 
    showPointCloud = true;
}
 
void
HandEyeCalibrationWidgetController::disableGuiElements()
{
    widget.horizontalSlider_croppingBox->setEnabled(false);
    widget.horizontalSlider_x->setEnabled(false);
    widget.horizontalSlider_y->setEnabled(false);
    widget.horizontalSlider_z->setEnabled(false);
    widget.checkBox_croppingActive->setEnabled(false);
    widget.checkBox_offsetInverted->setEnabled(false);
    widget.pushButton_copyToClipboard->setEnabled(false);
    widget.pushButton_resetEndeffector->setEnabled(false);
    widget.pushButton_pointcloudColor->setEnabled(false);
    widget.pushButton_backgroundColor->setEnabled(false);
    widget.spinBox_pointSize->setEnabled(false);
    widget.pushButton_changeFile->setEnabled(false);
    widget.pushButton_saveDatapoint->setEnabled(false);
    widget.lineEdit_file->setEnabled(false);
 
    widget.doubleSpinBox_camera_x->setEnabled(false);
    widget.doubleSpinBox_camera_y->setEnabled(false);
    widget.doubleSpinBox_camera_z->setEnabled(false);
    widget.doubleSpinBox_camera_roll->setEnabled(false);
    widget.doubleSpinBox_camera_pitch->setEnabled(false);
    widget.doubleSpinBox_camera_yaw->setEnabled(false);
 
    widget.label_offsetMatrix->setText(QString("No Endeffector selected"));
    widget.label_offsetMatrix->setStyleSheet("QLabel { color : red; }");
 
    showPointCloud = false;
}
 
void
HandEyeCalibrationWidgetController::updateManipulatorVisualization()
{
    if (activeEndEffector && manipulatorVisu)
    {
        Eigen::Matrix4f modelPoseInRoot =
            localRobot->getRobotNode(activeEndEffector->getTcp()->getName())->getPoseInRootFrame();
        Eigen::Matrix4f offset = getOffsetMatrixForEEF(activeEndEffector);
        // Apply offset in root frame (multiplication order matters!!)
        Eigen::Matrix4f pointCloudInRoot = offset * modelPoseInRoot;
        Eigen::Matrix4f pointCloudGlobal = localRobot->getGlobalPose() * pointCloudInRoot;
 
        // Apply global pose to manipulator visu
        manipulatorVisu->setUserDesiredPose(pointCloudGlobal);
 
        // Update the local eefRobot
        getEEFRobot(activeEndEffector)
            ->setGlobalPoseForRobotNode(activeEndEffector->getTcp(), pointCloudGlobal);
 
        // Update Matrix label
        if (widget.checkBox_offsetInverted->isChecked())
        {
            offset = offset.inverse();
        }
        widget.label_offsetMatrix->setText(
            QString::fromStdString(formatTransformationMatrix(offset)));
    }
}
 
void
HandEyeCalibrationWidgetController::process()
{
    if (!waitForPointClouds(providerName, 500))
    {
        ARMARX_WARNING << "Timeout or error in waiting for pointclouds from provider: "
                       << providerName;
    }
    else
    {
        getPointClouds(providerName, providerBuffer);
 
        emit pointCloudUpdated();
    }
}
 
void
HandEyeCalibrationWidgetController::processPointCloud()
{
    if (showPointCloud && pointcloudRobot)
    {
        // Copy buffer to be able to edit it
        pcl::PointCloud<PointT>::Ptr localBufferIn(new pcl::PointCloud<PointT>());
        pcl::copyPointCloud(*providerBuffer, *localBufferIn);
        pcl::PointCloud<PointT>::Ptr localBufferOut(new pcl::PointCloud<PointT>());
 
        Eigen::Matrix4f sourceFrameGlobalPose = Eigen::Matrix4f::Identity();
        if (referenceFrame != "Global")
        {
            visionx::MetaPointCloudFormatPtr info = this->getPointCloudFormat(providerName);
 
            try
            {
                RemoteRobot::synchronizeLocalCloneToTimestamp(
                    pointcloudRobot, robotStateComponentPrx, info->timeProvided);
            }
            catch (...)
            {
                ARMARX_ERROR << "Exception while synchronizing robot for pointcloud-processing";
                handleExceptions();
            }
 
            sourceFrameGlobalPose = pointcloudRobot->getRobotNode(referenceFrame)->getGlobalPose();
        }
        Eigen::Matrix4f cameraToGlobal = sourceFrameGlobalPose;
 
        // Crop pointcloud around endeffector
        if (croppingActive && manipulatorVisu && manipulatorVisu->getIsVisualizing())
        {
            Eigen::Matrix4f eefGlobal = manipulatorVisu->getUserDesiredPose();
            Eigen::Matrix4f eefInReferenceFrame = sourceFrameGlobalPose.inverse() * eefGlobal;
 
            float cropRange = (float)widget.horizontalSlider_croppingBox->value();
 
            Eigen::Vector3f cropMin;
            cropMin << eefInReferenceFrame(0, 3) - cropRange, eefInReferenceFrame(1, 3) - cropRange,
                eefInReferenceFrame(2, 3) - cropRange;
            Eigen::Vector3f cropMax;
            cropMax << eefInReferenceFrame(0, 3) + cropRange, eefInReferenceFrame(1, 3) + cropRange,
                eefInReferenceFrame(2, 3) + cropRange;
 
            pcl::PassThrough<PointT> pass;
 
            pass.setInputCloud(localBufferIn);
            pass.setFilterFieldName("z");
            pass.setFilterLimits(cropMin(2), cropMax(2));
            pass.filter(*localBufferOut);
            localBufferOut.swap(localBufferIn);
 
            pass.setInputCloud(localBufferIn);
            pass.setFilterFieldName("x");
            pass.setFilterLimits(cropMin(0), cropMax(0));
            pass.filter(*localBufferOut);
            localBufferOut.swap(localBufferIn);
 
            pass.setInputCloud(localBufferIn);
            pass.setFilterFieldName("y");
            pass.setFilterLimits(cropMin(1), cropMax(1));
            pass.filter(*localBufferOut);
            localBufferOut.swap(localBufferIn);
        }
 
 
        // Transform pointcloud into GlobalFrame
        pcl::transformPointCloud(*localBufferIn, *localBufferOut, cameraToGlobal);
        localBufferOut.swap(localBufferIn);
 
        if (pointCloudVisu)
        {
            pointCloudVisu->setVisualization(localBufferIn);
        }
    }
}
 
void
HandEyeCalibrationWidgetController::cb_inverted_changed(bool checked)
{
    if (checked)
    {
        widget.label_titleOffsetMatrix->setText("Offset from Pointcloud to Model:");
    }
    else
    {
        widget.label_titleOffsetMatrix->setText("Offset from Model to Pointcloud:");
    }
}
 
void
HandEyeCalibrationWidgetController::btn_copyToClipboard_pressed()
{
    if (activeEndEffector)
    {
        Eigen::Matrix4f offset = this->getOffsetMatrixForEEF(activeEndEffector);
 
        if (widget.checkBox_offsetInverted->isChecked())
        {
            offset = offset.inverse();
        }
 
        PosePtr pose = new Pose(offset);
 
        JSONObjectPtr obj = new JSONObject();
        obj->serializeIceObject(pose);
 
        QClipboard* clipboard = QApplication::clipboard();
        clipboard->setText(QString::fromStdString(obj->asString(true)));
    }
}
 
void
HandEyeCalibrationWidgetController::btn_resetToModel_pressed()
{
    if (activeEndEffector)
    {
        setOffsetMatrixForEEF(activeEndEffector, Eigen::Matrix4f::Identity());
        updateManipulatorVisualization();
        widget.displayWidget->getDisplay()->cameraViewNode((SoNode*)manipulatorVisu, 1.0);
    }
}
 
void
HandEyeCalibrationWidgetController::btn_backgroundColor_pressed()
{
    QColor selectedColor = this->colorDialog.getColor();
    widget.displayWidget->getDisplay()->setBackgroundColor(SbColor(selectedColor.red() / 255.0f,
                                                                   selectedColor.green() / 255.0f,
                                                                   selectedColor.blue() / 255.0f));
}
 
void
HandEyeCalibrationWidgetController::btn_pointCloudColor_pressed()
{
    QColor selectedColor = this->colorDialog.getColor();
    this->pointCloudVisu->setDrawColor(SbColor(selectedColor.red() / 255.0f,
                                               selectedColor.green() / 255.0f,
                                               selectedColor.blue() / 255.0f));
}
 
void
HandEyeCalibrationWidgetController::sB_pointSize_changed(int value)
{
    this->pointCloudVisu->setPointSize(value);
}
 
void
HandEyeCalibrationWidgetController::cameraNodeTransformationChanged(double value)
{
    if (cameraNode)
    {
        auto t = VirtualRobot::MathTools::eigen4f2posrpy(cameraNode->getLocalTransformation());
        QObject* obj = sender();
        if (obj == widget.doubleSpinBox_camera_x)
        {
            t(0) = (float)value;
        }
        else if (obj == widget.doubleSpinBox_camera_y)
        {
            t(1) = (float)value;
        }
        else if (obj == widget.doubleSpinBox_camera_z)
        {
            t(2) = (float)value;
        }
        else if (obj == widget.doubleSpinBox_camera_roll)
        {
            t(3) = VirtualRobot::MathTools::deg2rad((float)value);
        }
        else if (obj == widget.doubleSpinBox_camera_pitch)
        {
            t(4) = VirtualRobot::MathTools::deg2rad((float)value);
        }
        else if (obj == widget.doubleSpinBox_camera_yaw)
        {
            t(5) = VirtualRobot::MathTools::deg2rad((float)value);
        }
        Eigen::Matrix4f transformation =
            VirtualRobot::MathTools::posrpy2eigen4f(t(0), t(1), t(2), t(3), t(4), t(5));
        cameraNode->setLocalTransformation(transformation);
    }
}
 
void
HandEyeCalibrationWidgetController::taskEEFManipulationCB()
{
    float x[3];
    x[0] = (float)widget.horizontalSlider_x->value();
    x[1] = (float)widget.horizontalSlider_y->value();
    x[2] = (float)widget.horizontalSlider_z->value();
    if (activeEndEffector)
    {
        Eigen::Matrix4f offsetInRoot = Eigen::Matrix4f::Identity();
        offsetInRoot(0, 3) = x[0];
        offsetInRoot(1, 3) = x[1];
        offsetInRoot(2, 3) = x[2];
 
        setOffsetMatrixForEEF(activeEndEffector, offsetInRoot);
    }
}
 
void
HandEyeCalibrationWidgetController::taskLocalRobotUpdateCB()
{
    if (!robotStateComponentPrx)
    {
        return;
    }
    try
    {
        RemoteRobot::synchronizeLocalClone(localRobot, robotStateComponentPrx);
        updateManipulatorVisualization();
    }
    catch (...)
    {
    }
}
 
void
HandEyeCalibrationWidgetController::activeEndEffectorChanged(QString endEffectorName)
{
    if (localRobot && manipulatorVisu)
    {
        std::string eefName = endEffectorName.toStdString();
        if (!eefName.empty())
        {
            if (robotEEFMap.count(eefName) == 1)
            {
                activeEndEffector = robotEEFMap.at(eefName).first->getEndEffector(eefName);
                manipulatorVisu->setVisualization(activeEndEffector);
                widget.displayWidget->getDisplay()->cameraViewNode((SoNode*)manipulatorVisu, 1.0);
                enableGuiElements();
            }
            else
            {
                ARMARX_ERROR << "Robot " << localRobot->getName()
                             << " does not have endeffector with name " << eefName;
            }
        }
    }
}
 
void
HandEyeCalibrationWidgetController::slider_croppingRange_changed(int value)
{
    this->widget.label_croppingBoxValue->setText(QString::fromStdString(ValueToString(value)));
}
 
void
HandEyeCalibrationWidgetController::cb_croppingActive_changed(int state)
{
    if (state == Qt::Unchecked)
    {
        this->croppingActive = true;
    }
    else
    {
        this->croppingActive = false;
    }
}