Issue #1: Getting unit test of single-shot-registration pipeline work…

…ing.

sksurgerysurfacematch/pipelines/register_cloud_to_stereo_mosaic.py

@@ -0,0 +1,122 @@
+# -*- coding: utf-8 -*-
+
+""" Pipeline to register 3D point cloud to mosaiced surface reconstruction.  """
+
+import copy
+import numpy as np
+import cv2
+import sksurgerysurfacematch.interfaces.video_segmentor as vs
+import sksurgerysurfacematch.interfaces.stereo_reconstructor as sr
+import sksurgerysurfacematch.interfaces.rigid_registration as rr
+
+
+# pylint:disable=too-many-instance-attributes
+class Register3DToMosaicedStereoVideo:
+    """
+    Class to register a point cloud to a series of surfaces
+    derived from stereo video, and stitched together.
+    """
+    def __init__(self,
+                 video_segmentor: vs.VideoSegmentor,
+                 surface_reconstructor: sr.StereoReconstructor,
+                 rigid_registration: rr.RigidRegistration,
+                 number_of_keypoints: int = 25,
+                 left_mask: np.ndarray = None,
+                 voxel_reduction: list = None,
+                 statistical_outlier_reduction: list = None
+                 ):
+        """
+        Uses Dependency Injection for each main component.
+
+        :param video_segmentor: Optional class to pre-segment the video.
+        :param surface_reconstructor: Mandatory class to do reconstruction.
+        :param rigid_registration: Mandatory class to perform rigid alignment.
+        :param number_of_keypoints: Number of keypoints to use for matching.
+        :param left_mask: a static mask to apply to stereo reconstruction.
+        :param voxel_reduction: [vx, vy, vz] parameters for PCL
+        Voxel Grid reduction.
+        :param statistical_outlier_reduction: [meanK, StdDev] parameters for
+        PCL Statistical Outlier Removal filter.
+        """
+        self.video_segmentor = video_segmentor
+        self.surface_reconstructor = surface_reconstructor
+        self.rigid_registration = rigid_registration
+        self.number_of_keypoints = number_of_keypoints
+        self.left_static_mask = left_mask
+        self.voxel_reduction = voxel_reduction
+        self.statistical_outlier_reduction = statistical_outlier_reduction
+        self.previous_image = None
+        self.previous_recon = None
+        self.previous_keypoints = None
+        self.previous_descriptors = None
+
+    def reset(self):
+        """
+        Reset's internal data members, so that you can start accumulating
+        data again.
+        """
+        self.previous_image = None
+        self.previous_recon = None
+        self.previous_keypoints = None
+        self.previous_descriptors = None
+
+    def grab(self,
+             left_image: np.ndarray,
+             left_camera_matrix: np.ndarray,
+             left_dist_coeffs: np.ndarray,
+             right_image: np.ndarray,
+             right_camera_matrix: np.ndarray,
+             right_dist_coeffs: np.ndarray,
+             left_to_right_rmat: np.ndarray,
+             left_to_right_tvec: np.ndarray):
+        """
+        To do, explain.
+
+        :param left_image:
+        :param left_camera_matrix:
+        :param left_dist_coeffs:
+        :param right_image:
+        :param right_camera_matrix:
+        :param right_dist_coeffs:
+        :param left_to_right_rmat:
+        :param left_to_right_tvec:
+        :return:
+        """
+        left_mask = np.ones((left_image.shape[0],
+                             left_image.shape[1])) * 255
+
+        if self.video_segmentor is not None:
+            left_mask = self.video_segmentor.segment(left_image)
+
+        if self.left_static_mask is not None:
+            left_mask = np.logical_and(left_mask, self.left_static_mask)
+
+        orb = cv2.ORB_create()
+
+        current_key_points, current_descriptors = \
+            orb.detectAndCompute(left_image, None)
+
+        if self.previous_image is None:
+            full_reconstruction = \
+                self.surface_reconstructor.reconstruct(left_image,
+                                                       left_camera_matrix,
+                                                       left_dist_coeffs,
+                                                       right_image,
+                                                       right_camera_matrix,
+                                                       right_dist_coeffs,
+                                                       left_to_right_rmat,
+                                                       left_to_right_tvec,
+                                                       left_mask
+                                                       )
+            self.previous_image = copy.deepcopy(left_image)
+            self.previous_recon = full_reconstruction
+            self.previous_keypoints = current_key_points
+            self.previous_descriptors = current_descriptors
+
+
+        # Match
+        b_f = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
+        matches = b_f.match(self.previous_descriptors, current_descriptors)
+        matches = sorted(matches, key=lambda x: x.distance)
+        best_matches = matches[:self.number_of_keypoints]
+        print(best_matches)

sksurgerysurfacematch/pipelines/register_cloud_to_stereo_reconstruction.py

@@ -3,6 +3,7 @@
 """ Pipeline to register 3D point cloud to 2D stereo video """
 
 import numpy as np
+import sksurgerycore.transforms.matrix as mt
 import sksurgerypclpython as pclp
 import sksurgerysurfacematch.interfaces.video_segmentor as vs
 import sksurgerysurfacematch.interfaces.stereo_reconstructor as sr
@@ -70,14 +71,22 @@ def register(self,
         :param initial_transform: [4x4] of initial rigid transform.
         :return: residual, [4x4] matrix, of point_cloud to left camera space.
         """
-        left_mask = np.ones((left_image.shape[0],
-                             left_image.shape[1])) * 255
+        left_mask = None
+
+        if self.left_static_mask is not None:
+            left_mask = self.left_static_mask
 
         if self.video_segmentor is not None:
-            left_mask = self.video_segmentor.segment(left_image)
+            dynamic_mask = self.video_segmentor.segment(left_image)
 
-        if self.left_static_mask is not None:
-            left_mask = np.logical_and(left_mask, self.left_static_mask)
+            if left_mask is None:
+                left_mask = dynamic_mask
+            else:
+                left_mask = np.logical_and(left_mask, dynamic_mask)
+
+        if left_mask is None:
+            left_mask = np.ones((left_image.shape[0],
+                                 left_image.shape[1])) * 255
 
         reconstruction = \
             self.surface_reconstructor.reconstruct(left_image,
@@ -90,21 +99,19 @@ def register(self,
                                                    left_to_right_tvec,
                                                    left_mask
                                                    )
-
-        reconstruction = reconstruction[:, 0:3]
-
+        recon_points = reconstruction[:, 0:3]
         if self.voxel_reduction is not None:
-            reconstruction = \
+            recon_points = \
                 pclp.down_sample_points(
-                    reconstruction,
+                    recon_points,
                     self.voxel_reduction[0],
                     self.voxel_reduction[1],
                     self.voxel_reduction[2])
 
         if self.statistical_outlier_reduction is not None:
-            reconstruction = \
+            recon_points = \
                 pclp.remove_outlier_points(
-                    reconstruction,
+                    recon_points,
                     self.statistical_outlier_reduction[0],
                     self.statistical_outlier_reduction[1])
 
@@ -116,11 +123,26 @@ def register(self,
                 + initial_transform[0:3, 3]
             point_cloud = np.transpose(point_cloud)
 
-        # We register the fixed point cloud to the reconstructed point cloud.
-        residual, transform = self.rigid_registration.register(point_cloud,
-                                                               reconstruction
-                                                               )
-        # .. and then invert the result.
-        transform = np.linalg.inv(transform)
+        # Check sizes. Register fewest points to most points.
+        if point_cloud.shape[0] > recon_points.shape[0]:
+            residual, transform = \
+                self.rigid_registration.register(recon_points,
+                                                 point_cloud
+                                                 )
+            transform = np.linalg.inv(transform)
+        else:
+            residual, transform = \
+                self.rigid_registration.register(point_cloud,
+                                                 recon_points
+                                                 )
+
+        # Combine initial, if we have one.
+        if initial_transform is not None:
+            init_mat = \
+                mt.construct_rigid_transformation(
+                    initial_transform[0:3, 0:3],
+                    initial_transform[0:3, 3]
+                )
+            transform = np.matmul(transform, init_mat)
 
         return residual, transform

tests/test_register_point_cloud.py

@@ -2,23 +2,18 @@
 import cv2
 import numpy as np
 
-import sksurgerysurfacematch.algorithms.stoyanov_reconstructor as sr
+import sksurgerysurfacematch.algorithms.sgbm_reconstructor as sr
 import sksurgerysurfacematch.algorithms.pcl_icp_registration as pir
 import sksurgerysurfacematch.pipelines.register_cloud_to_stereo_reconstruction \
     as reg
 
 
 def test_point_cloud_registration():
 
-    reg_points_to_vid = \
-        reg.Register3DToStereoVideo(None,
-                                    sr.StoyanovReconstructor(),
-                                    pir.RigidRegistration(),
-                                    None, None, None
-                                    )
-
     pointcloud = np.loadtxt('tests/data/synthetic_liver/liver-H07.xyz')
     left_image = cv2.imread('tests/data/synthetic_liver/synthetic-left.png')
+    left_mask = cv2.imread('tests/data/synthetic_liver/synthetic-left-mask.png')
+    left_mask = cv2.cvtColor(left_mask, cv2.COLOR_BGR2GRAY)
     right_image = cv2.imread('tests/data/synthetic_liver/synthetic-right.png')
     left_intrinsics = np.loadtxt('tests/data/synthetic_liver/calib/calib.left.intrinsics.txt')
     right_intrinsics = np.loadtxt('tests/data/synthetic_liver/calib/calib.right.intrinsics.txt')
@@ -27,13 +22,21 @@ def test_point_cloud_registration():
     l2r_matrix = np.loadtxt('tests/data/synthetic_liver/calib/calib.l2r.txt')
 
     l2r_rmat = l2r_matrix[:3, :3]
-    l2r_rvec = (cv2.Rodrigues(l2r_rmat))[0]
     l2r_tvec = l2r_matrix[3, :]
 
     pointcloud = pointcloud[::5, :]
 
     print(f'{len(pointcloud)} points in point cloud')
 
+    reg_points_to_vid = \
+        reg.Register3DToStereoVideo(None,
+                                    sr.SGBMReconstructor(),
+                                    pir.RigidRegistration(),
+                                    left_mask=left_mask,
+                                    voxel_reduction=[5, 5, 5],
+                                    statistical_outlier_reduction=[500, 5]
+                                    )
+
     residual, registration = reg_points_to_vid.register(pointcloud,
                                                         left_image,
                                                         left_intrinsics,
@@ -42,7 +45,6 @@ def test_point_cloud_registration():
                                                         right_intrinsics,
                                                         right_distortion,
                                                         l2r_rmat,
-                                                        l2r_tvec,
-                                                        None)
+                                                        l2r_tvec)
     print(residual)
     print(registration)