From 1fe6bf46c447745e5b8fb4333d7dc96eaff7b236 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Manuel=20L=C3=B3pez=20Antequera?=
 <mlopezantequera@gmail.com>
Date: Thu, 4 Jun 2020 09:36:41 +0200
Subject: [PATCH 1/3] feat: improvements to import_colmap:

- missing image_list.txt
- missing undistorted images
- projecting depth into images of undistorted_max_size
---
 bin/import_colmap | 40 +++++++++++++++++++++++++++++++---------
 1 file changed, 31 insertions(+), 9 deletions(-)

diff --git a/bin/import_colmap b/bin/import_colmap
index e7ae2878c..ce8a7db43 100755
--- a/bin/import_colmap
+++ b/bin/import_colmap
@@ -19,6 +19,7 @@ from collections import defaultdict
 from pathlib import Path
 from struct import unpack
 
+import cv2
 import matplotlib.pyplot as pl
 import numpy as np
 from matplotlib import cm
@@ -48,10 +49,10 @@ camera_models = {
 }
 
 
-def compute_and_save_undistorted_reconstruction(reconstruction, tracks_manager, udata):
+def compute_and_save_undistorted_reconstruction(reconstruction, tracks_manager, data, udata):
     urec = types.Reconstruction()
     utracks_manager = pysfm.TracksManager()
-    undistorted_shots = {}
+    undistorted_shots = []
     for shot in reconstruction.shots.values():
         if shot.camera.projection_type == 'perspective':
             ucamera = osfm_u.perspective_camera_from_perspective(shot.camera)
@@ -67,7 +68,15 @@ def compute_and_save_undistorted_reconstruction(reconstruction, tracks_manager,
         urec.add_shot(ushot)
         if tracks_manager:
             osfm_u.add_subshot_tracks(tracks_manager, utracks_manager, shot, ushot)
-        undistorted_shots[shot.id] = ushot
+        undistorted_shots.append(ushot)
+
+        image = data.load_image(shot.id, unchanged=True, anydepth=True)
+        if image is not None:
+            max_size = data.config['undistorted_image_max_size']
+            undistorted = osfm_u.undistort_image(shot, undistorted_shots, image,
+                                                 cv2.INTER_AREA, max_size)
+            for k, v in undistorted.items():
+                udata.save_undistorted_image(k, v)
 
     udata.save_undistorted_reconstruction([urec])
     if tracks_manager:
@@ -415,15 +424,17 @@ def import_depthmaps_from_fused_pointcloud(udata, urec, image_ix_to_shot_id, pat
     points_seen = read_vis(path_ply.with_suffix('.ply.vis'), image_ix_to_shot_id)
 
     # Project to shots and save as depthmaps
+    max_size = udata.config['undistorted_image_max_size']
     for shot_id, points_seen_ixs in points_seen.items():
         print("Projecting shot {}".format(shot_id))
         project_pointcloud_save_depth(udata,
                                       urec,
                                       points[points_seen_ixs],
-                                      shot_id)
+                                      shot_id,
+                                      max_size)
 
 
-def project_pointcloud_save_depth(udata, urec, points, shot_id, max_sz=1024):
+def project_pointcloud_save_depth(udata, urec, points, shot_id, max_sz):
     # Project points to the undistorted image
     shot = urec.shots[shot_id]
     w, h = shot.camera.width, shot.camera.height
@@ -486,8 +497,7 @@ def quaternion_to_angle_axis(quaternion):
     angle = 2 * math.acos(qw)
     return [angle * x, angle * y, angle * z]
 
-
-if __name__ == "__main__":
+def main():
     parser = argparse.ArgumentParser(
         description='Convert COLMAP database to OpenSfM dataset')
     parser.add_argument('database', help='path to the database to be processed')
@@ -506,9 +516,17 @@ if __name__ == "__main__":
     data = dataset.DataSet(export_folder)
     db = sqlite3.connect(p_db.as_posix())
     camera_map, image_map = import_cameras_images(db, data)
+
+    # Create image_list.txt
+    with open(export_folder / 'image_list.txt', 'w') as f:
+        for image_id, (filename, camera_id) in image_map.items():
+            f.write('images/' + filename + '\n')
+    data._load_image_list()
+
     keypoints = import_features(db, data, image_map, camera_map)
     import_matches(db, data, image_map)
 
+
     rec_cameras = p_db.parent / 'cameras.bin'
     rec_points = p_db.parent / 'points3D.bin'
     rec_images = p_db.parent / 'images.bin'
@@ -529,7 +547,7 @@ if __name__ == "__main__":
 
         # Save undistorted reconstruction as well
         udata = dataset.UndistortedDataSet(data, 'undistorted')
-        urec = compute_and_save_undistorted_reconstruction(reconstruction, tracks_manager, udata)
+        urec = compute_and_save_undistorted_reconstruction(reconstruction, tracks_manager, data, udata)
 
         # Project colmap's fused pointcloud to save depths in opensfm format
         path_ply = p_db.parent / 'dense/fused.ply'
@@ -544,6 +562,10 @@ if __name__ == "__main__":
             print("Not importing dense reconstruction: Didn't find {}".format(path_ply))
 
     else:
-        print("Didn't find reconstruction files in text format")
+        print("Didn't find some of the reconstruction files at {}".format(p_db.parent))
 
     db.close()
+
+
+if __name__ == "__main__":
+    main()

From b1eeca5d8c02ff29107ebb44758f6c6f0983491b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Manuel=20L=C3=B3pez=20Antequera?=
 <mlopezantequera@gmail.com>
Date: Thu, 4 Jun 2020 09:39:28 +0200
Subject: [PATCH 2/3] feat: added binary mode to export_colmap.py

---
 opensfm/commands/export_colmap.py | 274 +++++++++++++++++++-----------
 1 file changed, 173 insertions(+), 101 deletions(-)

diff --git a/opensfm/commands/export_colmap.py b/opensfm/commands/export_colmap.py
index 139382507..1ad9a55a4 100644
--- a/opensfm/commands/export_colmap.py
+++ b/opensfm/commands/export_colmap.py
@@ -31,34 +31,35 @@
 
 # This script is based on an original implementation by True Price.
 
-from __future__ import print_function
+from __future__ import absolute_import
 from __future__ import division
+from __future__ import print_function
 from __future__ import unicode_literals
-from __future__ import absolute_import
 
-import sys
-import sqlite3
 import logging
-import os
 import math
-import argparse
+import os
+import sqlite3
+import sys
+from struct import pack
+
 import numpy as np
 
 from opensfm import dataset
-from opensfm import matching
 from opensfm import features
 from opensfm import io
-
+from opensfm import matching
 
 logger = logging.getLogger(__name__)
 
 
 class Command:
-    name = 'export_bundler'
-    help = "Export reconstruction to bundler format"
+    name = 'export_colmap'
+    help = "Export reconstruction to colmap format"
 
     def add_arguments(self, parser):
         parser.add_argument('dataset', help='dataset to process')
+        parser.add_argument('--binary', help='export using binary format', action='store_true')
 
     def run(self, args):
         data = dataset.DataSet(args.dataset)
@@ -80,12 +81,10 @@ def run(self, args):
 
         if data.reconstruction_exists():
             export_ini_file(export_folder, database_path, images_path)
-            export_cameras_reconstruction(data, db, export_folder, camera_map)
-            points_map = export_points_reconstruction(data, db, export_folder,
-                                                      camera_map, images_map)
-            export_shots_reconstruction(data, db, export_folder,
-                                        camera_map, images_map,
-                                        features_map, points_map)
+            export_cameras_reconstruction(data, export_folder, camera_map, args.binary)
+            points_map = export_points_reconstruction(data, export_folder, images_map, args.binary)
+            export_images_reconstruction(data, export_folder, camera_map, images_map,
+                                         features_map, points_map, args.binary)
         db.commit()
         db.close()
 
@@ -361,106 +360,179 @@ def export_matches(data, db, features_map, images_map):
             db.add_matches(images_map[pair[0]], images_map[pair[1]], inliers)
 
 
-def export_cameras_reconstruction(data, db, path, camera_map):
+def export_cameras_reconstruction(data, path, camera_map, binary=False):
     reconstructions = data.load_reconstruction()
-    with io.open_wt(os.path.join(path, 'cameras.txt')) as fout:
-        for reconstruction in reconstructions:
-            for camera_id, camera in reconstruction.cameras.items():
-                w = camera.width
-                h = camera.height
-                normalizer = max(w, h)
-                colmap_id = camera_map[camera_id]
-                colmap_type = COLMAP_TYPES_MAP[camera.projection_type]
-                if camera.projection_type == 'perspective':
-                    f = camera.focal*normalizer
-                    k1 = camera.k1
-                    k2 = camera.k2
-                    fout.write('%d %s %d %d %f %f %f %f %f\n' %
-                               (colmap_id, colmap_type, w, h, f, w*0.5, h*0.5, k1, k2))
-                elif camera.projection_type == 'brown':
-                    f_x = camera.focal_x*normalizer
-                    f_y = camera.focal_y*normalizer
-                    c_x = (w-1)*0.5 + normalizer*camera.c_x
-                    c_y = (h-1)*0.5 + normalizer*camera.c_y
-                    k1 = camera.k1
-                    k2 = camera.k2
-                    k3 = camera.k3
-                    p1 = camera.p1
-                    p2 = camera.p2
-                    fout.write('%d %s %d %d %f %f %f %f %f %f %f %f %f %f %f %f\n' %
-                               (colmap_id, colmap_type, w, h, f_x, f_y, c_x, c_y,
-                               k1, k2, p1, p2, k3,
-                               0.0, 0.0, 0.0))
-                elif camera.projection_type == 'fisheye':
-                    f = camera.focal*normalizer
-                    k1 = camera.k1
-                    k2 = camera.k2
-                    fout.write('%d %s %d %d %f %f %f %f %f\n' %
-                               (colmap_id, colmap_type, w, h, f, w*0.5, h*0.5, k1, k2))
-
-
-def export_shots_reconstruction(data, db, path, camera_map, images_map,
-                                features_map, points_map):
+    cameras = {}
+    for reconstruction in reconstructions:
+        for camera_id, camera in reconstruction.cameras.items():
+            cameras[camera_id] = camera
+
+    if binary:
+        fout = open(os.path.join(path, 'cameras.bin'), 'wb')
+        fout.write(pack('<Q', len(cameras)))
+    else:
+        fout = io.open_wt(os.path.join(path, 'cameras.txt'))
+
+    for camera_id, camera in cameras.items():
+        w = camera.width
+        h = camera.height
+        normalizer = max(w, h)
+        colmap_id = camera_map[camera_id]
+        colmap_type = COLMAP_TYPES_MAP[camera.projection_type]
+        if camera.projection_type == 'perspective':
+            f = camera.focal*normalizer
+            k1 = camera.k1
+            k2 = camera.k2
+            if binary:
+                fout.write(pack('<2i', colmap_id, COLMAP_ID_MAP[camera.projection_type]))
+                fout.write(pack('<2Q', w, h))
+                fout.write(pack('<5d', f, w*0.5, h*0.5, k1, k2))
+            else:
+                fout.write('%d %s %d %d %f %f %f %f %f\n' %
+                           (colmap_id, colmap_type, w, h, f, w*0.5, h*0.5, k1, k2))
+        elif camera.projection_type == 'brown':
+            f_x = camera.focal_x*normalizer
+            f_y = camera.focal_y*normalizer
+            c_x = (w-1)*0.5 + normalizer*camera.c_x
+            c_y = (h-1)*0.5 + normalizer*camera.c_y
+            k1 = camera.k1
+            k2 = camera.k2
+            k3 = camera.k3
+            p1 = camera.p1
+            p2 = camera.p2
+            if binary:
+                fout.write(pack('<2i', colmap_id, COLMAP_ID_MAP[camera.projection_type]))
+                fout.write(pack('<2Q', w, h))
+                fout.write(pack('<12d', f_x, f_y, c_x, c_y, k1, k2, p1, p2, k3, 0.0, 0.0, 0.0))
+            else:
+                fout.write('%d %s %d %d %f %f %f %f %f %f %f %f %f %f %f %f\n' %
+                           (colmap_id, colmap_type, w, h, f_x, f_y, c_x, c_y,
+                           k1, k2, p1, p2, k3,
+                           0.0, 0.0, 0.0))
+        elif camera.projection_type == 'fisheye':
+            f = camera.focal*normalizer
+            k1 = camera.k1
+            k2 = camera.k2
+            if binary:
+                fout.write(pack('<2i', colmap_id, COLMAP_ID_MAP[camera.projection_type]))
+                fout.write(pack('<2Q', w, h))
+                fout.write(pack('<5d', f, w*0.5, h*0.5, k1, k2))
+            else:
+                fout.write('%d %s %d %d %f %f %f %f %f\n' %
+                           (colmap_id, colmap_type, w, h, f, w*0.5, h*0.5, k1, k2))
+    fout.close()
+
+
+def export_images_reconstruction(data, path, camera_map, images_map,
+                                 features_map, points_map, binary=False):
     reconstructions = data.load_reconstruction()
     tracks_manager = data.load_tracks_manager()
 
-    with io.open_wt(os.path.join(path, 'images.txt')) as fout:
+    if binary:
+        fout = open(os.path.join(path, 'images.bin'), 'wb')
+        n_ims = 0
         for reconstruction in reconstructions:
-
-            for shot_id, shot in reconstruction.shots.items():
-                colmap_camera_id = camera_map[shot.camera.id]
-                colmap_shot_id = images_map[shot_id]
-
-                t = shot.pose.translation
-                q = angle_axis_to_quaternion(shot.pose.rotation)
-
-                format_line = '%d %f %f %f %f %f %f %f %d %s\n'
-                format_tuple = [colmap_shot_id,
-                                q[0], q[1], q[2], q[3],
-                                t[0], t[1], t[2],
-                                colmap_camera_id, shot_id]
-
-                point_per_feat = {obs.id: k for k, obs in tracks_manager.get_shot_observations(shot_id).items()}
-                for feature_id in range(len(features_map[shot_id])):
-                    colmap_point_id = -1
-                    if feature_id in point_per_feat:
-                        point_id = point_per_feat[feature_id]
-                        if point_id in points_map:
-                            colmap_point_id = points_map[point_id]
-
+            n_ims += len(reconstruction.shots)
+        fout.write(pack('<Q', n_ims))
+    else:
+        fout = io.open_wt(os.path.join(path, 'images.txt'))
+
+    for reconstruction in reconstructions:
+
+        for shot_id, shot in reconstruction.shots.items():
+            colmap_camera_id = camera_map[shot.camera.id]
+            colmap_shot_id = images_map[shot_id]
+
+            t = shot.pose.translation
+            q = angle_axis_to_quaternion(shot.pose.rotation)
+
+            if binary:
+                fout.write(pack('<I', colmap_shot_id))
+                fout.write(pack('<7d', *(list(q) + list(t))))
+                fout.write(pack('<I', colmap_camera_id))
+                for char in shot_id:
+                    fout.write(pack('<c', char.encode("utf-8")))
+                fout.write(pack('<c', b"\x00"))
+            format_line = '%d %f %f %f %f %f %f %f %d %s\n'
+            format_tuple = [colmap_shot_id,
+                            q[0], q[1], q[2], q[3],
+                            t[0], t[1], t[2],
+                            colmap_camera_id, shot_id]
+
+            point_per_feat = {obs.id: k for k, obs in tracks_manager.get_shot_observations(shot_id).items()}
+
+            points_tuple = []
+            for feature_id in range(len(features_map[shot_id])):
+                colmap_point_id = -1
+                if feature_id in point_per_feat:
+                    point_id = point_per_feat[feature_id]
+                    if point_id in points_map:
+                        colmap_point_id = points_map[point_id]
+
+                if colmap_point_id != -1:
                     x, y = features_map[shot_id][feature_id]
                     format_line += '%f %f %d '
-                    format_tuple += [x, y, colmap_point_id]
-                format_line += '\n'
-                format_tuple = tuple(format_tuple)
+                    points_tuple += [x, y, colmap_point_id]
+            format_line += '\n'
 
-                fout.write(format_line % format_tuple)
+            if binary:
+                fout.write(pack('<Q', len(points_tuple) // 3))
+                for i in range(0, len(points_tuple), 3):
+                    x, y, colmap_point_id = points_tuple[i:i + 3]
+                    fout.write(pack('<2d', x, y))
+                    fout.write(pack('<Q', colmap_point_id))
+            else:
+                fout.write(format_line % tuple(format_tuple + points_tuple))
+    fout.close()
 
 
-def export_points_reconstruction(data, db, path, camera_map, images_map):
+def export_points_reconstruction(data, path, images_map, binary=False):
     reconstructions = data.load_reconstruction()
     tracks_manager = data.load_tracks_manager()
 
     points_map = {}
-    with io.open_wt(os.path.join(path, 'points3D.txt')) as fout:
+
+    if binary:
+        fout = open(os.path.join(path, 'points3D.bin'), 'wb')
+        n_points = 0
         for reconstruction in reconstructions:
-            for i, point in enumerate(reconstruction.points.values()):
-                c = point.coordinates
-                cl = point.color
-                format_line = '%d %f %f %f %d %d %d %f '
-                format_tuple = [int(i), c[0], c[1], c[2],
-                                int(cl[0]), int(cl[1]), int(cl[2]), 0.0]
-
-                for image, obs in tracks_manager.get_track_observations(point.id).items():
-                    if image not in reconstruction.shots:
-                        continue
-                    format_line += '%d %d '
-                    format_tuple += [images_map[image], obs.id]
-                format_line += '\n'
-                format_tuple = tuple(format_tuple)
-
-                fout.write(format_line % format_tuple)
-                points_map[point.id] = i
+            n_points += len(reconstruction.points)
+        fout.write(pack('<Q', n_points))
+    else:
+        fout = io.open_wt(os.path.join(path, 'points3D.txt'))
+
+    i = 0
+    for reconstruction in reconstructions:
+        for point in reconstruction.points.values():
+            c = point.coordinates
+            cl = point.color
+            format_line = '%d %f %f %f %d %d %d %f '
+            format_tuple = [int(i), c[0], c[1], c[2],
+                            int(cl[0]), int(cl[1]), int(cl[2]), 0.0]
+
+            if binary:
+                fout.write(pack('<Q', int(i)))
+                fout.write(pack('<3d', c[0], c[1], c[2])) # Position
+                fout.write(pack('<3B', *[int(i) for i in cl])) # Color
+                fout.write(pack('<d', 0.)) # Error
+
+            track_tuple = []
+            for image, obs in tracks_manager.get_track_observations(point.id).items():
+                if image not in reconstruction.shots:
+                    continue
+                format_line += '%d %d '
+                track_tuple += [images_map[image], obs.id]
+            format_line += '\n'
+
+            if binary:
+                fout.write(pack('<Q', len(track_tuple) // 2)) # Track length
+                for el in track_tuple:
+                    fout.write(pack('<i', el)) # Track
+            else:
+                fout.write(format_line % tuple(format_tuple + track_tuple))
+            points_map[point.id] = i
+            i += 1
+    fout.close()
     return points_map
 
 

From ee951c539be27decbcef7c902c52138fe1b37724 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Manuel=20L=C3=B3pez=20Antequera?=
 <mlopezantequera@gmail.com>
Date: Thu, 4 Jun 2020 13:18:47 +0200
Subject: [PATCH 3/3] feat: tweaks to import_colmap

- Save depthmap visualizations in a separate directory
- Include (empty) plane and score arrays when saving the projected depth for compatibility.
- Rename saved depth to 'clean'
- Use maximum resolution and range from the config
---
 bin/import_colmap | 13 +++++++++----
 1 file changed, 9 insertions(+), 4 deletions(-)

diff --git a/bin/import_colmap b/bin/import_colmap
index ce8a7db43..553a21b00 100755
--- a/bin/import_colmap
+++ b/bin/import_colmap
@@ -424,7 +424,7 @@ def import_depthmaps_from_fused_pointcloud(udata, urec, image_ix_to_shot_id, pat
     points_seen = read_vis(path_ply.with_suffix('.ply.vis'), image_ix_to_shot_id)
 
     # Project to shots and save as depthmaps
-    max_size = udata.config['undistorted_image_max_size']
+    max_size = udata.config['depthmap_resolution']
     for shot_id, points_seen_ixs in points_seen.items():
         print("Projecting shot {}".format(shot_id))
         project_pointcloud_save_depth(udata,
@@ -464,15 +464,16 @@ def project_pointcloud_save_depth(udata, urec, points, shot_id, max_sz):
     distances = np.linalg.norm(points - shot.pose.get_origin(), axis=1)
     viewing_angles = np.arctan2(np.linalg.norm(points_2d, axis=1), shot.camera.focal)
     depths = distances * np.cos(viewing_angles)
+    depths[depths > udata.config['depthmap_max_depth']] = 0
 
     # Create depth image
     depth_image = np.zeros([h, w])
     depth_image[pixel_coords[:, 1], pixel_coords[:, 0]] = depths[mask]
 
     # Save numpy
-    filepath = Path(udata._depthmap_file(shot_id, 'fused.npz'))
+    filepath = Path(udata._depthmap_file(shot_id, 'clean.npz'))
     filepath.parent.mkdir(exist_ok=True, parents=True)
-    np.savez_compressed(filepath, points=depth_image)
+    np.savez_compressed(filepath, depth=depth_image, plane=np.zeros(1), score=np.zeros(1))
 
     # Save jpg for visualization
     import matplotlib.pyplot as plt
@@ -480,7 +481,7 @@ def project_pointcloud_save_depth(udata, urec, points, shot_id, max_sz):
     rgb, sm = depth_colormap(depth_image)
     plt.imshow(rgb)
     small_colorbar(plt.gca(), mappable=sm)
-    filepath = Path(udata._depthmap_file(shot_id, 'fused_viz.png'))
+    filepath = Path(udata.data_path) / 'plot_depthmaps' / '{}.png'.format(shot_id)
     filepath.parent.mkdir(exist_ok=True, parents=True)
     plt.savefig(filepath, dpi=300)
     plt.close(fig)
@@ -513,6 +514,10 @@ def main():
     if not images_path.exists():
         os.symlink(args.images, images_path, target_is_directory=True)
 
+    # Copy the config if this is an colmap export of an opensfm export
+    if p_db.parent.name == 'colmap_export' and not (export_folder/'config.yaml').exists():
+        os.symlink(p_db.parent.parent / 'config.yaml', export_folder / 'config.yaml')
+
     data = dataset.DataSet(export_folder)
     db = sqlite3.connect(p_db.as_posix())
     camera_map, image_map = import_cameras_images(db, data)