private void estimateAllFeatures ( View seedA , View seedB ) { List < View > open = new ArrayList <> ( ) ; // Add features for all the other views connected to the root view and determine the translation scale factor addUnvistedToStack ( seedA , open ) ; addUnvistedToStack ( seedB , open ) ; // Do a breath first search. The queue is first in first out while ( ! open . isEmpty ( ) ) { if ( stopRequested ) return ; if ( verbose != null ) verbose . println ( "### open.size=" + open . size ( ) ) ; // select the view with the 3D features. This view can be estimated which the highest degree of confience int bestCount = countFeaturesWith3D ( open . get ( 0 ) ) ; int bestIndex = 0 ; for ( int i = 1 ; i < open . size ( ) ; i ++ ) { int count = countFeaturesWith3D ( open . get ( i ) ) ; if ( count > bestCount ) { bestCount = count ; bestIndex = i ; } } View v = open . remove ( bestIndex ) ; if ( verbose != null ) verbose . println ( " processing view=" + v . index + " | 3D Features=" + bestCount ) ; // Determine the view's location in the 3D view. This might have been previously estimated using // stereo and the estimated scale factor. That will be ignored and the new estimate used instead if ( ! determinePose ( v ) ) { // // The pose could not be determined, so remove it from the graph // if( verbose != null ) // verbose.println(" Removing connection"); // for (CameraMotion m : v.connections) { // CameraView a = m.destination(v); // a.connections.remove(m); // graph.edges.remove(m); // } // graph.nodes.remove(v); // // for (int i = 0; i < graph.nodes.size(); i++) { // graph.nodes.get(i).index = i; // } // TODO mark instead of remove? Need a unit test for remove throw new RuntimeException ( "Crap handle this" ) ; } else { // If possible use triangulation from stereo addTriangulatedFeaturesForAllEdges ( v ) ; triangulateNoLocation ( v ) ; viewsAdded . add ( v ) ; // Update the open list addUnvistedToStack ( v , open ) ; } } }
Perform a breath first search to find the structure of all the remaining camrea views