private boolean estimateMotion ( ) { List < PointTrack > active = tracker . getActiveTracks ( null ) ; List < Point2D3D > obs = new ArrayList <> ( ) ; for ( PointTrack t : active ) { Point2D3D p = t . getCookie ( ) ; pixelToNorm . compute ( t . x , t . y , p . observation ) ; obs . add ( p ) ; } // estimate the motion up to a scale factor in translation if ( ! motionEstimator . process ( obs ) ) return false ; if ( doublePass ) { if ( ! performSecondPass ( active , obs ) ) return false ; } tracker . finishTracking ( ) ; Se3_F64 keyToCurr ; if ( refine != null ) { keyToCurr = new Se3_F64 ( ) ; refine . fitModel ( motionEstimator . getMatchSet ( ) , motionEstimator . getModelParameters ( ) , keyToCurr ) ; } else { keyToCurr = motionEstimator . getModelParameters ( ) ; } keyToCurr . invert ( currToKey ) ; // mark tracks as being inliers and add to inlier list int N = motionEstimator . getMatchSet ( ) . size ( ) ; for ( int i = 0 ; i < N ; i ++ ) { int index = motionEstimator . getInputIndex ( i ) ; Point2D3DTrack t = active . get ( index ) . getCookie ( ) ; t . lastInlier = tick ; inlierTracks . add ( t ) ; } return true ; }
Estimates motion from the set of tracks and their 3D location