MRConeApproximator.h

Go to the documentation of this file.

#pragma once
 
#include "MRMeshFwd.h"
#include "MRCone3.h"
#include "MRConstants.h"
#include "MRParallelFor.h"
#include "MRToFromEigen.h"
#include "MRPch/MREigenCore.h"
#include "MRPch/MRSuppressWarning.h"
 
#include <Eigen/SVD>
 
#ifdef _MSC_VER
#pragma warning(push)
#pragma warning(disable: 4244) 
#pragma warning(disable: 4464) 
#endif
#include <unsupported/Eigen/NonLinearOptimization>
#include <unsupported/Eigen/NumericalDiff>
#ifdef _MSC_VER
#pragma warning(pop)
#endif
 
#include <algorithm>
 
 
namespace MR
{
 
 
template <typename T>
struct ConeFittingFunctor
{
    using Scalar = T;
    using InputType = Eigen::Matrix<T, Eigen::Dynamic, 1>;
    using ValueType = Eigen::Matrix<T, Eigen::Dynamic, 1>;
    using JacobianType = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
 
    std::vector <Eigen::Vector3<T>> points;
 
    void setPoints( const std::vector<MR::Vector3<T>>& pointsMR )
    {
        points.reserve( pointsMR.size() );
        for ( auto i = 0; i < pointsMR.size(); ++i )
        {
            points.push_back( toEigen( pointsMR[i] ) );
        }
    }
 
    int inputs() const
    {
        return 6;
    }
    int values() const
    {
        return static_cast< int > ( points.size() );
    }
 
    int operator()( const InputType& x, ValueType& F ) const
    {
        Eigen::Vector3<T> V;
        V( 0 ) = x( 0 );
        V( 1 ) = x( 1 );
        V( 2 ) = x( 2 );
 
        Eigen::Vector3<T> W;
        W( 0 ) = x( 3 );
        W( 1 ) = x( 4 );
        W( 2 ) = x( 5 );
 
        for ( int i = 0; i < points.size(); ++i )
        {
            Eigen::Vector3<T> delta = V - points[i];
            T deltaDotW = delta.dot( W );
            F[i] = delta.squaredNorm() - deltaDotW * deltaDotW;
        }
        return 0;
    }
 
    int df( const InputType& x, JacobianType& J ) const
    {
 
        Eigen::Vector3<T> V;
        V( 0 ) = x( 0 );
        V( 1 ) = x( 1 );
        V( 2 ) = x( 2 );
 
        Eigen::Vector3<T> W;
        W( 0 ) = x( 3 );
        W( 1 ) = x( 4 );
        W( 2 ) = x( 5 );
 
        for ( int i = 0; i < points.size(); ++i )
        {
            const Eigen::Vector3<T>& P = points[i];
            Eigen::Vector3<T> D = ( V - P );
            T PW = D.dot( W );
            Eigen::Vector3<T> PVW = V - P - PW * W;
            Eigen::Vector3<T> PWD = PW * D;
 
            J( i, 0 ) = 2 * PVW.x();
            J( i, 1 ) = 2 * PVW.y();
            J( i, 2 ) = 2 * PVW.z();
 
            J( i, 3 ) = -2 * PWD.x();
            J( i, 4 ) = -2 * PWD.y();
            J( i, 5 ) = -2 * PWD.z();
        }
        return 0;
    }
 
};
 
 
enum class ConeFitterType
{
    ApproximationPCM, 
    HemisphereSearchFit,
    SpecificAxisFit
};
 
struct Cone3ApproximationParams {
    int levenbergMarquardtMaxIteration = 40;
    ConeFitterType coneFitterType = ConeFitterType::HemisphereSearchFit;
    int hemisphereSearchPhiResolution = 30;
    int hemisphereSearchThetaResolution = 30;
};
 
template <typename T>
class Cone3Approximation
{
public:
 
    Cone3Approximation() = default;
 
    T solve( const std::vector<MR::Vector3<T>>& points,
        Cone3<T>& cone, const Cone3ApproximationParams& params = {} )
 
    {
        params_ = params;
 
        switch ( params_.coneFitterType )
        {
        case ConeFitterType::SpecificAxisFit:
            return solveSpecificAxisFit_( points, cone );
            break;
        case ConeFitterType::HemisphereSearchFit:
            return solveHemisphereSearchFit_( points, cone );
            break;
        case ConeFitterType::ApproximationPCM:
            return solveApproximationPCM_( points, cone );
            break;
        default:
            return std::numeric_limits<T>::max();
            break;
        };
    }
 
 
private:
 
    Cone3ApproximationParams params_;
 
    T solveFixedAxis_( const std::vector<MR::Vector3<T>>& points,
        Cone3<T>& cone, bool useConeInputAsInitialGuess = false )
 
    {
        ConeFittingFunctor<T> coneFittingFunctor;
        coneFittingFunctor.setPoints( points );
        Eigen::LevenbergMarquardt<ConeFittingFunctor<T>, T> lm( coneFittingFunctor );
        lm.parameters.maxfev = params_.levenbergMarquardtMaxIteration;
 
        MR::Vector3<T> center, U;
        computeCenterAndNormal_( points, center, U );
 
        MR::Vector3<T>& coneAxis = cone.direction();
        if ( useConeInputAsInitialGuess )
        {
            coneAxis = coneAxis.normalized();
        }
        else
        {
            cone = computeInitialCone_( points, center, U );
        }
 
        Eigen::VectorX<T> fittedParams( 6 );
        coneToFitParams_( cone, fittedParams );
        [[maybe_unused]] Eigen::LevenbergMarquardtSpace::Status result = lm.minimize( fittedParams );
 
 
        fitParamsToCone_( fittedParams, cone );
 
        T const one_v = static_cast< T >( 1 );
        auto cosAngle = std::clamp( one_v / coneAxis.length(), static_cast< T >( 0 ), one_v );
        cone.angle = std::acos( cosAngle );
        cone.direction() = cone.direction().normalized();
        cone.height = calculateConeHeight_( points, cone );
 
        return getApproximationRMS_( points, cone );
    }
 
    T solveApproximationPCM_( const std::vector<MR::Vector3<T>>& points, Cone3<T>& cone )
    {
        return solveFixedAxis_( points, cone, false );
    }
 
    T solveSpecificAxisFit_( const std::vector<MR::Vector3<T>>& points, Cone3<T>& cone )
    {
        return solveFixedAxis_( points, cone, true );
    }
 
    T solveHemisphereSearchFit_( const std::vector<MR::Vector3<T>>& points, Cone3<T>& cone )
    {
        Vector3<T> center = computeCenter_( points );
        ConeFittingFunctor<T> coneFittingFunctor;
        coneFittingFunctor.setPoints( points );
 
        constexpr T pi2 = static_cast< T >( PI2 );
        T const theraStep = static_cast< T >( 2 * PI ) / params_.hemisphereSearchPhiResolution;
        T const phiStep = pi2 / params_.hemisphereSearchPhiResolution;
 
        struct BestCone {
            Cone3<T> bestCone;
            T minError = std::numeric_limits<T> ::max();
        };
        std::vector<BestCone> bestCones;
        bestCones.resize( params_.hemisphereSearchPhiResolution + 1 );
 
        ParallelFor( 0, params_.hemisphereSearchPhiResolution + 1, [&] ( int j )
        {
            T phi = phiStep * j; 
            T cosPhi = std::cos( phi );
            T sinPhi = std::sin( phi );
            for ( size_t i = 0; i < params_.hemisphereSearchThetaResolution; ++i )
            {
                T theta = theraStep * i; 
                T cosTheta = std::cos( theta );
                T sinTheta = std::sin( theta );
 
                Vector3<T> U( cosTheta * sinPhi, sinTheta * sinPhi, cosPhi );
 
                auto tmpCone = computeInitialCone_( points, center, U );
 
                Eigen::VectorX<T> fittedParams( 6 );
                coneToFitParams_( tmpCone, fittedParams );
 
                Eigen::LevenbergMarquardt<ConeFittingFunctor<T>, T> lm( coneFittingFunctor );
                lm.parameters.maxfev = params_.levenbergMarquardtMaxIteration;
                [[maybe_unused]] Eigen::LevenbergMarquardtSpace::Status result = lm.minimize( fittedParams );
 
 
                fitParamsToCone_( fittedParams, tmpCone );
 
                T const one_v = static_cast< T >( 1 );
                auto cosAngle = std::clamp( one_v / tmpCone.direction().length(), static_cast< T >( 0 ), one_v );
                tmpCone.angle = std::acos( cosAngle );
                tmpCone.direction() = tmpCone.direction().normalized();
 
                T error = getApproximationRMS_( points, tmpCone );
                if ( error < bestCones[j].minError )
                {
                    bestCones[j].minError = error;
                    bestCones[j].bestCone = tmpCone;
                }
            }
        } );
 
        auto bestAppox = std::min_element( bestCones.begin(), bestCones.end(), [] ( const BestCone& a, const BestCone& b )
        {
            return a.minError < b.minError;
        } );
 
        cone = bestAppox->bestCone;
 
        cone.height = calculateConeHeight_( points, cone );
 
        return bestAppox->minError;
    }
 
    T calculateConeHeight_( const std::vector<MR::Vector3<T>>& points, Cone3<T>& cone )
    {
        T length = static_cast< T > ( 0 );
        for ( auto i = 0; i < points.size(); ++i )
        {
            length = std::max( length, std::abs( MR::dot( points[i] - cone.apex(), cone.direction() ) ) );
        }
        return length;
    }
 
    T getApproximationRMS_( const std::vector<MR::Vector3<T>>& points, const Cone3<T>& cone )
    {
        if ( points.size() == 0 )
            return std::numeric_limits<T>::max();
 
        T error = 0;
        for ( auto p : points )
            error = error + distanceSq( cone.projectPoint( p ), p );
 
        return error / points.size();
    }
 
    MR::Vector3<T> computeCenter_( const std::vector<MR::Vector3<T>>& points )
    {
        MR::Vector3<T> center;  
        for ( auto i = 0; i < points.size(); ++i )
        {
            center += points[i];
        }
        center = center / static_cast< T >( points.size() );
        return center;
    }
 
 
    void computeCenterAndNormal_( const std::vector<MR::Vector3<T>>& points, MR::Vector3<T>& center, MR::Vector3<T>& U )
    {
        center = computeCenter_( points );
 
        U = Vector3f();  
        for ( auto i = 0; i < points.size(); ++i )
        {
            Vector3<T> Z = points[i] - center;
            U += Z * MR::dot( Z, Z );
        }
        U = U.normalized();
    }
 
 
    Cone3<T> computeInitialCone_( const std::vector<MR::Vector3<T>>& points, const MR::Vector3<T>& center, const MR::Vector3<T>& axis )
    {
        Cone3<T> result;
        MR::Vector3<T>& coneApex = result.apex();
        result.direction() = axis;
        MR::Vector3<T>& U = result.direction();  
        T& coneAngle = result.angle;
 
 
        std::vector<Vector2<T>> hrPairs( points.size() );
        T hMin = std::numeric_limits<T>::max(), hMax = -hMin;
        for ( auto i = 0; i < points.size(); ++i )
        {
            MR::Vector3<T> delta = points[i] - center;
            T h = MR::dot( U, delta );
            hMin = std::min( hMin, h );
            hMax = std::max( hMax, h );
            Vector3<T> projection = delta - MR::dot( U, delta ) * U;
            T r = projection.length();
            hrPairs[i] = { h, r };
        }
 
 
        MR::Vector2<T> avgPoint;
        T a, b; 
        findBestFitLine_( hrPairs, a, b, &avgPoint );
        T hAverage = avgPoint.x;
        T rAverage = avgPoint.y;
        T hrSlope = a;
 
        if ( hrSlope < 0 )
        {
            U = -U;
            hrSlope = -hrSlope;
            std::swap( hMin, hMax );
            hMin = -hMin;
            hMax = -hMax;
        }
 
        T rMin = rAverage + hrSlope * ( hMin - hAverage );
        T rMax = rAverage + hrSlope * ( hMax - hAverage );
        T hRange = hMax - hMin;
        T rRange = rMax - rMin;
 
        T tanAngle = rRange / hRange;
        coneAngle = std::atan2( rRange, hRange );
 
        T offset = rMax / tanAngle - hMax;
        coneApex = center - offset * U;
        return result;
    }
 
    void findBestFitLine_( const std::vector<MR::Vector2<T>>& xyPairs, T& lineA, T& lineB, MR::Vector2<T>* avg = nullptr )
    {
        auto numPoints = xyPairs.size();
        Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> A( numPoints, 2 );
        Eigen::Vector<T, Eigen::Dynamic>  b( numPoints );
 
        for ( auto i = 0; i < numPoints; ++i )
        {
            A( i, 0 ) = xyPairs[i].x;  
            A( i, 1 ) = 1.0;           
            b( i ) = xyPairs[i].y;     
            if ( avg )
                *avg = *avg + xyPairs[i];
        }
        if ( avg )
        {
            *avg = *avg / static_cast < T > ( xyPairs.size() );
        }
        MR_SUPPRESS_WARNING_PUSH
        MR_SUPPRESS_WARNING( "-Wdeprecated-declarations", 4996 )
        Eigen::Matrix<T, Eigen::Dynamic, 1> x = A.bdcSvd( Eigen::ComputeThinU | Eigen::ComputeThinV ).solve( b );
        MR_SUPPRESS_WARNING_POP
 
        lineA = x( 0 );
        lineB = x( 1 );
 
        if ( avg )
        {
            *avg = *avg / static_cast < T > ( xyPairs.size() );
            avg->y = lineA * avg->x + lineB;
        }
    }
 
    void fitParamsToCone_( Eigen::Vector<T, Eigen::Dynamic>& fittedParams, Cone3<T>& cone )
    {
        cone.apex().x = fittedParams[0];
        cone.apex().y = fittedParams[1];
        cone.apex().z = fittedParams[2];
 
        cone.direction().x = fittedParams[3];
        cone.direction().y = fittedParams[4];
        cone.direction().z = fittedParams[5];
    }
 
    void coneToFitParams_( Cone3<T>& cone, Eigen::Vector<T, Eigen::Dynamic>& fittedParams )
    {
        fittedParams[0] = cone.apex().x;
        fittedParams[1] = cone.apex().y;
        fittedParams[2] = cone.apex().z;
 
        T coneCosAngle = std::cos( cone.angle );
        fittedParams[3] = cone.direction().x / coneCosAngle;
        fittedParams[4] = cone.direction().y / coneCosAngle;
        fittedParams[5] = cone.direction().z / coneCosAngle;
    }
 
 
};
 
}