MultiViewPointCloudProcessor.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::ArmarXObjects::MultiViewPointCloudProcessor
 * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * @date       2020
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "MultiViewPointCloudProcessor.h"
 
#include <pcl/common/transforms.h>
#include <pcl/filters/approximate_voxel_grid.h>
 
#include <ArmarXCore/core/util/OnScopeExit.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <VisionX/interface/components/Calibration.h>
#include <VisionX/interface/core/PointCloudProcessorInterface.h>
 
#define call_template_function(F)                                                                  \
    switch (_pointCloudProviderInfo.pointCloudFormat->type)                                        \
    {                                                                                              \
        case visionx::PointContentType::ePoints:                                                   \
            F<pcl::PointXYZ>();                                                                    \
            break;                                                                                 \
        case visionx::PointContentType::eColoredPoints:                                            \
            F<pcl::PointXYZRGBA>();                                                                \
            break;                                                                                 \
        case visionx::PointContentType::eColoredOrientedPoints:                                    \
            F<pcl::PointXYZRGBNormal>();                                                           \
            break;                                                                                 \
        case visionx::PointContentType::eLabeledPoints:                                            \
            F<pcl::PointXYZL>();                                                                   \
            break;                                                                                 \
        case visionx::PointContentType::eColoredLabeledPoints:                                     \
            F<pcl::PointXYZRGBL>();                                                                \
            break;                                                                                 \
        case visionx::PointContentType::eIntensity:                                                \
            F<pcl::PointXYZI>();                                                                   \
            break;                                                                                 \
        case visionx::PointContentType::eOrientedPoints:                                           \
            ARMARX_ERROR << "eOrientedPoints NOT HANDLED IN VISIONX";                              \
            [[fallthrough]];                                                                       \
        default:                                                                                   \
            ARMARX_ERROR << "Could not call " #F ", because format '"                              \
                         << _pointCloudProviderInfo.pointCloudFormat->type << "' is unknown";      \
    }                                                                                              \
    do                                                                                             \
    {                                                                                              \
    } while (false)
 
//common
namespace armarx
{
    std::string
    MultiViewPointCloudProcessor::getDefaultName() const
    {
        return "MultiViewPointCloudProcessor";
    }
 
    armarx::PropertyDefinitionsPtr
    MultiViewPointCloudProcessor::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr prop =
            new visionx::PointCloudProcessorPropertyDefinitions(getConfigIdentifier());
        prop->defineOptionalProperty<std::string>(
            "ProviderReferenceFrame",
            "",
            "Name of the point cloud refence frame (empty = autodetect)");
        std::lock_guard guard{_cfgBufWriteMutex};
        prop->optional(_cfgBuf.getWriteBuffer(), "", "");
        return prop;
    }
} // namespace armarx
 
//gui
namespace armarx
{
    RemoteGui::WidgetPtr
    MultiViewPointCloudProcessor::buildGui()
    {
        std::lock_guard guard{_cfgBufWriteMutex};
        return RemoteGui::makeVBoxLayout()
            .addChild(RemoteGui::MakeGuiConfig("cfg", _cfgBuf.getWriteBuffer()))
            .addChild(RemoteGui::makeHBoxLayout()
                          .addChild(RemoteGui::makeButton("start"))
                          .addChild(RemoteGui::makeButton("stop"))
                          .addChild(RemoteGui::makeButton("clear"))
                          .addHSpacer());
    }
 
    void
    MultiViewPointCloudProcessor::processGui(RemoteGui::TabProxy& prx)
    {
        prx.receiveUpdates();
        std::lock_guard guard{_cfgBufWriteMutex};
 
        prx.getValue(_cfgBuf.getWriteBuffer(), "cfg");
        if (prx.getButtonClicked("start"))
        {
            startRecordingNextViewPoint();
        }
        if (prx.getButtonClicked("stop"))
        {
            stopRecordingViewPoint();
        }
        if (prx.getButtonClicked("clear"))
        {
            clearViewPoints();
        }
        prx.sendUpdates();
        _cfgBuf.commitWrite();
    }
} // namespace armarx
 
//hooks
namespace armarx
{
    void
    MultiViewPointCloudProcessor::onInitPointCloudProcessor()
    {
        _pointCloudProviderName = getProperty<std::string>("ProviderName");
        usingPointCloudProvider(_pointCloudProviderName);
    }
 
    void
    MultiViewPointCloudProcessor::onConnectPointCloudProcessor()
    {
        //provider
        {
            _pointCloudProviderInfo = getPointCloudProvider(_pointCloudProviderName, true);
            _pointCloudProvider = _pointCloudProviderInfo.proxy;
            _pointCloudProviderRefFrame = getProperty<std::string>("ProviderReferenceFrame");
            if (_pointCloudProviderRefFrame.empty())
            {
                _pointCloudProviderRefFrame = "root";
                auto frameprov =
                    visionx::ReferenceFrameInterfacePrx::checkedCast(_pointCloudProvider);
                if (frameprov)
                {
                    _pointCloudProviderRefFrame = frameprov->getReferenceFrame();
                }
            }
            ARMARX_INFO << VAROUT(_pointCloudProviderRefFrame);
 
            if (!_providerThread.joinable())
            {
                //result cloud of point type
                call_template_function(enableResultPointClouds);
                _providerThread = std::thread{[&] { call_template_function(doProvide); }};
            }
        }
        //robot
        {
            _robot = addRobot("_robot", VirtualRobot::RobotIO::eStructure);
        }
        //gui
        createOrUpdateRemoteGuiTab(buildGui(),
                                   [this](RemoteGui::TabProxy& prx) { processGui(prx); });
    }
 
    void
    MultiViewPointCloudProcessor::onExitPointCloudProcessor()
    {
        _stopProviding = true;
        _providerThread.join();
    }
} // namespace armarx
 
//Ice methods
namespace armarx
{
    void
    MultiViewPointCloudProcessor::startRecordingNextViewPoint(const Ice::Current&)
    {
        ++_currentViewpoint;
        _doRecord = true;
    }
 
    void
    MultiViewPointCloudProcessor::stopRecordingViewPoint(const Ice::Current&)
    {
        _doRecord = false;
    }
 
    void
    MultiViewPointCloudProcessor::clearViewPoints(const Ice::Current&)
    {
        _doClear = true;
    }
 
    void
    MultiViewPointCloudProcessor::setMaxNumberOfFramesPerViewPoint(Ice::Int n, const Ice::Current&)
    {
        std::lock_guard guard{_cfgBufWriteMutex};
        _cfgBuf.getWriteBuffer().maxNumberOfFramesPerViewPoint = n;
        _cfgBuf.commitWrite();
    }
 
    void
    MultiViewPointCloudProcessor::setDownsamplingGridSize(Ice::Float s, const Ice::Current&)
    {
        std::lock_guard guard{_cfgBufWriteMutex};
        _cfgBuf.getWriteBuffer().gridSize = s;
        _cfgBuf.commitWrite();
    }
 
    void
    MultiViewPointCloudProcessor::setFrameRate(Ice::Float r, const Ice::Current&)
    {
        std::lock_guard guard{_cfgBufWriteMutex};
        _cfgBuf.getWriteBuffer().frameRate = r;
        _cfgBuf.commitWrite();
    }
} // namespace armarx
 
//process
namespace armarx
{
    void
    MultiViewPointCloudProcessor::process()
    {
        call_template_function(process);
    }
 
    template <class PointType>
    void
    MultiViewPointCloudProcessor::process()
    {
        ARMARX_ON_SCOPE_EXIT
        {
            sendDebugObserverBatch();
        };
        const auto doRecord = _doRecord.load();
        if (_doClear)
        {
            std::lock_guard guard{_viewPointsMutex};
            _viewPoints.clear();
            _currentViewpoint = doRecord ? 0 : -1;
            _doClear = false;
        }
        if (!doRecord)
        {
            return;
        }
        const auto currentViewpoint = _currentViewpoint.load();
        ARMARX_CHECK_GREATER_EQUAL(currentViewpoint, 0);
        using cloud_t = pcl::PointCloud<PointType>;
        using cloud_ptr_t = typename cloud_t::Ptr;
 
        std::unique_lock lock{_cfgBufReadMutex};
        const auto buf = _cfgBuf.getUpToDateReadBuffer();
        lock.unlock();
        setDebugObserverDatafield("cfg", buf);
        setDebugObserverDatafield("currentViewpoint", currentViewpoint);
        setDebugObserverDatafield("doRecord", doRecord);
 
        //get cloud + transform to global
        cloud_ptr_t transformedCloud;
        {
            if (!waitForPointClouds(_pointCloudProviderName, 100))
            {
                ARMARX_INFO << deactivateSpam(10, _pointCloudProviderName)
                            << "Timeout or error while waiting for point cloud data of provider "
                            << _pointCloudProviderName;
                return;
            }
            cloud_ptr_t inputCloud(new cloud_t);
            getPointClouds(_pointCloudProviderName, inputCloud);
            synchronizeLocalClone(_robot);
 
            const Eigen::Matrix4f providerInRootFrame = [&]() -> Eigen::Matrix4f
            {
                FramedPose fp{
                    Eigen::Matrix4f::Identity(), _pointCloudProviderRefFrame, _robot->getName()};
                fp.changeFrame(_robot, "Global");
                return fp.toEigen();
            }();
 
            transformedCloud = cloud_ptr_t(new cloud_t);
            pcl::transformPointCloud(*inputCloud, *transformedCloud, providerInRootFrame);
        }
        setDebugObserverDatafield("cloud_size_in", transformedCloud->size());
        std::lock_guard guard{_viewPointsMutex};
        const auto p = static_cast<std::size_t>(currentViewpoint);
        if (p + 1 > _viewPoints.size())
        {
            _viewPoints.resize(p + 1);
        }
        _viewPoints.at(p).clouds.emplace_back(transformedCloud);
        if (buf.maxNumberOfFramesPerViewPoint > 0)
        {
            while (_viewPoints.at(p).clouds.size() >
                   static_cast<std::size_t>(buf.maxNumberOfFramesPerViewPoint))
            {
                _viewPoints.at(p).clouds.pop_front();
            }
        }
        setDebugObserverDatafield("current_view_num_clouds", _viewPoints.at(p).clouds.size());
    }
 
    template <class PointType>
    void
    MultiViewPointCloudProcessor::doProvide()
    {
        using cloud_t = pcl::PointCloud<PointType>;
        using cloud_ptr_t = typename cloud_t::Ptr;
        std::deque<std::uint64_t> idxVec;
        while (!_stopProviding)
        {
            const auto timeBeg = std::chrono::high_resolution_clock::now();
 
            std::unique_lock lock{_cfgBufReadMutex};
            const auto buf = _cfgBuf.getUpToDateReadBuffer();
            lock.unlock();
 
            cloud_ptr_t cloud(new cloud_t);
            {
                std::lock_guard guard{_viewPointsMutex};
                if (idxVec.size() < _viewPoints.size())
                {
                    idxVec.resize(_viewPoints.size());
                }
                for (std::size_t i = 0; i < idxVec.size(); ++i)
                {
                    const auto& inVec = _viewPoints.at(i).clouds;
                    const auto idx = (idxVec.at(i)++) % inVec.size();
                    const auto& in = *std::any_cast<cloud_ptr_t>(inVec.at(idx));
                    auto& out = cloud->points;
                    out.insert(out.end(), in.begin(), in.end());
                }
            }
            //downsample
            if (buf.gridSize > 0)
            {
                pcl::ApproximateVoxelGrid<PointType> grid;
                grid.setLeafSize(Eigen::Vector3f::Constant(buf.gridSize));
                grid.setInputCloud(cloud);
                cloud_ptr_t tempCloud(new cloud_t);
                grid.filter(*tempCloud);
                tempCloud.swap(cloud);
            }
            //provide
            provideResultPointClouds(cloud);
 
            //sleep for frame rate
            if (frameRate > 0)
            {
                const long dtms = 1 / frameRate * 1000;
                std::this_thread::sleep_until(timeBeg + std::chrono::milliseconds{dtms});
            }
        }
    }
} // namespace armarx