double triangulationAngle ( Point2D_F64 normA , Point2D_F64 normB , Se3_F64 a_to_b ) { // the more parallel a line is worse the triangulation. Get rid of bad ideas early here arrowA . set ( normA . x , normA . y , 1 ) ; arrowB . set ( normB . x , normB . y , 1 ) ; GeometryMath_F64 . mult ( a_to_b . R , arrowA , arrowA ) ; // put them into the same reference frame return UtilVector3D_F64 . acute ( arrowA , arrowB ) ; }
Computes the acture angle between two vectors . Larger this angle is the better the triangulation of the features 3D location is in general