UserAssistedSegmenterGuiWidgetController.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    SceneUnderstanding::gui-plugins::UserAssistedSegmenterGuiWidgetController
 * \author     Rainer Kartmann ( rainer dot kartmann at student dot kit dot edu )
 * \date       2018
 * \copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "UserAssistedSegmenterGuiWidgetController.h"
 
#include <boost/lexical_cast.hpp>
#include <pcl/common/colors.h>
 
#include <RobotAPI/libraries/core/visualization/GlasbeyLUT.h>
 
#include <VisionX/components/pointcloud_core/PCLUtilities.h>
#include <VisionX/components/pointcloud_core/PointCloudConversions.h>
 
#include "UserAssistedSegmenterConfigDialog.h"
#include "widgets/UserGroupingLineEdit.h"
 
 
using namespace armarx;
using namespace Eigen;
 
 
template <class T>
std::string str(const T& t)
{
    return boost::lexical_cast<std::string>(t);
}
 
template <class T>
QString qstr(const T& t)
{
    return QString::fromStdString(boost::lexical_cast<std::string>(t));
}
 
namespace visionx
{
 
    QString UserAssistedSegmenterGuiWidgetController::GetWidgetName()
    {
        return "VisionX.UserAssistedSegmenterGui";
    }
 
 
    UserAssistedSegmenterGuiWidgetController::UserAssistedSegmenterGuiWidgetController()
    {
        widget.setupUi(getWidget());
 
 
        this->segmentsOverviewTable = new PointCloudSegmentsTable();
        widget.tableSegmentsOverviewLayout->addWidget(segmentsOverviewTable);
        widget.tableSegmentsOverviewPlaceholder->deleteLater();
 
 
        // Setup tableUserSegmentation.
        widget.tableUserGrouping->setColumnCount(3);
        widget.tableUserGrouping->setRowCount(0);
 
        QStringList header;
        header << "C" << "ID" << "Space seperated segment IDs";
        widget.tableUserGrouping->setHorizontalHeaderLabels(header);
        widget.tableUserGrouping->setColumnWidth(0, 30);
        widget.tableUserGrouping->setColumnWidth(1, 40);
        // last column is stretched
 
    }
 
 
    UserAssistedSegmenterGuiWidgetController::~UserAssistedSegmenterGuiWidgetController()
    {
    }
 
 
    void UserAssistedSegmenterGuiWidgetController::loadSettings(QSettings* settings)
    {
        (void) settings; // unused
    }
 
    void UserAssistedSegmenterGuiWidgetController::saveSettings(QSettings* settings)
    {
        (void) settings; // unused
    }
 
    QPointer<QDialog> UserAssistedSegmenterGuiWidgetController::getConfigDialog(QWidget* parent)
    {
        if (!configDialog)
        {
            configDialog = new UserAssistedSegmenterConfigDialog(parent);
        }
        return qobject_cast<UserAssistedSegmenterConfigDialog*>(configDialog);
    }
 
    void UserAssistedSegmenterGuiWidgetController::configured()
    {
        userAssistedSegmenterProxyName = configDialog->getUserAssistedSegmenterProxyName();
        userAssistedSegmenterTopicName = configDialog->getUserAssistedSegmenterTopicName();
    }
 
 
    void UserAssistedSegmenterGuiWidgetController::onInitComponent()
    {
        usingTopic(userAssistedSegmenterTopicName);
        drawer.offeringTopic(*this);
 
        usingProxy(userAssistedSegmenterProxyName);
    }
 
 
    void UserAssistedSegmenterGuiWidgetController::onConnectComponent()
    {
        drawer.getTopic(*this);
 
        segmenterProxy = getProxy<visionx::UserAssistedSegmenterInterfacePrx>(userAssistedSegmenterProxyName);
        if (!segmenterProxy)
        {
            ARMARX_ERROR << "Could not get proxy '" << userAssistedSegmenterProxyName << "'.";
        }
 
        connect(this, SIGNAL(newSourcePointCloud()),
                this, SLOT(onNewSourcePointCloud()));
 
        connect(widget.spinBoxFilterSmallSegmentsNum, SIGNAL(editingFinished()),
                this, SLOT(onFilterSmallSegmentsNumEdited()));
 
        connect(widget.checkBoxFilterSegment0, SIGNAL(toggled(bool)),
                this, SLOT(updateFilters()));
        connect(widget.checkBoxFilterSmallSegments, SIGNAL(toggled(bool)),
                this, SLOT(updateFilters()));
 
        connect(widget.checkBoxShowIDs, SIGNAL(toggled(bool)),
                this, SLOT(onShowIDsToggled(bool)));
        connect(widget.radioButtonShowInput, SIGNAL(toggled(bool)),
                this, SLOT(onRadioButtonShowInputToggled(bool)));
        connect(widget.radioButtonShowResult, SIGNAL(toggled(bool)),
                this, SLOT(onRadioButtonShowResultToggle(bool)));
 
        connect(widget.spinBoxNumObjects, SIGNAL(valueChanged(int)),
                this, SLOT(updateUserGroupingRows(int)));
        connect(widget.tableUserGrouping, SIGNAL(cellChanged(int, int)),
                this, SLOT(onUserGroupingChanged(int, int)));
 
        connect(widget.buttonClearVisu, SIGNAL(released()),
                this, SLOT(clearVisualization()));
        connect(widget.buttonPublish, SIGNAL(released()),
                this, SLOT(publishSegmentation()));
 
        connect(widget.tableUserGrouping, SIGNAL(cellChanged(int, int)),
                this, SLOT(onTableGroupingCellChanged(int, int)));
 
 
        for (auto& spinBox :
             {
                 widget.spinTextSize, widget.spinSphereSize
             })
        {
            connect(spinBox, SIGNAL(valueChanged(int)), this, SLOT(updateSegmentIDs()));
        }
    }
 
    void UserAssistedSegmenterGuiWidgetController::reportSegmentation(
        const visionx::ColoredLabeledPointCloud& receivedPointCloud, const Ice::Current&)
    {
        if (!widget.checkBoxListening->isChecked())
        {
            return;
        }
 
        ARMARX_IMPORTANT << "Received new point cloud segmentation";
 
        ARMARX_INFO << "Setting source point cloud";
        visionx::tools::convertToPCL(receivedPointCloud, sourcePointCloud);
 
        ARMARX_INFO << "Emittinging signal receivedSegmentation().";
        emit newSourcePointCloud();
    }
 
    void UserAssistedSegmenterGuiWidgetController::onNewSourcePointCloud()
    {
        ARMARX_INFO << "onNewSourcePointCloud()";
 
        widget.labelPcStatus->setText(QString("Processing..."));
 
        this->pointCloud = sourcePointCloud;
 
        computeSegmentIndex(pointCloud);
 
        applyFilters(pointCloud, segmentIndex);
 
        updatePointCloud(pointCloud, segmentIndex);
 
        // updateUserGroupingRows();
 
        widget.labelPcStatus->setText(QString("Current point cloud:"));
    }
 
    void UserAssistedSegmenterGuiWidgetController::onFilterSmallSegmentsNumEdited()
    {
        widget.checkBoxFilterSmallSegments->setChecked(
            widget.spinBoxFilterSmallSegmentsNum->value() > 0);
    }
 
    void UserAssistedSegmenterGuiWidgetController::updateFilters()
    {
        applyFilters(pointCloud, segmentIndex);
        updatePointCloud(pointCloud, segmentIndex);
    }
 
    void UserAssistedSegmenterGuiWidgetController::onRadioButtonShowInputToggled(bool checked)
    {
        if (checked)
        {
            drawInputPointCloud(false);
        }
    }
 
    void UserAssistedSegmenterGuiWidgetController::onRadioButtonShowResultToggle(bool checked)
    {
        if (checked)
        {
            drawResultPointCloud(false);
        }
    }
 
    void UserAssistedSegmenterGuiWidgetController::onShowIDsToggled(bool checked)
    {
        if (checked)
        {
            drawSegmentIDs(false);
        }
        else
        {
            drawer.clearLayer(layerSegmentIDs);
        }
    }
 
    void UserAssistedSegmenterGuiWidgetController::updateSegmentIDs()
    {
        drawSegmentIDs();
    }
 
 
    void UserAssistedSegmenterGuiWidgetController::applyFilters(
        PointCloudT& pointCloud, std::map<uint32_t, pcl::PointIndices>& segmentIndex)
    {
        ARMARX_INFO << "Applying filters";
 
        if (widget.checkBoxFilterSegment0)
        {
            auto ifLabelZero = [](const PointT & p)
            {
                return p.label == 0;
            };
            pointCloud.erase(std::remove_if(pointCloud.begin(), pointCloud.end(), ifLabelZero),
                             pointCloud.end());
            // Recompute index.
            computeSegmentIndex(pointCloud);
        }
 
        if (widget.checkBoxFilterSmallSegments->isChecked())
        {
            const int minPoints = std::max(0, widget.spinBoxFilterSmallSegmentsNum->value());
            ARMARX_CHECK_NONNEGATIVE(minPoints);
 
            if (minPoints > 1)
            {
                auto ifLessPoints = [&segmentIndex, minPoints](const PointT & p)
                {
                    return segmentIndex.at(p.label).indices.size() < static_cast<std::size_t>(minPoints);
                };
                pointCloud.erase(std::remove_if(pointCloud.begin(), pointCloud.end(), ifLessPoints),
                                 pointCloud.end());
                // Recompute index.
                computeSegmentIndex(pointCloud);
            }
        }
    }
 
 
    void UserAssistedSegmenterGuiWidgetController::updatePointCloud(const PointCloudT& pointCloud, const std::map<uint32_t, pcl::PointIndices>& segmentIndex)
    {
        ARMARX_INFO << "Updating point cloud.";
        computeCenters(pointCloud, segmentIndex);
 
        widget.labelPcPointsNum->setText(qstr(pointCloud.size()));
        widget.labelPcSegmentsNum->setText(qstr(segmentIndex.size()));
 
        updateTableOverview();
        drawInputPointCloud();
        drawSegmentIDs(true);
    }
 
 
    void UserAssistedSegmenterGuiWidgetController::computeSegmentIndex(const PointCloudT& pointCloud)
    {
        segmentIndex.clear();
        const bool excludeZero = false;
        visionx::tools::fillLabelMap(pointCloud, segmentIndex, excludeZero);
    }
 
 
    void UserAssistedSegmenterGuiWidgetController::computeCenters(
        const PointCloudT& pointCloud, const std::map<uint32_t, pcl::PointIndices>& segmentIndex)
    {
        ARMARX_INFO << "Computing centers";
        centers.clear();
        for (const auto& [segmentID, indices] : segmentIndex)
        {
            Vector3f center(0, 0, 0);
            for (int i : indices.indices)
            {
                const PointT& p = pointCloud[static_cast<std::size_t>(i)];
                center += Vector3f(p.x, p.y, p.z);
            }
            center /= segmentIndex.at(segmentID).indices.size();
            centers[segmentID] = center;
        }
    }
 
 
    void UserAssistedSegmenterGuiWidgetController::updateTableOverview()
    {
        segmentsOverviewTable->setData(segmentIndex);
    }
 
 
    void UserAssistedSegmenterGuiWidgetController::drawSegmentIDs(bool onlyIfChecked)
    {
        if (onlyIfChecked && !widget.checkBoxShowIDs->isChecked())
        {
            return;
        }
 
        drawer.clearLayer(layerSegmentIDs, true);
 
        for (const auto& [segmentID, _] : segmentIndex)
        {
            const int textSize = widget.spinTextSize->value();
            const int sphereSize = widget.spinSphereSize->value();
 
            const DrawColor segColor = dcolor(segmentID);
            const DrawColor textColor =
                (segColor.r + segColor.g + segColor.b) > 1.f
                ? DrawColor {0, 0, 0, 1}
                :
                DrawColor {1, 1, 1, 1};
 
            drawer.drawText({layerSegmentIDs, "id" + str(segmentID)},
                            centers[segmentID], str(segmentID), textSize, textColor);
            drawer.drawSphere({layerSegmentIDs, "sph" + str(segmentID)},
                              centers[segmentID], sphereSize, segColor);
        }
    }
 
    void UserAssistedSegmenterGuiWidgetController::drawInputPointCloud(bool onlyIfChecked)
    {
        if (onlyIfChecked && !widget.radioButtonShowInput->isChecked())
        {
            return;
        }
 
        ARMARX_INFO << "Drawing input point cloud";
 
        // Draw point cloud.
        drawer.drawPointCloud({layerPointCloud, "PointCloud"}, pointCloud,
                              [this](const PointT & p)
        {
            return dcolor(p.label);
        });
 
        //    drawSegmentIDs(true);
    }
 
    void UserAssistedSegmenterGuiWidgetController::drawResultPointCloud(bool onlyIfChecked)
    {
        if (onlyIfChecked && !widget.radioButtonShowResult->isChecked())
        {
            return;
        }
 
        ARMARX_INFO << "Drawing result point cloud";
 
        DebugDrawerColoredPointCloud debugPointCloud;
 
        for (std::size_t group = 0; group < userGrouping.size(); ++group)
        {
            for (uint32_t segment : userGrouping[group])
            {
                for (int i : segmentIndex.at(segment).indices)
                {
                    PointT& p = pointCloud[static_cast<std::size_t>(i)];
                    debugPointCloud.points.push_back(DebugDrawerColoredPointCloudElement
                    {
                        p.x, p.y, p.z, dcolor(group)
                    });
                }
            }
        }
 
        drawer.drawPointCloud({layerPointCloud, "PointCloud"}, debugPointCloud);
 
        //    drawSegmentIDs(true);
    }
 
    DrawColor UserAssistedSegmenterGuiWidgetController::dcolor(std::size_t id) const
    {
        return armarx::GlasbeyLUT::at(id);
    }
 
    int toByte(float f)
    {
        return static_cast<int>(f * 255);
    }
 
    QColor UserAssistedSegmenterGuiWidgetController::qcolor(std::size_t id) const
    {
        const DrawColor c = dcolor(id);
        return QColor(toByte(c.r), toByte(c.g), toByte(c.b), toByte(c.a));
    }
 
    void UserAssistedSegmenterGuiWidgetController::updateUserGroupingRows(int numRows)
    {
        int previousNumRows = widget.tableUserGrouping->rowCount();
        widget.tableUserGrouping->setRowCount(numRows);
 
        std::set<uint32_t> segmentIDs;
        for (const auto& [segmentID, _] : segmentIndex)
        {
            segmentIDs.insert(segmentID);
        }
 
        // Initialize new rows (does nothing if previousNumRows >= numRows).
        for (int id = previousNumRows; id < numRows; ++id)
        {
            QTableWidgetItem* itemColor = new QTableWidgetItem(QString());
            itemColor->setBackgroundColor(qcolor(static_cast<std::size_t>(id)));
            widget.tableUserGrouping->setItem(id, 0, itemColor);
 
            QTableWidgetItem* itemID = new QTableWidgetItem(qstr(id));
            itemID->setTextAlignment(Qt::AlignCenter);
            widget.tableUserGrouping->setItem(id, 1, itemID);
 
            UserGroupingLineEdit* itemList = new UserGroupingLineEdit({segmentIDs}, static_cast<uint32_t>(id));
            connect(itemList, SIGNAL(groupingChanged(uint32_t, std::vector<uint32_t>)),
                    this, SLOT(onUserGroupingChanged(uint32_t, std::vector<uint32_t>)));
            widget.tableUserGrouping->setCellWidget(id, 2, itemList);
        }
 
        userGrouping.resize(static_cast<std::size_t>(numRows));
    }
 
    void UserAssistedSegmenterGuiWidgetController::onUserGroupingChanged(
        uint32_t groupID, std::vector<uint32_t> segmentIDs)
    {
        if (groupID < userGrouping.size())
        {
            userGrouping[groupID] = segmentIDs;
 
            ARMARX_INFO << "Updated user grouping " << groupID << ": \n"
                        << userGrouping[groupID];
            drawResultPointCloud(true);
        }
    }
 
    void UserAssistedSegmenterGuiWidgetController::onTableGroupingCellChanged(int row, int col)
    {
        if (col == 0)
        {
            widget.tableUserGrouping->item(row, col)->setText(QString());
        }
        else if (col == 1)
        {
            widget.tableUserGrouping->item(row, col)->setText(qstr(row));
        }
    }
 
    void UserAssistedSegmenterGuiWidgetController::publishSegmentation()
    {
        if (!segmenterProxy)
        {
            ARMARX_ERROR << "Segmenter proxy not set.";
            return;
        }
 
        visionx::ColoredLabeledPointCloud resultCloud;
 
        for (std::size_t groupID = 0; groupID < userGrouping.size(); ++groupID)
        {
            for (uint32_t segment : userGrouping[groupID])
            {
                for (int i : segmentIndex.at(segment).indices)
                {
                    PointT& p = pointCloud.at(static_cast<std::size_t>(i));
                    resultCloud.push_back(visionx::ColoredLabeledPoint3D
                    {
                        visionx::Point3D { p.x, p.y, p.z },
                        visionx::RGBA { p.r, p.g, p.b, p.a },
                        static_cast<int>(groupID)
                    });
                }
            }
        }
        segmenterProxy->publishSegmentation(resultCloud);
    }
 
    void UserAssistedSegmenterGuiWidgetController::clearVisualization()
    {
        drawer.clearLayer(layerSegmentIDs);
        drawer.clearLayer(layerPointCloud);
        drawer.clearColoredPointCloud({layerPointCloud, "PointCloud"});
    }
 
}