Ponca  40f245e28b920cbb763a1c6282156c87c626f24c
Point Cloud Analysis library
Loading...
Searching...
No Matches
covariancePlaneFit.hpp
1/*
2 Copyright (C) 2014 Nicolas Mellado <nmellado0@gmail.com>
3 Copyright (C) 2015 Gael Guennebaud <gael.guennebaud@inria.fr>
4
5 This Source Code Form is subject to the terms of the Mozilla Public
6 License, v. 2.0. If a copy of the MPL was not distributed with this
7 file, You can obtain one at http://mozilla.org/MPL/2.0/.
8*/
9
10#include PONCA_MULTIARCH_INCLUDE_STD(cmath)
11#include PONCA_MULTIARCH_INCLUDE_STD(limits)
12
13template < class DataPoint, class _WFunctor, typename T>
16{
17 if (Base::finalize() == STABLE) {
18 if (Base::plane().isValid()) Base::m_eCurrentState = CONFLICT_ERROR_FOUND;
19 Base::setPlane(Base::m_solver.eigenvectors().col(0), Base::barycenterLocal());
20 }
21
22 return Base::m_eCurrentState;
23}
24
25template < class DataPoint, class _WFunctor, typename T>
26template <bool ignoreTranslation>
29{
30 if (ignoreTranslation)
31 return Base::m_solver.eigenvectors().transpose() * _q;
32 else {
33 // apply rotation and translation to get uv coordinates
34 return Base::m_solver.eigenvectors().transpose() * (Base::m_w.convertToLocalBasis(_q));
35 }
36}
37
38template < class DataPoint, class _WFunctor, typename T>
39template <bool ignoreTranslation>
40typename CovariancePlaneFitImpl<DataPoint, _WFunctor, T>::VectorType
41CovariancePlaneFitImpl<DataPoint, _WFunctor, T>::tangentPlaneToWorld (const VectorType& _lq) const
42{
43 if (ignoreTranslation)
44 return Base::m_solver.eigenvectors().transpose().inverse() * _lq;
45 else {
46 return Base::m_w.convertToGlobalBasis(Base::m_solver.eigenvectors().transpose().inverse() * _lq);
47 }
48}
49
50
51
52template < class DataPoint, class _WFunctor, int DiffType, typename T>
55{
56 PONCA_MULTIARCH_STD_MATH(sqrt);
57 PONCA_MULTIARCH_STD_MATH(numeric_limits);
58
59 Base::finalize();
60 // Test if base finalize end on a viable case (stable / unstable)
61 if (this->isReady())
62 {
63 VectorType barycenter = Base::barycenterLocal();
64 VectorArray dBarycenter = Base::barycenterDerivatives();
65
66 // pre-compute shifted eigenvalues to apply the pseudo inverse of C - lambda_0 I
67 Scalar epsilon = Scalar(2) * Eigen::NumTraits<Scalar>::epsilon();
68 Scalar consider_as_zero = Scalar(2) * numeric_limits<Scalar>::denorm_min();
69
70 // This is where the limitation to 3d comes from.
71 // \fixme Replace shift in 2d subspace by any subspace with co-dimension 1
72 Eigen::Matrix<Scalar,2,1> shifted_eivals = Base::m_solver.eigenvalues().template tail<2>().array() - Base::m_solver.eigenvalues()(0);
73 if(shifted_eivals(0) < consider_as_zero || shifted_eivals(0) < epsilon * shifted_eivals(1)) shifted_eivals(0) = 0;
74 if(shifted_eivals(1) < consider_as_zero) shifted_eivals(1) = 0;
75
76
77 for(int k=0; k<Base::NbDerivatives; ++k)
78 {
79 VectorType normal = Base::primitiveGradient();
80 // The derivative of 'normal' is the derivative of the smallest eigenvector.
81 // Since the covariance matrix is real and symmetric, it is equal to:
82 // n' = - (C - lambda_0 I)^+ C' n
83 // Where ^+ denotes the pseudo-inverse.
84 // Since we already performed the eigenvalue decomposition of the matrix C,
85 // we can directly apply the pseudo inverse by observing that:
86 // (C - lambda_0 I) = V (L - lambda_0 I) V^T
87 // where V is the eigenvector matrix, and L the eigenvalue diagonal matrix.
88 Eigen::Matrix<Scalar,2,1> z = - Base::m_solver.eigenvectors().template rightCols<2>().transpose() * (Base::m_dCov[k] * normal);
89 if(shifted_eivals(0)>0) z(0) /= shifted_eivals(0);
90 if(shifted_eivals(1)>0) z(1) /= shifted_eivals(1);
91 m_dNormal.col(k) = Base::m_solver.eigenvectors().template rightCols<2>() * z;
92
93 VectorType dDiff = dBarycenter.col(k);
94 if(k>0 || !Base::isScaleDer())
95 dDiff(Base::isScaleDer() ? k-1 : k) += 1;
96 m_dDist(k) = m_dNormal.col(k).dot(barycenter) + normal.dot(dDiff);
97
98 // \fixme we shouldn't need this normalization, however currently the derivatives are overestimated by a factor 2
99 m_dNormal /= Scalar(2.);
100 }
101 }
102
103 return Base::m_eCurrentState;
104}
[CovariancePlaneFit Definition]
typename DataPoint::Scalar Scalar
Alias to scalar type.
typename Base::VectorType VectorType
Alias to vector type.
typename Base::VectorArray VectorArray
Alias to vector derivatives array.
Plane fitting procedure using only points position.
typename Base::VectorType VectorType
Alias to vector type.
FIT_RESULT
Enum corresponding to the state of a fitting method (and what the finalize function returns)
Definition enums.h:15
@ CONFLICT_ERROR_FOUND
Multiple classes of the fitting procedure initialize the primitive.
Definition enums.h:27
@ STABLE
The fitting is stable and ready to use.
Definition enums.h:17