private void triangulateFeatures ( List < AssociatedTriple > inliers , DMatrixRMaj P1 , DMatrixRMaj P2 , DMatrixRMaj P3 ) { List < DMatrixRMaj > cameraMatrices = new ArrayList <> ( ) ; cameraMatrices . add ( P1 ) ; cameraMatrices . add ( P2 ) ; cameraMatrices . add ( P3 ) ; // need elements to be non-empty so that it can use set(). probably over optimization List < Point2D_F64 > triangObs = new ArrayList <> ( ) ; triangObs . add ( null ) ; triangObs . add ( null ) ; triangObs . add ( null ) ; Point4D_F64 X = new Point4D_F64 ( ) ; for ( int i = 0 ; i < inliers . size ( ) ; i ++ ) { AssociatedTriple t = inliers . 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 ) ) { structure . points [ i ] . set ( X . x , X . y , X . z , X . w ) ; } else { throw new RuntimeException ( "Failed to triangulate a point in the inlier set?! Handle if this is common" ) ; } } }
Triangulates the location of each features in homogenous space