public boolean estimateMotion ( Planar < I > input , Se3_F32 hintKeyToInput ) { InputSanityCheck . checkSameShape ( derivX , input ) ; initMotion ( input ) ; keyToCurrent . set ( hintKeyToInput ) ; boolean foundSolution = false ; float previousError = Float . MAX_VALUE ; for ( int i = 0 ; i < maxIterations ; i ++ ) { constructLinearSystem ( input , keyToCurrent ) ; if ( ! solveSystem ( ) ) break ; if ( Math . abs ( previousError - errorOptical ) / previousError < convergeTol ) break ; else { // update the estimated motion from the computed twist previousError = errorOptical ; keyToCurrent . concat ( motionTwist , tmp ) ; keyToCurrent . set ( tmp ) ; foundSolution = true ; } } return foundSolution ; }
Estimates the motion relative to the key frame .