private void triangulateNoLocation ( View target ) { Se3_F64 otherToTarget = new Se3_F64 ( ) ; Se3_F64 worldToTarget = target . viewToWorld . invert ( null ) ; for ( Motion c : target . connections ) { boolean isSrc = c . viewSrc == target ; View other = c . destination ( target ) ; if ( other . state != ViewState . PROCESSED ) continue ; other . viewToWorld . concat ( worldToTarget , otherToTarget ) ; triangulationError . configure ( target . camera . pinhole , other . camera . pinhole ) ; for ( int i = 0 ; i < c . associated . size ( ) ; i ++ ) { AssociatedIndex a = c . associated . get ( i ) ; int indexTarget = isSrc ? a . src : a . dst ; int indexOther = isSrc ? a . dst : a . src ; if ( target . features3D [ indexTarget ] != null || other . features3D [ indexOther ] != null ) continue ; Point2D_F64 normOther = other . observationNorm . get ( indexOther ) ; Point2D_F64 normTarget = target . observationNorm . get ( indexTarget ) ; // Skip points with poor geometry double angle = triangulationAngle ( normOther , normTarget , otherToTarget ) ; if ( angle < TRIANGULATE_MIN_ANGLE ) continue ; Feature3D f = new Feature3D ( ) ; if ( ! triangulate . triangulate ( normOther , normTarget , otherToTarget , f . worldPt ) ) continue ; // must be in front of the camera if ( f . worldPt . z <= 0 ) continue ; double error = triangulationError . process ( normOther , normTarget , otherToTarget , f . worldPt ) ; if ( error > maxPixelError * maxPixelError ) continue ; other . viewToWorld . transform ( f . worldPt , f . worldPt ) ; f . views . add ( target ) ; f . views . add ( other ) ; f . obsIdx . add ( indexTarget ) ; f . obsIdx . add ( indexOther ) ; graph . features3D . add ( f ) ; target . features3D [ indexTarget ] = f ; other . features3D [ indexOther ] = f ; } } }
Go through all connections to the view and triangulate all features which have not been triangulated already