test_ProjectionFactorRollingShutter.py
Go to the documentation of this file.
1 """
2 GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
4 All Rights Reserved
5 
6 See LICENSE for the license information
7 
8 ProjectionFactorRollingShutter unit tests.
9 Author: Yotam Stern
10 """
11 import unittest
12 
13 import numpy as np
14 
15 import gtsam
16 import gtsam_unstable
17 from gtsam.utils.test_case import GtsamTestCase
18 
19 
20 pose1 = gtsam.Pose3()
21 pose2 = gtsam.Pose3(np.array([[ 0.9999375 , 0.00502487, 0.00998725, 0.1 ],
22  [-0.00497488, 0.999975 , -0.00502487, 0.02 ],
23  [-0.01001225, 0.00497488, 0.9999375 , 1. ],
24  [ 0. , 0. , 0. , 1. ]]))
25 point = np.array([2, 0, 15])
26 point_noise = gtsam.noiseModel.Diagonal.Sigmas(np.ones(2))
28 body_p_sensor = gtsam.Pose3()
29 alpha = 0.1
30 measured = np.array([0.13257015, 0.0004157])
31 
32 
34 
35  def test_constructor(self):
36  '''
37  test constructor for the ProjectionFactorRollingShutter
38  '''
39  factor = gtsam_unstable.ProjectionFactorRollingShutter(measured, alpha, point_noise, 0, 1, 2, cal)
40  factor = gtsam_unstable.ProjectionFactorRollingShutter(measured, alpha, point_noise, 0, 1, 2, cal,
41  body_p_sensor)
42  factor = gtsam_unstable.ProjectionFactorRollingShutter(measured, alpha, point_noise, 0, 1, 2, cal, True, False)
43  factor = gtsam_unstable.ProjectionFactorRollingShutter(measured, alpha, point_noise, 0, 1, 2, cal, True, False,
44  body_p_sensor)
45 
46  def test_error(self):
47  '''
48  test the factor error for a specific example
49  '''
50  values = gtsam.Values()
51  values.insert(0, pose1)
52  values.insert(1, pose2)
53  values.insert(2, point)
54  factor = gtsam_unstable.ProjectionFactorRollingShutter(measured, alpha, point_noise, 0, 1, 2, cal)
55  self.gtsamAssertEquals(factor.error(values), np.array(0), tol=1e-9)
56 
57 
58 if __name__ == '__main__':
59  unittest.main()
def gtsamAssertEquals(self, actual, expected, tol=1e-9)
Definition: test_case.py:18
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
Definition: NoiseModel.cpp:270


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:37:46