private void initializeStructure ( List < AssociatedTriple > listObs , DMatrixRMaj P2 , DMatrixRMaj P3 ) { List < DMatrixRMaj > cameraMatrices = new ArrayList <> ( ) ; cameraMatrices . add ( P1 ) ; cameraMatrices . add ( P2 ) ; cameraMatrices . add ( P3 ) ; List < Point2D_F64 > triangObs = new ArrayList <> ( ) ; triangObs . add ( null ) ; triangObs . add ( null ) ; triangObs . add ( null ) ; structure = new SceneStructureProjective ( true ) ; structure . initialize ( 3 , listObs . size ( ) ) ; observations = new SceneObservations ( 3 ) ; structure . setView ( 0 , true , P1 , 0 , 0 ) ; structure . setView ( 1 , false , P2 , 0 , 0 ) ; structure . setView ( 2 , false , P3 , 0 , 0 ) ; boolean needsPruning = false ; Point4D_F64 X = new Point4D_F64 ( ) ; for ( int i = 0 ; i < listObs . size ( ) ; i ++ ) { AssociatedTriple t = listObs . get ( i ) ; triangObs . set ( 0 , t . p1 ) ; triangObs . set ( 1 , t . p2 ) ; triangObs . set ( 2 , t . p3 ) ; // triangulation can fail if all 3 views have the same pixel value. This has been observed in // simulated 3D scenes if ( triangulator . triangulate ( triangObs , cameraMatrices , X ) ) { observations . getView ( 0 ) . add ( i , ( float ) t . p1 . x , ( float ) t . p1 . y ) ; observations . getView ( 1 ) . add ( i , ( float ) t . p2 . x , ( float ) t . p2 . y ) ; observations . getView ( 2 ) . add ( i , ( float ) t . p3 . x , ( float ) t . p3 . y ) ; structure . points [ i ] . set ( X . x , X . y , X . z , X . w ) ; } else { needsPruning = true ; } } if ( needsPruning ) { PruneStructureFromSceneProjective pruner = new PruneStructureFromSceneProjective ( structure , observations ) ; pruner . prunePoints ( 1 ) ; } }
Sets up data structures for SBA