From ba98adc7196475d3453784d8eae77515d5ebb4dd Mon Sep 17 00:00:00 2001 From: Pau Gargallo Date: Thu, 1 Mar 2018 19:39:10 +0100 Subject: [PATCH] fix: keep vector_angle for single vector pairs. --- opensfm/matching.py | 4 ++-- opensfm/multiview.py | 22 +++++++++++++++++++--- 2 files changed, 21 insertions(+), 5 deletions(-) diff --git a/opensfm/matching.py b/opensfm/matching.py index f14804940..731c983aa 100644 --- a/opensfm/matching.py +++ b/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 diff --git a/opensfm/multiview.py b/opensfm/multiview.py index c3066a690..a1468b53f 100644 --- a/opensfm/multiview.py +++ b/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)