OOBB.hpp

Go to the documentation of this file.

 
#pragma once
 
#ifdef CGAL_FOUND
 
#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/util/CPPUtility/trace.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.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;
    }
 
}
 
#endif