You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
218 lines
6.9 KiB
218 lines
6.9 KiB
# Copyright (c) 2022, ETH Zurich and UNC Chapel Hill.
|
|
# All rights reserved.
|
|
#
|
|
# Redistribution and use in source and binary forms, with or without
|
|
# modification, are permitted provided that the following conditions are met:
|
|
#
|
|
# * Redistributions of source code must retain the above copyright
|
|
# notice, this list of conditions and the following disclaimer.
|
|
#
|
|
# * Redistributions in binary form must reproduce the above copyright
|
|
# notice, this list of conditions and the following disclaimer in the
|
|
# documentation and/or other materials provided with the distribution.
|
|
#
|
|
# * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
|
|
# its contributors may be used to endorse or promote products derived
|
|
# from this software without specific prior written permission.
|
|
#
|
|
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
|
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
|
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
|
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
|
|
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
|
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
|
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
|
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
|
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
|
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
# POSSIBILITY OF SUCH DAMAGE.
|
|
|
|
import argparse
|
|
import numpy as np
|
|
import open3d
|
|
|
|
from read_write_model import read_model, write_model, qvec2rotmat, rotmat2qvec
|
|
|
|
|
|
class Model:
|
|
def __init__(self):
|
|
self.cameras = []
|
|
self.images = []
|
|
self.points3D = []
|
|
self.__vis = None
|
|
|
|
def read_model(self, path, ext=""):
|
|
self.cameras, self.images, self.points3D = read_model(path, ext)
|
|
|
|
def add_points(self, min_track_len=3, remove_statistical_outlier=True):
|
|
pcd = open3d.geometry.PointCloud()
|
|
|
|
xyz = []
|
|
rgb = []
|
|
for point3D in self.points3D.values():
|
|
track_len = len(point3D.point2D_idxs)
|
|
if track_len < min_track_len:
|
|
continue
|
|
xyz.append(point3D.xyz)
|
|
rgb.append(point3D.rgb / 255)
|
|
|
|
pcd.points = open3d.utility.Vector3dVector(xyz)
|
|
pcd.colors = open3d.utility.Vector3dVector(rgb)
|
|
|
|
# remove obvious outliers
|
|
if remove_statistical_outlier:
|
|
[pcd, _] = pcd.remove_statistical_outlier(nb_neighbors=20,
|
|
std_ratio=2.0)
|
|
|
|
# open3d.visualization.draw_geometries([pcd])
|
|
self.__vis.add_geometry(pcd)
|
|
self.__vis.poll_events()
|
|
self.__vis.update_renderer()
|
|
|
|
def add_cameras(self, scale=1):
|
|
frames = []
|
|
for img in self.images.values():
|
|
# rotation
|
|
R = qvec2rotmat(img.qvec)
|
|
|
|
# translation
|
|
t = img.tvec
|
|
|
|
# invert
|
|
t = -R.T @ t
|
|
R = R.T
|
|
|
|
# intrinsics
|
|
cam = self.cameras[img.camera_id]
|
|
|
|
if cam.model in ("SIMPLE_PINHOLE", "SIMPLE_RADIAL", "RADIAL"):
|
|
fx = fy = cam.params[0]
|
|
cx = cam.params[1]
|
|
cy = cam.params[2]
|
|
elif cam.model in ("PINHOLE", "OPENCV", "OPENCV_FISHEYE"):
|
|
fx = cam.params[0]
|
|
fy = cam.params[1]
|
|
cx = cam.params[2]
|
|
cy = cam.params[3]
|
|
else:
|
|
raise Exception("Camera model not supported")
|
|
|
|
# intrinsics
|
|
K = np.identity(3)
|
|
K[0, 0] = fx
|
|
K[1, 1] = fy
|
|
K[0, 2] = cx
|
|
K[1, 2] = cy
|
|
|
|
# create axis, plane and pyramed geometries that will be drawn
|
|
cam_model = draw_camera(K, R, t, cam.width, cam.height, scale)
|
|
frames.extend(cam_model)
|
|
|
|
# add geometries to visualizer
|
|
for i in frames:
|
|
self.__vis.add_geometry(i)
|
|
|
|
def create_window(self):
|
|
self.__vis = open3d.visualization.Visualizer()
|
|
self.__vis.create_window()
|
|
|
|
def show(self):
|
|
self.__vis.poll_events()
|
|
self.__vis.update_renderer()
|
|
self.__vis.run()
|
|
self.__vis.destroy_window()
|
|
|
|
|
|
def draw_camera(K, R, t, w, h,
|
|
scale=1, color=[0.8, 0.2, 0.8]):
|
|
"""Create axis, plane and pyramed geometries in Open3D format.
|
|
:param K: calibration matrix (camera intrinsics)
|
|
:param R: rotation matrix
|
|
:param t: translation
|
|
:param w: image width
|
|
:param h: image height
|
|
:param scale: camera model scale
|
|
:param color: color of the image plane and pyramid lines
|
|
:return: camera model geometries (axis, plane and pyramid)
|
|
"""
|
|
|
|
# intrinsics
|
|
K = K.copy() / scale
|
|
Kinv = np.linalg.inv(K)
|
|
|
|
# 4x4 transformation
|
|
T = np.column_stack((R, t))
|
|
T = np.vstack((T, (0, 0, 0, 1)))
|
|
|
|
# axis
|
|
axis = open3d.geometry.TriangleMesh.create_coordinate_frame(size=0.5 * scale)
|
|
axis.transform(T)
|
|
|
|
# points in pixel
|
|
points_pixel = [
|
|
[0, 0, 0],
|
|
[0, 0, 1],
|
|
[w, 0, 1],
|
|
[0, h, 1],
|
|
[w, h, 1],
|
|
]
|
|
|
|
# pixel to camera coordinate system
|
|
points = [Kinv @ p for p in points_pixel]
|
|
|
|
# image plane
|
|
width = abs(points[1][0]) + abs(points[3][0])
|
|
height = abs(points[1][1]) + abs(points[3][1])
|
|
plane = open3d.geometry.TriangleMesh.create_box(width, height, depth=1e-6)
|
|
plane.paint_uniform_color(color)
|
|
plane.translate([points[1][0], points[1][1], scale])
|
|
plane.transform(T)
|
|
|
|
# pyramid
|
|
points_in_world = [(R @ p + t) for p in points]
|
|
lines = [
|
|
[0, 1],
|
|
[0, 2],
|
|
[0, 3],
|
|
[0, 4],
|
|
]
|
|
colors = [color for i in range(len(lines))]
|
|
line_set = open3d.geometry.LineSet(
|
|
points=open3d.utility.Vector3dVector(points_in_world),
|
|
lines=open3d.utility.Vector2iVector(lines))
|
|
line_set.colors = open3d.utility.Vector3dVector(colors)
|
|
|
|
# return as list in Open3D format
|
|
return [axis, plane, line_set]
|
|
|
|
|
|
def parse_args():
|
|
parser = argparse.ArgumentParser(description="Visualize COLMAP binary and text models")
|
|
parser.add_argument("--input_model", required=True, help="path to input model folder")
|
|
parser.add_argument("--input_format", choices=[".bin", ".txt"],
|
|
help="input model format", default="")
|
|
args = parser.parse_args()
|
|
return args
|
|
|
|
|
|
def main():
|
|
args = parse_args()
|
|
|
|
# read COLMAP model
|
|
model = Model()
|
|
model.read_model(args.input_model, ext=args.input_format)
|
|
|
|
print("num_cameras:", len(model.cameras))
|
|
print("num_images:", len(model.images))
|
|
print("num_points3D:", len(model.points3D))
|
|
|
|
# display using Open3D visualization tools
|
|
model.create_window()
|
|
model.add_points()
|
|
model.add_cameras(scale=0.25)
|
|
model.show()
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|