covariancePlaneFit.hpp

/*
 Copyright (C) 2014 Nicolas Mellado <nmellado0@gmail.com>
 Copyright (C) 2015 Gael Guennebaud <gael.guennebaud@inria.fr>
 
 This Source Code Form is subject to the terms of the Mozilla Public
 License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
 
template <class DataPoint, class _NFilter, typename T>
    requires COVARIANCE_PLANE_FIT_REQUIREMENTS
FIT_RESULT CovariancePlaneFitImpl<DataPoint, _NFilter, T>::finalize()
{
    if (Base::finalize() == STABLE)
    {
        if (Base::plane().isValid())
            Base::m_eCurrentState = CONFLICT_ERROR_FOUND;
        Base::setPlane(Base::m_solver.eigenvectors().col(0), Base::barycenterLocal());
    }
 
    return Base::m_eCurrentState;
}
 
template <class DataPoint, class _NFilter, typename T>
    requires COVARIANCE_PLANE_FIT_REQUIREMENTS
typename CovariancePlaneFitImpl<DataPoint, _NFilter, T>::VectorType CovariancePlaneFitImpl<
    DataPoint, _NFilter, T>::worldToTangentPlane(const VectorType& _q, bool _isPositionVector) const
{
    return Base::m_solver.eigenvectors().transpose() *
           Base::getNeighborFrame().convertToLocalBasis(_q, _isPositionVector);
}
 
template <class DataPoint, class _NFilter, typename T>
    requires COVARIANCE_PLANE_FIT_REQUIREMENTS
typename CovariancePlaneFitImpl<DataPoint, _NFilter, T>::VectorType CovariancePlaneFitImpl<
    DataPoint, _NFilter, T>::tangentPlaneToWorld(const VectorType& _lq, bool _isPositionVector) const
{
    return Base::getNeighborFrame().convertToGlobalBasis(Base::m_solver.eigenvectors().transpose().inverse() * _lq,
                                                         _isPositionVector);
}
 
template <class DataPoint, class _NFilter, int DiffType, typename T>
    requires COVARIANCE_PLANE_DER_REQUIREMENTS
FIT_RESULT CovariancePlaneDerImpl<DataPoint, _NFilter, DiffType, T>::finalize()
{
    PONCA_MULTIARCH_CU_STD_FUNC(numeric_limits);
 
    Base::finalize();
    // Test if base finalize end on a viable case (stable / unstable)
    if (this->isReady())
    {
        VectorType barycenter   = Base::barycenterLocal();
        VectorArray dBarycenter = Base::barycenterDerivatives();
 
        // pre-compute shifted eigenvalues to apply the pseudo inverse of C - lambda_0 I
        Scalar epsilon          = Scalar(2) * Eigen::NumTraits<Scalar>::epsilon();
        Scalar consider_as_zero = Scalar(2) * numeric_limits<Scalar>::denorm_min();
 
        // This is where the limitation to 3d comes from.
        // \fixme Replace shift in 2d subspace by any subspace with co-dimension 1
        Eigen::Matrix<Scalar, 2, 1> shifted_eivals =
            Base::m_solver.eigenvalues().template tail<2>().array() - Base::m_solver.eigenvalues()(0);
        if (shifted_eivals(0) < consider_as_zero || shifted_eivals(0) < epsilon * shifted_eivals(1))
            shifted_eivals(0) = 0;
        if (shifted_eivals(1) < consider_as_zero)
            shifted_eivals(1) = 0;
 
        for (int k = 0; k < Base::NbDerivatives; ++k)
        {
            VectorType normal = Base::primitiveGradient();
            // The derivative of 'normal' is the derivative of the smallest eigenvector.
            // Since the covariance matrix is real and symmetric, it is equal to:
            //    n' = - (C - lambda_0 I)^+ C' n
            // Where ^+ denotes the pseudo-inverse.
            // Since we already performed the eigenvalue decomposition of the matrix C,
            // we can directly apply the pseudo inverse by observing that:
            //    (C - lambda_0 I) = V (L - lambda_0 I) V^T
            // where V is the eigenvector matrix, and L the eigenvalue diagonal matrix.
            Eigen::Matrix<Scalar, 2, 1> z =
                -Base::m_solver.eigenvectors().template rightCols<2>().transpose() * (Base::m_dCov[k] * normal);
            if (shifted_eivals(0) > 0)
                z(0) /= shifted_eivals(0);
            if (shifted_eivals(1) > 0)
                z(1) /= shifted_eivals(1);
            m_dNormal.col(k) = Base::m_solver.eigenvectors().template rightCols<2>() * z;
 
            VectorType dDiff = dBarycenter.col(k);
            if (k > 0 || !Base::isScaleDer())
                dDiff(Base::isScaleDer() ? k - 1 : k) += 1;
            m_dDist(k) = m_dNormal.col(k).dot(barycenter) + normal.dot(dDiff);
        }
    }
 
    return Base::m_eCurrentState;
}