EfficientRANSACPrimitiveExtractor.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::EfficientRANSACPrimitiveExtractor
 * @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 <pcl/filters/statistical_outlier_removal.h>
#include <pcl/common/transforms.h>
 
#include <ArmarXCore/core/util/OnScopeExit.h>
 
#include <ArmarXGui/libraries/RemoteGui/RemoteGui.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <VisionX/interface/components/Calibration.h>
#include <SimoxUtility/meta/type_name.h>
 
#include "EfficientRANSACPrimitiveExtractor.h"
 
//gui
namespace armarx
{
    RemoteGui::WidgetPtr EfficientRANSACPrimitiveExtractor::buildGui()
    {
        const auto& buf = _cfgBuf.getWriteBuffer();
        ARMARX_TRACE;
        return RemoteGui::makeSimpleGridLayout().cols(2)
 
               .addTextLabel("outlierMeanK")
               .addChild(RemoteGui::makeIntSpinBox("outlierMeanK")
                         .value(buf.outlierMeanK).min(1).max(1000))
 
               .addTextLabel("outlierStddevMulThresh")
               .addChild(RemoteGui::makeFloatSpinBox("outlierStddevMulThresh")
                         .value(buf.outlierStddevMulThresh).min(0).max(10))
 
               .addEmptyWidget()
               .addEmptyWidget()
 
               .addTextLabel("ransacEpsilon")
               .addChild(RemoteGui::makeFloatSpinBox("ransacEpsilon")
                         .value(buf.ransacEpsilon).min(1).max(1000))
 
               .addTextLabel("ransacBitmapEpsilon")
               .addChild(RemoteGui::makeFloatSpinBox("ransacBitmapEpsilon")
                         .value(buf.ransacBitmapEpsilon).min(1).max(10000))
 
               .addTextLabel("ransacNormalThresh")
               .addChild(RemoteGui::makeFloatSpinBox("ransacNormalThresh")
                         .value(buf.ransacNormalThresh).min(0).max(1).decimals(3))
 
               .addTextLabel("ransacProbability")
               .addChild(RemoteGui::makeFloatSpinBox("ransacProbability")
                         .value(buf.ransacProbability).min(0).max(1).decimals(3))
 
               .addTextLabel("ransacMinSupport")
               .addChild(RemoteGui::makeIntSpinBox("ransacMinSupport")
                         .value(buf.ransacMinSupport).min(1).max(1000))
 
               .addTextLabel("ransacMaxruntimeMs")
               .addChild(RemoteGui::makeIntSpinBox("ransacMaxruntimeMs")
                         .value(buf.ransacMaxruntimeMs).min(1).max(10000));
    }
 
    void EfficientRANSACPrimitiveExtractor::processGui(RemoteGui::TabProxy& prx)
    {
        ARMARX_TRACE;
        prx.receiveUpdates();
 
        auto& buf = _cfgBuf.getWriteBuffer();
        prx.getValue(buf.outlierMeanK, "outlierMeanK");
        prx.getValue(buf.outlierStddevMulThresh, "outlierStddevMulThresh");
 
        prx.getValue(buf.ransacEpsilon, "ransacEpsilon");
        prx.getValue(buf.ransacNormalThresh, "ransacNormalThresh");
        prx.getValue(buf.ransacMinSupport, "ransacMinSupport");
        prx.getValue(buf.ransacBitmapEpsilon, "ransacBitmapEpsilon");
        prx.getValue(buf.ransacProbability, "ransacProbability");
        prx.getValue(buf.ransacMaxruntimeMs, "ransacMaxruntimeMs");
        _cfgBuf.commitWrite();
        prx.sendUpdates();
    }
}
 
#define result_name_transformed getName() + "_Transformed"
#define result_name_filtered getName() + "_Filtered"
 
namespace armarx
{
    EfficientRANSACPrimitiveExtractorPropertyDefinitions::EfficientRANSACPrimitiveExtractorPropertyDefinitions(std::string prefix) :
        visionx::PointCloudProcessorPropertyDefinitions(prefix)
    {
        defineOptionalProperty<std::string>("ProviderReferenceFrame", "", "Name of the point cloud refence frame (empty = autodetect)");
 
    }
 
    std::string EfficientRANSACPrimitiveExtractor::getDefaultName() const
    {
        return "EfficientRANSACPrimitiveExtractor";
    }
 
    void EfficientRANSACPrimitiveExtractor::onInitPointCloudProcessor()
    {
        _pointCloudProviderName = getProperty<std::string>("ProviderName");
        usingPointCloudProvider(_pointCloudProviderName);
    }
    void EfficientRANSACPrimitiveExtractor::onConnectPointCloudProcessor()
    {
        //provider
        {
            enableResultPointClouds<pcl::PointXYZ>(result_name_transformed);
            enableResultPointClouds<pcl::PointXYZ>(result_name_filtered);
 
            _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);
        }
        //robot
        {
            addRobot("_localRobot", VirtualRobot::RobotIO::eStructure);
            _localRobot = getRobotData("_localRobot");
        }
        //gui
        createOrUpdateRemoteGuiTab(buildGui(), [this](RemoteGui::TabProxy & prx)
        {
            processGui(prx);
        });
        arviz = createArVizClient();
    }
    void EfficientRANSACPrimitiveExtractor::onDisconnectPointCloudProcessor()
    {
 
    }
    void EfficientRANSACPrimitiveExtractor::onExitPointCloudProcessor()
    {
 
    }
    void EfficientRANSACPrimitiveExtractor::process()
    {
 
#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 process point cloud, because format '"                           \
                         << _pointCloudProviderInfo.pointCloudFormat->type << "' is unknown";           \
    } do{}while(false)
        call_template_function(process);
    }
 
 
    armarx::PropertyDefinitionsPtr EfficientRANSACPrimitiveExtractor::createPropertyDefinitions()
    {
        return armarx::PropertyDefinitionsPtr(new EfficientRANSACPrimitiveExtractorPropertyDefinitions(
                getConfigIdentifier()));
    }
 
}
 
#include "EfficientRANSAC/PointCloud.h"
#include "EfficientRANSAC/RansacShapeDetector.h"
 
#include "EfficientRANSAC/PlanePrimitiveShape.h"
#include "EfficientRANSAC/CylinderPrimitiveShape.h"
#include "EfficientRANSAC/SpherePrimitiveShape.h"
#include "EfficientRANSAC/ConePrimitiveShape.h"
#include "EfficientRANSAC/TorusPrimitiveShape.h"
 
#include "EfficientRANSAC/PlanePrimitiveShapeConstructor.h"
#include "EfficientRANSAC/CylinderPrimitiveShapeConstructor.h"
#include "EfficientRANSAC/SpherePrimitiveShapeConstructor.h"
#include "EfficientRANSAC/ConePrimitiveShapeConstructor.h"
#include "EfficientRANSAC/TorusPrimitiveShapeConstructor.h"
 
namespace armarx
{
    template<class PointType>
    void EfficientRANSACPrimitiveExtractor::process()
    {
        const auto& buf = _cfgBuf.getUpToDateReadBuffer();
        using cloud_t = pcl::PointCloud<PointType>;
        using cloud_ptr_t = typename cloud_t::Ptr;
        const auto now = [] {return std::chrono::high_resolution_clock::now();};
        const auto timeBeg = now();
        ARMARX_ON_SCOPE_EXIT
        {
            const auto dt = now() - timeBeg;
            const int us = std::chrono::duration_cast<std::chrono::microseconds>(dt).count();
            setDebugObserverDatafield("time_full_process_call_s", us / 1e6f);
            sendDebugObserverBatch();
        };
        //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);
            setDebugObserverDatafield("size_cloud_in", inputCloud->size());
            synchronizeLocalClone(_localRobot);
 
            const Eigen::Matrix4f providerInRootFrame = [&]() -> Eigen::Matrix4f
            {
                FramedPose fp {Eigen::Matrix4f::Identity(), _pointCloudProviderRefFrame, _localRobot.robot->getName()};
                //fp.changeFrame(_localRobot.robot, _localRobot.robot->getRootNode()->getName());
                fp.changeFrame(_localRobot.robot, "Global");
                return fp.toEigen();
            }();
 
            transformedCloud = cloud_ptr_t(new cloud_t);
            pcl::transformPointCloud(*inputCloud, *transformedCloud, providerInRootFrame);
 
            provideResultPointClouds(transformedCloud, result_name_transformed);
        }
 
        //remove outliers
        {
            cloud_ptr_t filtered(new cloud_t);
            pcl::StatisticalOutlierRemoval<PointType> sor;
            sor.setInputCloud(transformedCloud);
            sor.setMeanK(buf.outlierMeanK);
            sor.setStddevMulThresh(buf.outlierStddevMulThresh);
            sor.filter(*filtered);
            setDebugObserverDatafield("size_cloud_filtered", filtered->size());
            provideResultPointClouds(filtered, result_name_filtered);
            std::swap(filtered, transformedCloud);
        }
 
        //ransac stuff
        {
            ARMARX_INFO << "creating point vec";
            std::vector<Point> points;
            points.reserve(transformedCloud->size());
            static constexpr float inf = std::numeric_limits<float>::infinity();
            float minX = inf;
            float minY = inf;
            float minZ = inf;
            float maxX = -inf;
            float maxY = -inf;
            float maxZ = -inf;
            for (const auto& p : *transformedCloud)
            {
                points.emplace_back(Vec3f(p.x, p.y, p.z));
                std::tie(minX, maxX) = std::minmax({minX, maxX, p.x});
                std::tie(minY, maxY) = std::minmax({minY, maxY, p.y});
                std::tie(minZ, maxZ) = std::minmax({minZ, maxZ, p.z});
            }
            ARMARX_INFO << "creating cloud";
            PointCloud pc(points.data(), points.size());
            pc.setBBox({minX, minY, minZ}, {maxX, maxY, maxZ});
            ARMARX_INFO << "calc cloud normals";
            pc.calcNormals(100);
            ARMARX_INFO << VAROUT(pc.getScale());
 
            ARMARX_INFO << "configure ransac";
            RansacShapeDetector::Options ransacOptions;
            {
                ransacOptions.m_epsilon = buf.ransacEpsilon; // set distance threshold to .01f of bounding box width
                // NOTE: Internally the distance threshold is taken as 3 * ransacOptions.m_epsilon!!!
                ransacOptions.m_bitmapEpsilon = buf.ransacBitmapEpsilon; // set bitmap resolution to .02f of bounding box width
                // NOTE: This threshold is NOT multiplied internally!
                ransacOptions.m_normalThresh = buf.ransacNormalThresh; // this is the cos of the maximal normal deviation
                ransacOptions.m_minSupport = buf.ransacMinSupport; // this is the minimal numer of points required for a primitive
                ransacOptions.m_probability = buf.ransacProbability; // this is the "probability" with which a primitive is overlooked
                ransacOptions.m_maxruntime = std::chrono::milliseconds{buf.ransacMaxruntimeMs};
            }
 
            RansacShapeDetector detector(ransacOptions); // the detector object
            {
                // set which primitives are to be detected by adding the respective constructors
                detector.Add(new PlanePrimitiveShapeConstructor());
                detector.Add(new SpherePrimitiveShapeConstructor());
                detector.Add(new CylinderPrimitiveShapeConstructor());
                //detector.Add(new ConePrimitiveShapeConstructor());
                //detector.Add(new TorusPrimitiveShapeConstructor());
            }
 
 
            ARMARX_INFO << "run ransac";
            MiscLib::Vector< std::pair< MiscLib::RefCountPtr< PrimitiveShape >, size_t > > shapes; // stores the detected shapes
            size_t remaining = detector.Detect(pc, 0, pc.size(), &shapes); // run detection
            // returns number of unassigned points
            // the array shapes is filled with pointers to the detected shapes
            // the second element per shapes gives the number of points assigned to that primitive (the support)
            // the points belonging to the first shape (shapes[0]) have been sorted to the end of pc,
            // i.e. into the range [ pc.size() - shapes[0].second, pc.size() )
            // the points of shape i are found in the range
            // [ pc.size() - \sum_{j=0..i} shapes[j].second, pc.size() - \sum_{j=0..i-1} shapes[j].second )
 
            ARMARX_INFO << "remaining unassigned points " << remaining << std::endl;
 
            setDebugObserverDatafield("ransac_num_remaining_points", remaining);
            setDebugObserverDatafield("ransac_num_shapes", shapes.size());
 
 
            viz::Layer cloudLayer = arviz.layer("Cloud");
            {
                cloudLayer.add(viz::PointCloud("points")
                               .position(Eigen::Vector3f::Zero())
                               .transparency(0.0f)
                               .pointSizeInPixels(8)
                               .pointCloud(*transformedCloud));
            }
            viz::Layer ransacLayer = arviz.layer("Ransac");
            //draw shapes and cloud
            std::size_t numCyl = 0;
            std::size_t numSph = 0;
            std::size_t numPla = 0;
            std::size_t numUnk = 0;
 
            static int maxnum = 0;
            int i = 0;
            int idx = 0;
 
            const auto draw_cloud = [&](std::size_t num, std::string desc)
            {
                viz::Layer cloudLayer = arviz.layer("Cloud " + std::to_string(i) + " " + desc);
                viz::PointCloud cloud("points");
                cloud.position(Eigen::Vector3f::Zero());
                cloud.transparency(0.0f);
                cloud.pointSizeInPixels(8);
 
                for (std::size_t i2 = 0; i2 < num; ++i2, ++idx)
                {
                    const auto& pt = pc.at(pc.size() - 1 - idx);
                    cloud.addPoint(pt.pos.getValue()[0], pt.pos.getValue()[1], pt.pos.getValue()[2], i);
                }
                cloud.colorGlasbeyLUT(i);
                cloudLayer.add(cloud);
                arviz.commit({cloudLayer});
            };
 
            for (; i < shapes.size(); ++i)
            {
                using plane_t = const PlanePrimitiveShape;
                using sphere_t = const SpherePrimitiveShape;
                using cylinder_t = const CylinderPrimitiveShape;
 
                const auto& [s, sz] = shapes.at(i);
                const std::string name = std::to_string(i);
 
                if (plane_t* ptr = dynamic_cast<plane_t*>(s.Ptr()); ptr)
                {
                    ++numPla;
                    Eigen::Vector3f c;
                    ptr->Internal().getPosition().getValue(c.x(), c.y(), c.z());
 
                    Eigen::Vector3f n;
                    ptr->Internal().getNormal().getValue(n.x(), n.y(), n.z());
 
                    ransacLayer.add(viz::Polygon(name)
                                    .colorGlasbeyLUT(i)
                                    .lineColorGlasbeyLUT(i)
                                    .lineWidth(1.0f)
                                    .circle(c, n, 200, 64));
                }
                else if (sphere_t* ptr = dynamic_cast<sphere_t*>(s.Ptr()); ptr)
                {
                    ++numSph;
                    Eigen::Vector3f c;
                    ptr->Internal().Center().getValue(c.x(), c.y(), c.z());
 
                    ransacLayer.add(viz::Sphere(name)
                                    .colorGlasbeyLUT(i)
                                    .position(c)
                                    .radius(ptr->Internal().Radius()));
                }
                else if (cylinder_t* ptr = dynamic_cast<cylinder_t*>(s.Ptr()); ptr)
                {
                    ++numCyl;
                    Eigen::Vector3f c;
                    ptr->Internal().AxisPosition().getValue(c.x(), c.y(), c.z());
 
                    ransacLayer.add(viz::Cylinder(name)
                                    .colorGlasbeyLUT(i)
                                    .position(c)
                                    .color(viz::Color::green())
                                    .radius(ptr->Internal().Radius())
                                    .height(ptr->Height()));
                }
                else
                {
                    ++numUnk;
                    ARMARX_WARNING << "UNKNOWN shape " << simox::meta::get_type_name(s.Ptr());
                }
                //std::string desc;
                //s->Description(&desc);
                draw_cloud(sz, "dummy");
            }
            draw_cloud(remaining, "dummy");
            for (; i < maxnum; ++i)
            {
                draw_cloud(0, "dummy");
            }
 
            setDebugObserverDatafield("ransac_num_cylinders", numCyl);
            setDebugObserverDatafield("ransac_num_spheres", numSph);
            setDebugObserverDatafield("ransac_num_planes", numPla);
            setDebugObserverDatafield("ransac_num_unknown", numUnk);
            arviz.commit({cloudLayer, ransacLayer});
        }
    }
}