From 3d477f3a38e9cf1ce2ea22f93849e2e88673da57 Mon Sep 17 00:00:00 2001
From: Frank dellaert <dellaert@gmail.com>
Date: Thu, 20 Aug 2020 14:47:55 -0400
Subject: [PATCH] Fixed Point2 and Point3 to have similar behavior as in C++ -
 to also take vectors - to be initialized to Nan

---
 python/gtsam/__init__.py          | 11 +++++++++--
 python/gtsam/tests/test_Point2.py | 29 +++++++++++++++++++++++++++++
 python/gtsam/tests/test_Point3.py | 29 +++++++++++++++++++++++++++++
 3 files changed, 67 insertions(+), 2 deletions(-)
 create mode 100644 python/gtsam/tests/test_Point2.py
 create mode 100644 python/gtsam/tests/test_Point3.py

diff --git a/python/gtsam/__init__.py b/python/gtsam/__init__.py
index e6fd8c9c8..a828e836f 100644
--- a/python/gtsam/__init__.py
+++ b/python/gtsam/__init__.py
@@ -5,17 +5,24 @@ def _init():
     """This function is to add shims for the long-gone Point2 and Point3 types"""
 
     import numpy as np
+    import math
 
     global Point2  # export function
 
-    def Point2(x=0, y=0):
+    def Point2(x=math.nan, y=math.nan):
         """Shim for the deleted Point2 type."""
+        if isinstance(x, np.ndarray):
+            assert x.shape == (2,), "Point2 takes 2-vector"
+            return x  # "copy constructor"
         return np.array([x, y], dtype=float)
 
     global Point3  # export function
 
-    def Point3(x=0, y=0, z=0):
+    def Point3(x=math.nan, y=math.nan, z=math.nan):
         """Shim for the deleted Point3 type."""
+        if isinstance(x, np.ndarray):
+            assert x.shape == (3,), "Point3 takes 3-vector"
+            return x  # "copy constructor"
         return np.array([x, y, z], dtype=float)
 
     # for interactive debugging
diff --git a/python/gtsam/tests/test_Point2.py b/python/gtsam/tests/test_Point2.py
new file mode 100644
index 000000000..52ac92970
--- /dev/null
+++ b/python/gtsam/tests/test_Point2.py
@@ -0,0 +1,29 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Point2 unit tests.
+Author: Frank Dellaert & Fan Jiang
+"""
+import unittest
+
+import gtsam
+import numpy as np
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestPoint2(GtsamTestCase):
+    """Test selected Point2 methods."""
+
+    def test_constructors(self):
+        """Test constructors from doubles and vectors."""
+        expected = gtsam.Point2(1, 2)
+        actual = gtsam.Point2(np.array([1, 2]))
+        np.testing.assert_array_equal(actual, expected)
+
+
+if __name__ == "__main__":
+    unittest.main()
diff --git a/python/gtsam/tests/test_Point3.py b/python/gtsam/tests/test_Point3.py
new file mode 100644
index 000000000..f7a1c0f06
--- /dev/null
+++ b/python/gtsam/tests/test_Point3.py
@@ -0,0 +1,29 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Point3 unit tests.
+Author: Frank Dellaert & Fan Jiang
+"""
+import unittest
+
+import gtsam
+import numpy as np
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestPoint3(GtsamTestCase):
+    """Test selected Point3 methods."""
+
+    def test_constructors(self):
+        """Test constructors from doubles and vectors."""
+        expected = gtsam.Point3(1, 2, 3)
+        actual = gtsam.Point3(np.array([1, 2, 3]))
+        np.testing.assert_array_equal(actual, expected)
+
+
+if __name__ == "__main__":
+    unittest.main()