void defineCoordinateSystem ( View viewA , Motion motion ) { View viewB = motion . destination ( viewA ) ; viewA . viewToWorld . reset ( ) ; // identity since it's the origin viewB . viewToWorld . set ( motion . motionSrcToDst ( viewB ) ) ; // translation is only known up to a scale factor so pick a reasonable scale factor double scale = viewB . viewToWorld . T . norm ( ) ; viewB . viewToWorld . T . scale ( 1.0 / scale ) ; viewsAdded . add ( viewA ) ; viewsAdded . add ( viewB ) ; viewA . state = ViewState . PROCESSED ; viewB . state = ViewState . PROCESSED ; // Take the already triangulated points and turn them into official 3D features boolean originIsDst = viewA == motion . viewDst ; for ( int i = 0 ; i < motion . stereoTriangulations . size ( ) ; i ++ ) { Feature3D f = motion . stereoTriangulations . get ( i ) ; if ( f . obsIdx . size != 2 ) throw new RuntimeException ( "BUG" ) ; int indexSrc = f . obsIdx . get ( 0 ) ; int indexDst = f . obsIdx . get ( 1 ) ; motion . viewSrc . features3D [ indexSrc ] = f ; motion . viewDst . features3D [ indexDst ] = f ; if ( originIsDst ) { SePointOps_F64 . transform ( motion . a_to_b , f . worldPt , f . worldPt ) ; } f . worldPt . scale ( 1.0 / scale ) ; graph . features3D . add ( f ) ; } // free memory and mark as already processed motion . stereoTriangulations = new ArrayList <> ( ) ; // All features which can be added using triangulation should now be added addTriangulatedFeaturesForAllEdges ( viewA ) ; addTriangulatedFeaturesForAllEdges ( viewB ) ; if ( verbose != null ) { verbose . println ( "root = " + viewA . index ) ; verbose . println ( "other = " + viewB . index ) ; verbose . println ( "-------------" ) ; } }
Sets the origin and scale of the coordinate system