Merge pull request #69 from vaughantnrc/dev-tv-registration-fix

BUG: Incorrect comparison direction in register_corresponding_points

Author: Rampex1

src/pose_solver/util/register_corresponding_points.py

@@ -19,7 +19,7 @@ def register_corresponding_points(
     """
     if len(point_set_from) != len(point_set_to):
         raise ValueError("Input point sets must be of identical length.")
-    if len(point_set_from) > 3:
+    if len(point_set_from) < 3:
         raise ValueError("Input point sets must be of length 3 or higher.")
     if collinearity_do_check:
         for point_set in (point_set_from, point_set_to):

test/test_register_corresponding_points.py

@@ -22,7 +22,7 @@ def assertRotationCloseToIdentity(
         self.assertAlmostEqual(float(matrix[2, 1]), 0.0, delta=tolerance)
         self.assertAlmostEqual(float(matrix[2, 2]), 1.0, delta=tolerance)
 
-    def test_identity(self):
+    def test_identity_3_points(self):
         point_set_from = [
             [0.0, 0.0, 0.0],
             [1.0, 0.0, 0.0],
@@ -38,6 +38,23 @@ def test_identity(self):
         self.assertEqual(matrix[3, 1], 0.0)
         self.assertEqual(matrix[3, 2], 0.0)
 
+    def test_identity_4_points(self):
+        point_set_from = [
+            [0.0, 0.0, 0.0],
+            [1.0, 0.0, 0.0],
+            [0.0, 1.0, 0.0],
+            [1.0, 1.0, 0.0]]
+        point_set_to = point_set_from
+        matrix = register_corresponding_points(point_set_from, point_set_to)
+        self.assertRotationCloseToIdentity(matrix)
+        self.assertAlmostEqual(matrix[0, 3], 0.0)
+        self.assertAlmostEqual(matrix[1, 3], 0.0)
+        self.assertAlmostEqual(matrix[2, 3], 0.0)
+        self.assertEqual(matrix[3, 3], 1.0)
+        self.assertEqual(matrix[3, 0], 0.0)
+        self.assertEqual(matrix[3, 1], 0.0)
+        self.assertEqual(matrix[3, 2], 0.0)
+
     def test_translation(self):
         point_set_from = [
             [0.0, 0.0, 0.0],