# 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()