fix: keep vector_angle for single vector pairs.

Author: paulinus

opensfm/matching.py

@@ -122,8 +122,8 @@ def _compute_inliers_bearings(b1, b2, T, threshold=0.01):
     br2 = R.T.dot((p - t).T).T
     br2 /= np.linalg.norm(br2, axis=1)[:, np.newaxis]
 
-    ok1 = multiview.vector_angle(br1, b1) < threshold
-    ok2 = multiview.vector_angle(br2, b2) < threshold
+    ok1 = multiview.vector_angle_many(br1, b1) < threshold
+    ok2 = multiview.vector_angle_many(br2, b2) < threshold
     return ok1 * ok2
 
 

opensfm/multiview.py

@@ -105,13 +105,29 @@ def rq(A):
 
 
 def vector_angle(u, v):
-    '''
+    """Angle between two vectors.
+
+    >>> u = [ 0.99500417, -0.33333333, -0.09983342]
+    >>> v = [ 0.99500417, -0.33333333, -0.09983342]
+    >>> vector_angle(u, v)
+    0.0
+    """
+    cos = np.dot(u, v) / math.sqrt(np.dot(u, u) * np.dot(v, v))
+    if cos >= 1.0:
+        return 0.0
+    else:
+        return math.acos(cos)
+
+
+def vector_angle_many(u, v):
+    """Angles between to lists of vectors.
+
     >>> u = [[0.99500417, -0.33333333, -0.09983342], [0, -1, 0], [0, 1, 0]]
     >>> v = [[0.99500417, -0.33333333, -0.09983342], [0, +1, 0], [0, 0, 1]]
-    >>> angles = vector_angle(u, v)
+    >>> angles = vector_angle_many(u, v)
     >>> np.allclose(angles, [0., 3.1416, 1.5708])
     True
-    '''
+    """
     ua = np.array(u, dtype=np.float64, copy=False).reshape(-1, 3)
     va = np.array(v, dtype=np.float64, copy=False).reshape(-1, 3)
     return tf.angle_between_vectors(ua, va, axis=1)