@ Override public double computeDistance ( AssociatedPair obs ) { // triangulate the point in 3D space triangulate . triangulate ( obs . p1 , obs . p2 , keyToCurr , p ) ; if ( p . z < 0 ) return Double . MAX_VALUE ; // compute observational error in each view double error = errorCam1 . errorSq ( obs . p1 . x , obs . p1 . y , p . x / p . z , p . y / p . z ) ; SePointOps_F64 . transform ( keyToCurr , p , p ) ; if ( p . z < 0 ) return Double . MAX_VALUE ; error += errorCam2 . errorSq ( obs . p2 . x , obs . p2 . y , p . x / p . z , p . y / p . z ) ; return error ; }
Computes the error given the motion model