def test_change_viewpoint(self): vp1 = np.array((1.0, 0.0, 0.0)) vp2 = np.array((2.0, 150.0, 0.0)) h = hyp.I3DHypothesis(forward_model=None, viewpoint=[vp1, vp2], params=None) for i in range(1000): hp, p0, p1 = change_viewpoint( h, {'CHANGE_VIEWPOINT_VARIANCE': np.square(5.0 / 180.0 * np.pi)}) self.assertAlmostEqual(p0, 1.0) self.assertAlmostEqual(p1, 1.0) vp = hp.viewpoint[0] self.assertAlmostEqual(vp[0], 1.0) # because the variance is 1.0, we would expect the angle between vectors to be small self.assertTrue( geom_3d.angle_between_vectors( geom_3d.spherical_to_cartesian(vp), geom_3d.spherical_to_cartesian(vp1)) < 20.0) vp = hp.viewpoint[1] self.assertAlmostEqual(vp[0], 2.0) self.assertTrue( geom_3d.angle_between_vectors( geom_3d.spherical_to_cartesian(vp), geom_3d.spherical_to_cartesian(vp2)) < 20.0)
def test_change_viewpoint(self): vp1 = np.array((1.0, 0.0, 0.0)) vp2 = np.array((2.0, 150.0, 0.0)) h = hyp.I3DHypothesis(forward_model=None, viewpoint=[vp1, vp2], params=None) for i in range(1000): hp, p0, p1 = change_viewpoint(h, {'CHANGE_VIEWPOINT_VARIANCE': np.square(5.0 / 180.0 * np.pi)}) self.assertAlmostEqual(p0, 1.0) self.assertAlmostEqual(p1, 1.0) vp = hp.viewpoint[0] self.assertAlmostEqual(vp[0], 1.0) # because the variance is 1.0, we would expect the angle between vectors to be small self.assertTrue(geom_3d.angle_between_vectors(geom_3d.spherical_to_cartesian(vp), geom_3d.spherical_to_cartesian(vp1)) < 20.0) vp = hp.viewpoint[1] self.assertAlmostEqual(vp[0], 2.0) self.assertTrue(geom_3d.angle_between_vectors(geom_3d.spherical_to_cartesian(vp), geom_3d.spherical_to_cartesian(vp2)) < 20.0)
def _get_joint_angle(self, joint_id): if joint_id < 1 or joint_id >= self.joint_count-1: raise ValueError('Joint id out out bounds.') return geom_3d.angle_between_vectors(self.joint_positions[joint_id-1] - self.joint_positions[joint_id], self.joint_positions[joint_id+1] - self.joint_positions[joint_id])