diff --git a/.gitignore b/.gitignore index e6e38132f..0e34eed34 100644 --- a/.gitignore +++ b/.gitignore @@ -17,3 +17,4 @@ # for QtCreator: CMakeLists.txt.user* xcode/ +/Dockerfile diff --git a/python/gtsam_unstable/tests/test_ProjectionFactorRollingShutter.py b/python/gtsam_unstable/tests/test_ProjectionFactorRollingShutter.py new file mode 100644 index 000000000..23a9d0831 --- /dev/null +++ b/python/gtsam_unstable/tests/test_ProjectionFactorRollingShutter.py @@ -0,0 +1,59 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +ProjectionFactorRollingShutter unit tests. +Author: Yotam Stern +""" +import unittest + +import numpy as np + +import gtsam +import gtsam_unstable +from gtsam.utils.test_case import GtsamTestCase + + +pose1 = gtsam.Pose3() +pose2 = gtsam.Pose3(np.array([[ 0.9999375 , 0.00502487, 0.00998725, 0.1 ], + [-0.00497488, 0.999975 , -0.00502487, 0.02 ], + [-0.01001225, 0.00497488, 0.9999375 , 1. ], + [ 0. , 0. , 0. , 1. ]])) +point = np.array([2, 0, 15]) +point_noise = gtsam.noiseModel.Diagonal.Sigmas(np.ones(2)) +cal = gtsam.Cal3_S2() +body_p_sensor = gtsam.Pose3() +alpha = 0.1 +measured = array([0.13257015, 0.0004157]) + + +class TestProjectionFactorRollingShutter(GtsamTestCase): + + def test_constructor(self): + ''' + test constructor for the ProjectionFactorRollingShutter + ''' + factor = gtsam_unstable.ProjectionFactorRollingShutter(measured, alpha, point_noise, 0, 1, 2, cal) + factor = gtsam_unstable.ProjectionFactorRollingShutter(measured, alpha, point_noise, 0, 1, 2, cal, + body_p_sensor) + factor = gtsam_unstable.ProjectionFactorRollingShutter(measured, alpha, point_noise, 0, 1, 2, cal, True, False) + factor = gtsam_unstable.ProjectionFactorRollingShutter(measured, alpha, point_noise, 0, 1, 2, cal, True, False, + body_p_sensor) + + def test_error(self): + ''' + test the factor error for a specific example + ''' + values = gtsam.Values() + values.insert(0, pose1) + values.insert(1, pose2) + values.insert(2, point) + factor = gtsam_unstable.ProjectionFactorRollingShutter(measured, alpha, point_noise, 0, 1, 2, cal) + self.assertEqual(factor.error(values), 0) + + +if __name__ == '__main__': + unittest.main()