FeatureExtractor.cpp

Go to the documentation of this file.

#include "FeatureExtractor.h"
 
#include <algorithm>
#include <cmath>
#include <iterator>
#include <numeric>
#include <optional>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/src/Geometry/AngleAxis.h>
#include <Eigen/src/Geometry/Transform.h>
 
#include <pcl/PointIndices.h>
#include <pcl/point_cloud.h>
 
#include <opencv2/core/types.hpp>
#include <opencv2/imgproc.hpp>
 
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/math/Helpers.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#include <armarx/navigation/components/laser_scanner_feature_extraction/ChainApproximation.h>
#include <armarx/navigation/components/laser_scanner_feature_extraction/EnclosingEllipsoid.h>
#include <armarx/navigation/components/laser_scanner_feature_extraction/ScanClustering.h>
#include <armarx/navigation/components/laser_scanner_feature_extraction/laser_scanner_conversion.h>
 
#include "Path.h"
#include "conversions/eigen.h"
#include "conversions/opencv.h"
#include "conversions/opencv_eigen.h"
#include "conversions/opencv_pcl.h"
#include "conversions/pcl_eigen.h"
 
namespace armarx::navigation::components::laser_scanner_feature_extraction
{
 
    // Features
 
    std::vector<Ellipsoid>
    Features::linesAsEllipsoids(const float axisLength) const
    {
        if (not chain)
        {
            return {};
        }
 
        const auto asEllipsoid = [&](const Path::Segment& segment) -> Ellipsoid
        {
            const Eigen::Vector2f center = (segment.first + segment.second) / 2;
            const Eigen::Vector2f v = segment.second - center;
            const float angle = math::Helpers::Angle(v);
            const float r = v.norm();
 
            Eigen::Isometry3f globalPose = Eigen::Isometry3f::Identity();
            globalPose.translation().head<2>() = center;
            globalPose.linear() =
                Eigen::AngleAxisf(angle, Eigen::Vector3f::UnitZ()).toRotationMatrix();
 
            return Ellipsoid{.pose = globalPose, .radii = Eigen::Vector2f{r, axisLength}};
        };
 
        const auto segments = Path{.points = *chain}.segments();
 
        std::vector<Ellipsoid> ellipsoids;
        std::transform(
            segments.begin(), segments.end(), std::back_inserter(ellipsoids), asEllipsoid);
 
        return ellipsoids;
    }
 
    // FeatureExtractor
 
    FeatureExtractor::FeatureExtractor(const ScanClustering::Params& scanClusteringParams,
                                       const ChainApproximation::Params& chainApproximationParams) :
        scanClusteringParams(scanClusteringParams),
        chainApproximationParams(chainApproximationParams)
    {
    }
 
    std::vector<Features>
    FeatureExtractor::onData(const armem::laser_scans::LaserScanStamped& data)
    {
        ARMARX_DEBUG << "on data";
        const auto clustersWithFeatures = features(data.data);
 
        ARMARX_DEBUG << "callback";
        return clustersWithFeatures;
    }
 
    std::vector<Features>
    FeatureExtractor::features(const LaserScan& scan) const
    {
        const auto clusters = detectClusters(scan);
 
        std::vector<Features> fs;
        std::transform(clusters.begin(),
                       clusters.end(),
                       std::back_inserter(fs),
                       [&](const LaserScan& cluster)
                       {
                           const auto points = toCartesian<Eigen::Vector2f>(cluster);
                           const auto hull = convexHull(points);
 
                           return Features{
                               .convexHull = hull,
                               .circle = circle(points),
                               .ellipsoid = std::nullopt, //ellipsoid(hull),
                               .chain = chainApproximation(points, hull, chainApproximationParams),
                               .points = points};
                       });
 
        return fs;
    }
 
    std::vector<LaserScan>
    FeatureExtractor::detectClusters(const LaserScan& scan) const
    {
        ScanClustering clustering(scanClusteringParams);
        return clustering.detectClusters(scan);
    }
 
    std::optional<VirtualRobot::MathTools::ConvexHull2D>
    FeatureExtractor::convexHull(const Points& points)
    {
 
        if (points.size() < 3) // no meaningful area
        {
            return std::nullopt;
        }
 
        try
        {
            return *VirtualRobot::MathTools::createConvexHull2D(points);
        }
        catch (std::exception& e)
        {
            ARMARX_WARNING << "Couldn't create convex hull: " << e.what();
        }
        return std::nullopt;
    }
 
    // Legacy OpenCV implementation. Didn't work as expected.
    // Also, tries to fit an ellipsoid which is NOT the enclosing ellipsoid!
 
    // std::optional<Ellipsoid> FeatureExtractor::ellipsoid(const PointCloud &pointCloud) const
    // {
    //     // OpenCV::fitEllipse requirement
    //     // "There should be at least 5 points to fit the ellipse"
    //     // => Too few points will cause algorithmic issues
    //     if (pointCloud.size() < 100)
    //     {
    //         return std::nullopt;
    //     }
 
    //     const auto points2f = conversions::pcl2cv(pointCloud);
    //     const auto points2i = conversions::cast<cv::Point2i>(points2f);
 
    //     cv::RotatedRect rect = cv::fitEllipse(points2i);
 
    //     Eigen::Affine2f pose;
    //     pose.translation() = conversions::cv2eigen(rect.center);
    //     pose.linear() =
    //         Eigen::Rotation2Df(VirtualRobot::MathTools::deg2rad(rect.angle)).toRotationMatrix();
 
    //     return Ellipsoid{.axisLengths = conversions::cv2eigen(rect.size), .pose = pose};
    // }
 
    std::optional<Ellipsoid>
    FeatureExtractor::ellipsoid(const std::optional<VirtualRobot::MathTools::ConvexHull2D>& hull)
    {
 
        if (not hull)
        {
            return std::nullopt;
        }
 
        // TODO(fabian.reister): it might result in multiple ellipsoids if hull->segments is considered
 
        // If there are not enough points, don't even try to fit an ellipse ...
        if (hull->vertices.size() < 5)
        {
            return std::nullopt;
        }
 
        EnclosingEllipsoid mvee(hull->vertices);
        return mvee;
    }
 
    std::optional<Circle>
    FeatureExtractor::circle(const Points& points)
    {
        // Too few points will cause algorithmic issues
        if (points.size() < 5)
        {
            return std::nullopt;
        }
 
        const auto points2f = conversions::eigen2cv(points);
        const auto points2i = conversions::cast<cv::Point2i>(points2f);
 
        cv::Point2f center;
        float radius = NAN;
        cv::minEnclosingCircle(points2i, center, radius);
 
        return Circle{.center = conversions::cv2eigen(center), .radius = radius};
    }
 
    std::optional<FeatureExtractor::Points>
    FeatureExtractor::chainApproximation(
        const Points& points,
        const std::optional<VirtualRobot::MathTools::ConvexHull2D>& convexHull,
        const ChainApproximation::Params& params)
    {
        if (not convexHull)
        {
            return std::nullopt;
        }
 
        ChainApproximation chApprx(points, params);
        const auto result = chApprx.approximate();
 
        if (result.condition !=
            ChainApproximation::ApproximationResult::TerminationCondition::Converged)
        {
            return std::nullopt;
        }
 
        return chApprx.approximatedChain();
    }
 
} // namespace armarx::navigation::components::laser_scanner_feature_extraction