private boolean estimateMotion ( ) { modelFitData . reset ( ) ; Point2D_F64 normLeft = new Point2D_F64 ( ) ; Point2D_F64 normRight = new Point2D_F64 ( ) ; // use 0 -> 1 stereo associations to estimate each feature's 3D position for ( int i = 0 ; i < quadViews . size ; i ++ ) { QuadView obs = quadViews . get ( i ) ; // convert old stereo view to normalized coordinates leftImageToNorm . compute ( obs . v0 . x , obs . v0 . y , normLeft ) ; rightImageToNorm . compute ( obs . v1 . x , obs . v1 . y , normRight ) ; // compute 3D location using triangulation triangulate . triangulate ( normLeft , normRight , leftToRight , obs . X ) ; // add to data set for fitting if not at infinity if ( ! Double . isInfinite ( obs . X . normSq ( ) ) ) { Stereo2D3D data = modelFitData . grow ( ) ; leftImageToNorm . compute ( obs . v2 . x , obs . v2 . y , data . leftObs ) ; rightImageToNorm . compute ( obs . v3 . x , obs . v3 . y , data . rightObs ) ; data . location . set ( obs . X ) ; } } // robustly match the data if ( ! matcher . process ( modelFitData . toList ( ) ) ) return false ; Se3_F64 oldToNew = matcher . getModelParameters ( ) ; // System.out.println("matcher rot = "+toString(oldToNew)); // optionally refine the results if ( modelRefiner != null ) { Se3_F64 found = new Se3_F64 ( ) ; if ( modelRefiner . fitModel ( matcher . getMatchSet ( ) , oldToNew , found ) ) { // System.out.println("matcher rot = "+toString(found)); found . invert ( newToOld ) ; } else { oldToNew . invert ( newToOld ) ; // System.out.println("Fit failed!"); } } else { oldToNew . invert ( newToOld ) ; } // compound the just found motion with the previously found motion Se3_F64 temp = new Se3_F64 ( ) ; newToOld . concat ( leftCamToWorld , temp ) ; leftCamToWorld . set ( temp ) ; return true ; }
Estimates camera egomotion between the two most recent image frames