public static DMatrixRMaj toHomogeneous ( Se3_F64 se , DMatrixRMaj ret ) { if ( ret == null ) ret = new DMatrixRMaj ( 4 , 4 ) ; else { ret . set ( 3 , 0 , 0 ) ; ret . set ( 3 , 1 , 0 ) ; ret . set ( 3 , 2 , 0 ) ; } CommonOps_DDRM . insert ( se . getR ( ) , ret , 0 , 0 ) ; Vector3D_F64 T = se . getT ( ) ; ret . set ( 0 , 3 , T . x ) ; ret . set ( 1 , 3 , T . y ) ; ret . set ( 2 , 3 , T . z ) ; ret . set ( 3 , 3 , 1 ) ; return ret ; }
Converts it into a 4 by 4 homogeneous matrix .