diff --git a/python/gtsam/tests/test_Utilities.py b/python/gtsam/tests/test_Utilities.py index ef53660bf..851684f12 100644 --- a/python/gtsam/tests/test_Utilities.py +++ b/python/gtsam/tests/test_Utilities.py @@ -103,6 +103,94 @@ class TestUtilites(GtsamTestCase): np.testing.assert_allclose(gtsam.utilities.extractPose3(initial), pose3.reshape(-1, 12)) + def test_perturbPoint2(self): + """Test perturbPoint2.""" + values = gtsam.Values() + values.insert(0, gtsam.Pose3()) + values.insert(1, gtsam.Point2(1, 1)) + gtsam.utilities.perturbPoint2(values, 1.0) + self.assertTrue( + not np.allclose(values.atPoint2(1), gtsam.Point2(1, 1))) + + def test_perturbPose2(self): + """Test perturbPose2.""" + values = gtsam.Values() + values.insert(0, gtsam.Pose2()) + values.insert(1, gtsam.Point2(1, 1)) + gtsam.utilities.perturbPose2(values, 1, 1) + self.assertTrue(values.atPose2(0) != gtsam.Pose2()) + + def test_perturbPoint3(self): + """Test perturbPoint3.""" + values = gtsam.Values() + point3 = gtsam.Point3(0, 0, 0) + values.insert(0, gtsam.Pose2()) + values.insert(1, point3) + gtsam.utilities.perturbPoint3(values, 1) + self.assertTrue(not np.allclose(values.atPoint3(1), point3)) + + def test_insertBackprojections(self): + """Test insertBackprojections.""" + values = gtsam.Values() + cam = gtsam.PinholeCameraCal3_S2() + gtsam.utilities.insertBackprojections( + values, cam, [0, 1, 2], np.asarray([[20, 30, 40], [20, 30, 40]]), + 10) + np.testing.assert_allclose(values.atPoint3(0), + gtsam.Point3(200, 200, 10)) + + def test_insertProjectionFactors(self): + """Test insertProjectionFactors.""" + graph = gtsam.NonlinearFactorGraph() + gtsam.utilities.insertProjectionFactors( + graph, 0, [0, 1], np.asarray([[20, 30], [20, 30]]), + gtsam.noiseModel.Isotropic.Sigma(2, 0.1), gtsam.Cal3_S2()) + self.assertEqual(graph.size(), 2) + + graph = gtsam.NonlinearFactorGraph() + gtsam.utilities.insertProjectionFactors( + graph, 0, [0, 1], np.asarray([[20, 30], [20, 30]]), + gtsam.noiseModel.Isotropic.Sigma(2, 0.1), gtsam.Cal3_S2(), + gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(1, 0, 0))) + self.assertEqual(graph.size(), 2) + + def test_reprojectionErrors(self): + """Test reprojectionErrors.""" + pixels = np.asarray([[20, 30], [20, 30]]) + I = [1, 2] + K = gtsam.Cal3_S2() + graph = gtsam.NonlinearFactorGraph() + gtsam.utilities.insertProjectionFactors( + graph, 0, I, pixels, gtsam.noiseModel.Isotropic.Sigma(2, 0.1), K) + values = gtsam.Values() + values.insert(0, gtsam.Pose3()) + cam = gtsam.PinholeCameraCal3_S2(gtsam.Pose3(), K) + gtsam.utilities.insertBackprojections(values, cam, I, pixels, 10) + errors = gtsam.utilities.reprojectionErrors(graph, values) + np.testing.assert_allclose(errors, np.zeros((2, 2))) + + def test_localToWorld(self): + """Test localToWorld.""" + local = gtsam.Values() + local.insert(0, gtsam.Point2(10, 10)) + local.insert(1, gtsam.Pose2(6, 11, 0.0)) + base = gtsam.Pose2(1, 0, 0) + world = gtsam.utilities.localToWorld(local, base) + + expected_point2 = gtsam.Point2(11, 10) + expected_pose2 = gtsam.Pose2(7, 11, 0) + np.testing.assert_allclose(world.atPoint2(0), expected_point2) + np.testing.assert_allclose( + world.atPose2(1).matrix(), expected_pose2.matrix()) + + user_keys = [1] + world = gtsam.utilities.localToWorld(local, base, user_keys) + np.testing.assert_allclose( + world.atPose2(1).matrix(), expected_pose2.matrix()) + + # Raise error since 0 is not in user_keys + self.assertRaises(RuntimeError, world.atPoint2, 0) + if __name__ == "__main__": unittest.main()