Issue #5: Normalise data by default for GoICP

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)

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

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
+

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

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