Merge pull request #29 from meyerls/dev_registration

Dev registration

.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:

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="",

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
 

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

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,18 +92,18 @@ 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
         for image_idx in self.photogrammetry_software.images.keys():
             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()

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