private void extractSolutionForQ ( DMatrix4x4 Q ) { DMatrixRMaj nv = new DMatrixRMaj ( 10 , 1 ) ; SingularOps_DDRM . nullVector ( svd , true , nv ) ; // Convert the solution into a fixed sized matrix because it's easier to read encodeQ ( Q , nv . data ) ; // diagonal elements must be positive because Q = [K*K' .. ; ... ] // If they are a mix of positive and negative that's bad and can't be fixed // All 3 are checked because technically they could be zero. Not a physically useful solution but... if ( Q . a11 < 0 || Q . a22 < 0 || Q . a33 < 0 ) { CommonOps_DDF4 . scale ( - 1 , Q ) ; } }
Extracts the null space and converts it into the Q matrix