60 lines
2.1 KiB
Python
60 lines
2.1 KiB
Python
"""
|
|
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 = np.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.gtsamAssertEquals(factor.error(values), np.array(0), tol=1e-9)
|
|
|
|
|
|
if __name__ == '__main__':
|
|
unittest.main()
|