52 #ifndef __PCL_AkimaInterpolation_h
53 #define __PCL_AkimaInterpolation_h
58 #include <pcl/Diagnostics.h>
95 template <
typename T =
double>
160 if ( y.Length() < 5 )
161 throw Error(
"AkimaInterpolation::Initialize(): Less than five data points specified." );
168 int n = m_y.Length();
184 for (
int i = 0; i < N; ++i )
186 T h = m_x[i+1] - m_x[i];
188 throw Error(
"AkimaInterpolation::Initialize(): Empty interpolation subinterval(s)." );
189 m[i] = (m_y[i+1] - m_y[i])/h;
192 for (
int i = 0; i < N; ++i )
193 m[i] = m_y[i+1] - m_y[i];
196 m[-2 ] = 3*m[ 0] - 2*m[ 1];
197 m[-1 ] = 2*m[ 0] - m[ 1];
198 m[ N ] = 2*m[N-1] - m[N-2];
199 m[N+1] = 3*m[N-1] - 2*m[N-2];
205 for (
int i = 0; i < n; ++i )
207 T f =
Abs( m[i-1] - m[i-2] );
208 T e =
Abs( m[i+1] - m[i] ) + f;
211 tL[i] = m[i-1] + f*(m[i] - m[i-1])/e;
227 for (
int i = 0; i < N; ++i )
229 m_c[i] = 3*m[i] - 2*m_b[i] - tL[i+1];
230 m_d[i] = m_b[i] + tL[i+1] - 2*m[i];
234 T h = m_x[i+1] - m_x[i];
257 PCL_PRECONDITION( IsValid() )
268 int i1 = m_x.Length() - 1;
270 if ( unlikely( x < m_x[0] || x > m_x[i1] ) )
274 int im = (i0 + i1) >> 1;
280 dx = x - double( m_x[i0] );
286 if ( x >= m_y.Length()-1 )
287 return m_y[m_y.Length()-1];
295 return m_y[i0] + dx*(m_b[i0] + dx*(m_c[i0] + dx*m_d[i0]));
315 return m_b && m_c && m_d;
324 coefficient_vector m_b;
325 coefficient_vector m_c;
326 coefficient_vector m_d;
Akima subspline interpolation algorithm.
AkimaInterpolation()=default
typename UnidimensionalInterpolation< T >::vector_type vector_type
PCL_HOT_FUNCTION double operator()(double x) const override
bool IsValid() const override
void Initialize(const vector_type &x, const vector_type &y) override
~AkimaInterpolation() override
AkimaInterpolation(const AkimaInterpolation &)=default
vector_type coefficient_vector
AkimaInterpolation(AkimaInterpolation &&)=default
A simple exception with an associated error message.
Generic vector of arbitrary length.
A generic interface to one-dimensional interpolation algorithms.
virtual void Initialize(const vector_type &x, const vector_type &y)
T Abs(const Complex< T > &c) noexcept
int TruncInt(T x) noexcept