covarianceLineFit.h

/*
 Copyright (C) 2021 aniket agarwalla <aniketagarwalla37@gmail.com>
 
 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/.
*/
 
#pragma once
#include "../../defines.h"
#include "../concepts.h"
 
#include "../primitive/linePrimitive.h" // used to define CovarianceLineFit
#include "../tools/mean.h"              // used to define CovarianceLineFit
#include "../tools/covariance.h"        // used to define CovarianceLineFit
 
#include <Eigen/Dense>
 
#define COVARIANCE_LINE_FIT_REQUIREMENTS ProvidesPositionCovariance<T>&& ProvidesLine<T>
 
namespace Ponca
{
 
    template <class DataPoint, class _NFilter, typename T>
        requires COVARIANCE_LINE_FIT_REQUIREMENTS
    class CovarianceLineFitImpl : public T
    {
        PONCA_FITTING_DECLARE_DEFAULT_TYPES
        PONCA_FITTING_DECLARE_MATRIX_TYPE
    public:
        PONCA_EXPLICIT_CAST_OPERATORS(CovarianceLineFitImpl, covarianceLineFit)
 
        PONCA_FITTING_APIDOC_FINALIZE
        PONCA_MULTIARCH inline FIT_RESULT finalize()
        {
            static const int smallestEigenValue = DataPoint::Dim - 1;
            if (Base::finalize() == STABLE)
            {
                if (Base::line().isValid())
                    Base::m_eCurrentState = CONFLICT_ERROR_FOUND;
                Base::setLine(Base::barycenterLocal(),
                              Base::m_solver.eigenvectors().col(smallestEigenValue).normalized());
            }
            return Base::m_eCurrentState;
        }
        PONCA_FITTING_IS_SIGNED(false)
    };
 
    template <class DataPoint, class _NFilter, typename T>
    using CovarianceLineFit = CovarianceLineFitImpl<
        DataPoint, _NFilter,
        CovarianceBase<DataPoint, _NFilter, MeanPosition<DataPoint, _NFilter, Line<DataPoint, _NFilter, T>>>>;
 
} // namespace Ponca