diff --git a/sksurgerysurfacematch/algorithms/goicp_registration.py b/sksurgerysurfacematch/algorithms/goicp_registration.py index 994e455..95df19d 100644 --- a/sksurgerysurfacematch/algorithms/goicp_registration.py +++ b/sksurgerysurfacematch/algorithms/goicp_registration.py @@ -30,19 +30,20 @@ def demean_and_normalise(points_a: np.ndarray, """ a_demean = points_a - np.mean(points_a, axis=0) b_demean = points_b - np.mean(points_b, axis=0) - - norm_factor = np.max(np.max(a_demean), np.max(b_demean)) + + norm_factor = np.max([np.max(np.abs(a_demean)), + np.max(np.abs(b_demean))]) a_normalised = a_demean / norm_factor - b_normalised = a_demean / norm_factor + b_normalised = b_demean / norm_factor return a_normalised, b_normalised -class GoICPRegistration(rr.RigidRegistration): +class RigidRegistration(rr.RigidRegistration): """ Class that uses GoICP implementation to register fixed/moving clouds. At the moment, we are just relying on all default parameters. - :param dt_size: GoICP distance transform size + :param dt_size: Nodes per dimension of distance transform :param dt_factor: GoICP distance transform factor """ @@ -54,17 +55,20 @@ def __init__(self, dt_size=300, dt_factor=2.0): def register(self, fixed_cloud: np.ndarray, moving_cloud: np.ndarray, + normalise=True ): """ Uses GoICP library, wrapped in scikit-surgerygoicp. :param fixed_cloud: [Nx3] fixed point cloud. :param moving_cloud: [Mx3] moving point cloud. + :normalise: If true, data will be centred around 0 and normalsied. :return: [4x4] transformation matrix, moving-to-fixed space. """ - fixed_cloud, moving_cloud = \ - demean_and_normalise(fixed_cloud, moving_cloud) + if normalise: + fixed_cloud, moving_cloud = \ + demean_and_normalise(fixed_cloud, moving_cloud) Nm, a_points = numpy_to_POINT3D_array(moving_cloud) Nd, b_points = numpy_to_POINT3D_array(fixed_cloud) diff --git a/tests/data/icp/data_rand.txt b/tests/data/icp/data_rand.txt new file mode 100644 index 0000000..2f84b8e --- /dev/null +++ b/tests/data/icp/data_rand.txt @@ -0,0 +1,100 @@ +-0.390945 -0.253648 -0.693646 +-0.209112 -0.788946 0.806259 +-0.244454 -0.630994 0.222520 +-0.018572 -0.013369 -0.968680 +-0.795680 0.213003 0.469272 +-0.129410 -0.758468 -0.841533 +-0.874140 0.376690 0.073802 +0.972439 0.805720 0.540446 +0.639408 0.517936 -0.085539 +0.041984 -0.567641 0.525445 +-0.627868 -0.626682 -0.682663 +-0.390540 -0.910843 0.614556 +-0.093783 0.383537 0.689436 +-0.340680 -0.130823 -0.818663 +0.585048 0.368079 0.830095 +-0.524509 -0.227004 -0.694724 +0.805901 0.617765 0.547100 +-0.014319 0.920549 -0.620875 +-0.948211 -0.705225 -0.308715 +0.598218 -0.846279 -0.971523 +0.866325 -0.425606 -0.320275 +0.878305 -0.543165 0.731280 +-0.551634 0.122239 -0.418976 +-0.809045 -0.222236 -0.835483 +-0.669562 -0.568575 0.902713 +0.640050 -0.124511 0.619277 +0.004342 -0.336516 0.701013 +0.193722 -0.674728 -0.456951 +-0.551709 -0.026097 0.762519 +0.220608 0.452412 0.698366 +-0.984362 -0.830217 -0.733272 +0.275721 -0.381809 0.669618 +0.893837 -0.663012 -0.430258 +0.929187 0.540413 0.857103 +-0.721998 0.609023 0.199136 +0.082503 -0.118703 -0.543592 +-0.500554 -0.347211 0.659898 +0.919482 0.094603 0.864206 +-0.336649 0.410775 -0.994273 +0.032443 0.941984 -0.214181 +-0.328830 0.445457 0.371844 +0.325111 0.235720 -0.881337 +-0.460650 -0.230771 -0.525362 +-0.930396 0.274761 -0.467282 +-0.388081 0.830520 0.556153 +-0.569780 0.964451 -0.647045 +-0.503215 0.297412 -0.720650 +-0.435641 0.552125 0.155869 +-0.341545 0.034978 0.636255 +0.263576 -0.400496 -0.873924 +-0.432517 -0.689693 0.525743 +0.097252 0.257235 0.200841 +-0.659993 -0.858351 -0.422616 +0.553212 -0.617077 0.950776 +-0.666821 0.929519 0.154664 +-0.155741 -0.279532 -0.726886 +-0.541045 -0.541962 0.957706 +0.222851 0.459064 0.493044 +0.025348 0.623475 0.178610 +-0.759422 0.186110 -0.718520 +-0.688227 0.173035 0.649013 +-0.216700 0.694449 0.542953 +-0.972530 -0.154074 0.896830 +-0.865882 -0.708373 0.356959 +-0.741257 0.202092 0.727488 +0.964848 -0.359001 0.269442 +0.652869 0.249336 -0.199819 +-0.974141 -0.180387 0.625854 +-0.427451 -0.971566 0.629372 +0.911110 0.797895 -0.478867 +-0.865690 0.675552 0.929710 +0.366356 0.734283 0.529405 +0.387733 0.478012 0.031460 +-0.933848 0.510250 -0.630657 +-0.441093 0.965525 -0.582941 +0.219504 -0.735712 -0.541959 +-0.387027 0.525647 -0.816564 +-0.418526 0.667827 0.209249 +-0.504858 0.628997 -0.337728 +-0.234295 0.857608 -0.370971 +-0.126532 -0.250294 -0.417554 +-0.095219 -0.958002 0.881987 +-0.606688 0.059193 0.489544 +0.206382 0.132974 0.104455 +-0.975392 0.750054 0.938282 +0.498338 0.585213 0.263634 +0.450925 0.285074 0.861187 +-0.571254 -0.097008 0.039436 +-1.010648 -0.655652 0.887168 +-0.830783 0.554990 0.148806 +-0.169736 -0.961859 0.291496 +0.772313 0.524162 -0.166129 +0.001199 0.259918 -0.742469 +0.281014 0.447398 -0.913083 +-0.619791 -0.259713 0.763154 +-0.192184 -0.902064 -0.207704 +-0.064109 -0.357203 0.952066 +0.876370 0.603648 -0.897100 +-0.824621 -0.587793 -0.818184 +0.535244 -0.763384 -0.909095 diff --git a/tests/data/icp/model_rand.txt b/tests/data/icp/model_rand.txt new file mode 100644 index 0000000..199f5e0 --- /dev/null +++ b/tests/data/icp/model_rand.txt @@ -0,0 +1,101 @@ +0.891472 0.164805 -0.955314 +0.809038 -0.917668 0.198116 +0.900167 -0.474560 -0.255627 +0.852279 0.679822 -0.897447 +-0.061554 -0.674927 -0.567396 +1.462851 0.214494 -0.813023 +-0.044407 -0.394357 -0.903964 +-0.293255 0.621115 0.731754 +0.123039 0.726665 0.099793 +0.809097 -0.491692 0.203452 +1.192926 -0.131155 -1.090970 +0.993202 -0.951571 -0.044605 +-0.168530 -0.315542 0.074971 +0.847547 0.344356 -1.046240 +-0.081948 0.052107 0.670425 +0.841575 0.114205 -1.096014 +-0.161038 0.474140 0.618961 +-0.119751 0.820865 -0.786561 +1.038907 -0.611916 -1.096341 +1.716085 0.731287 -0.342688 +1.124265 0.645839 0.251174 +0.832298 -0.081293 0.955536 +0.442408 0.017335 -0.967022 +0.847352 0.000271 -1.385553 +0.535181 -1.178774 -0.109779 +0.467348 -0.010067 0.632341 +0.518452 -0.555805 0.259907 +1.275175 0.213775 -0.329232 +0.130129 -0.783844 -0.176465 +-0.166791 -0.070545 0.301661 +1.316648 -0.412634 -1.353849 +0.606552 -0.367068 0.423828 +1.368911 0.624917 0.245115 +-0.193887 0.292195 0.933718 +-0.279933 -0.279192 -0.765136 +0.804966 0.429169 -0.534185 +0.450879 -0.835961 -0.162066 +0.212349 0.107564 0.997032 +0.417951 0.674364 -1.190164 +-0.312481 0.589021 -0.479724 +-0.121688 -0.214579 -0.306769 +0.637287 0.953640 -0.618865 +0.821740 0.027142 -0.919133 +0.250653 -0.094283 -1.277081 +-0.570891 -0.202971 -0.293967 +-0.259160 0.499580 -1.222150 +0.398977 0.314432 -1.119246 +-0.200709 -0.095603 -0.543558 +0.129142 -0.582392 -0.110384 +1.234782 0.634074 -0.586755 +0.823509 -0.811335 -0.138905 +0.187958 0.080826 -0.093637 +1.285037 -0.420406 -0.900434 +0.801948 -0.450514 0.813595 +-0.531444 -0.093548 -0.763167 +0.995463 0.323762 -0.803779 +0.470887 -1.138835 0.023181 +-0.107263 0.081053 0.174508 +-0.174405 0.200542 -0.198951 +0.459629 0.113366 -1.337517 +-0.033295 -0.742887 -0.364665 +-0.420594 -0.171931 -0.151170 +0.110108 -1.208862 -0.362297 +0.836565 -1.024992 -0.607595 +-0.118826 -0.805765 -0.353701 +0.869614 0.368124 0.684688 +0.440130 0.712192 0.070918 +0.240290 -1.042213 -0.560852 +1.016407 -1.014421 -0.052200 +0.097433 1.277945 0.045402 +-0.636162 -0.838280 -0.410838 +-0.321169 0.234910 0.282842 +0.101430 0.469194 0.014662 +0.117664 0.077075 -1.464952 +-0.268332 0.549733 -1.102880 +1.376281 0.270486 -0.345013 +0.220909 0.562012 -1.116908 +-0.286204 -0.055884 -0.507191 +-0.055982 0.210019 -0.940920 +-0.203870 0.496085 -0.788243 +0.849596 0.158882 -0.610943 +1.012071 -0.950827 0.381258 +0.130640 -0.615347 -0.396846 +0.365915 0.149899 -0.062392 +-0.723044 -0.875811 -0.455569 +-0.065262 0.430453 0.224190 +-0.040891 -0.102308 0.587432 +0.444330 -0.388311 -0.649969 +0.564429 -1.393917 -0.365897 +-0.224104 -0.341029 -0.857896 +1.226559 -0.624932 -0.068242 +0.196959 0.847497 0.144423 +0.506330 0.664479 -0.749120 +0.444183 1.012243 -0.694046 +0.308319 -0.937891 -0.168803 +1.318222 -0.288644 -0.442094 +0.440013 -0.749676 0.372678 +0.402528 1.469447 -0.241091 +1.163921 -0.152044 -1.331625 +1.605365 0.700907 -0.349552 + diff --git a/tests/test_icp_goicp.py b/tests/test_icp_goicp.py new file mode 100644 index 0000000..cf61488 --- /dev/null +++ b/tests/test_icp_goicp.py @@ -0,0 +1,39 @@ +import pytest +import numpy as np + +model_data = np.loadtxt('tests/data/icp/model_rand.txt') +target_data = np.loadtxt('tests/data/icp/data_rand.txt') + +import sksurgerysurfacematch.algorithms.pcl_icp_registration as pir +import sksurgerysurfacematch.algorithms.goicp_registration as goicp + +def test_icp_reg(): + icp_reg = pir.RigidRegistration() + residual, transform = icp_reg.register(model_data, target_data) + + assert residual < 0.1 + + +def test_goicp_reg(): + goicp_reg = goicp.RigidRegistration() + residual, transform = goicp_reg.register(model_data, target_data) + + print(residual, transform) + assert residual < 0.1 + +def test_normalise(): + + # Create data between 0 and 2 + points_a = 2 * np.random.rand(100,3) + points_b = 2 * np.random.rand(100,3) + + norm_a, norm_b = goicp.demean_and_normalise(points_a, points_b) + + tol = 0.01 + assert np.abs(np.mean(norm_a)) < tol + assert np.abs(np.mean(norm_b)) < tol + + assert np.max(norm_a) <= 1 + assert np.max(norm_b) <= 1 + assert np.min(norm_a) >= -1 + assert np.min(norm_b) >= -1 diff --git a/tests/test_register_point_cloud.py b/tests/test_register_point_cloud.py index f959078..2758e94 100644 --- a/tests/test_register_point_cloud.py +++ b/tests/test_register_point_cloud.py @@ -68,65 +68,4 @@ def test_point_cloud_registration(): pu.reproject_and_save(left_undistorted, registration, point_cloud, left_intrinsics, output_file='tests/output/open_cas_tmi_registered.png') - assert residual < 5.0 - - -def test_point_cloud_registration_goicp(): - - left_intrinsics = np.loadtxt('tests/data/open_cas_tmi/Stereo_SD_d_complete_22/calib.left.intrinsics.txt') - right_intrinsics = np.loadtxt('tests/data/open_cas_tmi/Stereo_SD_d_complete_22/calib.right.intrinsics.txt') - left_distortion = np.loadtxt('tests/data/open_cas_tmi/Stereo_SD_d_complete_22/calib.left.distortion.txt') - right_distortion = np.loadtxt('tests/data/open_cas_tmi/Stereo_SD_d_complete_22/calib.right.distortion.txt') - l2r_matrix = np.loadtxt('tests/data/open_cas_tmi/Stereo_SD_d_complete_22/calib.l2r.txt') - - l2r_rmat = l2r_matrix[0:3, 0:3] - l2r_tvec = l2r_matrix[0:3, 3:4] - - pointcloud_file = 'tests/data/open_cas_tmi/Stereo_SD_d_complete_22/Stereo_SD_d_complete_22_CT_cut.xyz' - point_cloud = np.loadtxt(pointcloud_file) - - model_to_camera = np.eye(4) - - left_image = cv2.imread('tests/data/open_cas_tmi/Stereo_SD_d_complete_22/Stereo_SD_d_complete_22_IMG_left.bmp') - left_undistorted = cv2.undistort(left_image, left_intrinsics, left_distortion) - left_mask = cv2.imread('tests/data/open_cas_tmi/Stereo_SD_d_complete_22/Stereo_SD_d_complete_22_MASK_eval_inverted.png') - left_mask = cv2.cvtColor(left_mask, cv2.COLOR_BGR2GRAY) - right_image = cv2.imread('tests/data/open_cas_tmi/Stereo_SD_d_complete_22/Stereo_SD_d_complete_22_IMG_right.bmp') - right_undistorted = cv2.undistort(right_image, right_intrinsics, right_distortion) - - # Produce picture of gold standard registration. - pu.reproject_and_save(left_undistorted, model_to_camera, point_cloud, left_intrinsics, - output_file='tests/output/open_cas_tmi_gold.png') - - # Create registration pipeline. - reg_points_to_vid = \ - reg.Register3DToStereoVideo(None, - sr.SGBMReconstructor(), - goicp.GoICPRegistration(), - left_intrinsics, - left_distortion, - right_intrinsics, - right_distortion, - l2r_rmat, - l2r_tvec, - left_mask=left_mask, - z_range=[45, 65], - voxel_reduction=[5, 5, 5] - ) - - residual, registration = reg_points_to_vid.register(point_cloud, - left_undistorted, - right_undistorted, - model_to_camera - ) - - print(f'Model: {pointcloud_file}') - print(f'{len(point_cloud)} points in reference point cloud') - - print("Residual:" + str(residual)) - print("Registration, using single-view, with full point cloud:\n" + str(registration)) - - pu.reproject_and_save(left_undistorted, registration, point_cloud, left_intrinsics, - output_file='tests/output/open_cas_tmi_registered.png') - - assert residual < 5.0 + assert residual < 5.0 \ No newline at end of file