public Se3_F64 getWorldToCurr3D ( ) { // 2D to 3D coordinates worldToCurr3D . getT ( ) . set ( - worldToCurr2D . T . y , 0 , worldToCurr2D . T . x ) ; DMatrixRMaj R = worldToCurr3D . getR ( ) ; // set rotation around Y axis. // Transpose the 2D transform since the rotation are pointing in opposite directions R . unsafe_set ( 0 , 0 , worldToCurr2D . c ) ; R . unsafe_set ( 0 , 2 , - worldToCurr2D . s ) ; R . unsafe_set ( 1 , 1 , 1 ) ; R . unsafe_set ( 2 , 0 , worldToCurr2D . s ) ; R . unsafe_set ( 2 , 2 , worldToCurr2D . c ) ; worldToCurr3D . concat ( planeToCamera , worldToCurrCam3D ) ; return worldToCurrCam3D ; }
3D motion .