From 1de42f1e5dd5bff6534e05ede5bbd240ed742b4c Mon Sep 17 00:00:00 2001
From: "Hannes F. Kuchelmeister" <hannes@kuchelmeister.org>
Date: Thu, 22 Jul 2021 16:11:52 +0200
Subject: [PATCH] add icp registration however only 2d transformation matrix is
 supported yet

---
 situr/registration/__init__.py     |  2 +-
 situr/registration/registration.py | 18 ++++++++++++++++++
 2 files changed, 19 insertions(+), 1 deletion(-)

diff --git a/situr/registration/__init__.py b/situr/registration/__init__.py
index 3637663..510d5d3 100644
--- a/situr/registration/__init__.py
+++ b/situr/registration/__init__.py
@@ -1,4 +1,4 @@
-from .registration import Registration, RegistrationFunction, FilterregRegistrationFunction
+from .registration import Registration, RegistrationFunction, IcpRegistrationFunction, FilterregRegistrationFunction
 from .channel_registration import SituImageChannelRegistration, ChannelRegistration, AcrossRoundChannelRegistration
 from .round_registration import RoundRegistration, AllChannelRoundRegistration
 from .tile_registration import CombinedRegistration
diff --git a/situr/registration/registration.py b/situr/registration/registration.py
index fb3b15e..a2a27a4 100644
--- a/situr/registration/registration.py
+++ b/situr/registration/registration.py
@@ -54,6 +54,24 @@ class FilterregRegistrationFunction(RegistrationFunction):
                                              scale=tf_param.scale, offset=tf_param.t[0:2])
 
 
+class IcpRegistrationFunction(RegistrationFunction):
+    def __init__(self, max_correspondence_distance=50) -> None:
+        self.max_distance = max_correspondence_distance
+
+    def do_registration(self,
+                        data_peaks: np.ndarray,
+                        reference_peaks: np.ndarray) -> ScaleRotateTranslateTransform:
+        source = o3.geometry.PointCloud()
+        source.points = o3.utility.Vector3dVector(extend_dim(data_peaks))
+        target = o3.geometry.PointCloud()
+        target.points = o3.utility.Vector3dVector(extend_dim(reference_peaks))
+        reg_p2p = o3.pipelines.registration.registration_icp(
+            source, target, 200)
+        print('Transformation: ')
+        print(reg_p2p.transformation)
+        return ScaleRotateTranslateTransform(reg_p2p.transformation[0:2, 0:2])
+
+
 class Registration:
     __metaclass__ = abc.ABCMeta