"""Synthetic checks of geometry-only rejection and registration conventions.""" import io import unittest from unittest.mock import patch import numpy as np from scipy.spatial.transform import Rotation from g1_registration_cost import geometry_only, icp, sample_surface, starting_poses class RegistrationChecks(unittest.TestCase): def test_nondegenerate_starts_split_four_and_four(self): x=np.vstack([np.diag([4.,2.,1.]),-np.diag([4.,2.,1.])]) branches,meta=starting_poses(x,x) self.assertEqual([len(branches[1]),len(branches[-1])],[4,4]) self.assertEqual(meta['added_rotation_axes'],[]) def test_near_degenerate_plane_adds_both_parities(self): x=np.vstack([np.diag([2.,2.,1.]),-np.diag([2.,2.,1.])]) branches,meta=starting_poses(x,x) self.assertEqual(meta['added_rotation_axes'],[2]) for parity,poses in branches.items(): self.assertGreaterEqual(len(poses),12) for pose in poses:self.assertAlmostEqual(np.linalg.det(pose['rotation']),parity) def test_label_association_rejected_before_decode(self): xml=b'poison' with patch('g1_registration_cost.decode_array') as decode: with self.assertRaisesRegex(ValueError,'Label association'): geometry_only(io.BytesIO(xml)) decode.assert_not_called() def test_non_geometry_array_rejected(self): xml=b'poison' with patch('g1_registration_cost.decode_array') as decode: with self.assertRaisesRegex(ValueError,'Non-geometry'): geometry_only(io.BytesIO(xml)) decode.assert_not_called() def test_icp_recovers_small_rigid_motion(self): rng=np.random.default_rng(12) target=rng.uniform(-1,1,(600,3)) axis=np.arange(600)%3;sign=np.where(np.arange(600)%2,1.,-1.) target[np.arange(600),axis]=sign normals=np.zeros_like(target);normals[np.arange(600),axis]=sign r=Rotation.from_rotvec([.01,-.015,.008]).as_matrix();t=np.array([.006,-.004,.005]) source=(target-t)@r result=icp(source,target,normals,np.eye(3),np.zeros(3)) predicted=source@np.asarray(result['rotation']).T+result['translation'] self.assertLess(np.max(np.abs(predicted-target)),1e-8) self.assertEqual(result['min_rank'],6) def test_reflection_is_preserved_and_reported(self): rng=np.random.default_rng(4);target=rng.normal(size=(90,3));normals=target/np.linalg.norm(target,axis=1)[:,None] mirror=np.diag([-1.,1.,1.]);source=target@mirror result=icp(source,target,normals,mirror,np.zeros(3)) self.assertLess(result['point_plane_mse'],1e-20) self.assertAlmostEqual(result['determinant'],-1.) def test_area_pps_samples_stay_on_triangles_and_are_seeded(self): g={'points':np.array([[0.,0,0],[2.,0,0],[0,1.,0]]),'boundary':np.array([[0,1,2]])} a,n=sample_surface(g,73);b,_=sample_surface(g,73) np.testing.assert_array_equal(a,b) self.assertTrue(np.all(a[:,0]/2+a[:,1]<=1+1e-14)) self.assertTrue(np.all(a[:,:2]>=0)) np.testing.assert_allclose(n,np.tile([0,0,1],(2000,1))) if __name__=='__main__':unittest.main(verbosity=2)