OOBB.hpp

Go to the documentation of this file.

#pragma once
 
#include <type_traits>
 
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wpedantic"
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/convex_hull_2.h>
#include <CGAL/min_quadrilateral_2.h>
#pragma GCC diagnostic pop
 
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
 
#include <SimoxUtility/shapes/OrientedBox.h>
#include <SimoxUtility/shapes/XYConstrainedOrientedBox.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include "../VisitPointLikeContainer.h"
 
namespace armarx
{
    template <class ST>
    Eigen::Matrix<ST, 3, 3>
    normalToRotation(const Eigen::Matrix<ST, 3, 1>& normal)
    {
        using VectorT = Eigen::Matrix<ST, 3, 1>;
        using MatrixT = Eigen::Matrix<ST, 3, 3>;
 
        MatrixT mx;
        mx.col(0) = normal.normalized();
        const VectorT other = (mx.col(0) == VectorT::UnitZ()) ? VectorT::UnitX() : VectorT::UnitZ();
        mx.col(1) = mx.col(0).cross(other).normalized();
        mx.col(2) = mx.col(0).cross(mx.col(1)).normalized();
 
        return mx;
    }
 
    template <class T>
    simox::OrientedBox<double>
    calculate2dOOBB(const T& cloud, const Eigen::Vector3d& dir)
    {
        ARMARX_CHECK_GREATER(dir.norm(), 0.01) << VAROUT(dir.transpose());
        const Eigen::Vector3d normal = dir.normalized();
        using Kernel = CGAL::Exact_predicates_inexact_constructions_kernel;
        using Point_2 = Kernel::Point_2;
        using Polygon_2 = CGAL::Polygon_2<Kernel>;
 
        const Eigen::Matrix3d boxFrame = normalToRotation(normal);
        //        const Eigen::Matrix3d boxFrame = Eigen::Matrix3d::Identity();
 
        std::vector<Point_2> allProjected;
        double allMaxX = -std::numeric_limits<double>::infinity();
        double allMinX = +std::numeric_limits<double>::infinity();
        {
            const auto& consume = [&](auto x, auto y, auto z)
            {
                const Eigen::Vector3d t = boxFrame.transpose() * Eigen::Vector3d{x, y, z};
                allProjected.emplace_back(t.y(), t.z());
                allMinX = std::min(allMinX, t.x());
                allMaxX = std::max(allMaxX, t.x());
            };
            const auto& resize = [&](auto size) { allProjected.reserve(size); };
            VisitPointLikeContainer(cloud, resize, consume);
        }
        ARMARX_CHECK(!allProjected.empty());
 
        std::vector<Point_2> chullAll;
        CGAL::convex_hull_2(allProjected.begin(), allProjected.end(), std::back_inserter(chullAll));
 
        Polygon_2 polyAll;
        CGAL::min_rectangle_2(chullAll.begin(), chullAll.end(), std::back_inserter(polyAll));
        const auto& polyps = polyAll.container();
 
        ARMARX_CHECK_EQUAL(polyps.size(), 4) << '\n'
                                             << VAROUT(normal.transpose()) << '\n'
                                             << VAROUT(boxFrame) << '\n'
                                             << VAROUT(allProjected.size()) << '\n'
                                             << VAROUT(chullAll.size()) << '\n'
                                             << VAROUT(polyAll.container().size());
 
        const auto v0 = polyps.at(0) - polyps.at(1);
        const auto v1 = polyps.at(2) - polyps.at(1);
        return simox::OrientedBox<double>{
            Eigen::Vector3d{allMinX, polyps.at(1).x(), polyps.at(1).y()}, //corner
            Eigen::Vector3d{allMaxX - allMinX, 0, 0}, //v3 (in local frame it is x)
            Eigen::Vector3d{0, v0.x(), v0.y()}, //v1
            Eigen::Vector3d{0, v1.x(), v1.y()} //v2
        }
            .transformed(boxFrame);
    }
 
    template <class T>
    simox::XYConstrainedOrientedBox<double>
    calculateXYOOBB(const T& cloud)
    {
        const auto oobb = calculate2dOOBB(cloud, Eigen::Vector3d::UnitZ());
        ARMARX_CHECK_GREATER(std::abs(oobb.axis_x().dot(Eigen::Vector3d::UnitZ())), 0.99)
            << '\n'
            << VAROUT(oobb.transformation()) << '\n'
            << VAROUT(oobb.dimensions()) << '\n'
            << VAROUT(oobb.axis_x().transpose());
 
 
        const Eigen::Vector3d e1 = oobb.extend(1);
        const Eigen::Vector3d e2 = oobb.extend(2);
 
        return {oobb.translation(), {e1(0), e1(1)}, {e2(0), e2(1)}, oobb.dimension(0)};
    }
 
    template <class T>
    simox::OrientedBox<double>
    calculateDouble2dOOBB(const T& cloud)
    {
        const auto box1 = calculate2dOOBB(cloud, Eigen::Vector3d::UnitZ());
 
        ARMARX_CHECK_GREATER(std::abs(box1.axis_x().normalized().dot(Eigen::Vector3d::UnitZ())),
                             0.99)
            << '\n'
            << VAROUT(box1.transformation());
 
        const Eigen::Vector3d norm =
            (box1.axis_y().norm() < box1.axis_z().norm()) ? box1.axis_y() : box1.axis_z();
 
        const auto box2 = calculate2dOOBB(cloud, norm);
        ARMARX_CHECK_GREATER(std::abs(box2.axis_x().normalized().dot(norm.normalized())), 0.99)
            << '\n'
            << VAROUT(box2.transformation()) << '\n'
            << VAROUT(norm.transpose());
 
        return box2;
    }
 
    template <class T>
    inline std::vector<Eigen::Vector3f>
    trimXYZHistogram(const T& cloud, const Eigen::Matrix3f& transform, const float trimEachSideBy)
    {
        std::vector<float> valuesX;
        std::vector<float> valuesY;
        std::vector<float> valuesZ;
        std::size_t sz = 0;
        {
            const auto& consume = [&](auto x, auto y, auto z)
            {
                const Eigen::Vector3f ted = transform * Eigen::Vector3f{x, y, z};
                valuesX.emplace_back(ted.x());
                valuesY.emplace_back(ted.y());
                valuesZ.emplace_back(ted.z());
            };
            const auto& resize = [&](auto size)
            {
                sz = size;
                valuesX.reserve(size);
                valuesY.reserve(size);
                valuesZ.reserve(size);
            };
            VisitPointLikeContainer(cloud, resize, consume);
        }
 
        std::sort(valuesX.begin(), valuesX.end());
        std::sort(valuesY.begin(), valuesY.end());
        std::sort(valuesZ.begin(), valuesZ.end());
 
        const std::size_t idxLo = sz * trimEachSideBy;
        const std::size_t idxHi = sz - std::max(idxLo, 1ul);
        const float loX = valuesX.at(idxLo);
        const float hiX = valuesX.at(idxHi);
        const float loY = valuesY.at(idxLo);
        const float hiY = valuesY.at(idxHi);
        const float loZ = valuesZ.at(idxLo);
        const float hiZ = valuesZ.at(idxHi);
 
 
        std::vector<Eigen::Vector3f> result;
        {
            const auto& consume = [&](auto x, auto y, auto z)
            {
                const Eigen::Vector3f ted = transform * Eigen::Vector3f{x, y, z};
                if (ted.x() <= hiX && ted.x() >= loX && ted.y() <= hiY && ted.y() >= loY &&
                    ted.z() <= hiZ && ted.z() >= loZ)
                {
                    result.emplace_back(x, y, z);
                }
            };
            const auto& resize = [&](auto) {};
            VisitPointLikeContainer(cloud, resize, consume);
        }
 
        return result;
    }
 
} // namespace armarx