boolean determinePose ( View target ) { // Find all Features which are visible in this view and have a known 3D location List < Point2D3D > list = new ArrayList <> ( ) ; List < Feature3D > features = new ArrayList <> ( ) ; GrowQueue_I32 featureIndexes = new GrowQueue_I32 ( ) ; // TODO mark need to handle casees where the target's index has changed due to node removal // Find all the known 3D features which are visible in this view for ( Motion c : target . connections ) { boolean isSrc = c . viewSrc == target ; View other = c . destination ( target ) ; if ( other . state != ViewState . PROCESSED ) continue ; for ( int i = 0 ; i < c . associated . size ( ) ; i ++ ) { AssociatedIndex a = c . associated . get ( i ) ; Feature3D f = other . features3D [ isSrc ? a . dst : a . src ] ; if ( f == null || f . mark == target . index ) continue ; f . mark = target . index ; features . add ( f ) ; featureIndexes . add ( isSrc ? a . src : a . dst ) ; Point2D_F64 norm = target . observationNorm . get ( isSrc ? a . src : a . dst ) ; Point2D3D p = new Point2D3D ( ) ; p . location . set ( f . worldPt ) ; p . observation . set ( norm ) ; list . add ( p ) ; } } // Estimate the target's location using robust PNP ransacPnP . setIntrinsic ( 0 , target . camera . pinhole ) ; if ( list . size ( ) < 20 || ! ransacPnP . process ( list ) ) { if ( verbose != null ) verbose . println ( " View=" + target . index + " RANSAC failed. list.size=" + list . size ( ) ) ; return false ; } target . state = ViewState . PROCESSED ; // add inliers to the features int N = ransacPnP . getMatchSet ( ) . size ( ) ; if ( verbose != null ) verbose . println ( " View=" + target . index + " PNP RANSAC " + N + "/" + list . size ( ) ) ; for ( int i = 0 ; i < N ; i ++ ) { int which = ransacPnP . getInputIndex ( i ) ; Feature3D f = features . get ( which ) ; if ( f . views . contains ( target ) ) continue ; f . views . add ( target ) ; f . obsIdx . add ( featureIndexes . get ( which ) ) ; target . features3D [ featureIndexes . get ( which ) ] = f ; if ( f . views . size ( ) != f . obsIdx . size ) throw new RuntimeException ( "BUG!" ) ; } Se3_F64 worldToView = ransacPnP . getModelParameters ( ) ; target . viewToWorld . set ( worldToView . invert ( null ) ) ; return true ; }
Uses the previously found motion between the two cameras to estimate the scale and 3D point of common features . If a feature already has a known 3D point that is not modified . Scale is found by computing the 3D coordinate of all points with a 3D point again then dividing the two distances . New features are also triangulated and have their location s update using this scale .