public static boolean isIdentical ( Se3_F64 a , Se3_F64 b , double tolT , double tolR ) { if ( Math . abs ( a . T . x - b . T . x ) > tolT ) return false ; if ( Math . abs ( a . T . y - b . T . y ) > tolT ) return false ; if ( Math . abs ( a . T . z - b . T . z ) > tolT ) return false ; DMatrixRMaj D = new DMatrixRMaj ( 3 , 3 ) ; CommonOps_DDRM . multTransA ( a . R , b . R , D ) ; Rodrigues_F64 rod = new Rodrigues_F64 ( ) ; ConvertRotation3D_F64 . matrixToRodrigues ( D , rod ) ; return rod . theta <= tolR ; }
Can be used to see if two transforms are identical to within tolerance