public Se3_F64 getWorldToCurr3D ( ) { // compute transform in 2D space Se2_F64 currToWorld = getCurrToWorld2D ( ) ; // 2D to 3D coordinates currPlaneToWorld3D . getT ( ) . set ( - currToWorld . T . y , 0 , currToWorld . T . x ) ; DMatrixRMaj R = currPlaneToWorld3D . getR ( ) ; // set rotation around Y axis. // Transpose the 2D transform since the rotation are pointing in opposite directions R . unsafe_set ( 0 , 0 , currToWorld . c ) ; R . unsafe_set ( 0 , 2 , - currToWorld . s ) ; R . unsafe_set ( 1 , 1 , 1 ) ; R . unsafe_set ( 2 , 0 , currToWorld . s ) ; R . unsafe_set ( 2 , 2 , currToWorld . c ) ; currPlaneToWorld3D . invert ( worldToCurrPlane3D ) ; worldToCurrPlane3D . concat ( planeToCamera , worldToCurrCam3D ) ; return worldToCurrCam3D ; }
Converts 2D motion estimate into a 3D motion estimate