public void selectWorldControlPoints ( List < Point3D_F64 > worldPts , FastQueue < Point3D_F64 > controlWorldPts ) { UtilPoint3D_F64 . mean ( worldPts , meanWorldPts ) ; // covariance matrix elements, summed up here for speed double c11 = 0 , c12 = 0 , c13 = 0 , c22 = 0 , c23 = 0 , c33 = 0 ; final int N = worldPts . size ( ) ; for ( int i = 0 ; i < N ; i ++ ) { Point3D_F64 p = worldPts . get ( i ) ; double dx = p . x - meanWorldPts . x ; double dy = p . y - meanWorldPts . y ; double dz = p . z - meanWorldPts . z ; c11 += dx * dx ; c12 += dx * dy ; c13 += dx * dz ; c22 += dy * dy ; c23 += dy * dz ; c33 += dz * dz ; } c11 /= N ; c12 /= N ; c13 /= N ; c22 /= N ; c23 /= N ; c33 /= N ; DMatrixRMaj covar = new DMatrixRMaj ( 3 , 3 , true , c11 , c12 , c13 , c12 , c22 , c23 , c13 , c23 , c33 ) ; // find the data's orientation and check to see if it is planar svd . decompose ( covar ) ; double [ ] singularValues = svd . getSingularValues ( ) ; DMatrixRMaj V = svd . getV ( null , false ) ; SingularOps_DDRM . descendingOrder ( null , false , singularValues , 3 , V , false ) ; // planar check if ( singularValues [ 0 ] < singularValues [ 2 ] * 1e13 ) { numControl = 4 ; } else { numControl = 3 ; } // put control points along the data's major axises controlWorldPts . reset ( ) ; for ( int i = 0 ; i < numControl - 1 ; i ++ ) { double m = Math . sqrt ( singularValues [ 1 ] ) * magicNumber ; double vx = V . unsafe_get ( 0 , i ) * m ; double vy = V . unsafe_get ( 1 , i ) * m ; double vz = V . unsafe_get ( 2 , i ) * m ; controlWorldPts . grow ( ) . set ( meanWorldPts . x + vx , meanWorldPts . y + vy , meanWorldPts . z + vz ) ; } // set a control point to be the centroid controlWorldPts . grow ( ) . set ( meanWorldPts . x , meanWorldPts . y , meanWorldPts . z ) ; }
Selects control points along the data s axis and the data s centroid . If the data is determined to be planar then only 3 control points are selected .