Efficient PnP.

Summary:
Efficient PnP algorithm to fit 2D to 3D correspondences under perspective assumption.

Benchmarked both variants of nullspace and pick one; SVD takes 7 times longer in the 100K points case.

Reviewed By: davnov134, gkioxari

Differential Revision: D20095754

fbshipit-source-id: 2b4519729630e6373820880272f674829eaed073

tests/common_testing.py

@@ -1,12 +1,15 @@
 # Copyright (c) Facebook, Inc. and its affiliates. All rights reserved.
 
 import unittest
-from typing import Optional
+from typing import Callable, Optional, Union
 
 import numpy as np
 import torch
 
 
+TensorOrArray = Union[torch.Tensor, np.ndarray]
+
+
 class TestCaseMixin(unittest.TestCase):
     def assertSeparate(self, tensor1, tensor2) -> None:
         """
@@ -28,10 +31,11 @@ class TestCaseMixin(unittest.TestCase):
         ptrs = [i.storage().data_ptr() for i in tensor_list]
         self.assertCountEqual(ptrs, set(ptrs))
 
-    def assertClose(
+    def assertNormsClose(
         self,
-        input,
-        other,
+        input: TensorOrArray,
+        other: TensorOrArray,
+        norm_fn: Callable[[TensorOrArray], TensorOrArray],
         *,
         rtol: float = 1e-05,
         atol: float = 1e-08,
@@ -39,7 +43,60 @@ class TestCaseMixin(unittest.TestCase):
         msg: Optional[str] = None,
     ) -> None:
         """
-        Verify that two tensors or arrays are the same shape and close.
+        Verifies that two tensors or arrays have the same shape and are close
+            given absolute and relative tolerance; raises AssertionError otherwise.
+            A custom norm function is computed before comparison. If no such pre-
+            processing needed, pass `torch.abs` or, equivalently, call `assertClose`.
+        Args:
+            input, other: two tensors or two arrays.
+            norm_fn: The function evaluates
+                `all(norm_fn(input - other) <= atol + rtol * norm_fn(other))`.
+                norm_fn is a tensor -> tensor function; the output has:
+                    * all entries non-negative,
+                    * shape defined by the input shape only.
+            rtol, atol, equal_nan: as for torch.allclose.
+            msg: message in case the assertion is violated.
+        Note:
+            Optional arguments here are all keyword-only, to avoid confusion
+            with msg arguments on other assert functions.
+        """
+
+        self.assertEqual(np.shape(input), np.shape(other))
+
+        diff = norm_fn(input - other)
+        other_ = norm_fn(other)
+
+        # We want to generalise allclose(input, output), which is essentially
+        #  all(diff <= atol + rtol * other)
+        # but with a sophisticated handling non-finite values.
+        # We work that around by calling allclose() with the following arguments:
+        # allclose(diff + other_, other_). This computes what we want because
+        #  all(|diff + other_ - other_| <= atol + rtol * |other_|) ==
+        #    all(|norm_fn(input - other)| <= atol + rtol * |norm_fn(other)|) ==
+        #    all(norm_fn(input - other) <= atol + rtol * norm_fn(other)).
+
+        backend = torch if torch.is_tensor(input) else np
+        close = backend.allclose(
+            diff + other_, other_, rtol=rtol, atol=atol, equal_nan=equal_nan
+        )
+
+        self.assertTrue(close, msg)
+
+    def assertClose(
+        self,
+        input: TensorOrArray,
+        other: TensorOrArray,
+        *,
+        rtol: float = 1e-05,
+        atol: float = 1e-08,
+        equal_nan: bool = False,
+        msg: Optional[str] = None,
+    ) -> None:
+        """
+        Verifies that two tensors or arrays have the same shape and are close
+            given absolute and relative tolerance, i.e. checks
+            `all(|input - other| <= atol + rtol * |other|)`;
+            raises AssertionError otherwise.
         Args:
             input, other: two tensors or two arrays.
             rtol, atol, equal_nan: as for torch.allclose.
@@ -51,10 +108,9 @@ class TestCaseMixin(unittest.TestCase):
 
         self.assertEqual(np.shape(input), np.shape(other))
 
-        if torch.is_tensor(input):
-            close = torch.allclose(
-                input, other, rtol=rtol, atol=atol, equal_nan=equal_nan
-            )
-        else:
-            close = np.allclose(input, other, rtol=rtol, atol=atol, equal_nan=equal_nan)
+        backend = torch if torch.is_tensor(input) else np
+        close = backend.allclose(
+            input, other, rtol=rtol, atol=atol, equal_nan=equal_nan
+        )
+
         self.assertTrue(close, msg)