VoxelGridMappingProvider.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::ArmarXObjects::VoxelGridMappingProvider
 * @author     Mirko Waechter ( mirko dot waechter at kit dot edu )
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "VoxelGridMappingProvider.h"
 
#include <pcl/common/transforms.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/filters/frustum_culling.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/uniform_sampling.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
 
#include <ArmarXCore/core/util/IceReportSkipper.h>
#include <ArmarXCore/core/util/StringHelperTemplates.h>
#include <ArmarXCore/core/util/algorithm.h>
 
#include <RobotAPI/components/DebugDrawer/DebugDrawerUtils.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
//#include <RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.h>
#include <VirtualRobot/CollisionDetection/CDManager.h>
#include <VirtualRobot/CollisionDetection/CollisionChecker.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
#include <VirtualRobot/Visualization/TriMeshUtils.h>
 
#include <VisionX/components/pointcloud_core/PointCloudConversions.h>
#include <VisionX/interface/components/Calibration.h>
 
#include <MotionPlanning/CSpace/CSpaceSampled.h>
#include <MotionPlanning/Planner/BiRrt.h>
#include <MotionPlanning/PostProcessing/ShortcutProcessor.h>
 
 
using namespace armarx;
 
namespace visionx
{
    VoxelGridMappingProvider::VoxelGridMappingProvider()
    {
    }
 
    std::string
    VoxelGridMappingProvider::getDefaultName() const
    {
        return "VoxelGridMappingProvider";
    }
 
    PropertyDefinitionsPtr
    VoxelGridMappingProvider::createPropertyDefinitions()
    {
        return PropertyDefinitionsPtr(
            new VoxelGridMappingProviderPropertyDefinitions(getConfigIdentifier()));
    }
 
    void
    VoxelGridMappingProvider::onInitPointCloudProcessor()
    {
        skipper.reset(
            new IceReportSkipper(getProperty<float>("PointCloudStorageFrequency").getValue()));
        cycleKeeper.reset(new CycleUtil(1000.0f / getProperty<float>("UpdateRate").getValue()));
        providerName = getProperty<std::string>("providerName").getValue();
        sourceFrameName = getProperty<std::string>("sourceFrameName").getValue();
        updateBoundaries();
        gridLeafSize = getProperty<float>("GridSize");
 
        usingPointCloudProvider(providerName);
        accumulatedPointCloud.reset(new pcl::PointCloud<PointType>());
        if (!getProperty<std::string>("PointCloudLoadFilepath").getValue().empty())
        {
            ARMARX_INFO << "Loading pointcloud from "
                        << getProperty<std::string>("PointCloudLoadFilepath").getValue();
            pcl::io::loadPCDFile(getProperty<std::string>("PointCloudLoadFilepath"),
                                 *accumulatedPointCloud);
        }
    }
 
    void
    VoxelGridMappingProvider::onConnectPointCloudProcessor()
    {
        if (getProperty<std::string>("RobotStateComponentName").getValue() != "")
        {
            robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(
                getProperty<std::string>("RobotStateComponentName").getValue());
            auto localRobot = RemoteRobot::createLocalCloneFromFile(
                robotStateComponent, VirtualRobot::RobotIO::RobotDescription::eCollisionModel);
            robotPool.reset(new RobotPool(localRobot, 3));
        }
        dd = getTopic<DebugDrawerInterfacePrx>("DebugDrawerUpdates");
        collectingPointClouds = getProperty<bool>("ActivateOnStartUp");
        octtree.reset(new pcl::octree::OctreePointCloud<PointType>(gridLeafSize));
 
        if (sourceFrameName.empty())
        {
 
            visionx::ReferenceFrameInterfacePrx proxy =
                getProxy<visionx::ReferenceFrameInterfacePrx>(providerName);
            if (proxy)
            {
                sourceFrameName = proxy->getReferenceFrame();
                ARMARX_INFO << "source frame name is " << sourceFrameName;
            }
        }
 
        enableResultPointClouds<PointType>();
    }
 
    void
    VoxelGridMappingProvider::onExitPointCloudProcessor()
    {
    }
 
    void
    VoxelGridMappingProvider::updateBoundaries()
    {
        auto limits = Split(getProperty<std::string>("BoundingBox").getValue(), ";", true);
        int i = 0;
        ARMARX_CHECK_EQUAL(limits.size(), 3);
        for (auto& pos : limits)
        {
            auto posVec = Split<float>(pos, ",", true);
            ARMARX_CHECK_EQUAL(posVec.size(), 2);
            croppingMin(i) = posVec.at(0);
            croppingMax(i) = posVec.at(1);
            i++;
        }
    }
 
    void
    VoxelGridMappingProvider::process()
    {
 
        typename pcl::PointCloud<PointType>::Ptr inputCloudPtr(new pcl::PointCloud<PointType>());
        typename pcl::PointCloud<PointType>::Ptr downsampledTransformedInputCloudPtr(
            new pcl::PointCloud<PointType>());
        typename pcl::PointCloud<PointType>::Ptr outputCloudPtr(new pcl::PointCloud<PointType>());
 
        if (!collectingPointClouds)
        {
            ARMARX_DEBUG << deactivateSpam(10) << "Pointcloud processing is disabled";
            usleep(10000);
            return;
        }
        if (!waitForPointClouds(providerName, 10000))
        {
            ARMARX_INFO << "Timeout or error while waiting for point cloud data" << flush;
            return;
        }
        else
        {
            getPointClouds(providerName, inputCloudPtr);
        }
        TIMING_START(FullCalc);
 
        ARMARX_INFO << "Got cloud of size: " << inputCloudPtr->size();
        auto localRobot = robotPool->getRobot();
        visionx::MetaPointCloudFormatPtr format = getPointCloudFormat(providerName);
        ARMARX_INFO << deactivateSpam(1) << "pc timestamp: "
                    << IceUtil::Time::microSeconds(format->timeProvided).toDateTime();
        if (robotStateComponent)
        {
            if (!RemoteRobot::synchronizeLocalCloneToTimestamp(
                    localRobot, robotStateComponent, format->timeProvided))
            {
                ARMARX_WARNING << deactivateSpam(5)
                               << "failed to sync robot to timestamp! conversion to globalpose "
                                  "will fail! aborting";
                return;
            }
        }
 
        Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
        //    Eigen::Matrix4f sourceFrameGlobalPose = Eigen::Matrix4f::Identity();
        if (robotStateComponent && sourceFrameName != "Global")
        {
            transform = localRobot->getRobotNode(sourceFrameName)->getGlobalPose();
        }
 
        Eigen::Matrix4f cam2robot;
        cam2robot << 0, 0, 1, 0, 0, -1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1;
        // .. read or fill the source cloud
        pcl::FrustumCulling<PointType> fc;
        fc.setInputCloud(inputCloudPtr);
        fc.setNegative(false);
        fc.setVerticalFOV(35);
        fc.setHorizontalFOV(45);
        fc.setNearPlaneDistance(0.0);
        fc.setFarPlaneDistance(3500);
 
        fc.setCameraPose(cam2robot);
 
        fc.filter(*outputCloudPtr);
        ARMARX_INFO << "size before frustrum culling: " << inputCloudPtr->size()
                    << " size after: " << outputCloudPtr->size();
        inputCloudPtr.swap(outputCloudPtr);
 
        ARMARX_INFO << "Robot camera transform is: " << transform;
        //    float N = 0.001f;
        //    transform (0,0) = transform (0,0) * N;
        //    transform (1,1) = transform (1,1) * N;
        //    transform (2,2) = transform (2,2) * N;
        TIMING_START(PCl_TRANSFORM);
        // Transform to intermediate cropping frame
        pcl::transformPointCloud(*inputCloudPtr, *outputCloudPtr, transform);
        outputCloudPtr.swap(inputCloudPtr);
        TIMING_END(PCl_TRANSFORM);
 
        TIMING_START(PCL_FILTER);
        //    // assumes that z axis goes up in robot's root frame
        pcl::PassThrough<PointType> pass;
 
 
        pass.setInputCloud(inputCloudPtr);
        pass.setFilterFieldName("z");
        pass.setFilterLimits(croppingMin(2), croppingMax(2));
        pass.filter(*outputCloudPtr);
        outputCloudPtr.swap(inputCloudPtr);
 
        pass.setInputCloud(inputCloudPtr);
        pass.setFilterFieldName("x");
        pass.setFilterLimits(croppingMin(0), croppingMax(0));
        pass.filter(*outputCloudPtr);
        outputCloudPtr.swap(inputCloudPtr);
 
        pass.setInputCloud(inputCloudPtr);
        pass.setFilterFieldName("y");
        pass.setFilterLimits(croppingMin(1), croppingMax(1));
        pass.filter(*outputCloudPtr);
        outputCloudPtr.swap(inputCloudPtr);
        TIMING_END(PCL_FILTER);
 
 
        TIMING_START(PCl_DOWNSAMPLING);
        gridLeafSize = getProperty<float>("GridSize");
        pcl::VoxelGrid<PointType> grid;
        grid.setMinimumPointsNumberPerVoxel(
            getProperty<int>("MinimumPointNumberPerVoxel").getValue());
        //    grid.setDownsampleAllData(false);
        grid.setLeafSize(gridLeafSize, gridLeafSize, gridLeafSize);
        grid.setInputCloud(inputCloudPtr);
        grid.filter(*outputCloudPtr);
        ARMARX_VERBOSE << "Got cloud of size: " << inputCloudPtr->size()
                       << " after downsampling: " << outputCloudPtr->size();
        outputCloudPtr.swap(inputCloudPtr);
 
 
        if (!lastPointCloud)
        {
            lastPointCloud.reset(new Cloud());
        }
        //    {
        //        *inputCloudPtr += *lastPointCloud;
 
        //        pcl::VoxelGrid<PointType> grid;
        //        grid.setMinimumPointsNumberPerVoxel(2);
        //        //    grid.setDownsampleAllData(false);
        //        grid.setLeafSize(gridLeafSize, gridLeafSize, gridLeafSize);
        //        grid.setInputCloud(inputCloudPtr);
        //        grid.filter(*outputCloudPtr);
        //        ARMARX_VERBOSE << "Cloud size after removing of temporal points (e.g. flickering): " << outputCloudPtr->size();
        //        outputCloudPtr.swap(inputCloudPtr);
        //        lastPointCloud = downsampledTransformedInputCloudPtr;
        //    }
 
 
        TIMING_END(PCl_DOWNSAMPLING);
 
        //    Eigen::Matrix4f transformScaleUp = Eigen::Matrix4f::Identity();
        //    float N2 = 1000.f;
        //    transformScaleUp (0,0) = transformScaleUp (0,0) * N2;
        //    transformScaleUp (1,1) = transformScaleUp (1,1) * N2;
        //    transformScaleUp (2,2) = transformScaleUp (2,2) * N2;
 
        //    pcl::transformPointCloud(*inputCloudPtr, *outputCloudPtr, transformScaleUp);
        //    outputCloudPtr.swap(inputCloudPtr);
 
 
        typename pcl::PointCloud<PointType>::Ptr occludedPoints(new pcl::PointCloud<PointType>());
        typename pcl::PointCloud<PointType>::Ptr oldPointsInView(new pcl::PointCloud<PointType>());
        {
            TIMING_START(PCL_FrustumCulling);
            std::unique_lock lock(dataMutex);
            Eigen::Matrix4f cam2robot;
            cam2robot << 0, 0, 1, 0, 0, -1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1;
            // .. read or fill the source cloud
            pcl::FrustumCulling<PointType> fc;
            fc.setInputCloud(accumulatedPointCloud);
            fc.setNegative(false);
            fc.setVerticalFOV(40);
            fc.setHorizontalFOV(50);
            fc.setNearPlaneDistance(0.0);
            fc.setFarPlaneDistance(2500);
 
            fc.setCameraPose(transform * cam2robot);
 
            fc.filter(*oldPointsInView);
 
 
            fc.setNegative(true);
            fc.filter(*outputCloudPtr);
            outputCloudPtr.swap(accumulatedPointCloud);
            TIMING_END(PCL_FrustumCulling);
 
 
            TIMING_START(OCCLUSION);
            Eigen::Vector3f cameraPos = transform.block<3, 1>(0, 3);
            for (auto& p : *oldPointsInView)
            {
                Eigen::Vector3f currentPoint(
                    p._PointXYZRGBA::x, p._PointXYZRGBA::y, p._PointXYZRGBA::z);
                VirtualRobot::MathTools::BaseLine<Eigen::Vector3f> line(transform.block<3, 1>(0, 3),
                                                                        currentPoint - cameraPos);
                float distP1 = (currentPoint - cameraPos).norm();
                for (auto& p2pcl : *inputCloudPtr)
                {
                    Eigen::Vector3f p2(
                        p2pcl._PointXYZRGBA::x, p2pcl._PointXYZRGBA::y, p2pcl._PointXYZRGBA::z);
                    float distP2 = (cameraPos - p2).norm();
                    if (distP1 > distP2)
                    {
                        auto distance = VirtualRobot::MathTools::distPointLine(line, p2);
                        ARMARX_CHECK_GREATER_EQUAL(distance, 0.0f);
                        if (distance < gridLeafSize)
                        {
                            auto newP = p;
                            //                    newP._PointXYZRGBA::r = 255;
                            //                    newP._PointXYZRGBA::g = 0;
                            //                    newP._PointXYZRGBA::b = 0;
                            occludedPoints->push_back(newP);
                            break;
                        }
                    }
                }
            }
            TIMING_END(OCCLUSION);
 
            TIMING_START(RemoveRobotVoxels);
            inputCloudPtr = removeRobotVoxels(inputCloudPtr, localRobot, gridLeafSize);
            ARMARX_VERBOSE << "Size after removal of robot voxels: " << inputCloudPtr->size();
            TIMING_END(RemoveRobotVoxels);
 
            TIMING_START(Accumulation);
            //        octtree->setInputCloud(inputCloudPtr);
            //        octtree->addPointsFromInputCloud();
            //        octtree->setInputCloud(occludedPoints);
            //        octtree->addPointsFromInputCloud();
 
            *accumulatedPointCloud += *inputCloudPtr;
            *accumulatedPointCloud += *occludedPoints;
 
 
            *accumulatedPointCloud += *occludedPoints;
            ARMARX_VERBOSE << "Accumulated pc size: " << accumulatedPointCloud->size()
                           << " new point count: " << inputCloudPtr->size()
                           << " number of occluded points: " << occludedPoints->size();
            TIMING_END(Accumulation);
            TIMING_START(Downsampling2);
            //     filter again to remove duplicated points
 
            pcl::VoxelGrid<PointType> grid2;
            grid2.setLeafSize(gridLeafSize, gridLeafSize, gridLeafSize);
            grid2.setInputCloud(accumulatedPointCloud);
            grid2.filter(*outputCloudPtr);
            outputCloudPtr.swap(accumulatedPointCloud);
            ARMARX_VERBOSE << "Accumulated pc size after grid filtering: "
                           << accumulatedPointCloud->size() << " " << VAROUT(gridLeafSize);
            TIMING_END(Downsampling2);
 
 
            provideResultPointClouds(accumulatedPointCloud);
 
            TIMING_START(PREPARATION);
 
            //    std::map<Eigen::Vector3i, int> grid;
            std::vector<VirtualRobot::VisualizationFactory::Color> gridColors;
            gridPositions.reserve(accumulatedPointCloud->size());
            gridColors.reserve(accumulatedPointCloud->size());
 
            DebugDrawerTriMesh ddTriMesh;
 
            this->gridPositions.clear();
            this->gridPositions.reserve(accumulatedPointCloud->size());
            //        pcl::octree::OctreePointCloud<PointType>::AlignedPointTVector points;
            //        octtree->getOccupiedVoxelCenters(points);
            for (auto& p : *accumulatedPointCloud)
            {
                if (!std::isnan(p._PointXYZRGBA::x) && !std::isnan(p._PointXYZRGBA::y) &&
                    !std::isnan(p._PointXYZRGBA::z))
                {
 
                    this->gridPositions.push_back(
                        {p._PointXYZRGBA::x, p._PointXYZRGBA::y, p._PointXYZRGBA::z});
                    gridColors.push_back(
                        VirtualRobot::VisualizationFactory::Color(p._PointXYZRGBA::r / 255.f,
                                                                  p._PointXYZRGBA::g / 255.f,
                                                                  p._PointXYZRGBA::b / 255.f,
                                                                  0.8));
                }
            }
            TIMING_END(PREPARATION);
 
            std::vector<Eigen::Vector3f> gridPositionsEigen = armarx::transform(
                this->gridPositions,
                +[](const Vector3f& p) -> Eigen::Vector3f {
                    return {p.e0, p.e1, p.e2};
                });
 
 
            ARMARX_INFO << VAROUT(gridPositionsEigen.size());
 
            TIMING_START(MESHING);
 
            gridMesh = VirtualRobot::TriMeshUtils::CreateSparseBoxGrid(
                Eigen::Matrix4f::Identity(),
                gridPositionsEigen,
                gridLeafSize,
                gridLeafSize,
                gridLeafSize,
                VirtualRobot::VisualizationFactory::Color::Blue(),
                gridColors);
            ddTriMesh = DebugDrawerUtils::convertTriMesh(*gridMesh);
            TIMING_END(MESHING);
            TIMING_START(VISU);
            //        DebugDrawer24BitColoredPointCloud ddPC;
            //        visionx::tools::convertFromPCLToDebugDrawer(accumulatedPointCloud, ddPC, visionx::tools::eConvertAsColors);
            //        dd->set24BitColoredPointCloudVisu("VoxelGrid", "PointCloud", ddPC);
            //        ARMARX_INFO << "Sending point cloud to debug drawer - size: " << ddTriMesh.faces.size() << " first: " << ddTriMesh.vertices.at(0).x << ", " << ddTriMesh.vertices.at(0).y << ", " << ddTriMesh.vertices.at(0).z;
            dd->setTriMeshVisu("VoxelGrid", "PointCloudCollisionGrid", ddTriMesh);
            TIMING_END(VISU);
        }
 
 
        if (accumulatedPointCloud->size() > 0 && skipper->checkFrequency("SavingPCD") &&
            !getProperty<std::string>("PointCloudStoreFilepath").getValue().empty())
        {
            std::unique_lock lock(dataMutex);
            ARMARX_INFO << "Saving point cloud to "
                        << getProperty<std::string>("PointCloudStoreFilepath").getValue();
            pcl::io::savePCDFile(getProperty<std::string>("PointCloudStoreFilepath").getValue(),
                                 *accumulatedPointCloud);
        }
        TIMING_END(FullCalc);
 
 
        cycleKeeper->waitForCycleDuration();
    }
 
    VoxelGridMappingProvider::CloudPtr
    VoxelGridMappingProvider::removeRobotVoxels(const VoxelGridMappingProvider::CloudPtr& cloud,
                                                const VirtualRobot::RobotPtr& robot,
                                                float distanceThreshold)
    {
        CloudPtr result(new Cloud());
        VirtualRobot::CDManager cdm;
        auto colModels = robot->getCollisionModels();
        auto collisionChecker = robot->getCollisionChecker();
        for (auto& p : *cloud)
        {
            bool collisionFound = false;
            for (auto& colModel : colModels)
            {
                Eigen::Vector3f pEigen(p._PointXYZRGBA::x, p._PointXYZRGBA::y, p._PointXYZRGBA::z);
                if (collisionChecker->checkCollision(colModel, pEigen, distanceThreshold))
                {
                    collisionFound = true;
                    break;
                }
            }
            if (!collisionFound)
            {
                result->push_back(p);
            }
        }
        return result;
    }
 
    Vector3fSeq
    VoxelGridMappingProvider::getFilledGridPositions(const Ice::Current&) const
    {
        std::unique_lock lock(dataMutex);
        return gridPositions;
    }
 
    Ice::Float
    VoxelGridMappingProvider::getGridSize(const Ice::Current&) const
    {
        return gridLeafSize;
    }
 
    void
    VoxelGridMappingProvider::startCollectingPointClouds(const Ice::Current&)
    {
        collectingPointClouds = true;
    }
 
    void
    VoxelGridMappingProvider::stopCollectingPointClouds(const Ice::Current&)
    {
        collectingPointClouds = false;
    }
 
    void
    VoxelGridMappingProvider::reset(const Ice::Current&)
    {
        std::unique_lock lock(dataMutex);
        ARMARX_INFO << "Clearing voxel grid";
        accumulatedPointCloud->clear();
    }
 
    void
    VoxelGridMappingProvider::componentPropertiesUpdated(
        const std::set<std::string>& changedProperties)
    {
        if (changedProperties.count("BoundingBox"))
        {
            updateBoundaries();
        }
    }
} // namespace visionx