EnclosingEllipsoid.cpp

Go to the documentation of this file.

#include "EnclosingEllipsoid.h"
 
#include <algorithm>
#include <cmath>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/src/Geometry/AngleAxis.h>
 
#include <SimoxUtility/math/periodic/periodic_clamp.h>
#include <VirtualRobot/math/Helpers.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
 
namespace armarx::navigation::components::laser_scanner_feature_extraction
{
 
    // Eigen::Affine2f Ellipsoid::pose() const noexcept
    // {
    //     Eigen::Affine2f pose;
    //     pose.translation() = center;
    //     pose.linear()      = Eigen::Rotation2Df(angle).toRotationMatrix();
 
    //     return pose;
    // }
 
    // TODO(fabian.reister): use Eigen built-in stuff.
    size_t
    argmin(const Eigen::VectorXf& v)
    {
        const std::vector<float> vec(v.data(), v.data() + v.rows() * v.cols());
        return std::distance(std::begin(vec), std::max_element(vec.begin(), vec.end()));
    }
 
    // https://github.com/minillinim/ellipsoid/blob/master/ellipsoid.py
 
    EnclosingEllipsoid::EnclosingEllipsoid(const Points& points)
    {
        ARMARX_CHECK(compute(points));
    }
 
    bool
    EnclosingEllipsoid::compute(const Points& points)
    {
        const float tolerance = 0.01;
 
        const int N = static_cast<int>(points.size());
        const int d = 2;
 
        Eigen::MatrixXf P(N, d);
 
        for (int i = 0; i < N; i++)
        {
            P.row(i) = points.at(i);
        }
 
        Eigen::MatrixXf Q(d + 1, N);
        Q.topRows(d) = P.transpose();
        Q.bottomRows(1).setOnes(); // last row
 
        Eigen::MatrixXf Qt = Q.transpose();
 
        float err = 1.F + tolerance;
        Eigen::VectorXf u = (1.F / N) * Eigen::VectorXf::Ones(N);
 
        // Khachiyan Algorithm
        while (err > tolerance)
        {
            const Eigen::MatrixXf V = Q * (u.asDiagonal() * Qt);
            const Eigen::VectorXf M = (Qt * (V.inverse() * Q)).diagonal();
 
            const int j = static_cast<int>(argmin(M));
            const float maximum = M(j);
 
            const float stepSize = (maximum - d - 1.0) / ((d + 1.0) * (maximum - 1.0));
 
            Eigen::VectorXf uNew = (1.0 - stepSize) * u;
            uNew(j) += stepSize;
 
            err = (uNew - u).norm();
            u = uNew;
        }
 
        const Eigen::VectorXf center = P.transpose() * u;
 
        Eigen::MatrixXf K(d, d);
        for (int i = 0; i < d; i++)
        {
            for (int j = 0; j < d; j++)
            {
                K(i, j) = center(i) * center(j);
            }
        }
 
        const Eigen::MatrixXf H = (P.transpose() * (u.asDiagonal() * P)) - K;
        const Eigen::MatrixXf A = H.inverse() / d;
 
        const Eigen::JacobiSVD svd(A, Eigen::ComputeThinV);
 
        // angle
        const Eigen::VectorXf v0 = svd.matrixV().col(0);
        const float angle = math::Helpers::Angle(v0);
 
        // radii
        const Eigen::Vector2f radii = svd.singularValues().cwiseSqrt().cwiseInverse();
 
        // As the singular values are sorted descending, the radii are sorted ascending.
        // To fix this, the vector is reversed.
        this->radii = radii.reverse();
 
        // Thus, the angle does no longer match the radii. It has to be altered by 90°.
        const float angle1 = simox::math::periodic_clamp(angle + M_PI_2, -M_PI, M_PI);
 
        this->pose.setIdentity();
        this->pose.translation().head<2>() = center;
        this->pose.linear() =
            Eigen::AngleAxisf(angle1, Eigen::Vector3f::UnitZ()).toRotationMatrix();
 
        return true;
    }
 
} // namespace armarx::navigation::components::laser_scanner_feature_extraction