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 "EfficientRANSACPrimitiveExtractor.h"
 
#include <pcl/common/transforms.h>
#include <pcl/filters/statistical_outlier_removal.h>
 
#include <SimoxUtility/meta/type_name.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>
 
//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();
    }
} // namespace armarx
 
#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()));
    }
 
} // namespace armarx
 
#include "EfficientRANSAC/ConePrimitiveShape.h"
#include "EfficientRANSAC/ConePrimitiveShapeConstructor.h"
#include "EfficientRANSAC/CylinderPrimitiveShape.h"
#include "EfficientRANSAC/CylinderPrimitiveShapeConstructor.h"
#include "EfficientRANSAC/PlanePrimitiveShape.h"
#include "EfficientRANSAC/PlanePrimitiveShapeConstructor.h"
#include "EfficientRANSAC/PointCloud.h"
#include "EfficientRANSAC/RansacShapeDetector.h"
#include "EfficientRANSAC/SpherePrimitiveShape.h"
#include "EfficientRANSAC/SpherePrimitiveShapeConstructor.h"
#include "EfficientRANSAC/TorusPrimitiveShape.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});
        }
    }
} // namespace armarx