From 9ca603db8006e232c68cb899d7f82df4638a1eb1 Mon Sep 17 00:00:00 2001
From: Lukas Meyer <lukas.meyer@fau.de>
Date: Mon, 19 Dec 2022 12:27:47 +0100
Subject: [PATCH 01/14] wrong import

---
 aruco_estimator/visualization/__init__.py | 4 ++--
 setup.py                                  | 2 +-
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/aruco_estimator/visualization/__init__.py b/aruco_estimator/visualization/__init__.py
index 96026b5..cb5663a 100644
--- a/aruco_estimator/visualization/__init__.py
+++ b/aruco_estimator/visualization/__init__.py
@@ -1,5 +1,5 @@
 #!/usr/bin/env python
 # -*- coding: utf-8 -*-
 
-from visualization import *
-from visualization_scale_factor_estimator import *
\ No newline at end of file
+from aruco_estimator.visualization.visualization import *
+from aruco_estimator.visualization.visualization_scale_factor_estimator import *
\ No newline at end of file
diff --git a/setup.py b/setup.py
index ad9d2db..4882bd2 100644
--- a/setup.py
+++ b/setup.py
@@ -16,7 +16,7 @@
 
 setuptools.setup(
     name='aruco-estimator',
-    version='1.1.8',
+    version='1.1.9',
     description='Aruco Scale Factor Estimation',
     license="MIT",
     long_description=long_description,

From 013430ff52a2c4fafb3711fbf33bd4f40ae1f62e Mon Sep 17 00:00:00 2001
From: Lukas Meyer <lukas.meyer@fau.de>
Date: Mon, 19 Dec 2022 13:38:31 +0100
Subject: [PATCH 02/14] added test file

---
 .github/workflows/python-package.yml          | 2 +-
 scale_estimator.py => aruco_estimator/test.py | 0
 2 files changed, 1 insertion(+), 1 deletion(-)
 rename scale_estimator.py => aruco_estimator/test.py (100%)

diff --git a/.github/workflows/python-package.yml b/.github/workflows/python-package.yml
index 46d80d1..69333df 100644
--- a/.github/workflows/python-package.yml
+++ b/.github/workflows/python-package.yml
@@ -31,7 +31,7 @@ jobs:
       - name: 'Test Aruco Scale Factor Estimation'
         run: |
           exiftool -ver
-          python3 scale_estimator.py --test_data
+          python3 aruco_estimator/test.py --test_data
       #- name: 'Upload Artifact'
       #  uses: actions/upload-artifact@v3
       #  with:
diff --git a/scale_estimator.py b/aruco_estimator/test.py
similarity index 100%
rename from scale_estimator.py
rename to aruco_estimator/test.py

From 55ab9e19dd051842c57444680b048669e48dad8d Mon Sep 17 00:00:00 2001
From: Lukas Meyer <lukas.meyer@fau.de>
Date: Mon, 19 Dec 2022 13:45:43 +0100
Subject: [PATCH 03/14] wrong import

---
 aruco_estimator/__init__.py               | 2 +-
 aruco_estimator/visualization/__init__.py | 4 ++--
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/aruco_estimator/__init__.py b/aruco_estimator/__init__.py
index 26cea44..fd20ed0 100644
--- a/aruco_estimator/__init__.py
+++ b/aruco_estimator/__init__.py
@@ -17,5 +17,5 @@
 from aruco_estimator import base
 from aruco_estimator import download
 from aruco_estimator import opt
-from aruco_estimator import visualization
+from aruco_estimator.visualization import *
 
diff --git a/aruco_estimator/visualization/__init__.py b/aruco_estimator/visualization/__init__.py
index cb5663a..a2dc54e 100644
--- a/aruco_estimator/visualization/__init__.py
+++ b/aruco_estimator/visualization/__init__.py
@@ -1,5 +1,5 @@
 #!/usr/bin/env python
 # -*- coding: utf-8 -*-
 
-from aruco_estimator.visualization.visualization import *
-from aruco_estimator.visualization.visualization_scale_factor_estimator import *
\ No newline at end of file
+from .visualization import *
+from .visualization_scale_factor_estimator import *
\ No newline at end of file

From 24cd8d303480d65a355b0f02eb99485a884c090c Mon Sep 17 00:00:00 2001
From: Lukas Meyer <lukas.meyer@fau.de>
Date: Mon, 19 Dec 2022 13:50:04 +0100
Subject: [PATCH 04/14] wrong import

---
 aruco_estimator/__init__.py | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/aruco_estimator/__init__.py b/aruco_estimator/__init__.py
index fd20ed0..767fdf5 100644
--- a/aruco_estimator/__init__.py
+++ b/aruco_estimator/__init__.py
@@ -12,10 +12,10 @@
 # ...
 
 # Own modules
-from aruco_estimator import aruco
-from aruco_estimator import aruco_scale_factor
-from aruco_estimator import base
-from aruco_estimator import download
-from aruco_estimator import opt
-from aruco_estimator.visualization import *
+from . import aruco
+from . import aruco_scale_factor
+from . import base
+from . import download
+from . import opt
+from .visualization import *
 

From 16e3f4aa7d83345b68cccc3b1b8727848f7335d8 Mon Sep 17 00:00:00 2001
From: Lukas Meyer <lukas.meyer@fau.de>
Date: Mon, 19 Dec 2022 14:01:13 +0100
Subject: [PATCH 05/14] changed action

---
 .github/workflows/python-package.yml | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/.github/workflows/python-package.yml b/.github/workflows/python-package.yml
index 69333df..9e683dc 100644
--- a/.github/workflows/python-package.yml
+++ b/.github/workflows/python-package.yml
@@ -19,7 +19,7 @@ jobs:
       - name: 'Install dependencies'
         run: |
           python -m pip install --upgrade pip
-          pip install .
+          pip install -e .
       - name: 'Install ExifTools'
         run: |
           wget https://exiftool.org/Image-ExifTool-12.51.tar.gz
@@ -31,6 +31,7 @@ jobs:
       - name: 'Test Aruco Scale Factor Estimation'
         run: |
           exiftool -ver
+          pip install -e . 
           python3 aruco_estimator/test.py --test_data
       #- name: 'Upload Artifact'
       #  uses: actions/upload-artifact@v3

From 96429f4e67a1990e19640a32009327b3f153deea Mon Sep 17 00:00:00 2001
From: Lukas Meyer <lukas.meyer@fau.de>
Date: Tue, 10 Jan 2023 12:40:02 +0100
Subject: [PATCH 06/14] aruco marker registration + scaling

---
 aruco_estimator/__init__.py                   |   3 +-
 aruco_estimator/aruco.py                      |   8 +-
 aruco_estimator/aruco_scale_factor.py         |  67 +++--
 aruco_estimator/base.py                       |   2 +-
 aruco_estimator/registration.py               | 230 ++++++++++++++++++
 aruco_estimator/utils.py                      | 102 ++++++++
 .../visualization_scale_factor_estimator.py   |  14 +-
 7 files changed, 388 insertions(+), 38 deletions(-)
 create mode 100644 aruco_estimator/registration.py
 create mode 100644 aruco_estimator/utils.py

diff --git a/aruco_estimator/__init__.py b/aruco_estimator/__init__.py
index 767fdf5..4c2fc1c 100644
--- a/aruco_estimator/__init__.py
+++ b/aruco_estimator/__init__.py
@@ -17,5 +17,6 @@
 from . import base
 from . import download
 from . import opt
-from .visualization import *
+from . import visualization
+from . import utils
 
diff --git a/aruco_estimator/aruco.py b/aruco_estimator/aruco.py
index b4c1007..02bf603 100644
--- a/aruco_estimator/aruco.py
+++ b/aruco_estimator/aruco.py
@@ -123,12 +123,14 @@ def detect_aruco_marker(image: np.ndarray, dict_type: int = aruco.DICT_4X4_1000,
     else:
         raise NotImplementedError
 
+    image_size = image.shape
     gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
-    corners, aruco_id, rejected_img_points = aruco.detectMarkers(gray, aruco_dict,
+    corners, aruco_id, rejected_img_points = aruco.detectMarkers(gray,
+                                                                 aruco_dict,
                                                                  parameters=aruco_parameters)
 
     if aruco_id is None:
-        return None, None
+        return None, None, image_size
 
     if False:
         if len(corners) > 0:
@@ -167,4 +169,4 @@ def detect_aruco_marker(image: np.ndarray, dict_type: int = aruco.DICT_4X4_1000,
     del gray
     del image
 
-    return corners, aruco_id
+    return corners, aruco_id, image_size
diff --git a/aruco_estimator/aruco_scale_factor.py b/aruco_estimator/aruco_scale_factor.py
index d2be2f6..4ad517a 100644
--- a/aruco_estimator/aruco_scale_factor.py
+++ b/aruco_estimator/aruco_scale_factor.py
@@ -5,20 +5,19 @@
 Licensed under the MIT License.
 See LICENSE file for more information.
 """
+from multiprocessing import Pool
+
 # Built-in/Generic Imports
 from copy import deepcopy
 import os
 import time
 from functools import partial
 from functools import wraps
-from multiprocessing import Pool
-
 # Libs
 from tqdm import tqdm
+
 # Own modules
-from colmap_wrapper.colmap.colmap import COLMAPProject
 from colmap_wrapper.colmap.utils import generate_colmap_sparse_pc
-
 from aruco_estimator.aruco import *
 from aruco_estimator.opt import *
 from aruco_estimator.base import *
@@ -41,7 +40,8 @@ def timeit_wrapper(*args, **kwargs):
 
 
 class ArucoScaleFactor(ScaleFactorBase):
-    def __init__(self, photogrammetry_software: COLMAPProject, aruco_size: float, dense_path: str = 'fused.ply'):
+    def __init__(self, photogrammetry_software: Union[COLMAPProject, COLMAP], aruco_size: float,
+                 dense_path: str = 'fused.ply'):
         """
         This class is used to determine 3D points of the aruco marker, which are used to compute a scaling factor.
         In the following the workflow is shortly described.
@@ -91,7 +91,7 @@ def __init__(self, photogrammetry_software: COLMAPProject, aruco_size: float, de
 
         # Multi Processing
         self.progress_bar = True
-        self.num_processes = 8 if os.cpu_count() > 8 else os.cpu_count()
+        self.num_processes = 12 if os.cpu_count() > 12 else os.cpu_count()
         print('Num process: ', self.num_processes)
         self.image_names = []
         # Prepare parsed data for multi processing
@@ -99,10 +99,10 @@ def __init__(self, photogrammetry_software: COLMAPProject, aruco_size: float, de
             self.image_names.append(self.photogrammetry_software._src_image_path.joinpath(
                 self.photogrammetry_software.images[image_idx].name).__str__())
 
-        #if os.path.exists(self.photogrammetry_software._project_path.joinpath('aruco_size.txt')):
+        # if os.path.exists(self.photogrammetry_software._project_path.joinpath('aruco_size.txt')):
         #    self.aruco_size = float(
         #        open(self.photogrammetry_software._project_path.joinpath('aruco_size.txt'), 'r').read())
-        #else:
+        # else:
         self.aruco_size = aruco_size
 
     def run(self) -> [np.ndarray, None]:
@@ -121,7 +121,7 @@ def run(self) -> [np.ndarray, None]:
                                                        N=self.N.reshape(len(self.N) // 12, 4, 3))
         self.aruco_distance = self.__evaluate(self.aruco_corners_3d)
 
-        return self.aruco_distance
+        return self.aruco_distance, self.aruco_corners_3d
 
     @timeit
     def __detect(self):
@@ -146,12 +146,26 @@ def __detect(self):
         # else:
         #    self.aruco_marker_detected = True
 
-        for image_idx in self.photogrammetry_software.images.keys():
-            self.photogrammetry_software.images[image_idx].aruco_corners = result[image_idx - 1][0]
-            self.photogrammetry_software.images[image_idx].aruco_id = result[image_idx - 1][1]
-            self.photogrammetry_software.images[image_idx].image_path = self.image_names[image_idx - 1]
+        aruco_ids = []
+
+        for image_idx, image_key in enumerate(self.photogrammetry_software.images.keys()):
+            ratio_x = self.photogrammetry_software.cameras[1].width / result[image_idx][2][1]
+            ratio_y = self.photogrammetry_software.cameras[1].height / result[image_idx][2][0]
+            if result[image_idx][0] != None:
+                corners = (np.expand_dims(np.vstack([result[image_idx][0][0][0, :, 0] * ratio_y,
+                                                     result[image_idx][0][0][0, :, 1] * ratio_x]).T, axis=0),)
+                self.photogrammetry_software.images[image_key].aruco_corners = corners
+                aruco_ids.append(result[image_idx][1][0][0])
+            else:
+                self.photogrammetry_software.images[image_key].aruco_corners = result[image_idx][0]
+
+            self.photogrammetry_software.images[image_key].aruco_id = result[image_idx][1]
+            self.photogrammetry_software.images[image_key].image_path = self.image_names[image_idx]
             # self.images[image_idx].image = cv2.resize(result[image_idx - 1][2], (0, 0), fx=0.3, fy=0.3)
 
+        # Only one aruco marker is allowed
+        self.dominant_aruco_id = np.argmax(np.bincount(aruco_ids))
+
     def __ray_cast(self):
         """
         This function casts a ray from the origin of the camera center C_i (also the translational part of the extrinsic
@@ -164,21 +178,28 @@ def __ray_cast(self):
         """
         for image_idx in self.photogrammetry_software.images.keys():
             if self.photogrammetry_software.images[image_idx].aruco_corners is not None:
-                p0, n = ray_cast_aruco_corners(extrinsics=self.photogrammetry_software.images[image_idx].extrinsics,
-                                               intrinsics=self.photogrammetry_software.images[image_idx].intrinsics.K,
-                                               corners=self.photogrammetry_software.images[image_idx].aruco_corners)
-                self.photogrammetry_software.images[image_idx].p0 = p0
-                self.photogrammetry_software.images[image_idx].n = n
-
-                self.P0 = np.append(self.P0, p0)
-                self.N = np.append(self.N, n)
+                if self.photogrammetry_software.images[image_idx].aruco_id[0, 0] == self.dominant_aruco_id:
+                    p0, n = ray_cast_aruco_corners(extrinsics=self.photogrammetry_software.images[image_idx].extrinsics,
+                                                   intrinsics=self.photogrammetry_software.images[
+                                                       image_idx].intrinsics.K,
+                                                   corners=self.photogrammetry_software.images[image_idx].aruco_corners)
+                    self.photogrammetry_software.images[image_idx].p0 = p0
+                    self.photogrammetry_software.images[image_idx].n = n
+
+                    self.P0 = np.append(self.P0, p0)
+                    self.N = np.append(self.N, n)
+                else:
+                    self.photogrammetry_software.images[image_idx].aruco_corners = None
+                    self.photogrammetry_software.images[image_idx].aruco_id = None
 
     @staticmethod
     def __evaluate(aruco_corners_3d: np.ndarray) -> np.ndarray:
         """
         Calculates the L2 norm between every neighbouring aruco corner. Finally the distances are averaged and returned
 
-        :return:
+
+        @param aruco_corners_3d:
+        @return:
         """
         dist1 = np.linalg.norm(aruco_corners_3d[0] - aruco_corners_3d[1])
         dist2 = np.linalg.norm(aruco_corners_3d[1] - aruco_corners_3d[2])
@@ -213,7 +234,7 @@ def analyze(self):
         plt.show()
 
     def get_dense_scaled(self):
-        return self.dense_scaled
+        return self.photogrammetry_software.dense_scaled
 
     def get_sparse_scaled(self):
         return generate_colmap_sparse_pc(self.sparse_scaled)
diff --git a/aruco_estimator/base.py b/aruco_estimator/base.py
index 4d44cfb..72eb96b 100644
--- a/aruco_estimator/base.py
+++ b/aruco_estimator/base.py
@@ -40,7 +40,7 @@ def __init__(self, photogrammetry_software: COLMAPProject):
             |     Apply   |
             ---------------
         """
-        self.photogrammetry_software = photogrammetry_software
+        self.photogrammetry_software = photogrammetry_software.projects
 
     def __detect(self):
         return NotImplemented
diff --git a/aruco_estimator/registration.py b/aruco_estimator/registration.py
new file mode 100644
index 0000000..f6190f4
--- /dev/null
+++ b/aruco_estimator/registration.py
@@ -0,0 +1,230 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+"""
+Copyright (c) 2022 Lukas Meyer
+Licensed under the MIT License.
+See LICENSE file for more information.
+"""
+import numpy as np
+import os
+import open3d as o3d
+from copy import deepcopy
+from typing import Union
+
+from colmap_wrapper.colmap import COLMAP
+# Own modules
+from aruco_estimator.aruco_scale_factor import ArucoScaleFactor
+from aruco_estimator.visualization import ArucoVisualization
+from aruco_estimator.utils import align_point_set, plot_aligned_pointset, manual_registration
+
+
+class ArucoMarkerScaledRegistration(object):
+    def __init__(self, project_path_a: str, project_path_b: str, dense_pc: str = 'fused.ply'):
+        # Name of both subprojects
+        self.project_path_a = project_path_a
+        self.project_path_b = project_path_b
+
+        # Name of dense point cloud (incase it is cropped)
+        self.dense_pc = dense_pc
+
+        self.project_a: COLMAP = None
+        self.project_b: COLMAP = None
+
+        # Load COLMAP projects
+        self.load_projects()
+
+        # ArucoScaleFactor class for estimating scale factor
+        self.aruco_scale_factor_a: ArucoScaleFactor = None
+        self.aruco_scale_factor_b: ArucoScaleFactor = None
+
+        # Sorted array with 3d location of all 4 aruco corners
+        self.aruco_corners_3d_a: np.ndarray = None
+        self.aruco_corners_3d_b: np.ndarray = None
+
+        # Scaled point cloud
+        self.pcd_a: o3d.geometry.PointCloud = None
+        self.pcd_b: o3d.geometry.PointCloud = None
+        self.pcd_combined: o3d.geometry.PointCloud = None
+
+        # Scale factor for individual point clouds
+        self.scale_factor_a: float = None
+        self.scale_factor_b: float = None
+
+        # Transformation matrix (4x4) to scale, rotate and translate pcd
+        self.transformation_a2b: np.ndarray = None
+
+    def load_projects(self):
+        self.project_a = COLMAP(self.project_path_a, image_resize=0.3, dense_pc=self.dense_pc)
+        self.project_b = COLMAP(self.project_path_b, image_resize=0.3, dense_pc=self.dense_pc)
+
+    def scale(self, debug=False):
+        self.aruco_scale_factor_a = ArucoScaleFactor(photogrammetry_software=self.project_a, aruco_size=0.3)
+        self.aruco_scale_factor_b = ArucoScaleFactor(photogrammetry_software=self.project_b, aruco_size=0.3)
+
+        aruco_distance_a, self.aruco_corners_3d_a = self.aruco_scale_factor_a.run()
+        aruco_distance_b, self.aruco_corners_3d_b = self.aruco_scale_factor_b.run()
+
+        self.pcd_a, self.scale_factor_a = self.aruco_scale_factor_a.apply()
+        self.pcd_b, self.scale_factor_b = self.aruco_scale_factor_b.apply()
+
+        if debug:
+            # Visualization of the scene and rays
+            vis = ArucoVisualization(aruco_colmap=self.aruco_scale_factor_a)
+            vis.visualization(frustum_scale=0.3, point_size=0.1)
+
+            # Visualization of the scene and rays
+            vis = ArucoVisualization(aruco_colmap=self.aruco_scale_factor_b)
+            vis.visualization(frustum_scale=0.3, point_size=0.1)
+
+            o3d.visualization.draw_geometries([self.pcd_a, self.pcd_b])
+
+    def registrate(self, additional_points: Union[type(None), tuple], manual=False, debug=False):
+
+        if additional_points:
+            self.aruco_corners_3d_a = np.vstack([self.aruco_corners_3d_a, additional_points[0]])
+            self.aruco_corners_3d_b = np.vstack([self.aruco_corners_3d_b, additional_points[1]])
+
+        if manual:
+            manual_points_1, manual_points_2 = manual_registration(self.pcd_a, self.pcd_b)
+
+        # Scale 3d aruco corners for alignment
+        scaled_aruco_corners_a = self.scale_factor_a * self.aruco_corners_3d_a
+        scaled_aruco_corners_b = self.scale_factor_b * self.aruco_corners_3d_b
+
+        a, b, transformation_a2b = align_point_set(scaled_aruco_corners_a, scaled_aruco_corners_b)
+
+        if debug:
+            plot_aligned_pointset(a, b)
+
+        # Init 4x4 transformation matrix
+        self.transformation_a2b = np.eye(4)
+        self.transformation_a2b[:3, :4] = np.hstack(
+            [(transformation_a2b[1]) * transformation_a2b[0], np.expand_dims(transformation_a2b[2], axis=0).T])
+
+        pcd_b_transformed = self.pcd_b.transform(self.transformation_a2b)
+        self.pcd_combined = deepcopy(self.pcd_a)
+        self.pcd_combined += pcd_b_transformed
+
+        if debug:
+            viewer = o3d.visualization.Visualizer()
+            viewer.create_window(window_name='Combined PCD')
+
+            viewer.add_geometry(self.pcd_combined)
+            opt = viewer.get_render_option()
+            # opt.show_coordinate_frame = True
+            opt.point_size = 0.01
+            opt.line_width = 0.01
+            opt.background_color = np.asarray([1, 1, 1])
+            viewer.run()
+            viewer.destroy_window()
+
+    def write(self):
+        common_path = os.path.commonpath(([self.project_path_a, self.project_path_b]))
+        # Save combined pcd and transformation from a to b
+        o3d.io.write_point_cloud(os.path.join(common_path, './combined.ply'), self.pcd_combined)
+        np.savetxt(os.path.join(common_path, 'transformation_a2b.txt'), self.transformation_a2b)
+
+        self.aruco_scale_factor_a.write_data()
+        self.aruco_scale_factor_b.write_data()
+
+
+if __name__ == '__main__':
+    scaled_registration = ArucoMarkerScaledRegistration(project_path_a="/home/luigi/Documents/reco/Baum 8/side_1",
+                                                        project_path_b="/home/luigi/Documents/reco/Baum 8/side_2",
+                                                        dense_pc='cropped.ply')
+    scaled_registration.scale(debug=False)
+    point_set = None
+    # (np.asarray([-3.074686, -3.703092, 4.512500]), np.asarray([-4.271004, -4.733126, 3.378184])) # Baum 08
+    # (np.asarray([-4.037381, -1.749546, 6.646245]), np.asarray([2.538995, -4.001166, 4.676914])) # Baum 07
+    scaled_registration.registrate(additional_points=point_set, manual=True, debug=False)
+    scaled_registration.write()
+
+"""
+SHOW_ARUCO_ESTIMATION = False
+
+project1 = COLMAP("/home/luigi/Documents/reco/Baum 8/side_1", image_resize=0.3, dense_pc='cropped.ply')
+colmap_project1 = project1.projects
+aruco_scale_factor1 = ArucoScaleFactor(photogrammetry_software=project1, aruco_size=0.3)
+aruco_distance1, aruco_corners_3d1 = aruco_scale_factor1.run()
+print('Distance: ', aruco_distance1)
+
+pcd_1, scale_factor_1 = aruco_scale_factor1.apply()
+aruco_scale_factor1.write_data()
+
+if SHOW_ARUCO_ESTIMATION:
+    # Visualization of the scene and rays
+    vis = ArucoVisualization(aruco_colmap=aruco_scale_factor1)
+    vis.visualization(frustum_scale=0.3, point_size=0.1)
+
+project2 = COLMAP("/home/luigi/Documents/reco/Baum 8/side_2", image_resize=0.3, dense_pc='cropped.ply')
+colmap_project2 = project1.projects
+aruco_scale_factor2 = ArucoScaleFactor(photogrammetry_software=project2, aruco_size=0.3)
+aruco_distance2, aruco_corners_3d2 = aruco_scale_factor2.run()
+print('Distance: ', aruco_distance2)
+
+pcd_2, scale_factor_2 = aruco_scale_factor2.apply()
+aruco_scale_factor2.write_data()
+
+if False:
+    o3d.visualization.draw_geometries([pcd_1, pcd_2],
+                                      zoom=0.4459,
+                                      front=[0.9288, -0.2951, -0.2242],
+                                      lookat=[1.6784, 2.0612, 1.4451],
+                                      up=[-0.3402, -0.9189, -0.1996])
+
+if SHOW_ARUCO_ESTIMATION:
+    # Visualization of the scene and rays
+    vis = ArucoVisualization(aruco_colmap=aruco_scale_factor2)
+    vis.visualization(frustum_scale=0.3, point_size=0.1)
+
+# Baum 7, side 1: [-4.037381 -1.749546 6.646245]
+# Baum 7, Side 2: [2.538995 -4.001166 4.676914]
+# aruco_corners_3d1 = np.vstack([aruco_corners_3d1, np.asarray([-4.037381, -1.749546, 6.646245])])
+# aruco_corners_3d2 = np.vstack([aruco_corners_3d2, np.asarray([2.538995, -4.001166, 4.676914])])
+
+# Baum, 8, side 1: [-3.074686 -3.703092 4.512500]
+# Baum, 8, side 2: [-4.271004 -4.733126 3.378184]
+
+aruco_corners_3d1 = np.vstack([aruco_corners_3d1, np.asarray([-3.074686, -3.703092, 4.512500])])
+aruco_corners_3d2 = np.vstack([aruco_corners_3d2, np.asarray([-4.271004, -4.733126, 3.378184])])
+
+# manual_points_1, manual_points_2 = manual_registration(pcd_1, pcd_2)
+
+
+# aruco_corners_3d1 = np.vstack([aruco_corners_3d1, manual_points_1])
+# aruco_corners_3d2 = np.vstack([aruco_corners_3d2, manual_points_2])
+
+A, B, trafo_AB = align_point_set(scale_factor_1 * aruco_corners_3d1, scale_factor_2 * aruco_corners_3d2)
+
+plot_aligned_pointset(A, B)
+
+trans_init = np.eye(4)
+trans_init[:3, :4] = np.hstack([(trafo_AB[1]) * trafo_AB[0], np.expand_dims(trafo_AB[2], axis=0).T])
+
+pcd_2_transformed = pcd_2.transform(trans_init)
+np.savetxt('transformation.txt', trans_init)
+pcd_1 += pcd_2_transformed
+
+o3d.io.write_point_cloud('./combined.ply', pcd_1)
+
+o3d.visualization.draw_geometries([pcd_1],
+                                  zoom=0.4459,
+                                  front=[0.9288, -0.2951, -0.2242],
+                                  lookat=[1.6784, 2.0612, 1.4451],
+                                  up=[-0.3402, -0.9189, -0.1996])
+
+# total_trafo_AB = get_icp_transformation(pcd_A, pcd_B, trafo_AB, max_iteration=1000)
+#
+# pcd_B.transform(total_trafo_AB.transformation)
+#
+# o3d.visualization.draw_geometries([pcd_A, pcd_B],
+#                                  zoom=0.4459,
+#                                  front=[0.9288, -0.2951, -0.2242],
+#                                  lookat=[1.6784, 2.0612, 1.4451],
+#                                  up=[-0.3402, -0.9189, -0.1996])
+#
+# pcd_A += pcd_B
+#
+# o3d.io.write_point_cloud('./combined2.ply', pcd_A)
+
+"""
diff --git a/aruco_estimator/utils.py b/aruco_estimator/utils.py
new file mode 100644
index 0000000..64b67c1
--- /dev/null
+++ b/aruco_estimator/utils.py
@@ -0,0 +1,102 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+"""
+Copyright (c) 2022 Lukas Meyer
+Licensed under the MIT License.
+See LICENSE file for more information.
+"""
+
+import numpy as np
+import open3d as o3d
+from copy import copy
+
+
+def kabsch_umeyama(A, B):
+    assert A.shape == B.shape
+    n, m = A.shape
+
+    EA = np.mean(A, axis=0)
+    EB = np.mean(B, axis=0)
+    VarA = np.mean(np.linalg.norm(A - EA, axis=1) ** 2)
+
+    H = ((A - EA).T @ (B - EB)) / n
+    U, D, VT = np.linalg.svd(H)
+    d = np.sign(np.linalg.det(U) * np.linalg.det(VT))
+    S = np.diag([1] * (m - 1) + [d])
+
+    R = U @ S @ VT
+    c = VarA / np.trace(np.diag(D) @ S)
+    t = EA - c * R @ EB
+
+    return R, c, t
+
+
+def align_point_set(point_set_A, point_set_B):
+    R, c, t = kabsch_umeyama(np.asarray(point_set_A), np.asarray(point_set_B))
+
+    point_set_B = np.array([t + c * R @ b for b in point_set_B])
+
+    return point_set_A, point_set_B, [R, c, t]
+
+
+def plot_aligned_pointset(A, B):
+    pcd1 = o3d.geometry.PointCloud()
+    pcd1.points = o3d.utility.Vector3dVector(A)
+
+    pcd2 = o3d.geometry.PointCloud()
+    pcd2.points = o3d.utility.Vector3dVector(B)
+
+    o3d.visualization.draw_geometries([pcd1, pcd2])
+
+
+def get_icp_transformation(source, target, trafo, max_iteration=2000):
+    threshold = 0.02
+    trans_init = np.eye(4)
+    trans_init[:3, :4] = np.hstack([trafo[1] * trafo[0], np.expand_dims(trafo[2], axis=0).T])
+
+    print("Initial alignment")
+    evaluation = o3d.pipelines.registration.evaluate_registration(
+        source, target, threshold, trans_init)
+    print(evaluation)
+
+    print("Apply point-to-point ICP")
+    reg_p2p = o3d.pipelines.registration.registration_icp(
+        source, target, threshold, trans_init,
+        o3d.pipelines.registration.TransformationEstimationPointToPlane(),
+        o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=max_iteration))
+
+    return reg_p2p
+
+
+def manual_registration(pcd_1, pcd_2):
+    def pick_points(pcd):
+        print("")
+        print(
+            "1) Please pick at least three correspondences using [shift + left click]"
+        )
+        print("   Press [shift + right click] to undo point picking")
+        print("2) After picking points, press 'Q' to close the window")
+        vis = o3d.visualization.VisualizerWithEditing()
+        vis.create_window(window_name='Picker')
+        vis.add_geometry(pcd)
+        vis.run()  # user picks points
+        vis.destroy_window()
+        print("")
+        return vis.get_picked_points()
+
+    def draw_registration_result(source, target, transformation):
+        source_temp = copy.deepcopy(source)
+        target_temp = copy.deepcopy(target)
+        source_temp.paint_uniform_color([1, 0.706, 0])
+        target_temp.paint_uniform_color([0, 0.651, 0.929])
+        source_temp.transform(transformation)
+        o3d.visualization.draw_geometries([source_temp, target_temp])
+
+    # pick points from two point clouds and builds correspondences
+    picked_id_source = pick_points(pcd_1)
+    picked_id_target = pick_points(pcd_2)
+
+    picked_points_1 = pcd_1.select_by_index(picked_id_source)
+    picked_points_2 = pcd_1.select_by_index(picked_id_target)
+
+    return np.asarray(picked_points_1.points), np.asarray(picked_points_2.points)
diff --git a/aruco_estimator/visualization/visualization_scale_factor_estimator.py b/aruco_estimator/visualization/visualization_scale_factor_estimator.py
index 73a2268..58c2354 100644
--- a/aruco_estimator/visualization/visualization_scale_factor_estimator.py
+++ b/aruco_estimator/visualization/visualization_scale_factor_estimator.py
@@ -6,17 +6,12 @@
 See LICENSE file for more information.
 """
 
-import open3d as o3d
-import numpy as np
-
-from colmap_wrapper.colmap import COLMAP
-from colmap_wrapper.colmap.colmap_project import PhotogrammetrySoftware
-from colmap_wrapper.visualization import draw_camera_viewport
 from aruco_estimator.visualization import *
+from aruco_estimator.aruco_scale_factor import ArucoScaleFactor
 
 
 class ScaleFactorExtimatorVisualization():
-    def __init__(self, photogrammetry_software: PhotogrammetrySoftware):
+    def __init__(self, photogrammetry_software: ArucoScaleFactor):
         self.scale_factor_estimator = photogrammetry_software
         self.photogrammetry_software = photogrammetry_software.photogrammetry_software
 
@@ -30,7 +25,7 @@ def show_dense(self):
 
 
 class ArucoVisualization(ScaleFactorExtimatorVisualization):
-    def __init__(self, aruco_colmap: COLMAP, bg_color: np.ndarray = np.asarray([1, 1, 1])):
+    def __init__(self, aruco_colmap: ArucoScaleFactor, bg_color: np.ndarray = np.asarray([1, 1, 1])):
         super().__init__(aruco_colmap)
 
         self.vis_bg_color = bg_color
@@ -91,7 +86,6 @@ def add_colmap_frustums2geometrie(self, frustum_scale: float = 1., image_type: s
 
         self.geometries.extend(geometries)
 
-
     def visualization(self, frustum_scale: float = 1, point_size: float = 1., sphere_size: float = 0.02):
         """
 
@@ -125,7 +119,7 @@ def visualization(self, frustum_scale: float = 1, point_size: float = 1., sphere
                                           color=[[0, 0, 0],
                                                  [1, 0, 0],
                                                  [0, 0, 1],
-                                                 [1, 1, 1]],
+                                                 [1, 1, 0]],
                                           radius=sphere_size)
 
         aruco_rect = generate_line_set(points=[self.scale_factor_estimator.aruco_corners_3d[0],

From 184b89900f240a659dbf73ab012b3a8c74213d66 Mon Sep 17 00:00:00 2001
From: Lukas Meyer <lukas.meyer@fau.de>
Date: Tue, 10 Jan 2023 13:38:47 +0100
Subject: [PATCH 07/14] Cleaned code and manual point add

---
 aruco_estimator/registration.py | 115 ++++----------------------------
 aruco_estimator/utils.py        |  77 ++++++++++++++++-----
 2 files changed, 72 insertions(+), 120 deletions(-)

diff --git a/aruco_estimator/registration.py b/aruco_estimator/registration.py
index f6190f4..81b2784 100644
--- a/aruco_estimator/registration.py
+++ b/aruco_estimator/registration.py
@@ -11,8 +11,8 @@
 from copy import deepcopy
 from typing import Union
 
-from colmap_wrapper.colmap import COLMAP
 # Own modules
+from colmap_wrapper.colmap import COLMAP
 from aruco_estimator.aruco_scale_factor import ArucoScaleFactor
 from aruco_estimator.visualization import ArucoVisualization
 from aruco_estimator.utils import align_point_set, plot_aligned_pointset, manual_registration
@@ -51,7 +51,7 @@ def __init__(self, project_path_a: str, project_path_b: str, dense_pc: str = 'fu
         self.scale_factor_b: float = None
 
         # Transformation matrix (4x4) to scale, rotate and translate pcd
-        self.transformation_a2b: np.ndarray = None
+        self.transformation_b2a: np.ndarray = None
 
     def load_projects(self):
         self.project_a = COLMAP(self.project_path_a, image_resize=0.3, dense_pc=self.dense_pc)
@@ -86,22 +86,24 @@ def registrate(self, additional_points: Union[type(None), tuple], manual=False,
 
         if manual:
             manual_points_1, manual_points_2 = manual_registration(self.pcd_a, self.pcd_b)
+            self.aruco_corners_3d_a = np.vstack([self.aruco_corners_3d_a, manual_points_1])
+            self.aruco_corners_3d_b = np.vstack([self.aruco_corners_3d_b, manual_points_2])
 
         # Scale 3d aruco corners for alignment
         scaled_aruco_corners_a = self.scale_factor_a * self.aruco_corners_3d_a
         scaled_aruco_corners_b = self.scale_factor_b * self.aruco_corners_3d_b
 
-        a, b, transformation_a2b = align_point_set(scaled_aruco_corners_a, scaled_aruco_corners_b)
+        A, B, transformation_b2a = align_point_set(scaled_aruco_corners_a, scaled_aruco_corners_b)
 
         if debug:
-            plot_aligned_pointset(a, b)
+            plot_aligned_pointset(A, B)
 
         # Init 4x4 transformation matrix
-        self.transformation_a2b = np.eye(4)
-        self.transformation_a2b[:3, :4] = np.hstack(
-            [(transformation_a2b[1]) * transformation_a2b[0], np.expand_dims(transformation_a2b[2], axis=0).T])
+        self.transformation_b2a = np.eye(4)
+        self.transformation_b2a[:3, :4] = np.hstack(
+            [(transformation_b2a[1]) * transformation_b2a[0], np.expand_dims(transformation_b2a[2], axis=0).T])
 
-        pcd_b_transformed = self.pcd_b.transform(self.transformation_a2b)
+        pcd_b_transformed = self.pcd_b.transform(self.transformation_b2a)
         self.pcd_combined = deepcopy(self.pcd_a)
         self.pcd_combined += pcd_b_transformed
 
@@ -122,7 +124,7 @@ def write(self):
         common_path = os.path.commonpath(([self.project_path_a, self.project_path_b]))
         # Save combined pcd and transformation from a to b
         o3d.io.write_point_cloud(os.path.join(common_path, './combined.ply'), self.pcd_combined)
-        np.savetxt(os.path.join(common_path, 'transformation_a2b.txt'), self.transformation_a2b)
+        np.savetxt(os.path.join(common_path, 'transformation_b2a.txt'), self.transformation_b2a)
 
         self.aruco_scale_factor_a.write_data()
         self.aruco_scale_factor_b.write_data()
@@ -133,98 +135,7 @@ def write(self):
                                                         project_path_b="/home/luigi/Documents/reco/Baum 8/side_2",
                                                         dense_pc='cropped.ply')
     scaled_registration.scale(debug=False)
-    point_set = None
-    # (np.asarray([-3.074686, -3.703092, 4.512500]), np.asarray([-4.271004, -4.733126, 3.378184])) # Baum 08
+    point_set = (np.asarray([-3.074686, -3.703092, 4.512500]), np.asarray([-4.271004, -4.733126, 3.378184])) # Baum 08
     # (np.asarray([-4.037381, -1.749546, 6.646245]), np.asarray([2.538995, -4.001166, 4.676914])) # Baum 07
-    scaled_registration.registrate(additional_points=point_set, manual=True, debug=False)
+    scaled_registration.registrate(additional_points=point_set, manual=False, debug=True)
     scaled_registration.write()
-
-"""
-SHOW_ARUCO_ESTIMATION = False
-
-project1 = COLMAP("/home/luigi/Documents/reco/Baum 8/side_1", image_resize=0.3, dense_pc='cropped.ply')
-colmap_project1 = project1.projects
-aruco_scale_factor1 = ArucoScaleFactor(photogrammetry_software=project1, aruco_size=0.3)
-aruco_distance1, aruco_corners_3d1 = aruco_scale_factor1.run()
-print('Distance: ', aruco_distance1)
-
-pcd_1, scale_factor_1 = aruco_scale_factor1.apply()
-aruco_scale_factor1.write_data()
-
-if SHOW_ARUCO_ESTIMATION:
-    # Visualization of the scene and rays
-    vis = ArucoVisualization(aruco_colmap=aruco_scale_factor1)
-    vis.visualization(frustum_scale=0.3, point_size=0.1)
-
-project2 = COLMAP("/home/luigi/Documents/reco/Baum 8/side_2", image_resize=0.3, dense_pc='cropped.ply')
-colmap_project2 = project1.projects
-aruco_scale_factor2 = ArucoScaleFactor(photogrammetry_software=project2, aruco_size=0.3)
-aruco_distance2, aruco_corners_3d2 = aruco_scale_factor2.run()
-print('Distance: ', aruco_distance2)
-
-pcd_2, scale_factor_2 = aruco_scale_factor2.apply()
-aruco_scale_factor2.write_data()
-
-if False:
-    o3d.visualization.draw_geometries([pcd_1, pcd_2],
-                                      zoom=0.4459,
-                                      front=[0.9288, -0.2951, -0.2242],
-                                      lookat=[1.6784, 2.0612, 1.4451],
-                                      up=[-0.3402, -0.9189, -0.1996])
-
-if SHOW_ARUCO_ESTIMATION:
-    # Visualization of the scene and rays
-    vis = ArucoVisualization(aruco_colmap=aruco_scale_factor2)
-    vis.visualization(frustum_scale=0.3, point_size=0.1)
-
-# Baum 7, side 1: [-4.037381 -1.749546 6.646245]
-# Baum 7, Side 2: [2.538995 -4.001166 4.676914]
-# aruco_corners_3d1 = np.vstack([aruco_corners_3d1, np.asarray([-4.037381, -1.749546, 6.646245])])
-# aruco_corners_3d2 = np.vstack([aruco_corners_3d2, np.asarray([2.538995, -4.001166, 4.676914])])
-
-# Baum, 8, side 1: [-3.074686 -3.703092 4.512500]
-# Baum, 8, side 2: [-4.271004 -4.733126 3.378184]
-
-aruco_corners_3d1 = np.vstack([aruco_corners_3d1, np.asarray([-3.074686, -3.703092, 4.512500])])
-aruco_corners_3d2 = np.vstack([aruco_corners_3d2, np.asarray([-4.271004, -4.733126, 3.378184])])
-
-# manual_points_1, manual_points_2 = manual_registration(pcd_1, pcd_2)
-
-
-# aruco_corners_3d1 = np.vstack([aruco_corners_3d1, manual_points_1])
-# aruco_corners_3d2 = np.vstack([aruco_corners_3d2, manual_points_2])
-
-A, B, trafo_AB = align_point_set(scale_factor_1 * aruco_corners_3d1, scale_factor_2 * aruco_corners_3d2)
-
-plot_aligned_pointset(A, B)
-
-trans_init = np.eye(4)
-trans_init[:3, :4] = np.hstack([(trafo_AB[1]) * trafo_AB[0], np.expand_dims(trafo_AB[2], axis=0).T])
-
-pcd_2_transformed = pcd_2.transform(trans_init)
-np.savetxt('transformation.txt', trans_init)
-pcd_1 += pcd_2_transformed
-
-o3d.io.write_point_cloud('./combined.ply', pcd_1)
-
-o3d.visualization.draw_geometries([pcd_1],
-                                  zoom=0.4459,
-                                  front=[0.9288, -0.2951, -0.2242],
-                                  lookat=[1.6784, 2.0612, 1.4451],
-                                  up=[-0.3402, -0.9189, -0.1996])
-
-# total_trafo_AB = get_icp_transformation(pcd_A, pcd_B, trafo_AB, max_iteration=1000)
-#
-# pcd_B.transform(total_trafo_AB.transformation)
-#
-# o3d.visualization.draw_geometries([pcd_A, pcd_B],
-#                                  zoom=0.4459,
-#                                  front=[0.9288, -0.2951, -0.2242],
-#                                  lookat=[1.6784, 2.0612, 1.4451],
-#                                  up=[-0.3402, -0.9189, -0.1996])
-#
-# pcd_A += pcd_B
-#
-# o3d.io.write_point_cloud('./combined2.ply', pcd_A)
-
-"""
diff --git a/aruco_estimator/utils.py b/aruco_estimator/utils.py
index 64b67c1..2bdba7b 100644
--- a/aruco_estimator/utils.py
+++ b/aruco_estimator/utils.py
@@ -11,19 +11,38 @@
 from copy import copy
 
 
-def kabsch_umeyama(A, B):
-    assert A.shape == B.shape
-    n, m = A.shape
+def kabsch_umeyama(pointset_A, pointset_B):
+    """
+    Kabsch–Umeyama algorithm is a method for aligning and comparing the similarity between two sets of points.
+    It finds the optimal translation, rotation and scaling by minimizing the root-mean-square deviation (RMSD)
+    of the point pairs.
 
-    EA = np.mean(A, axis=0)
-    EB = np.mean(B, axis=0)
-    VarA = np.mean(np.linalg.norm(A - EA, axis=1) ** 2)
+    Source and Explenation: https://zpl.fi/aligning-point-patterns-with-kabsch-umeyama-algorithm/
 
-    H = ((A - EA).T @ (B - EB)) / n
+    @param pointset_A: array of a set of points in n-dim
+    @param pointset_B: array of a set of points in n-dim
+    @return: Rotation Matrix (3x3), scaling (scalar) translation vector (3x1)
+    """
+    assert pointset_A.shape == pointset_B.shape
+    n, m = pointset_A.shape
+
+    # Find centroids of both point sets
+    EA = np.mean(pointset_A, axis=0)
+    EB = np.mean(pointset_B, axis=0)
+
+    VarA = np.mean(np.linalg.norm(pointset_A - EA, axis=1) ** 2)
+
+    # Covariance matrix
+    H = ((pointset_A - EA).T @ (pointset_B - EB)) / n
+
+    # SVD H = UDV^T
     U, D, VT = np.linalg.svd(H)
+
+    # Detect and prevent reflection
     d = np.sign(np.linalg.det(U) * np.linalg.det(VT))
     S = np.diag([1] * (m - 1) + [d])
 
+    # rotation, scaling and translation
     R = U @ S @ VT
     c = VarA / np.trace(np.diag(D) @ S)
     t = EA - c * R @ EB
@@ -40,13 +59,24 @@ def align_point_set(point_set_A, point_set_B):
 
 
 def plot_aligned_pointset(A, B):
-    pcd1 = o3d.geometry.PointCloud()
-    pcd1.points = o3d.utility.Vector3dVector(A)
+    """
+    Visualize transformed point set
+    @param A: array of a set of points in n-dim
+    @param B: array of a set of points in n-dim
+    @return: both point clouds
+    """
+
 
-    pcd2 = o3d.geometry.PointCloud()
-    pcd2.points = o3d.utility.Vector3dVector(B)
+    pcdA = o3d.geometry.PointCloud()
+    pcdA.points = o3d.utility.Vector3dVector(A)
 
-    o3d.visualization.draw_geometries([pcd1, pcd2])
+    pcdB = o3d.geometry.PointCloud()
+    pcdB.points = o3d.utility.Vector3dVector(B)
+
+
+    o3d.visualization.draw_geometries([pcdA, pcdB])
+
+    return pcdA, pcdB
 
 
 def get_icp_transformation(source, target, trafo, max_iteration=2000):
@@ -69,6 +99,14 @@ def get_icp_transformation(source, target, trafo, max_iteration=2000):
 
 
 def manual_registration(pcd_1, pcd_2):
+    """
+    Source: http://www.open3d.org/docs/latest/tutorial/Advanced/interactive_visualization.html
+
+    @param pcd_1:
+    @param pcd_2:
+    @return:
+    """
+
     def pick_points(pcd):
         print("")
         print(
@@ -76,13 +114,16 @@ def pick_points(pcd):
         )
         print("   Press [shift + right click] to undo point picking")
         print("2) After picking points, press 'Q' to close the window")
-        vis = o3d.visualization.VisualizerWithEditing()
-        vis.create_window(window_name='Picker')
-        vis.add_geometry(pcd)
-        vis.run()  # user picks points
-        vis.destroy_window()
+        viewer = o3d.visualization.VisualizerWithEditing()
+        viewer.create_window(window_name='Picker')
+        opt = viewer.get_render_option()
+        # opt.show_coordinate_frame = True
+        opt.point_size = 2.5
+        viewer.add_geometry(pcd)
+        viewer.run()  # user picks points
+        viewer.destroy_window()
         print("")
-        return vis.get_picked_points()
+        return viewer.get_picked_points()
 
     def draw_registration_result(source, target, transformation):
         source_temp = copy.deepcopy(source)

From f38db0e46a3a94d9c0c09f7b768ebd843b1ff912 Mon Sep 17 00:00:00 2001
From: Lukas Meyer <lukas.meyer@fau.de>
Date: Tue, 10 Jan 2023 14:03:05 +0100
Subject: [PATCH 08/14] changed missing links and adapted code

---
 README.md               | 18 +++++++++---------
 aruco_estimator/test.py |  6 +++---
 2 files changed, 12 insertions(+), 12 deletions(-)

diff --git a/README.md b/README.md
index 8d9af8f..ee5fbb8 100644
--- a/README.md
+++ b/README.md
@@ -1,5 +1,5 @@
 <p align="center" width="100%">
-    <img width="100%" src="https://github.com/meyerls/aruco-estimator/blob/dev/img/wood.png?raw=true">
+    <img width="100%" src="https://media.githubusercontent.com/media/meyerls/aruco-estimator/main/img/wood.png">
 </p>
 
 # Automatic Estimation of the Scale Factor Based on Aruco Markers (Work in Progress!)
@@ -59,7 +59,7 @@ using ``aruco_scale_factor.apply(true_scale)``.
 from aruco_estimator.aruco_scale_factor import ArucoScaleFactor
 from aruco_estimator.visualization import ArucoVisualization
 from aruco_estimator import download
-from colmap_wrapper.colmap import COLMAPProject
+from colmap_wrapper.colmap import COLMAP
 import os
 import open3d as o3d
 
@@ -68,11 +68,11 @@ dataset = download.Dataset()
 dataset.download_door_dataset()
 
 # Load Colmap project folder
-project = COLMAPProject(project_path=dataset.dataset_path, image_resize=0.4)
+project = COLMAP(project_path=dataset.dataset_path, image_resize=0.4)
 
 # Init & run pose estimation of corners in 3D & estimate mean L2 distance between the four aruco corners
 aruco_scale_factor = ArucoScaleFactor(photogrammetry_software=project, aruco_size=dataset.scale)
-aruco_distance = aruco_scale_factor.run()
+aruco_distance, aruco_corners_3d = aruco_scale_factor.run()
 print('Size of the unscaled aruco markers: ', aruco_distance)
 
 # Calculate scaling factor, apply it to the scene and save scaled point cloud
@@ -104,14 +104,14 @@ chmod u+x init_env.sh
 To test the code on your local machine try the example project by using:
 
 ````angular2html
-python3 scale_estimator.py --test_data
+python3 scale_estimator.py --test_data --visualize --frustum_size 0.4
 ````
 <p align="center" width="100%">
-    <img width="100%" src="https://github.com/meyerls/aruco-estimator/blob/dev/img/door.png?raw=true">
+    <img width="100%" src="https://github.com/meyerls/aruco-estimator/blob/main/img/door.png?raw=true">
 </p>
 
 <p align="center" width="100%">
-    <img width="100%" src="https://github.com/meyerls/aruco-estimator/blob/dev/img/output.gif?raw=true">
+    <img width="100%" src="https://github.com/meyerls/aruco-estimator/blob/main/img/output.gif?raw=true">
 </p>
 
 ## Limitation / Improvements
@@ -132,7 +132,7 @@ repo [COLMAP Utility Scripts](https://github.com/uzh-rpg/colmap_utils) by [uzh-r
 
 ## Trouble Shooting
 
-*In some cases cv2 does not detect the aruco marker module. Reinstalling opencv-python and opencv-python-python might
+* In some cases cv2 does not detect the aruco marker module. Reinstalling opencv-python and opencv-python-python might
   help [Source](https://stackoverflow.com/questions/45972357/python-opencv-aruco-no-module-named-cv2-aruco)
 * [PyExifTool](https://github.com/sylikc/pyexiftool): A library to communicate with the [ExifTool](https://exiftool.org)
   command- application. If you have trouble installing it please refer to the PyExifTool-Homepage. 
@@ -155,7 +155,7 @@ Please cite this paper, if this work helps you with your research:
 
 ```
 @InProceedings{ ,
-  author="H",
+  author="",
   title="",
   booktitle="",
   year="",
diff --git a/aruco_estimator/test.py b/aruco_estimator/test.py
index ef4619d..5299863 100644
--- a/aruco_estimator/test.py
+++ b/aruco_estimator/test.py
@@ -10,7 +10,7 @@
 # Built-in/Generic Imports
 import argparse
 
-from colmap_wrapper.colmap import COLMAPProject
+from colmap_wrapper.colmap import COLMAP
 
 # Own modules
 from aruco_estimator.aruco_scale_factor import ArucoScaleFactor, DEBUG
@@ -44,13 +44,13 @@
         raise ValueError('--colmap_project is empty! Please select a path to our colmap project or test it with our '
                          'dataset by setting the flag --test_data')
 
-    project = COLMAPProject(project_path=args.colmap_project, image_resize=0.4)
+    project = COLMAP(project_path=args.colmap_project, image_resize=0.4)
 
     # Init & run pose estimation of corners in 3D & estimate mean L2 distance between the four aruco corners
     aruco_scale_factor = ArucoScaleFactor(photogrammetry_software=project,
                                           aruco_size=args.aruco_size,
                                           dense_path=args.dense_model)
-    aruco_distance = aruco_scale_factor.run()
+    aruco_distance, aruco_corners_3d = aruco_scale_factor.run()
     print('Size of the unscaled aruco markers: ', aruco_distance)
 
     # Calculate scaling factor and apply to scene

From 8f11c73b2db0c2d7ac572690a38212a1b8b202a9 Mon Sep 17 00:00:00 2001
From: Lukas Meyer <lukas.meyer@fau.de>
Date: Tue, 10 Jan 2023 14:14:42 +0100
Subject: [PATCH 09/14] changed readme for registration

---
 README.md                             | 28 +++++++++++++++++++--------
 aruco_estimator/aruco_scale_factor.py |  2 +-
 2 files changed, 21 insertions(+), 9 deletions(-)

diff --git a/README.md b/README.md
index ee5fbb8..c929568 100644
--- a/README.md
+++ b/README.md
@@ -46,14 +46,9 @@ dataset = download.Dataset()
 dataset.download_door_dataset(output_path='.')
 ````
 
-### API
+### Scale Factor Estimation
 
-A use of the code on the provided dataset can be seen in the following block. The most important function is 
-``aruco_scale_factor.run()``. Here, an aruco marker is searched for in each image. If a marker is found in at 
-least 2 images, the position of the aruco corner in 3D is calculated based on the camera poses and the corners of 
-the aruco maker.Based on the positions of the corners of the square aruco marker, the size of the marker in the unscaled 
-reconstruction can be determined. With the correct metric size of the marker, the scene can be scaled true to scale 
-using ``aruco_scale_factor.apply(true_scale)``. 
+An example of how to use the aruco estimator is shown below.
 
 ````python
 from aruco_estimator.aruco_scale_factor import ArucoScaleFactor
@@ -88,6 +83,23 @@ vis.visualization(frustum_scale=0.7, point_size=0.1)
 aruco_scale_factor.write_data()
 ````
 
+### Registration and Scaling
+
+In some cases COLMAP is not able to registrate all images into one dense reconstruction. If appears to be reconstructed 
+into two seperated reconstruction. To registrate both (up to know only two are possible) reconstructions the aruco 
+markers are used to registrate both sides using ```ArucoMarkerScaledRegistration```.
+
+```python
+from aruco_estimator.registration import ArucoMarkerScaledRegistration
+
+scaled_registration = ArucoMarkerScaledRegistration(project_path_a=[path2part1],
+                                                    project_path_b=[path2part2])
+scaled_registration.scale(debug=True)
+scaled_registration.registrate(manual=False, debug=True)
+scaled_registration.write()
+```
+
+
 ## Source
 
 If you want to install the repo from source make sure that conda is installed. Afterwards clone this repository, give
@@ -104,7 +116,7 @@ chmod u+x init_env.sh
 To test the code on your local machine try the example project by using:
 
 ````angular2html
-python3 scale_estimator.py --test_data --visualize --frustum_size 0.4
+python3 aruco_estimator/test.py --test_data --visualize --frustum_size 0.4
 ````
 <p align="center" width="100%">
     <img width="100%" src="https://github.com/meyerls/aruco-estimator/blob/main/img/door.png?raw=true">
diff --git a/aruco_estimator/aruco_scale_factor.py b/aruco_estimator/aruco_scale_factor.py
index 4ad517a..12e7bea 100644
--- a/aruco_estimator/aruco_scale_factor.py
+++ b/aruco_estimator/aruco_scale_factor.py
@@ -163,7 +163,7 @@ def __detect(self):
             self.photogrammetry_software.images[image_key].image_path = self.image_names[image_idx]
             # self.images[image_idx].image = cv2.resize(result[image_idx - 1][2], (0, 0), fx=0.3, fy=0.3)
 
-        # Only one aruco marker is allowed
+        # Only one aruco marker is allowed. Todo: Extend to multiple possible aruco markers
         self.dominant_aruco_id = np.argmax(np.bincount(aruco_ids))
 
     def __ray_cast(self):

From 727181f1367dbcb3779499285e5042b4e9912fae Mon Sep 17 00:00:00 2001
From: Lukas Meyer <lukas.meyer@fau.de>
Date: Tue, 10 Jan 2023 15:10:36 +0100
Subject: [PATCH 10/14] write images, cameras and points3d, Closes #22

---
 aruco_estimator/aruco_scale_factor.py | 26 ++++++++++++++------------
 1 file changed, 14 insertions(+), 12 deletions(-)

diff --git a/aruco_estimator/aruco_scale_factor.py b/aruco_estimator/aruco_scale_factor.py
index 12e7bea..ea8d8eb 100644
--- a/aruco_estimator/aruco_scale_factor.py
+++ b/aruco_estimator/aruco_scale_factor.py
@@ -18,6 +18,7 @@
 
 # Own modules
 from colmap_wrapper.colmap.utils import generate_colmap_sparse_pc
+from colmap_wrapper.colmap.bin import write_cameras_text, write_images_text, write_points3D_text
 from aruco_estimator.aruco import *
 from aruco_estimator.opt import *
 from aruco_estimator.base import *
@@ -273,17 +274,18 @@ def apply(self) -> Tuple[o3d.pybind.geometry.PointCloud, float]:
     def write_data(self):
 
         pcd_scaled = self.photogrammetry_software._project_path
-        cameras_scaled = self.photogrammetry_software._project_path.joinpath('sparse_scaled/cameras')
-        images_scaled = self.photogrammetry_software._project_path.joinpath('sparse_scaled/images')
-        points_scaled = self.photogrammetry_software._project_path.joinpath('sparse_scaled/points3D')
+        sparse_scaled_path = self.photogrammetry_software._project_path.joinpath('sparse_scaled')
+        cameras_scaled = sparse_scaled_path.joinpath('cameras.txt')
+        images_scaled = sparse_scaled_path.joinpath('images.txt')
+        points_scaled = sparse_scaled_path.joinpath('points3D.txt')
 
-        cameras_scaled.mkdir(parents=True, exist_ok=True)
-        images_scaled.mkdir(parents=False, exist_ok=True)
-        points_scaled.mkdir(parents=False, exist_ok=True)
+        write_cameras_text(self.photogrammetry_software.cameras, cameras_scaled)
+        write_images_text(self.photogrammetry_software.images_scaled, images_scaled)
+        write_points3D_text(self.sparse_scaled, points_scaled)
 
-        for image_idx in self.photogrammetry_software.images_scaled.keys():
-            filename = images_scaled.joinpath('image_{:04d}.txt'.format(image_idx - 1))
-            np.savetxt(filename, self.photogrammetry_software.images[image_idx].extrinsics.flatten())
+        # for image_idx in self.photogrammetry_software.images_scaled.keys():
+        #    filename = images_scaled.joinpath('image_{:04d}.txt'.format(image_idx - 1))
+        #    np.savetxt(filename, self.photogrammetry_software.images[image_idx].extrinsics.flatten())
 
         o3d.io.write_point_cloud(os.path.join(pcd_scaled, 'scaled.ply'), self.photogrammetry_software.dense_scaled)
 
@@ -293,13 +295,13 @@ def write_data(self):
 
 
 if __name__ == '__main__':
-    from colmap_wrapper.colmap import COLMAPProject
+    from colmap_wrapper.colmap import COLMAP
     from aruco_estimator.visualization import ArucoVisualization
 
-    project = COLMAPProject(project_path='../data/door', image_resize=0.4)
+    project = COLMAP(project_path='../data/door', image_resize=0.4)
 
     aruco_scale_factor = ArucoScaleFactor(photogrammetry_software=project, aruco_size=0.15)
-    aruco_distance = aruco_scale_factor.run()
+    aruco_distance, aruco_points3d = aruco_scale_factor.run()
     print('Mean distance between aruco markers: ', aruco_distance)
 
     aruco_scale_factor.analyze()

From f0da791a49d51856066a935df0331d228cdc17dd Mon Sep 17 00:00:00 2001
From: Lukas Meyer <lukas.meyer@fau.de>
Date: Tue, 10 Jan 2023 15:14:56 +0100
Subject: [PATCH 11/14] fixed version for cv2 in setup.py

---
 setup.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/setup.py b/setup.py
index 4882bd2..e2f04f8 100644
--- a/setup.py
+++ b/setup.py
@@ -29,7 +29,7 @@
                       "colmap_wrapper",
                       "matplotlib",
                       "open3d",
-                      "opencv-contrib-python",
+                      "opencv-contrib-python==4.6.0.66",
                       "pyquaternion",
                       "pycolmap",
                       "tqdm",

From 91becb8f5cff5210a43ff5c50d6c4e8e491e0194 Mon Sep 17 00:00:00 2001
From: Lukas Meyer <lukas.meyer@fau.de>
Date: Tue, 10 Jan 2023 15:36:58 +0100
Subject: [PATCH 12/14] changed version for colmap-wrapper

---
 setup.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/setup.py b/setup.py
index e2f04f8..163e994 100644
--- a/setup.py
+++ b/setup.py
@@ -26,7 +26,7 @@
     url="https://github.com/meyerls/aruco-estimator",
     packages=['aruco_estimator'],
     install_requires=["numpy",
-                      "colmap_wrapper",
+                      "colmap_wrapper==1.1.5",
                       "matplotlib",
                       "open3d",
                       "opencv-contrib-python==4.6.0.66",

From ffbfe5c0b00e9848e9358900605441188ea70e08 Mon Sep 17 00:00:00 2001
From: Lukas Meyer <lukas.meyer@fau.de>
Date: Tue, 10 Jan 2023 16:41:15 +0100
Subject: [PATCH 13/14] create scaled folder

---
 aruco_estimator/aruco_scale_factor.py | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/aruco_estimator/aruco_scale_factor.py b/aruco_estimator/aruco_scale_factor.py
index ea8d8eb..1daa637 100644
--- a/aruco_estimator/aruco_scale_factor.py
+++ b/aruco_estimator/aruco_scale_factor.py
@@ -279,6 +279,8 @@ def write_data(self):
         images_scaled = sparse_scaled_path.joinpath('images.txt')
         points_scaled = sparse_scaled_path.joinpath('points3D.txt')
 
+        sparse_scaled_path.mkdir(parents=True, exist_ok=True)
+
         write_cameras_text(self.photogrammetry_software.cameras, cameras_scaled)
         write_images_text(self.photogrammetry_software.images_scaled, images_scaled)
         write_points3D_text(self.sparse_scaled, points_scaled)

From c26e4e2239f31c8b9b2b6417a36cf9bac278cfc6 Mon Sep 17 00:00:00 2001
From: Lukas Meyer <lukas.meyer@fau.de>
Date: Tue, 10 Jan 2023 18:15:53 +0100
Subject: [PATCH 14/14] changed version

---
 setup.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/setup.py b/setup.py
index 163e994..876e587 100644
--- a/setup.py
+++ b/setup.py
@@ -16,7 +16,7 @@
 
 setuptools.setup(
     name='aruco-estimator',
-    version='1.1.9',
+    version='1.1.10',
     description='Aruco Scale Factor Estimation',
     license="MIT",
     long_description=long_description,