def test_retract(self): expected = gtsam.Cal3Unified(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1) K = gtsam.Cal3Unified(100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1) d = np.array([2, 3, 4, 5, 6, 7, 8, 9, 10, 1], order='F') actual = K.retract(d) self.gtsamAssertEquals(actual, expected) np.testing.assert_allclose(d, K.localCoordinates(actual))
def test_jacobian(self): """Evaluate jacobian at optical axis""" obj_point_on_axis = np.array([0, 0, 1]) img_point = np.array([0.0, 0.0]) pose = gtsam.Pose3() camera = gtsam.Cal3Unified() state = gtsam.Values() camera_key, pose_key, landmark_key = K(0), P(0), L(0) state.insert_cal3unified(camera_key, camera) state.insert_point3(landmark_key, obj_point_on_axis) state.insert_pose3(pose_key, pose) g = gtsam.NonlinearFactorGraph() noise_model = gtsam.noiseModel.Unit.Create(2) factor = gtsam.GeneralSFMFactor2Cal3Unified(img_point, noise_model, pose_key, landmark_key, camera_key) g.add(factor) f = g.error(state) gaussian_factor_graph = g.linearize(state) H, z = gaussian_factor_graph.jacobian() self.assertAlmostEqual(f, 0) self.gtsamAssertEquals(z, np.zeros(2)) self.gtsamAssertEquals(H @ H.T, 4 * np.eye(2)) Dcal = np.zeros((2, 10), order='F') Dp = np.zeros((2, 2), order='F') camera.calibrate(img_point, Dcal, Dp) self.gtsamAssertEquals( Dcal, np.array([[0., 0., 0., -1., 0., 0., 0., 0., 0., 0.], [0., 0., 0., 0., -1., 0., 0., 0., 0., 0.]])) self.gtsamAssertEquals(Dp, np.array([[1., -0.], [-0., 1.]]))
def test_sfm_factor2(self): """Evaluate residual with camera, pose and point as state variables""" r = np.linalg.norm(self.obj_point) graph = gtsam.NonlinearFactorGraph() state = gtsam.Values() measured = self.img_point noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1) camera_key, pose_key, landmark_key = K(0), P(0), L(0) k = self.stereographic state.insert_cal3unified(camera_key, k) state.insert_pose3(pose_key, gtsam.Pose3()) state.insert_point3(landmark_key, self.obj_point) factor = gtsam.GeneralSFMFactor2Cal3Unified(measured, noise_model, pose_key, landmark_key, camera_key) graph.add(factor) score = graph.error(state) self.assertAlmostEqual(score, 0)
def evaluate_jacobian(obj_point, img_point): """Evaluate jacobian at given object point""" pose = gtsam.Pose3() camera = gtsam.Cal3Fisheye() state = gtsam.Values() camera_key, pose_key, landmark_key = K(0), P(0), L(0) state.insert_point3(landmark_key, obj_point) state.insert_pose3(pose_key, pose) g = gtsam.NonlinearFactorGraph() noise_model = gtsam.noiseModel.Unit.Create(2) factor = gtsam.GenericProjectionFactorCal3Fisheye( img_point, noise_model, pose_key, landmark_key, camera) g.add(factor) f = g.error(state) gaussian_factor_graph = g.linearize(state) H, z = gaussian_factor_graph.jacobian() return f, z, H
def test_Cal3Unified(self): K = gtsam.Cal3Unified() self.assertEqual(K.fx(), 1.) self.assertEqual(K.fx(), 1.)
def test_Cal3Fisheye(self): K = gtsam.Cal3Fisheye() self.assertEqual(K.fx(), 1.) self.assertEqual(K.fy(), 1.)