52 #ifndef __PCL_Matrix_h
53 #define __PCL_Matrix_h
58 #include <pcl/Diagnostics.h>
63 #include <pcl/Memory.h>
65 #include <pcl/Rotate.h>
69 #ifndef __PCL_NO_MATRIX_STATISTICS
74 #if !defined( __PCL_NO_MATRIX_IMAGE_RENDERING ) && !defined( __PCL_NO_MATRIX_IMAGE_CONVERSION )
79 #if !defined( __PCL_NO_MATRIX_PHASE_MATRICES ) && !defined( __PCL_NO_VECTOR_INSTANTIATE )
88 #define PCL_VALID_KERNEL_SIZE( n ) (n ? Max( 3, n|1 ) : 0)
121 template <
typename T>
170 PCL_PRECONDITION( rows >= 0 && cols >= 0 )
171 m_data =
new Data( rows, cols );
183 PCL_PRECONDITION( rows >= 0 && cols >= 0 )
184 m_data =
new Data( rows, cols );
185 pcl::Fill( m_data->Begin(), m_data->End(), x );
201 template <
typename T1>
204 PCL_PRECONDITION( rows >= 0 && cols >= 0 )
205 m_data =
new Data( rows, cols );
210 const T1* __restrict__ k = a;
212 for ( ; i < j; ++i, ++k )
227 template <
typename T1>
229 const T1& a10,
const T1& a11 )
231 m_data =
new Data( 2, 2 );
248 template <
typename T1>
250 const T1& a10,
const T1& a11,
const T1& a12,
251 const T1& a20,
const T1& a21,
const T1& a22 )
253 m_data =
new Data( 3, 3 );
307 m_data =
new Data( rows, cols );
308 for (
int i = 0; i < m_data->Rows(); ++i, ++i0 )
309 for (
int j = 0; j < m_data->Cols(); ++j, ++j0 )
310 m_data->v[i][j] = x.m_data->v[i0][j0];
313 #ifndef __PCL_NO_MATRIX_IMAGE_CONVERSION
378 #endif // !__PCL_NO_MATRIX_IMAGE_CONVERSION
386 if ( m_data !=
nullptr )
399 if ( m_data->IsUnique() )
400 m_data->Deallocate();
403 Data* newData =
new Data( 0, 0 );
516 Data* newData =
new Data( m_data->Rows(), m_data->Cols() );
520 pcl::Fill( m_data->Begin(), m_data->End(), x );
524 #define IMPLEMENT_SCALAR_ASSIGN_OP( op ) \
527 block_iterator __restrict__ i = m_data->Begin(); \
528 const_block_iterator __restrict__ j = m_data->End(); \
530 for ( ; i < j; ++i ) \
535 Data* newData = new Data( m_data->Rows(), m_data->Cols() ); \
536 block_iterator __restrict__ i = newData->Begin(); \
537 const_block_iterator __restrict__ j = newData->End(); \
538 const_block_iterator __restrict__ k = m_data->Begin(); \
540 for ( ; i < j; ++i, ++k ) \
557 IMPLEMENT_SCALAR_ASSIGN_OP( + )
570 IMPLEMENT_SCALAR_ASSIGN_OP( - )
583 IMPLEMENT_SCALAR_ASSIGN_OP( * )
596 IMPLEMENT_SCALAR_ASSIGN_OP( / )
619 Data* newData =
new Data( m_data->Rows(), m_data->Cols() );
624 for ( ; i < j; ++i, ++k )
632 #undef IMPLEMENT_SCALAR_ASSIGN_OP
634 #ifndef __PCL_NO_MATRIX_ELEMENT_WISE_ARITHMETIC_OPERATIONS
636 #define IMPLEMENT_ELEMENT_WISE_ASSIGN_OP( op ) \
639 block_iterator __restrict__ i = m_data->Begin(); \
640 const_block_iterator __restrict__ j = pcl::Min( m_data->End(), \
641 m_data->Begin() + x.NumberOfElements() ); \
642 const_block_iterator __restrict__ k = x.m_data->Begin(); \
644 for ( ; i < j; ++i, ++k ) \
649 Data* newData = new Data( m_data->Rows(), m_data->Cols() ); \
650 block_iterator __restrict__ i = newData->Begin(); \
651 const_block_iterator __restrict__ j = pcl::Min( newData->End(), \
652 newData->Begin() + x.NumberOfElements() ); \
653 const_block_iterator __restrict__ k = x.m_data->Begin(); \
654 const_block_iterator __restrict__ t = m_data->Begin(); \
656 for ( ; i < j; ++i, ++k, ++t ) \
677 IMPLEMENT_ELEMENT_WISE_ASSIGN_OP( + )
694 IMPLEMENT_ELEMENT_WISE_ASSIGN_OP( - )
711 IMPLEMENT_ELEMENT_WISE_ASSIGN_OP( * )
728 IMPLEMENT_ELEMENT_WISE_ASSIGN_OP( / )
751 for ( ; i < j; ++i, ++k )
756 Data* newData =
new Data( m_data->Rows(), m_data->Cols() );
763 for ( ; i < j; ++i, ++k, ++t )
771 #undef IMPLEMENT_ELEMENT_WISE_ASSIGN_OP
773 #endif // __PCL_NO_MATRIX_ELEMENT_WISE_ARITHMETIC_OPERATIONS
787 for ( ; i < j; ++i, ++k )
810 Data* newData =
new Data( m_data->Rows(), m_data->Cols() );
815 for ( ; i < j; ++i, ++k )
834 for ( ; i < j; ++i, ++k )
857 Data* newData =
new Data( m_data->Rows(), m_data->Cols() );
862 for ( ; i < j; ++i, ++k )
881 for ( ; i < j; ++i, ++k )
904 Data* newData =
new Data( m_data->Rows(), m_data->Cols() );
909 for ( ; i < j; ++i, ++k )
921 return m_data->IsUnique();
930 return m_data == x.m_data;
944 Data* newData =
new Data( m_data->Rows(), m_data->Cols() );
949 for ( ; i < j; ++i, ++k )
962 return m_data->Rows();
971 return m_data->Cols();
1002 return m_data !=
nullptr;
1011 return Rows() == 0 || Cols() == 0;
1019 operator bool() const noexcept
1030 return m_data->NumberOfElements();
1039 return m_data->Size();
1051 return IsAliasOf( x ) || SameDimensions( x ) &&
pcl::Equal( Begin(), x.Begin(), x.End() );
1065 return !IsAliasOf( x ) &&
pcl::Compare( Begin(), End(), x.Begin(), x.End() ) < 0;
1074 return Rows() == x.Rows() && Cols() == x.Cols();
1088 if ( unlikely( IsAliasOf( x ) ) )
1090 if ( unlikely( NumberOfElements() != x.NumberOfElements() ) )
1096 for ( ; i < j; ++i, ++k )
1111 return m_data->Element( i, j );
1120 return m_data->Element( i, j );
1135 return m_data->v[i];
1146 return m_data->v[i];
1164 return m_data->Begin();
1178 return m_data->Begin();
1226 return m_data->End();
1241 return m_data->End();
1287 return m_data->v[i];
1290 #ifndef __PCL_NO_STL_COMPATIBLE_ITERATORS
1322 #endif // !__PCL_NO_STL_COMPATIBLE_ITERATORS
1329 vector r( m_data->Cols() );
1330 for (
int j = 0; j < m_data->Cols(); ++j )
1331 r[j] = m_data->v[i][j];
1340 vector c( m_data->Rows() );
1341 for (
int i = 0; i < m_data->Rows(); ++i )
1342 c[i] = m_data->v[i][j];
1353 return ColumnVector( j );
1364 for (
int j = 0; j < m_data->Cols() && j < r.Length(); ++j )
1365 m_data->v[i][j] =
element( r[j] );
1376 for (
int i = 0; i < m_data->Rows() && i < c.Length(); ++i )
1377 m_data->v[i][j] =
element( c[i] );
1400 for (
int j = 0; j < r.
Length(); ++j )
1401 R.m_data->v[0][j] = r[j];
1413 for (
int i = 0; i < c.
Length(); ++i )
1414 C.m_data->v[i][0] = c[i];
1423 return FromColumnVector( c );
1435 for (
int i = 0; i < n; ++i )
1449 for (
int i = 0; i < m_data->Rows(); ++i )
1450 for (
int j = 0; j < m_data->Cols(); ++j )
1451 Tr.m_data->v[j][i] = m_data->v[i][j];
1489 pcl::Reverse( m_data->Begin(), m_data->End() );
1499 pcl::CopyReversed( Tf.m_data->End(), m_data->Begin(), m_data->End() );
1570 PCL_PRECONDITION( Rows() == 3 && Cols() == 3 )
1575 A0[2] =
element( A0[2] + A0[0]*dx + A0[1]*dy );
1576 A1[2] =
element( A1[2] + A1[0]*dx + A1[1]*dy );
1577 A2[2] =
element( A2[2] + A2[0]*dx + A2[1]*dy );
1591 Translate(
double( delta[0] ),
double( delta[1] ) );
1603 R.Translate( dx, dy );
1619 return Translated(
double( delta[0] ),
double( delta[1] ) );
1660 SinCos( phi, sphi, cphi );
1661 return RotationX( sphi, cphi );
1702 SinCos( phi, sphi, cphi );
1703 return RotationY( sphi, cphi );
1744 SinCos( phi, sphi, cphi );
1745 return RotationZ( sphi, cphi );
1773 PCL_PRECONDITION( Rows() == 3 && Cols() == 3 )
1777 double a10 = cphi*A1[0] + sphi*A2[0];
1778 double a11 = cphi*A1[1] + sphi*A2[1];
1779 double a12 = cphi*A1[2] + sphi*A2[2];
1780 double a20 = -sphi*A1[0] + cphi*A2[0];
1781 double a21 = -sphi*A1[1] + cphi*A2[1];
1782 double a22 = -sphi*A1[2] + cphi*A2[2];
1802 SinCos( phi, sphi, cphi );
1803 RotateX( sphi, cphi );
1814 R.RotateX( sphi, cphi );
1854 PCL_PRECONDITION( Rows() == 3 && Cols() == 3 )
1858 double a00 = cphi*A0[0] - sphi*A2[0];
1859 double a01 = cphi*A0[1] - sphi*A2[1];
1860 double a02 = cphi*A0[2] - sphi*A2[2];
1861 double a20 = sphi*A0[0] + cphi*A2[0];
1862 double a21 = sphi*A0[1] + cphi*A2[1];
1863 double a22 = sphi*A0[2] + cphi*A2[2];
1883 SinCos( phi, sphi, cphi );
1884 RotateY( sphi, cphi );
1895 R.RotateY( sphi, cphi );
1935 PCL_PRECONDITION( Rows() == 3 && Cols() == 3 )
1939 double a00 = cphi*A0[0] + sphi*A1[0];
1940 double a01 = cphi*A0[1] + sphi*A1[1];
1941 double a02 = cphi*A0[2] + sphi*A1[2];
1942 double a10 = -sphi*A0[0] + cphi*A1[0];
1943 double a11 = -sphi*A0[1] + cphi*A1[1];
1944 double a12 = -sphi*A0[2] + cphi*A1[2];
1964 SinCos( phi, sphi, cphi );
1965 RotateZ( sphi, cphi );
1976 R.RotateZ( sphi, cphi );
2009 for ( ; i < j; ++i )
2022 R.Truncate( f0, f1 );
2045 if ( v0 != f0 || v1 != f1 )
2054 double d = (double( f1 ) - double( f0 ))/(
double( v1 ) - double( v0 ));
2058 for ( ; i < j; ++i )
2064 for ( ; i < j; ++i )
2065 *i =
element( d*(*i - v0) + f0 );
2069 pcl::Fill( m_data->Begin(), m_data->End(), f0 );
2072 pcl::Fill( m_data->Begin(), m_data->End(),
pcl::Range( v0, f0, f1 ) );
2082 R.Rescale( f0, f1 );
2092 pcl::Sort( m_data->Begin(), m_data->End() );
2111 pcl::Sort( m_data->Begin(), m_data->End(),
2134 pcl::Sort( m_data->Begin(), m_data->End(), p );
2157 return (p != m_data->End()) ? p :
nullptr;
2177 return (p != m_data->End()) ? p :
nullptr;
2188 #ifndef __PCL_NO_MATRIX_STATISTICS
2197 return *
MinItem( m_data->Begin(), m_data->End() );
2213 p.y = int( d/m_data->Cols() );
2214 p.x = int( d%m_data->Cols() );
2228 return *
MaxItem( m_data->Begin(), m_data->End() );
2243 int d = m - m_data->Begin();
2244 p.y = d/m_data->Cols();
2245 p.x = d%m_data->Cols();
2259 return pcl::Sum( m_data->Begin(), m_data->End() );
2322 return pcl::Mean( m_data->Begin(), m_data->End() );
2378 return pcl::StdDev( m_data->Begin(), m_data->End() );
2388 return pcl::Median( m_data->Begin(), m_data->End() );
2405 double AvgDev(
double center )
const noexcept
2407 return pcl::AvgDev( m_data->Begin(), m_data->End(), center );
2444 return pcl::AvgDev( m_data->Begin(), m_data->End() );
2497 double MAD(
double center )
const
2499 return pcl::MAD( m_data->Begin(), m_data->End(), center );
2514 return pcl::MAD( m_data->Begin(), m_data->End() );
2573 double BiweightMidvariance(
double center,
double sigma,
int k = 9,
bool reducedLength =
false ) const noexcept
2609 double center =
Median();
2628 int k = 9,
bool reducedLength =
false ) const noexcept
2641 double center =
Median();
2722 return pcl::Sn( A.Begin(), A.End() );
2750 return pcl::Qn( A.Begin(), A.End() );
2753 #endif // !__PCL_NO_MATRIX_STATISTICS
2765 return pcl::Hash64( m_data->Begin(), m_data->Size(), seed );
2778 return pcl::Hash32( m_data->Begin(), m_data->Size(), seed );
2806 template <
class S,
typename SP>
2807 S& ToSeparated( S& s, SP separator )
const
2812 s.Append( S( *i ) );
2816 s.Append( separator );
2817 s.Append( S( *i ) );
2846 template <
class S,
typename SP,
class AF>
2847 S& ToSeparated( S& s, SP separator, AF append )
const
2852 append( s, S( *i ) );
2859 append( s, S( *i ) );
2876 S& ToCommaSeparated( S& s )
const
2878 return ToSeparated( s,
',' );
2890 S& ToSpaceSeparated( S& s )
const
2892 return ToSeparated( s,
' ' );
2904 S& ToTabSeparated( S& s )
const
2906 return ToSeparated( s,
'\t' );
2909 #ifndef __PCL_NO_MATRIX_PHASE_MATRICES
2917 if ( B.Rows() != A.Rows() || B.Cols() != A.Cols() )
2918 throw Error(
"Invalid matrix dimensions in PhaseCorrelationMatrix()" );
2930 if ( B.Rows() != A.Rows() || B.Cols() != A.Cols() )
2931 throw Error(
"Invalid matrix dimensions in CrossPowerSpectrumMatrix()" );
2937 #endif // !__PCL_NO_MATRIX_PHASE_MATRICES
2939 #ifndef __PCL_NO_MATRIX_IMAGE_RENDERING
2963 typename P::sample* __restrict__ v = *image;
2967 for ( ; i < j; ++i, ++v )
2968 *v = P::ToSample( *i );
2994 case 32: ToImage(
static_cast<Image&
>( *image ) );
break;
2995 case 64: ToImage(
static_cast<DImage&
>( *image ) );
break;
3000 case 32: ToImage(
static_cast<ComplexImage&
>( *image ) );
break;
3001 case 64: ToImage(
static_cast<DComplexImage&
>( *image ) );
break;
3006 case 8: ToImage(
static_cast<UInt8Image&
>( *image ) );
break;
3007 case 16: ToImage(
static_cast<UInt16Image&
>( *image ) );
break;
3008 case 32: ToImage(
static_cast<UInt32Image&
>( *image ) );
break;
3012 #endif // !__PCL_NO_MATRIX_IMAGE_RENDERING
3014 #ifndef __PCL_NO_MATRIX_IMAGE_CONVERSION
3060 if ( r == image.
Bounds() )
3063 const typename P::sample* __restrict__ v = image[channel];
3067 for ( ; i < j; ++i, ++v )
3068 P::FromSample( *i, *v );
3078 for (
int i = r.
y0; i < r.
y1; ++i )
3080 const typename P::sample* __restrict__ v = image.
PixelAddress( r.
x0, i, channel );
3082 for (
int j = 0; j < w; ++j )
3083 P::FromSample( *m++, *v++ );
3109 case 32:
return FromImage(
static_cast<const Image&
>( *image ), rect, channel );
3110 case 64:
return FromImage(
static_cast<const DImage&
>( *image ), rect, channel );
3115 case 32:
return FromImage(
static_cast<const ComplexImage&
>( *image ), rect, channel );
3116 case 64:
return FromImage(
static_cast<const DComplexImage&
>( *image ), rect, channel );
3121 case 8:
return FromImage(
static_cast<const UInt8Image&
>( *image ), rect, channel );
3122 case 16:
return FromImage(
static_cast<const UInt16Image&
>( *image ), rect, channel );
3123 case 32:
return FromImage(
static_cast<const UInt32Image&
>( *image ), rect, channel );
3129 #endif // !__PCL_NO_MATRIX_IMAGE_CONVERSION
3142 block_iterator* v =
nullptr;
3146 Data(
int rows,
int cols )
3148 if ( rows > 0 && cols > 0 )
3149 Allocate( rows, cols );
3157 int Rows() const noexcept
3162 int Cols() const noexcept
3167 size_type NumberOfElements() const noexcept
3174 return NumberOfElements()*
sizeof( element );
3177 block_iterator Begin()
const
3179 block_iterator p = (v !=
nullptr) ? *v :
nullptr;
3180 if ( likely( std::is_scalar<element>::value ) )
3181 return reinterpret_cast<block_iterator
>( PCL_ASSUME_ALIGNED_32( p ) );
3185 block_iterator End() const noexcept
3187 return (v !=
nullptr) ? *v + NumberOfElements() : nullptr;
3190 element& Element(
int i,
int j )
const noexcept
3195 void Allocate(
int rows,
int cols )
3199 v =
new block_iterator[ n ];
3200 if ( likely( std::is_scalar<element>::value ) )
3202 *v =
reinterpret_cast<block_iterator
>( PCL_ALIGNED_MALLOC( Size(), 32 ) );
3203 if ( unlikely( *v ==
nullptr ) )
3208 throw std::bad_alloc();
3212 *v =
new element[ NumberOfElements() ];
3213 for (
int i = 1; i < n; ++i )
3219 PCL_PRECONDITION( refCount == 0 )
3222 if ( likely( std::is_scalar<element>::value ) )
3223 PCL_ALIGNED_FREE( *v );
3237 Data* m_data =
nullptr;
3243 void DetachFromData()
3245 if ( !m_data->Detach() )
3253 static bool Invert( block_iterator* Ai, const_block_iterator
const* A,
int n ) noexcept
3264 const_block_iterator __restrict__ A0 = A[0];
3265 const_block_iterator __restrict__ A1 = A[1];
3266 element d = A0[0]*A1[1] - A0[1]*A1[0];
3270 Ai[0][1] = -A0[1]/d;
3271 Ai[1][0] = -A1[0]/d;
3277 const_block_iterator __restrict__ A0 = A[0];
3278 const_block_iterator __restrict__ A1 = A[1];
3279 const_block_iterator __restrict__ A2 = A[2];
3280 element d1 = A1[1]*A2[2] - A1[2]*A2[1];
3281 element d2 = A1[2]*A2[0] - A1[0]*A2[2];
3282 element d3 = A1[0]*A2[1] - A1[1]*A2[0];
3283 element d = A0[0]*d1 + A0[1]*d2 + A0[2]*d3;
3287 Ai[0][1] = (A2[1]*A0[2] - A2[2]*A0[1])/d;
3288 Ai[0][2] = (A0[1]*A1[2] - A0[2]*A1[1])/d;
3290 Ai[1][1] = (A2[2]*A0[0] - A2[0]*A0[2])/d;
3291 Ai[1][2] = (A0[2]*A1[0] - A0[0]*A1[2])/d;
3293 Ai[2][1] = (A2[0]*A0[1] - A2[1]*A0[0])/d;
3294 Ai[2][2] = (A0[0]*A1[1] - A0[1]*A1[0])/d;
3312 template <
typename T>
inline
3315 if ( Rows() != Cols() || Rows() == 0 )
3316 throw Error(
"Invalid matrix inversion: Non-square or empty matrix." );
3325 if ( !Invert( Ai.m_data->v, m_data->v, Rows() ) )
3326 throw Error(
"Invalid matrix inversion: Singular matrix." );
3338 template <
typename T>
inline
3342 Transfer( Inverse() );
3345 if ( Rows() != Cols() || Rows() == 0 )
3346 throw Error(
"Invalid matrix inversion: Non-square or empty matrix." );
3370 template <
typename T>
inline
3373 if ( B.Rows() != A.Rows() || B.Cols() != A.Cols() )
3374 throw Error(
"Invalid matrix addition." );
3381 for ( ; i < j; ++i, ++k, ++t )
3391 template <
typename T>
inline
3399 for ( ; i < j; ++i, ++k )
3412 template <
typename T>
inline
3428 template <
typename T>
inline
3431 if ( B.Rows() != A.Rows() || B.Cols() != A.Cols() )
3432 throw Error(
"Invalid matrix subtraction." );
3439 for ( ; i < j; ++i, ++k, ++t )
3449 template <
typename T>
inline
3457 for ( ; i < j; ++i, ++k )
3471 template <
typename T>
inline
3479 for ( ; i < j; ++i, ++k )
3498 template <
typename T>
inline
3504 if ( B.Rows() != p )
3505 throw Error(
"Invalid matrix multiplication." );
3507 for (
int i = 0; i < n; ++i )
3508 for (
int j = 0; j < m; ++j )
3511 for (
int k = 0; k < p; ++k )
3512 rij += A[i][k] * B[k][j];
3530 template <
typename T>
inline
3536 throw Error(
"Invalid matrix-vector multiplication." );
3538 for (
int i = 0; i < m; ++i )
3541 for (
int j = 0; j < n; ++j )
3542 ri += A[i][j] * x[j];
3553 template <
typename T>
inline
3561 for ( ; i < j; ++i, ++k )
3574 template <
typename T>
inline
3587 template <
typename T>
inline
3595 for ( ; i < j; ++i, ++k )
3608 template <
typename T>
inline
3616 for ( ; i < j; ++i, ++k )
3628 template <
typename T>
inline
3636 for ( ; i < j; ++i, ++k )
3649 template <
typename T>
inline
3657 for ( ; i < j; ++i, ++k )
3664 #ifndef __PCL_NO_MATRIX_INSTANTIATE
3677 using I8Matrix = GenericMatrix<int8>;
3808 using F64Matrix = GenericMatrix<double>;
3837 using C32Matrix = GenericMatrix<Complex32>;
3846 using C64Matrix = GenericMatrix<Complex64>;
3861 using F80Matrix = GenericMatrix<long double>;
3878 #endif // !__PCL_NO_MATRIX_INSTANTIATE
3884 #endif // __PCL_Matrix_h