videos_to_colmap.py 13.6 KB
Newer Older
Clement Pinard's avatar
Clement Pinard committed
1
2
3
4
5
6
from colmap_util import read_model as rm, database as db
import anafi_metadata as am
from wrappers import FFMpeg, PDraw
from sklearn.cluster import KMeans
from sklearn.metrics import pairwise_distances_argmin_min
from argparse import ArgumentParser, ArgumentDefaultsHelpFormatter
nicolas's avatar
nicolas committed
7
from edit_exif import set_gps_location
Clement Pinard's avatar
Clement Pinard committed
8
9
10
11
12
from path import Path
import pandas as pd
import numpy as np
from pyproj import Proj
from tqdm import tqdm
13
import tempfile
Clement Pinard's avatar
Clement Pinard committed
14
15
16
17
18
19
20
21
22

parser = ArgumentParser(description='Take all the drone videos of a folder and put the frame '
                                    'location in a COLMAP file for vizualisation',
                        formatter_class=ArgumentDefaultsHelpFormatter)

parser.add_argument('--video_folder', metavar='DIR',
                    help='path to videos', type=Path)
parser.add_argument('--system', default='epsg:2154')
parser.add_argument('--centroid_path', default=None)
Clément Pinard's avatar
Clément Pinard committed
23
parser.add_argument('--colmap_img_root', metavar='DIR', type=Path)
Clement Pinard's avatar
Clement Pinard committed
24
25
26
27
28
29
30
parser.add_argument('--output_format', metavar='EXT', default="bin")
parser.add_argument('--vid_ext', nargs='+', default=[".mp4", ".MP4"])
parser.add_argument('--pic_ext', nargs='+', default=[".jpg", ".JPG", ".png", ".PNG"])
parser.add_argument('--nw', default='',
                    help="native-wrapper.sh file location")
parser.add_argument('--fps', default=1, type=int,
                    help="framerate at which videos will be scanned WITH reconstruction")
31
parser.add_argument('--total_frames', default=200, type=int)
Clement Pinard's avatar
Clement Pinard committed
32
33
34
parser.add_argument('--orientation_weight', default=1, type=float)
parser.add_argument('--resolution_weight', default=1, type=float)
parser.add_argument('--save_space', action="store_true")
Clément Pinard's avatar
Clément Pinard committed
35
36
parser.add_argument('--thorough_db', type=Path)
parser.add_argument('-v', '--verbose', action="count", default=0)
Clement Pinard's avatar
Clement Pinard committed
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55


def world_coord_from_frame(frame_qvec, frame_tvec):
    '''
    frame_qvec is written in the NED system (north east down)
    frame_tvec is already is the world system (east norht up)
    '''
    world2NED = np.float32([[0, 1, 0],
                            [1, 0, 0],
                            [0, 0, -1]])
    NED2cam = np.float32([[0, 1, 0],
                          [0, 0, 1],
                          [1, 0, 0]])
    world2cam = NED2cam @ rm.qvec2rotmat(frame_qvec).T @ world2NED
    cam_tvec = - world2cam  @ frame_tvec
    cam_qvec = rm.rotmat2qvec(world2cam)
    return cam_qvec, cam_tvec


nicolas's avatar
nicolas committed
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
def set_gps(frames_list, metadata, image_path):
    for frame in frames_list:
        relative = str(frame.relpath(image_path))
        row = metadata[metadata["image_path"] == relative]
        if len(row) > 0:
            row = row.iloc[0]
            set_gps_location(frame,
                             lat=row["location_latitude"],
                             lng=row["location_longitude"],
                             altitude=row["location_altitude"])


def get_georef(metadata):
    relevant_data = metadata[["location_valid", "image_path", "x", "y", "z"]]
    path_list = []
    georef_list = []
    for _, (gps, path, x, y, alt) in relevant_data.iterrows():
        path_list.append(path)
        if gps == 1:
            georef_list.append("{} {} {} {}\n".format(path, x, y, alt))
    return georef_list, path_list


Clement Pinard's avatar
Clement Pinard committed
79
80
def optimal_sample(metadata, num_frames, orientation_weight, resolution_weight):
    metadata["sampled"] = False
nicolas's avatar
nicolas committed
81
    XYZ = metadata[["x", "y", "z"]].values
Clement Pinard's avatar
Clement Pinard committed
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
    axis_angle = metadata[["frame_quat_x", "frame_quat_y", "frame_quat_z"]].values
    if True in metadata["indoor"].unique():
        diameter = (XYZ.max(axis=0) - XYZ.min(axis=0))
        videos = metadata.loc[metadata["indoor"]]["video"].unique()
        new_centroids = 2 * diameter * np.linspace(0, 10, len(videos)).reshape(-1, 1)
        for centroid, v in zip(new_centroids, videos):
            video_index = (metadata["video"] == v).values
            XYZ[video_index] += centroid

    frame_size = metadata["video_quality"].values
    weighted_point_cloud = np.concatenate([XYZ, orientation_weight * axis_angle], axis=1)

    if resolution_weight == 0:
        weights = None
    else:
        weights = frame_size ** resolution_weight
    km = KMeans(n_clusters=num_frames).fit(weighted_point_cloud, sample_weight=weights)
    closest, _ = pairwise_distances_argmin_min(km.cluster_centers_, weighted_point_cloud)
    metadata.at[closest, "sampled"] = True
    return metadata


def register_new_cameras(cameras_dataframe, database, camera_dict, model_name="PINHOLE"):
    camera_ids = []
    for _, (w, h, f, hfov, vfov) in cameras_dataframe.iterrows():
        fx = w / (2 * np.tan(hfov * np.pi/360))
        fy = h / (2 * np.tan(vfov * np.pi/360))
        params = np.array([fx, fy, w/2, h/2])
        model_id = rm.CAMERA_MODEL_NAMES[model_name].model_id
        db_id = database.add_camera(model_id, w, h, params, prior_focal_length=True)
        camera_ids.append(db_id)
        camera_dict[db_id] = rm.Camera(id=db_id,
                                       model=model_name,
                                       width=int(w),
                                       height=int(h),
                                       params=params)
    ids_series = pd.Series(camera_ids)
    return cameras_dataframe.set_index(ids_series)


def process_video_folder(videos_list, existing_pictures, output_video_folder, image_path, system, centroid,
Clément Pinard's avatar
Clément Pinard committed
123
                         thorough_db, fps=1, total_frames=500, orientation_weight=1, resolution_weight=1,
Clément Pinard's avatar
Clément Pinard committed
124
                         output_colmap_format="bin", save_space=False, max_sequence_length=1000, **env):
Clement Pinard's avatar
Clement Pinard committed
125
126
127
128
129
130
    proj = Proj(system)
    indoor_videos = []
    final_metadata = []
    video_output_folders = {}
    images = {}
    colmap_cameras = {}
131
    tempfile_database = Path(tempfile.NamedTemporaryFile().name)
Clément Pinard's avatar
Clément Pinard committed
132
133
    if thorough_db.isfile():
        thorough_db.copy(thorough_db.stripext() + "_backup.db")
Clement Pinard's avatar
Clement Pinard committed
134
    path_lists_output = {}
135
    database = db.COLMAPDatabase.connect(thorough_db)
Clement Pinard's avatar
Clement Pinard committed
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
    database.create_tables()
    to_extract = total_frames - len(existing_pictures)

    print("extracting metadata for {} videos...".format(len(videos_list)))
    for v in tqdm(videos_list):
        width, height, framerate = env["ffmpeg"].get_size_and_framerate(v)
        video_output_folder = output_video_folder / "{}x{}".format(width, height) / v.namebase
        video_output_folder.makedirs_p()
        video_output_folders[v] = video_output_folder

        metadata = am.extract_metadata(v.parent, v, env["pdraw"], proj,
                                       width, height, framerate, centroid)
        final_metadata.append(metadata)
        if metadata["indoor"].iloc[0]:
            indoor_videos.append(v)
    final_metadata = pd.concat(final_metadata, ignore_index=True)
    print("{} outdoor videos".format(len(videos_list) - len(indoor_videos)))
    print("{} indoor videos".format(len(indoor_videos)))

    print("{} frames in total".format(len(final_metadata)))

    cam_fields = ["width", "height", "framerate", "picture_hfov", "picture_vfov"]
    cameras_dataframe = final_metadata[cam_fields].drop_duplicates()
    cameras_dataframe = register_new_cameras(cameras_dataframe, database, colmap_cameras, "PINHOLE")
nicolas's avatar
nicolas committed
160
    print("Cameras : ")
Clement Pinard's avatar
Clement Pinard committed
161
162
163
164
165
166
167
168
    print(cameras_dataframe)
    final_metadata["camera_id"] = 0
    for cam_id, row in cameras_dataframe.iterrows():
        final_metadata.loc[(final_metadata[cam_fields] == row).all(axis=1), "camera_id"] = cam_id
    if any(final_metadata["camera_id"] == 0):
        print("Error")
        print((final_metadata["camera_id"] == 0))

Clément Pinard's avatar
Clément Pinard committed
169
170
171
    if to_extract <= 0:
        final_metadata["sampled"] = False
    elif to_extract < len(final_metadata):
Clement Pinard's avatar
Clement Pinard committed
172
173
174
175
176
177
        print("subsampling based on K-Means, to get {}"
              " frames from videos, for a total of {} frames".format(to_extract, total_frames))
        final_metadata = optimal_sample(final_metadata, total_frames - len(existing_pictures),
                                        orientation_weight,
                                        resolution_weight)
        print("Done.")
Clément Pinard's avatar
Clément Pinard committed
178
179
    else:
        final_metadata["sampled"] = True
Clement Pinard's avatar
Clement Pinard committed
180
181
182

    print("Constructing COLMAP model with {:,} frames".format(len(final_metadata[final_metadata["sampled"]])))

183
184
185
186
    database.commit()
    thorough_db.copy(tempfile_database)
    temp_database = db.COLMAPDatabase.connect(tempfile_database)

Clement Pinard's avatar
Clement Pinard committed
187
    final_metadata["image_path"] = ""
188
189
    final_metadata["db_id"] = -1
    for current_id, row in tqdm(final_metadata.iterrows(), total=len(final_metadata)):
Clement Pinard's avatar
Clement Pinard committed
190
191
192
193
194
        video = row["video"]
        frame = row["frame"]
        camera_id = row["camera_id"]
        current_image_path = video_output_folders[video].relpath(image_path) / video.namebase + "_{:05d}.jpg".format(frame)

195
196
197
        final_metadata.at[current_id, "image_path"] = current_image_path
        db_image_id = temp_database.add_image(current_image_path, int(camera_id))
        final_metadata.at[current_id, "db_id"] = db_image_id
Clement Pinard's avatar
Clement Pinard committed
198
199
200
201
202
203

        if row["sampled"]:
            frame_qvec = row[["frame_quat_w",
                              "frame_quat_x",
                              "frame_quat_y",
                              "frame_quat_z"]].values
nicolas's avatar
nicolas committed
204
205
            x, y, z = row[["x", "y", "z"]]
            frame_tvec = np.array([x, y, z])
Clement Pinard's avatar
Clement Pinard committed
206
207
208
209
210
211
            if row["location_valid"]:
                frame_gps = row[["location_longitude", "location_latitude", "location_altitude"]]
            else:
                frame_gps = np.full(3, np.NaN)

            world_qvec, world_tvec = world_coord_from_frame(frame_qvec, frame_tvec)
212
213
214
215
            database.add_image(current_image_path, int(camera_id), prior_t=frame_gps, image_id=db_image_id)
            images[db_image_id] = rm.Image(id=db_image_id, qvec=world_qvec, tvec=world_tvec,
                                           camera_id=camera_id, name=current_image_path,
                                           xys=[], point3D_ids=[])
Clement Pinard's avatar
Clement Pinard committed
216
217
218

    database.commit()
    database.close()
219
220
    temp_database.commit()
    temp_database.close()
Clement Pinard's avatar
Clement Pinard committed
221
222
223
    rm.write_model(colmap_cameras, images, {}, output_video_folder, "." + output_colmap_format)
    print("COLMAP model created")

nicolas's avatar
nicolas committed
224
225
226
227
    thorough_georef, thorough_paths = get_georef(final_metadata[final_metadata["sampled"]])
    path_lists_output["thorough"] = {}
    path_lists_output["thorough"]["frames"] = thorough_paths
    path_lists_output["thorough"]["georef"] = thorough_georef
Clement Pinard's avatar
Clement Pinard committed
228
229
230
231
232

    print("Extracting frames from videos")

    for v in tqdm(videos_list):
        video_metadata = final_metadata[final_metadata["video"] == v]
nicolas's avatar
nicolas committed
233
        by_time = video_metadata.set_index(pd.to_datetime(video_metadata["time"], unit="us"))
Clement Pinard's avatar
Clement Pinard committed
234
235
        video_folder = video_output_folders[v]
        video_metadata.to_csv(video_folder/"metadata.csv")
nicolas's avatar
nicolas committed
236
237
238
239
240
        path_lists_output[v] = {}
        video_metadata_1fps = by_time.resample("{:.3f}S".format(1/fps)).first()
        georef, frame_paths = get_georef(video_metadata_1fps)
        path_lists_output[v]["frames_lowfps"] = frame_paths
        path_lists_output[v]["georef_lowfps"] = georef
Clément Pinard's avatar
Clément Pinard committed
241
        num_chunks = len(video_metadata) // max_sequence_length + 1
242
243
244
245
246
247
248
        chunks = [list(frames) for frames in np.array_split(video_metadata["image_path"],
                                                            num_chunks)]
        # Add some overlap between chunks, in order to ease the model merging afterwards
        for chunk, next_chunk in zip(chunks, chunks[1:]):
            chunk.extend(next_chunk[:10])
        path_lists_output[v]["frames_full"] = chunks

Clement Pinard's avatar
Clement Pinard committed
249
        if save_space:
Clément Pinard's avatar
Clément Pinard committed
250
251
            frame_ids = set(video_metadata[video_metadata["sampled"]]["frame"].values) | \
                set(video_metadata_1fps["frame"].values)
252
            frame_ids = sorted(list(frame_ids))
Clément Pinard's avatar
Clément Pinard committed
253
            if len(frame_ids) > 0:
nicolas's avatar
nicolas committed
254
                extracted_frames = env["ffmpeg"].extract_specific_frames(v, video_folder, frame_ids)
Clement Pinard's avatar
Clement Pinard committed
255
        else:
nicolas's avatar
nicolas committed
256
257
            extracted_frames = env["ffmpeg"].extract_images(v, video_folder)
        set_gps(extracted_frames, video_metadata, image_path)
Clement Pinard's avatar
Clement Pinard committed
258
259
260
261
262
263
264
265

    return path_lists_output, video_output_folders


if __name__ == '__main__':
    args = parser.parse_args()
    env = vars(args)
    env["videos_list"] = sum((list(args.video_folder.walkfiles('*{}'.format(ext))) for ext in args.vid_ext), [])
Clément Pinard's avatar
Clément Pinard committed
266
    output_video_folder = args.colmap_img_root / "Videos"
Clement Pinard's avatar
Clement Pinard committed
267
268
269
    output_video_folder.makedirs_p()
    env["image_path"] = args.output_folder
    env["output_video_folder"] = output_video_folder
Clément Pinard's avatar
Clément Pinard committed
270
271
    existing_pictures = sum((list(args.output_folder.walkfiles('*{}'.format(ext))) for ext in args.pic_ext), [])
    env["existing_pictures"] = [p.relpath(args.colmap_img_root) for p in existing_pictures]
Clément Pinard's avatar
Clément Pinard committed
272
273
274
    env["pdraw"] = PDraw(args.nw, verbose=args.verbose)
    env["ffmpeg"] = FFMpeg(verbose=args.verbose)
    env["output_colmap_format"] = args.output_format
Clement Pinard's avatar
Clement Pinard committed
275
276
277
278
279
280
281
282
283
284

    if args.centroid_path is not None:
        centroid = np.loadtxt(args.centroid_path)
    else:
        centroid = np.zeros(3)
    env["centroid"] = centroid
    lists, extracted_video_folders = process_video_folder(**env)

    if lists is not None:
        with open(args.output_folder/"video_frames_for_thorough_scan.txt", "w") as f:
Clément Pinard's avatar
Clément Pinard committed
285
            f.write("\n".join(env["existing_pictures"]) + "\n")
Clément Pinard's avatar
Clément Pinard committed
286
            f.write("\n".join(lists["thorough"]["frames"]))
Clement Pinard's avatar
Clement Pinard committed
287
        with open(args.output_folder/"georef.txt", "w") as f:
Clément Pinard's avatar
Clément Pinard committed
288
            f.write("\n".join(lists["thorough"]["georef"]))
Clement Pinard's avatar
Clement Pinard committed
289
        for v in env["videos_list"]:
Clément Pinard's avatar
Clément Pinard committed
290
291
292
293
294
295
296
297
298
            video_folder = extracted_video_folders[v]
            with open(video_folder / "lowfps.txt", "w") as f:
                f.write("\n".join(lists[v]["frames_lowfps"]) + "\n")
            with open(video_folder / "georef.txt", "w") as f:
                f.write("\n".join(lists["thorough"]["georef"]) + "\n")
                f.write("\n".join(lists[v]["georef_lowfps"]) + "\n")
            for j, l in enumerate(lists[v]["frames_full"]):
                with open(video_folder / "full_chunk_{}.txt".format(j), "w") as f:
                    f.write("\n".join(l) + "\n")