void addTriangulatedStereoFeatures ( View base , Motion edge , double scale ) { View viewA = edge . viewSrc ; View viewB = edge . viewDst ; boolean baseIsA = base == viewA ; View other = baseIsA ? viewB : viewA ; // Determine transform from other to world edge . a_to_b . T . scale ( scale ) ; Se3_F64 otherToBase = baseIsA ? edge . a_to_b . invert ( null ) : edge . a_to_b . copy ( ) ; otherToBase . concat ( base . viewToWorld , other . viewToWorld ) ; // Convert already computed stereo 3D features and turn them into real features for ( int i = 0 ; i < edge . stereoTriangulations . size ( ) ; i ++ ) { Feature3D edge3D = edge . stereoTriangulations . get ( i ) ; int indexSrc = edge3D . obsIdx . get ( 0 ) ; int indexDst = edge3D . obsIdx . get ( 1 ) ; Feature3D world3D = baseIsA ? viewA . features3D [ indexSrc ] : viewB . features3D [ indexDst ] ; // find the 3D location of the point in world frame edge3D . worldPt . scale ( scale ) ; if ( baseIsA ) { viewA . viewToWorld . transform ( edge3D . worldPt , edge3D . worldPt ) ; } else { edge . a_to_b . transform ( edge3D . worldPt , edge3D . worldPt ) ; viewB . viewToWorld . transform ( edge3D . worldPt , edge3D . worldPt ) ; } // See if the feature is already known if ( world3D != null ) { // Add the other view if another feature in the other view was not already associated with this feature if ( ! world3D . views . contains ( other ) ) { world3D . views . add ( other ) ; world3D . obsIdx . add ( baseIsA ? indexDst : indexSrc ) ; } // Retriangulate the point if it appears that this stereo pair is better than the one which originally // computed it if ( world3D . triangulationAngle >= edge3D . triangulationAngle ) { continue ; } world3D . worldPt . set ( edge3D . worldPt ) ; world3D . triangulationAngle = edge3D . triangulationAngle ; other . features3D [ baseIsA ? indexDst : indexSrc ] = edge3D ; } else { graph . features3D . add ( edge3D ) ; viewA . features3D [ indexSrc ] = edge3D ; viewB . features3D [ indexDst ] = edge3D ; } } // free memory edge . stereoTriangulations = new ArrayList <> ( ) ; }
Adds features which were triangulated using the stereo pair after the scale factor has been determined . Don t mark the other view as being processed . It s 3D pose will be estimated later on using PNP with the new features and features determined later on