diff --git a/src/nvmath/Fitting.cpp b/src/nvmath/Fitting.cpp index 9763b78..72d3c42 100644 --- a/src/nvmath/Fitting.cpp +++ b/src/nvmath/Fitting.cpp @@ -7,6 +7,7 @@ #include "nvcore/Utils.h" // max, swap #include // FLT_MAX +#include using namespace nv; @@ -187,6 +188,29 @@ Vector3 nv::Fit::computePrincipalComponent_EigenSolver(int n, const Vector3 *__r return firstEigenVector_EigenSolver(matrix); } +void ArvoSVD(int rows, int cols, float * Q, float * diag, float * R); + +Vector3 nv::Fit::computePrincipalComponent_SVD(int n, const Vector3 *__restrict points) +{ + // Store the points in an n x n matrix + std::vector Q(n*n, 0.0f); + for (int i = 0; i < n; ++i) + { + Q[i*n+0] = points[i].x; + Q[i*n+1] = points[i].y; + Q[i*n+2] = points[i].z; + } + + // Alloc space for the SVD outputs + std::vector diag(n, 0.0f); + std::vector R(n*n, 0.0f); + + ArvoSVD(n, n, &Q[0], &diag[0], &R[0]); + + // Get the principal component + return Vector3(R[0], R[1], R[2]); +} + Plane nv::Fit::bestPlane(int n, const Vector3 *__restrict points) @@ -232,16 +256,16 @@ bool nv::Fit::isPlanar(int n, const Vector3 * points, float epsilon/*=NV_EPSILON // Householder transforms followed by QL decomposition. // Seems to be based on the code from Numerical Recipes in C. -static void EigenSolver_Tridiagonal(double mat[3][3],double * diag,double * subd); -static bool EigenSolver_QLAlgorithm(double mat[3][3],double * diag,double * subd); +static void EigenSolver_Tridiagonal(float mat[3][3],float * diag,float * subd); +static bool EigenSolver_QLAlgorithm(float mat[3][3],float * diag,float * subd); bool nv::Fit::eigenSolveSymmetric(const float matrix[6], float eigenValues[3], Vector3 eigenVectors[3]) { nvDebugCheck(matrix != NULL && eigenValues != NULL && eigenVectors != NULL); - double subd[3]; - double diag[3]; - double work[3][3]; + float subd[3]; + float diag[3]; + float work[3][3]; work[0][0] = matrix[0]; work[0][1] = work[1][0] = matrix[1]; @@ -297,7 +321,7 @@ bool nv::Fit::eigenSolveSymmetric(const float matrix[6], float eigenValues[3], V return true; } -static void EigenSolver_Tridiagonal(double mat[3][3],double * diag,double * subd) +static void EigenSolver_Tridiagonal(float mat[3][3],float * diag,float * subd) { // Householder reduction T = Q^t M Q // Input: @@ -306,23 +330,23 @@ static void EigenSolver_Tridiagonal(double mat[3][3],double * diag,double * subd // mat, orthogonal matrix Q // diag, diagonal entries of T // subd, subdiagonal entries of T (T is symmetric) - const double epsilon = 1e-08f; + const float epsilon = 1e-08f; - double a = mat[0][0]; - double b = mat[0][1]; - double c = mat[0][2]; - double d = mat[1][1]; - double e = mat[1][2]; - double f = mat[2][2]; + float a = mat[0][0]; + float b = mat[0][1]; + float c = mat[0][2]; + float d = mat[1][1]; + float e = mat[1][2]; + float f = mat[2][2]; diag[0] = a; subd[2] = 0.f; if ( fabs(c) >= epsilon ) { - const double ell = sqrt(b*b+c*c); + const float ell = sqrtf(b*b+c*c); b /= ell; c /= ell; - const double q = 2*b*e+c*(f-d); + const float q = 2*b*e+c*(f-d); diag[1] = d+c*q; diag[2] = f-c*q; subd[0] = ell; @@ -343,7 +367,7 @@ static void EigenSolver_Tridiagonal(double mat[3][3],double * diag,double * subd } } -static bool EigenSolver_QLAlgorithm(double mat[3][3],double * diag,double * subd) +static bool EigenSolver_QLAlgorithm(float mat[3][3],float * diag,float * subd) { // QL iteration with implicit shifting to reduce matrix from tridiagonal // to diagonal @@ -357,34 +381,34 @@ static bool EigenSolver_QLAlgorithm(double mat[3][3],double * diag,double * subd int m; for (m = ell; m <= 1; m++) { - double dd = fabs(diag[m]) + fabs(diag[m+1]); + float dd = fabs(diag[m]) + fabs(diag[m+1]); if ( fabs(subd[m]) + dd == dd ) break; } if ( m == ell ) break; - double g = (diag[ell+1]-diag[ell])/(2*subd[ell]); - double r = sqrt(g*g+1); + float g = (diag[ell+1]-diag[ell])/(2*subd[ell]); + float r = sqrtf(g*g+1); if ( g < 0 ) g = diag[m]-diag[ell]+subd[ell]/(g-r); else g = diag[m]-diag[ell]+subd[ell]/(g+r); - double s = 1, c = 1, p = 0; + float s = 1, c = 1, p = 0; for (int i = m-1; i >= ell; i--) { - double f = s*subd[i], b = c*subd[i]; + float f = s*subd[i], b = c*subd[i]; if ( fabs(f) >= fabs(g) ) { c = g/f; - r = sqrt(c*c+1); + r = sqrtf(c*c+1); subd[i+1] = f*r; c *= (s = 1/r); } else { s = f/g; - r = sqrt(s*s+1); + r = sqrtf(s*s+1); subd[i+1] = g*r; s *= (c = 1/r); } @@ -504,3 +528,300 @@ int nv::Fit::compute4Means(int n, const Vector3 *__restrict points, const float } } + + +// Adaptation of James Arvo's SVD code, as found in ZOH. + +inline float Sqr(float x) { return x*x; } + +inline float svd_pythag( float a, float b ) +{ + float at = fabsf(a); + float bt = fabsf(b); + if( at > bt ) + return at * sqrtf( 1.0f + Sqr( bt / at ) ); + else if( bt > 0.0f ) + return bt * sqrtf( 1.0f + Sqr( at / bt ) ); + else return 0.0f; +} + +inline float SameSign( float a, float b ) +{ + float t; + if( b >= 0.0f ) t = fabsf( a ); + else t = -fabsf( a ); + return t; +} + +void ArvoSVD(int rows, int cols, float * Q, float * diag, float * R) +{ + static const int MaxIterations = 30; + + int i, j, k, l, p, q, iter; + float c, f, h, s, x, y, z; + float norm = 0.0f; + float g = 0.0f; + float scale = 0.0f; + + std::vector temp(cols, 0.0f); + + for( i = 0; i < cols; i++ ) + { + temp[i] = scale * g; + scale = 0.0f; + g = 0.0f; + s = 0.0f; + l = i + 1; + + if( i < rows ) + { + for( k = i; k < rows; k++ ) scale += fabsf( Q[k*cols+i] ); + if( scale != 0.0f ) + { + for( k = i; k < rows; k++ ) + { + Q[k*cols+i] /= scale; + s += Sqr( Q[k*cols+i] ); + } + f = Q[i*cols+i]; + g = -SameSign( sqrtf(s), f ); + h = f * g - s; + Q[i*cols+i] = f - g; + if( i != cols - 1 ) + { + for( j = l; j < cols; j++ ) + { + s = 0.0f; + for( k = i; k < rows; k++ ) s += Q[k*cols+i] * Q[k*cols+j]; + f = s / h; + for( k = i; k < rows; k++ ) Q[k*cols+j] += f * Q[k*cols+i]; + } + } + for( k = i; k < rows; k++ ) Q[k*cols+i] *= scale; + } + } + + diag[i] = scale * g; + g = 0.0f; + s = 0.0f; + scale = 0.0f; + + if( i < rows && i != cols - 1 ) + { + for( k = l; k < cols; k++ ) scale += fabsf( Q[i*cols+k] ); + if( scale != 0.0f ) + { + for( k = l; k < cols; k++ ) + { + Q[i*cols+k] /= scale; + s += Sqr( Q[i*cols+k] ); + } + f = Q[i*cols+l]; + g = -SameSign( sqrtf(s), f ); + h = f * g - s; + Q[i*cols+l] = f - g; + for( k = l; k < cols; k++ ) temp[k] = Q[i*cols+k] / h; + if( i != rows - 1 ) + { + for( j = l; j < rows; j++ ) + { + s = 0.0f; + for( k = l; k < cols; k++ ) s += Q[j*cols+k] * Q[i*cols+k]; + for( k = l; k < cols; k++ ) Q[j*cols+k] += s * temp[k]; + } + } + for( k = l; k < cols; k++ ) Q[i*cols+k] *= scale; + } + } + norm = max( norm, fabsf( diag[i] ) + fabsf( temp[i] ) ); + } + + + for( i = cols - 1; i >= 0; i-- ) + { + if( i < cols - 1 ) + { + if( g != 0.0f ) + { + for( j = l; j < cols; j++ ) R[i*cols+j] = ( Q[i*cols+j] / Q[i*cols+l] ) / g; + for( j = l; j < cols; j++ ) + { + s = 0.0f; + for( k = l; k < cols; k++ ) s += Q[i*cols+k] * R[j*cols+k]; + for( k = l; k < cols; k++ ) R[j*cols+k] += s * R[i*cols+k]; + } + } + for( j = l; j < cols; j++ ) + { + R[i*cols+j] = 0.0f; + R[j*cols+i] = 0.0f; + } + } + R[i*cols+i] = 1.0f; + g = temp[i]; + l = i; + } + + + for( i = cols - 1; i >= 0; i-- ) + { + l = i + 1; + g = diag[i]; + if( i < cols - 1 ) for( j = l; j < cols; j++ ) Q[i*cols+j] = 0.0f; + if( g != 0.0f ) + { + g = 1.0f / g; + if( i != cols - 1 ) + { + for( j = l; j < cols; j++ ) + { + s = 0.0f; + for( k = l; k < rows; k++ ) s += Q[k*cols+i] * Q[k*cols+j]; + f = ( s / Q[i*cols+i] ) * g; + for( k = i; k < rows; k++ ) Q[k*cols+j] += f * Q[k*cols+i]; + } + } + for( j = i; j < rows; j++ ) Q[j*cols+i] *= g; + } + else + { + for( j = i; j < rows; j++ ) Q[j*cols+i] = 0.0f; + } + Q[i*cols+i] += 1.0f; + } + + + for( k = cols - 1; k >= 0; k-- ) + { + for( iter = 1; iter <= MaxIterations; iter++ ) + { + int jump; + + for( l = k; l >= 0; l-- ) + { + q = l - 1; + if( fabsf( temp[l] ) + norm == norm ) { jump = 1; break; } + if( fabsf( diag[q] ) + norm == norm ) { jump = 0; break; } + } + + if( !jump ) + { + c = 0.0f; + s = 1.0f; + for( i = l; i <= k; i++ ) + { + f = s * temp[i]; + temp[i] *= c; + if( fabsf( f ) + norm == norm ) break; + g = diag[i]; + h = svd_pythag( f, g ); + diag[i] = h; + h = 1.0f / h; + c = g * h; + s = -f * h; + for( j = 0; j < rows; j++ ) + { + y = Q[j*cols+q]; + z = Q[j*cols+i]; + Q[j*cols+q] = y * c + z * s; + Q[j*cols+i] = z * c - y * s; + } + } + } + + z = diag[k]; + if( l == k ) + { + if( z < 0.0f ) + { + diag[k] = -z; + for( j = 0; j < cols; j++ ) R[k*cols+j] *= -1.0f; + } + break; + } + if( iter >= MaxIterations ) return; + x = diag[l]; + q = k - 1; + y = diag[q]; + g = temp[q]; + h = temp[k]; + f = ( ( y - z ) * ( y + z ) + ( g - h ) * ( g + h ) ) / ( 2.0f * h * y ); + g = svd_pythag( f, 1.0f ); + f = ( ( x - z ) * ( x + z ) + h * ( ( y / ( f + SameSign( g, f ) ) ) - h ) ) / x; + c = 1.0f; + s = 1.0f; + for( j = l; j <= q; j++ ) + { + i = j + 1; + g = temp[i]; + y = diag[i]; + h = s * g; + g = c * g; + z = svd_pythag( f, h ); + temp[j] = z; + c = f / z; + s = h / z; + f = x * c + g * s; + g = g * c - x * s; + h = y * s; + y = y * c; + for( p = 0; p < cols; p++ ) + { + x = R[j*cols+p]; + z = R[i*cols+p]; + R[j*cols+p] = x * c + z * s; + R[i*cols+p] = z * c - x * s; + } + z = svd_pythag( f, h ); + diag[j] = z; + if( z != 0.0f ) + { + z = 1.0f / z; + c = f * z; + s = h * z; + } + f = c * g + s * y; + x = c * y - s * g; + for( p = 0; p < rows; p++ ) + { + y = Q[p*cols+j]; + z = Q[p*cols+i]; + Q[p*cols+j] = y * c + z * s; + Q[p*cols+i] = z * c - y * s; + } + } + temp[l] = 0.0f; + temp[k] = f; + diag[k] = x; + } + } + + // Sort the singular values into descending order. + + for( i = 0; i < cols - 1; i++ ) + { + float biggest = diag[i]; // Biggest singular value so far. + int bindex = i; // The row/col it occurred in. + for( j = i + 1; j < cols; j++ ) + { + if( diag[j] > biggest ) + { + biggest = diag[j]; + bindex = j; + } + } + if( bindex != i ) // Need to swap rows and columns. + { + // Swap columns in Q. + for (int j = 0; j < rows; ++j) + swap(Q[j*cols+i], Q[j*cols+bindex]); + + // Swap rows in R. + for (int j = 0; j < rows; ++j) + swap(R[i*cols+j], R[bindex*cols+j]); + + // Swap elements in diag. + swap(diag[i], diag[bindex]); + } + } +} diff --git a/src/nvmath/Fitting.h b/src/nvmath/Fitting.h index a783753..c3f9cf8 100644 --- a/src/nvmath/Fitting.h +++ b/src/nvmath/Fitting.h @@ -23,6 +23,8 @@ namespace nv Vector3 computePrincipalComponent_EigenSolver(int n, const Vector3 * points); Vector3 computePrincipalComponent_EigenSolver(int n, const Vector3 * points, const float * weights, const Vector3 & metric); + Vector3 computePrincipalComponent_SVD(int n, const Vector3 * points); + Plane bestPlane(int n, const Vector3 * points); bool isPlanar(int n, const Vector3 * points, float epsilon = NV_EPSILON);