void decomposeEssential ( Motion motion ) { List < Se3_F64 > candidates = MultiViewOps . decomposeEssential ( motion . F ) ; int bestScore = 0 ; Se3_F64 best = null ; PositiveDepthConstraintCheck check = new PositiveDepthConstraintCheck ( ) ; for ( int i = 0 ; i < candidates . size ( ) ; i ++ ) { Se3_F64 a_to_b = candidates . get ( i ) ; int count = 0 ; for ( int j = 0 ; j < motion . associated . size ( ) ; j ++ ) { AssociatedIndex a = motion . associated . get ( j ) ; Point2D_F64 p0 = motion . viewSrc . observationNorm . get ( a . src ) ; Point2D_F64 p1 = motion . viewDst . observationNorm . get ( a . dst ) ; if ( check . checkConstraint ( p0 , p1 , a_to_b ) ) { count ++ ; } } if ( count > bestScore ) { bestScore = count ; best = a_to_b ; } } if ( best == null ) throw new RuntimeException ( "Problem!" ) ; motion . a_to_b . set ( best ) ; }
Sets the a_to_b transform for the motion given .