features.cpp

Go to the documentation of this file.

#include "features.h"
 
#include <SimoxUtility/algorithm/apply.hpp>
 
#include <armarx/navigation/components/laser_scanner_feature_extraction/conversions/eigen.h>
 
namespace armarx::conversions
{
    using Features = armarx::navigation::components::laser_scanner_feature_extraction::Features;
 
    Features
    transformFeature(const Features& feature, const Eigen::Isometry3f& transformation)
    {
        const auto transformPoint =
            [](const Eigen::Vector2f& point, const Eigen::Isometry3f& global_T_frame)
        { return to2D(global_T_frame * to3D(point)); };
 
        const auto transformPoints =
            [&](const std::vector<Eigen::Vector2f>& points, const Eigen::Isometry3f& global_T_frame)
        {
            return simox::alg::apply(points,
                                     [&](const Eigen::Vector2f& point)
                                     { return transformPoint(point, global_T_frame); });
        };
 
        Features transformed;
 
        if (feature.convexHull)
        {
            transformed.convexHull = {
                .vertices = transformPoints(feature.convexHull->vertices, transformation),
                .segments = feature.convexHull->segments};
        }
        if (feature.circle)
        {
            transformed.circle = {.center = transformPoint(feature.circle->center, transformation),
                                  .radius = feature.circle->radius};
        }
        if (feature.ellipsoid)
        {
            transformed.ellipsoid = {.pose = transformation * feature.ellipsoid->pose,
                                     .radii = feature.ellipsoid->radii};
        }
        if (feature.chain)
        {
            transformed.chain = transformPoints(feature.chain.value(), transformation);
        }
        transformed.points = transformPoints(feature.points, transformation);
 
        return transformed;
    }
 
} // namespace armarx::conversions