diff --git a/cython/gtsam/tests/test_Pose3.py b/cython/gtsam/tests/test_Pose3.py new file mode 100644 index 000000000..8fa50b90c --- /dev/null +++ b/cython/gtsam/tests/test_Pose3.py @@ -0,0 +1,44 @@ +import math +import unittest + +from gtsam import Point3, Rot3, Pose3 + + +class TestPose3(unittest.TestCase): + + def test__between(self): + T2 = Pose3(Rot3.Rodrigues(0.3,0.2,0.1),Point3(3.5,-8.2,4.2)) + T3 = Pose3(Rot3.Rodrigues(-90, 0, 0), Point3(1, 2, 3)) + expected = T2.inverse().compose(T3) + actual = T2.between(T3) + self.assertTrue(actual.equals(expected, 1e-6)) + + def test_transform_to(self): + transform = Pose3(Rot3.Rodrigues(0,0,-1.570796), Point3(2,4, 0)) + actual = transform.transform_to(Point3(3,2,10)) + expected = Point3 (2,1,10) + self.assertTrue(actual.equals(expected, 1e-6)) + + def test_range(self): + l1 = Point3(1, 0, 0) + l2 = Point3(1, 1, 0) + x1 = Pose3() + + xl1 = Pose3(Rot3.Ypr(0.0, 0.0, 0.0), Point3(1, 0, 0)) + xl2 = Pose3(Rot3.Ypr(0.0, 1.0, 0.0), Point3(1, 1, 0)) + + # establish range is indeed zero + self.assertEqual(1,x1.range(point=l1)) + + # establish range is indeed sqrt2 + self.assertEqual(math.sqrt(2.0),x1.range(point=l2)) + + # establish range is indeed zero + self.assertEqual(1,x1.range(pose=xl1)) + + # establish range is indeed sqrt2 + self.assertEqual(math.sqrt(2.0),x1.range(pose=xl2)) + + +if __name__ == "__main__": + unittest.main() diff --git a/cython/gtsam/tests/test_SimpleCamera.py b/cython/gtsam/tests/test_SimpleCamera.py index d6a095d23..7924a9b1c 100644 --- a/cython/gtsam/tests/test_SimpleCamera.py +++ b/cython/gtsam/tests/test_SimpleCamera.py @@ -6,7 +6,7 @@ from gtsam import Pose2, Point3, Rot3, Pose3, Cal3_S2, SimpleCamera K = Cal3_S2(625, 625, 0, 0, 0) -class TestCal3Unified(unittest.TestCase): +class TestSimpleCamera(unittest.TestCase): def test_constructor(self): pose1 = Pose3(Rot3(np.diag([1, -1, -1])), Point3(0, 0, 0.5)) diff --git a/gtsam.h b/gtsam.h index 282fafdc5..4bbca9896 100644 --- a/gtsam.h +++ b/gtsam.h @@ -496,6 +496,7 @@ class Rot3 { static gtsam::Rot3 Ypr(double y, double p, double r); static gtsam::Rot3 Quaternion(double w, double x, double y, double z); static gtsam::Rot3 Rodrigues(Vector v); + static gtsam::Rot3 Rodrigues(double wx, double wy, double wz); // Testable void print(string s) const;