protected int addConicPairConstraints ( AssociatedPairConic a , AssociatedPairConic b , DMatrixRMaj A , int rowA ) { // s*C[i] = H^T*V[i]*H // C[i] = a, C[j] = b // Conic in view 1 is C and view 2 is V, e.g. x' = H*x. x' is in view 2 and x in view 1 UtilCurves_F64 . convert ( a . p1 , C1 ) ; UtilCurves_F64 . convert ( a . p2 , V1 ) ; CommonOps_DDF3 . invert ( C1 , C1_inv ) ; CommonOps_DDF3 . invert ( V1 , V1_inv ) ; UtilCurves_F64 . convert ( b . p1 , C2 ) ; UtilCurves_F64 . convert ( b . p2 , V2 ) ; CommonOps_DDF3 . invert ( C2 , C2_inv ) ; CommonOps_DDF3 . invert ( V2 , V2_inv ) ; // L = inv(V[i])*V[j] CommonOps_DDF3 . mult ( V1_inv , V2 , L ) ; // R = C[i]*inv(C[j]) CommonOps_DDF3 . mult ( C1_inv , C2 , R ) ; // clear this row int idxA = rowA * 9 ; // Arrays.fill(A.data,idxA,9*9,0); <-- has already been zeroed // NOTE: adding all 9 rows is redundant. The source paper doesn't attempt to reduce the number of rows // maybe this can be made to run faster if the rows can be intelligently pruned // inv(V[i])*V[j]*H - H*C[i]*inv(C[j]) == 0 for ( int row = 0 ; row < 3 ; row ++ ) { for ( int col = 0 ; col < 3 ; col ++ ) { for ( int i = 0 ; i < 3 ; i ++ ) { A . data [ idxA + 3 * i + col ] += L . get ( row , i ) ; A . data [ idxA + 3 * row + i ] -= R . get ( i , col ) ; } idxA += 9 ; } } return rowA + 9 ; }
Add constraint for a pair of conics