diff --git a/.github/workflows/python-package.yml b/.github/workflows/python-package.yml
index 46d80d1..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,7 +31,8 @@ jobs:
       - name: 'Test Aruco Scale Factor Estimation'
         run: |
           exiftool -ver
-          python3 scale_estimator.py --test_data
+          pip install -e . 
+          python3 aruco_estimator/test.py --test_data
       #- name: 'Upload Artifact'
       #  uses: actions/upload-artifact@v3
       #  with:
diff --git a/README.md b/README.md
index 8d9af8f..c929568 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!)
@@ -46,20 +46,15 @@ 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
 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 +63,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
@@ -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,14 +116,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 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/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 +144,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 +167,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/__init__.py b/aruco_estimator/__init__.py
index 26cea44..4c2fc1c 100644
--- a/aruco_estimator/__init__.py
+++ b/aruco_estimator/__init__.py
@@ -12,10 +12,11 @@
 # ...
 
 # 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 import visualization
+from . import aruco
+from . import aruco_scale_factor
+from . import base
+from . import download
+from . import opt
+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..1daa637 100644
--- a/aruco_estimator/aruco_scale_factor.py
+++ b/aruco_estimator/aruco_scale_factor.py
@@ -5,20 +5,20 @@
 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 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 *
@@ -41,7 +41,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 +92,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 +100,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 +122,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 +147,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. Todo: Extend to multiple possible aruco markers
+        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 +179,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 +235,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)
@@ -252,17 +274,20 @@ 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')
+
+        sparse_scaled_path.mkdir(parents=True, exist_ok=True)
 
-        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)
 
@@ -272,13 +297,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()
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..81b2784
--- /dev/null
+++ b/aruco_estimator/registration.py
@@ -0,0 +1,141 @@
+#!/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
+
+# 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
+
+
+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_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)
+        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)
+            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_b2a = align_point_set(scaled_aruco_corners_a, scaled_aruco_corners_b)
+
+        if debug:
+            plot_aligned_pointset(A, B)
+
+        # Init 4x4 transformation matrix
+        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_b2a)
+        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_b2a.txt'), self.transformation_b2a)
+
+        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 = (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=False, debug=True)
+    scaled_registration.write()
diff --git a/scale_estimator.py b/aruco_estimator/test.py
similarity index 94%
rename from scale_estimator.py
rename to aruco_estimator/test.py
index ef4619d..5299863 100644
--- a/scale_estimator.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
diff --git a/aruco_estimator/utils.py b/aruco_estimator/utils.py
new file mode 100644
index 0000000..2bdba7b
--- /dev/null
+++ b/aruco_estimator/utils.py
@@ -0,0 +1,143 @@
+#!/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(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.
+
+    Source and Explenation: https://zpl.fi/aligning-point-patterns-with-kabsch-umeyama-algorithm/
+
+    @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
+
+    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):
+    """
+    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
+    """
+
+
+    pcdA = o3d.geometry.PointCloud()
+    pcdA.points = o3d.utility.Vector3dVector(A)
+
+    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):
+    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):
+    """
+    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(
+            "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")
+        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 viewer.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/__init__.py b/aruco_estimator/visualization/__init__.py
index 96026b5..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 visualization import *
-from 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
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],
diff --git a/setup.py b/setup.py
index ad9d2db..876e587 100644
--- a/setup.py
+++ b/setup.py
@@ -16,7 +16,7 @@
 
 setuptools.setup(
     name='aruco-estimator',
-    version='1.1.8',
+    version='1.1.10',
     description='Aruco Scale Factor Estimation',
     license="MIT",
     long_description=long_description,
@@ -26,10 +26,10 @@
     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",
+                      "opencv-contrib-python==4.6.0.66",
                       "pyquaternion",
                       "pycolmap",
                       "tqdm",