private void extractTransform ( DMatrixRMaj U , DMatrixRMaj V , DMatrixRMaj S , Se3_F64 se , boolean optionA , boolean optionB ) { DMatrixRMaj R = se . getR ( ) ; Vector3D_F64 T = se . getT ( ) ; // extract rotation if ( optionA ) CommonOps_DDRM . mult ( U , Rz , temp ) ; else CommonOps_DDRM . multTransB ( U , Rz , temp ) ; CommonOps_DDRM . multTransB ( temp , V , R ) ; // extract screw symmetric translation matrix if ( optionB ) CommonOps_DDRM . multTransB ( U , Rz , temp ) ; else CommonOps_DDRM . mult ( U , Rz , temp ) ; CommonOps_DDRM . mult ( temp , S , temp2 ) ; CommonOps_DDRM . multTransB ( temp2 , U , temp ) ; T . x = temp . get ( 2 , 1 ) ; T . y = temp . get ( 0 , 2 ) ; T . z = temp . get ( 1 , 0 ) ; }
There are four possible reconstructions from an essential matrix . This function will compute different permutations depending on optionA and optionB being true or false .