├── .gitignore ├── facap ├── __init__.py ├── data │ ├── __init__.py │ └── scan.py ├── geometry │ ├── __init__.py │ ├── torch.py │ ├── open3d.py │ ├── numpy.py │ └── allign_walls.py ├── colmap_scripts │ ├── __init__.py │ └── read_write_model.py ├── feature_errors │ ├── __init__.py │ ├── point_error.py │ ├── reprojection_error.py │ └── ray_error.py ├── optimization │ ├── __init__.py │ ├── project.py │ ├── unproject.py │ ├── floor_term.py │ ├── wall_segment_term.py │ ├── wall_term.py │ └── parameters.py ├── floorplan_metrics.py └── utils.py ├── imgs ├── BIMCaP.png ├── BIMCaP_v2.png ├── bimfloor.png └── pipeline.png ├── env.yaml ├── experiments ├── config_facap.yaml ├── config_bim.yaml └── config.yaml ├── Dockerfile ├── README.md ├── scripts └── run_experiment.py └── LICENSE /.gitignore: -------------------------------------------------------------------------------- 1 | .idea/* -------------------------------------------------------------------------------- /facap/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /facap/data/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /facap/geometry/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /facap/colmap_scripts/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /imgs/BIMCaP.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MigVega/BIMCaP/HEAD/imgs/BIMCaP.png -------------------------------------------------------------------------------- /imgs/BIMCaP_v2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MigVega/BIMCaP/HEAD/imgs/BIMCaP_v2.png -------------------------------------------------------------------------------- /imgs/bimfloor.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MigVega/BIMCaP/HEAD/imgs/bimfloor.png -------------------------------------------------------------------------------- /imgs/pipeline.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MigVega/BIMCaP/HEAD/imgs/pipeline.png -------------------------------------------------------------------------------- /facap/feature_errors/__init__.py: -------------------------------------------------------------------------------- 1 | from facap.feature_errors.point_error import point_error 2 | from facap.feature_errors.ray_error import ray_error 3 | from facap.feature_errors.reprojection_error import reprojection_error 4 | 5 | __all__ = ["point_error", "ray_error", "reprojection_error"] 6 | -------------------------------------------------------------------------------- /facap/feature_errors/point_error.py: -------------------------------------------------------------------------------- 1 | def point_error(left, right, unproject, distance_function, **kwargs): 2 | processed_left = unproject(left["depths"], left["points"], left["camera_idxs"]) 3 | processed_right = unproject(right["depths"], right["points"], right["camera_idxs"]) 4 | 5 | return distance_function(processed_left, processed_right) 6 | -------------------------------------------------------------------------------- /facap/feature_errors/reprojection_error.py: -------------------------------------------------------------------------------- 1 | 2 | def reprojection_error(left, right, unproject, project, distance_function, **kwargs): 3 | unprojected_left = unproject(left["depths"], left["points"], left["camera_idxs"]) 4 | projected_left_on_right = project(unprojected_left, right["camera_idxs"]) 5 | 6 | return distance_function(right["points"], projected_left_on_right) 7 | -------------------------------------------------------------------------------- /env.yaml: -------------------------------------------------------------------------------- 1 | name: FACaP 2 | channels: 3 | - pytorch 4 | - iopath 5 | - fvcore 6 | - conda-forge 7 | - bottler 8 | - pytorch3d 9 | dependencies: 10 | - python=3.8 11 | - pytorch=1.7.1 12 | - torchvision=0.8.2 13 | - torchaudio=0.7.2 14 | - cudatoolkit=11.0 15 | - fvcore 16 | - iopath 17 | - nvidiacub=1.10.0 18 | - pytorch3d=0.4.0 19 | - opencv 20 | - matplotlib 21 | - scipy 22 | - tqdm 23 | - pyyaml 24 | -------------------------------------------------------------------------------- /facap/optimization/__init__.py: -------------------------------------------------------------------------------- 1 | from facap.optimization.unproject import Unproject 2 | from facap.optimization.project import Project 3 | from facap.optimization.parameters import CameraParameters 4 | from facap.optimization.floor_term import FloorTerm 5 | from facap.optimization.wall_term import WallTerm 6 | from facap.optimization.wall_segment_term import WallSegmentTerm 7 | 8 | __all__ = ["Unproject", "Project", "CameraParameters", "FloorTerm", "WallTerm", "WallSegmentTerm"] 9 | -------------------------------------------------------------------------------- /facap/optimization/project.py: -------------------------------------------------------------------------------- 1 | import torch 2 | from torch import nn 3 | 4 | from facap.geometry.torch import project_points_rotvec 5 | 6 | 7 | class Project(nn.Module): 8 | def __init__(self, camera_parameters): 9 | super(Project, self).__init__() 10 | self.camera_parameters = camera_parameters 11 | 12 | def forward(self, pcd, cam_ids): 13 | if len(cam_ids) == 1: 14 | cam_ids = list(cam_ids) * len(pcd) 15 | rotvecs, translations, f, pp = self.camera_parameters.get_tensors(cam_ids) 16 | 17 | yxs = project_points_rotvec(pcd, f, pp, rotvecs, translations) 18 | 19 | return yxs 20 | -------------------------------------------------------------------------------- /facap/optimization/unproject.py: -------------------------------------------------------------------------------- 1 | import torch 2 | from torch import nn 3 | 4 | from facap.geometry.torch import unproject_points_rotvec 5 | 6 | 7 | class Unproject(nn.Module): 8 | def __init__(self, camera_parameters, scale): 9 | super(Unproject, self).__init__() 10 | self.camera_parameters = camera_parameters 11 | self.scale = scale 12 | 13 | def forward(self, depths, points, cam_ids): 14 | if len(cam_ids) == 1: 15 | cam_ids = list(cam_ids) * len(depths) 16 | rotvecs, translations, f, pp = self.camera_parameters.get_tensors(cam_ids) 17 | pcd = unproject_points_rotvec(depths, points, f, pp, rotvecs, translations, scale=self.scale) 18 | 19 | return pcd 20 | -------------------------------------------------------------------------------- /experiments/config_facap.yaml: -------------------------------------------------------------------------------- 1 | paths: 2 | scan_path: "../camera_refinement_BIM/scan_lowres_002_250/" 3 | save_path: "../camera_refinement_BIM/scan_lowres_002_250/results_gfw/" 4 | 5 | data: 6 | use_bim : False 7 | wall_sparsity: 150 8 | floor_sparsity: 30 9 | min_frame_difference: 1 10 | max_initial_distance: 0.4 11 | floor_percentiles: [ 0.5, 95 ] 12 | depths_scale: 1 13 | 14 | optimization: 15 | num_epoches: 20000 16 | lr: 0.1 17 | momentum: 0.9 18 | fixed_cameras_idx: [ ] 19 | 20 | error: 21 | error_type: point_error 22 | floor_term: True 23 | floor_weight: 0.02 24 | ceil_term: False 25 | ceil_weight: 0.02 26 | wall_term: True 27 | wall_term_type: "point" 28 | wall_weight: 0.0007 29 | include_columns: False 30 | -------------------------------------------------------------------------------- /experiments/config_bim.yaml: -------------------------------------------------------------------------------- 1 | paths: 2 | scan_path: "../camera_refinement_BIM/scan_lowres_002_250/" 3 | save_path: "../camera_refinement_BIM/scan_lowres_002_250/results_bim_noceil_nocol_gfw/" 4 | 5 | data: 6 | use_bim : True 7 | wall_sparsity: 150 8 | floor_sparsity: 30 9 | min_frame_difference: 1 10 | max_initial_distance: 0.4 11 | floor_percentiles: [ 0.5, 95 ] 12 | depths_scale: 1 13 | 14 | optimization: 15 | num_epoches: 20000 16 | lr: 0.1 17 | momentum: 0.9 18 | fixed_cameras_idx: [ ] 19 | 20 | error: 21 | error_type: point_error 22 | floor_term: True 23 | floor_weight: 0.02 24 | ceil_term: False 25 | ceil_weight: 0.02 26 | wall_term: True 27 | wall_term_type: "point" 28 | wall_weight: 0.0007 29 | include_columns: False 30 | -------------------------------------------------------------------------------- /experiments/config.yaml: -------------------------------------------------------------------------------- 1 | paths: 2 | scan_path: "./data/htkr" 3 | save_path: "./data/htkr/results_2d" 4 | 5 | data: 6 | wall_sparsity: 150 7 | floor_sparsity: 30 8 | #Minimum distance between frames that are used for optimization 9 | min_frame_difference: 3 10 | #Filter out matched keypoints with too much initial distance 11 | max_initial_distance: 0.4 12 | #Filter out floor points in lower and upper percentiles regarding Z axis 13 | floor_percentiles: [ 0.5, 95 ] 14 | #The scale at which the depth images are provided 15 | depths_scale: 1000 16 | 17 | optimization: 18 | num_epoches: 25000 19 | lr: 0.1 20 | momentum: 0.9 21 | fixed_cameras_idx: [ ] 22 | 23 | error: 24 | error_type: point_error 25 | floor_term: True 26 | floor_weight: 0.02 27 | wall_term: True 28 | wall_term_type: "segment" 29 | wall_weight: 0.0007 -------------------------------------------------------------------------------- /facap/floorplan_metrics.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | def nearest_line_distance(florplan_edges, pcd_2d, max_dist=10., border_margin=.1, percentile=.9): 5 | biases = np.array([np.array(i[0]) for i in florplan_edges]) 6 | directions = np.array([np.array(i[1]) - np.array(i[0]) for i in florplan_edges]) 7 | 8 | distances = [] 9 | 10 | for a, b in zip(directions, biases): 11 | c = pcd_2d - b 12 | dot_product = np.sum(c * a, axis=-1) / a.dot(a) 13 | 14 | mask = np.where((dot_product < -border_margin) | (dot_product > 1 + border_margin)) 15 | 16 | dst = np.abs(c[:, 0] * a[1] - c[:, 1] * a[0]) / np.linalg.norm(a) 17 | 18 | dst[mask] = max_dist 19 | dst[dst > max_dist] = max_dist 20 | distances.append(dst) 21 | 22 | distances = np.stack(distances) 23 | distances = np.min(distances, axis=0) 24 | return distances, np.mean(distances), np.percentile(distances, percentile) 25 | -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | FROM nvidia/cuda:11.0.3-cudnn8-devel-ubuntu20.04 2 | 3 | ENV TZ=Europe/Moscow 4 | RUN ln -snf /usr/share/zoneinfo/$TZ /etc/localtime && echo $TZ > /etc/timezone 5 | 6 | RUN apt-get update &&\ 7 | apt-get upgrade -y &&\ 8 | apt-get install -y curl git vim htop wget bzip2 g++ libgl1-mesa-dev libosmesa6-dev patchelf\ 9 | libglfw3 libglew-dev libglib2.0-0 libsm6 unzip xvfb 10 | 11 | ARG USERNAME=user 12 | RUN apt-get install -y sudo && \ 13 | addgroup --gid 1000 $USERNAME && \ 14 | adduser --uid 1000 --gid 1000 --disabled-password --gecos '' $USERNAME && \ 15 | adduser $USERNAME sudo && \ 16 | echo '%sudo ALL=(ALL) NOPASSWD:ALL' >> /etc/sudoers && \ 17 | USER=$USERNAME && \ 18 | GROUP=$USERNAME 19 | 20 | USER $USERNAME:$USERNAME 21 | WORKDIR "/home/$USERNAME" 22 | ENV PATH="/home/$USERNAME/.local/bin:${PATH}" 23 | 24 | ENV PATH=/home/$USERNAME/miniconda3/bin:${PATH} 25 | 26 | RUN wget -q "https://repo.continuum.io/miniconda/Miniconda3-py39_4.9.2-Linux-x86_64.sh" -O 'miniconda3.sh' && \ 27 | bash 'miniconda3.sh' -b -p '/home/$USERNAME/miniconda3' && \ 28 | rm 'miniconda3.sh' && \ 29 | conda update -y conda && \ 30 | conda install pip 31 | 32 | ENV HOME /home/$USERNAME 33 | ADD env.yaml env.yaml 34 | RUN conda env update -f env.yaml --prune 35 | 36 | RUN pip install open3d==0.10.0 -U 37 | 38 | 39 | SHELL ["/bin/bash", "-c"] 40 | -------------------------------------------------------------------------------- /facap/optimization/floor_term.py: -------------------------------------------------------------------------------- 1 | import torch 2 | from torch import nn 3 | 4 | 5 | def fit_plane(pcd): 6 | mean = torch.mean(pcd, dim=0) 7 | pcd_c = pcd - mean 8 | x = torch.matmul(pcd_c.T, pcd_c) 9 | u, s, v = torch.svd(x) 10 | abc = v[:, -1] 11 | d = -torch.dot(abc, mean) 12 | coefs = torch.cat([abc, d.view(1, )]) 13 | return coefs 14 | 15 | 16 | class FloorTerm(nn.Module): 17 | def __init__(self, floor, unproject, distance_function, bim_floor=None): 18 | super(FloorTerm, self).__init__() 19 | self.floor = floor 20 | self.unproject = unproject 21 | 22 | self.plane = self.get_initial_plane(bim_floor) 23 | self.distance_function = distance_function 24 | 25 | def get_initial_plane(self, bim_floor): 26 | with torch.no_grad(): 27 | if bim_floor is None: 28 | floor_pcd = self.unproject(self.floor["depths"], self.floor["points"], self.floor["camera_idxs"]) 29 | else: 30 | floor_pcd = bim_floor.to('cuda:0') 31 | print('FLOORPCD', floor_pcd.shape) 32 | normed_coefs = fit_plane(floor_pcd) 33 | return normed_coefs 34 | 35 | def forward(self): 36 | floor_pcd = self.unproject(self.floor["depths"], self.floor["points"], self.floor["camera_idxs"]) 37 | floor_distances = (torch.sum(floor_pcd * self.plane[:3], dim=-1) + self.plane[3]) 38 | zeros = torch.zeros_like(floor_distances) 39 | floor_term = self.distance_function(zeros, floor_distances) 40 | return floor_term 41 | -------------------------------------------------------------------------------- /facap/optimization/wall_segment_term.py: -------------------------------------------------------------------------------- 1 | import torch 2 | 3 | from torch import nn 4 | 5 | 6 | def point_segment_distance(pcd_2d, segments): 7 | biases = torch.stack([i[:2] for i in segments]) 8 | directions = torch.stack([i[2:] - i[:2] for i in segments]) 9 | 10 | c = pcd_2d - biases 11 | dot_product = torch.sum(c * directions, dim=-1) / torch.sum(directions * directions, dim=-1) 12 | 13 | par_dist = torch.zeros_like(dot_product) 14 | mask = dot_product < 0 15 | par_dist[mask] = - dot_product[mask] * torch.norm(directions, dim=-1)[mask] 16 | mask = dot_product > 1. 17 | par_dist[dot_product > 1.] = (dot_product[mask] - 1) * torch.norm(directions, dim=-1)[mask] 18 | ort_dst = torch.abs(c[:, 0] * directions[:, 1] - c[:, 1] * directions[:, 0]) / torch.norm(directions, dim=-1) 19 | 20 | return par_dist, ort_dst 21 | 22 | 23 | class WallSegmentTerm(nn.Module): 24 | def __init__(self, wall, unproject, distance_function, floorplan): 25 | super(WallSegmentTerm, self).__init__() 26 | self.wall = wall 27 | self.unproject = unproject 28 | self.distance_function = distance_function 29 | self.register_buffer("floorplan", floorplan) 30 | 31 | def forward(self): 32 | wall_pcd = self.unproject(self.wall["depths"], self.wall["points"], self.wall["camera_idxs"]) 33 | par_dist, ort_dst = point_segment_distance(wall_pcd[:, [0, 1]], self.wall["segments"]) #TODO 01 34 | zeros = torch.zeros_like(par_dist) 35 | 36 | return self.distance_function(zeros, par_dist) + self.distance_function(zeros, ort_dst) 37 | -------------------------------------------------------------------------------- /facap/optimization/wall_term.py: -------------------------------------------------------------------------------- 1 | import torch 2 | 3 | from torch import nn 4 | 5 | 6 | def nearest_segment_distance(floorplan, pcd_2d): 7 | biases = torch.stack([i[:2] for i in floorplan]) 8 | directions = torch.stack([i[2:] - i[:2] for i in floorplan]) 9 | 10 | distances = [] 11 | 12 | for a, b in zip(directions, biases): 13 | c = pcd_2d - b 14 | dot_product = torch.sum(c * a, dim=-1) / a.dot(a) 15 | 16 | par_dist = torch.zeros_like(dot_product) 17 | par_dist[dot_product < 0] = - dot_product[dot_product < 0] * torch.norm(a) 18 | par_dist[dot_product > 1.] = (dot_product[dot_product > 1.] - 1) * torch.norm(a) 19 | ort_dst = torch.abs(c[:, 0] * a[1] - c[:, 1] * a[0]) / torch.norm(a) 20 | distances.append(torch.sqrt(par_dist ** 2 + ort_dst ** 2)) 21 | 22 | distances = torch.stack(distances) 23 | distances = torch.min(distances, dim=0).values 24 | return distances 25 | 26 | 27 | class WallTerm(nn.Module): 28 | def __init__(self, wall, unproject, distance_function, floorplan): 29 | super(WallTerm, self).__init__() 30 | self.wall = wall 31 | self.unproject = unproject 32 | self.distance_function = distance_function 33 | self.register_buffer("floorplan", floorplan) 34 | 35 | def forward(self): 36 | wall_pcd = self.unproject(self.wall["depths"], self.wall["points"], self.wall["camera_idxs"]) 37 | distances = nearest_segment_distance(self.floorplan, wall_pcd[:, [0, 1]]) #TODO 01 38 | zeros = torch.zeros_like(distances) 39 | 40 | return self.distance_function(zeros, distances) 41 | -------------------------------------------------------------------------------- /facap/optimization/parameters.py: -------------------------------------------------------------------------------- 1 | import torch 2 | 3 | from torch import nn 4 | 5 | from facap.data.scan import Camera 6 | from collections import OrderedDict 7 | 8 | 9 | class CameraParameters(nn.Module): 10 | def __init__(self, cameras): 11 | super(CameraParameters, self).__init__() 12 | self.cameras = sorted(list(cameras.keys())) 13 | self.cam2id = dict(zip(self.cameras, range(len(self.cameras)))) 14 | self.rotvecs = nn.Parameter(torch.stack([torch.from_numpy(cameras[i].rotvec) for i in cameras])) 15 | self.translations = nn.Parameter(torch.stack([torch.from_numpy(cameras[i].translation) for i in cameras])) 16 | f = torch.stack([torch.tensor(cameras[i].f) for i in cameras]) 17 | pp = torch.stack([torch.tensor(cameras[i].pp) for i in cameras]) 18 | self.register_buffer("f", f) 19 | self.register_buffer("pp", pp) 20 | self.cached_indexes = OrderedDict() 21 | self.n_cached = 5 22 | 23 | def get_cameras(self): 24 | cameras = {} 25 | 26 | for cam_id in self.cameras: 27 | idx = self.cam2id[cam_id] 28 | rotvec = self.rotvecs[idx].cpu().detach().numpy() 29 | translation = self.translations[idx].cpu().detach().numpy() 30 | f = self.f[idx].cpu().detach().numpy() 31 | pp = self.pp[idx].cpu().detach().numpy() 32 | cameras[cam_id] = Camera(f, pp, rotvec, translation) 33 | 34 | return cameras 35 | 36 | def get_tensors(self, camera_idxs): 37 | idxs = self.get_cached_indexes(camera_idxs) 38 | rotvecs = self.rotvecs[idxs] 39 | translations = self.translations[idxs] 40 | f = self.f[idxs] 41 | pp = self.pp[idxs] 42 | return rotvecs, translations, f, pp 43 | 44 | def get_cached_indexes(self, cameras): 45 | for key in self.cached_indexes: 46 | if key == id(cameras): 47 | return self.cached_indexes[key] 48 | if len(self.cached_indexes) == self.n_cached: 49 | self.cached_indexes.popitem(last=True) 50 | idxs = [self.cam2id[i] for i in cameras] 51 | self.cached_indexes.update({id(cameras): idxs}) 52 | self.cached_indexes.move_to_end(id(cameras), last=False) 53 | return idxs 54 | -------------------------------------------------------------------------------- /facap/feature_errors/ray_error.py: -------------------------------------------------------------------------------- 1 | import torch 2 | 3 | 4 | def ray_distance(a0, a, b0, b): 5 | c = torch.cross(a, b) 6 | c = c / torch.norm(c, dim=-1, keepdim=True) 7 | dr = torch.unsqueeze(torch.sum(c * (b0 - a0), dim=-1), dim=-1) 8 | a0 += dr * c 9 | 10 | def get_coef(a, b, a0, b0): 11 | return ((b0[:, 1] - a0[:, 1]) * a[:, 0] - (b0[:, 0] - a0[:, 0]) * a[:, 1]) / \ 12 | (b[:, 0] * a[:, 1] - a[:, 0] * b[:, 1]) 13 | 14 | beta = get_coef(a, b, a0, b0) 15 | alpha = get_coef(b, a, b0, a0) 16 | 17 | beta = torch.unsqueeze(beta, dim=-1) 18 | alpha = torch.unsqueeze(alpha, dim=-1) 19 | 20 | return (dr.squeeze(dim=-1), 21 | alpha.squeeze(dim=-1), 22 | beta.squeeze(dim=-1)) 23 | 24 | 25 | def ray_error(left, right, unproject, distance_function, max_depth=3., parallel_eps=1e-2, 26 | depths_weight=0.1, depths_scale=1000, **kwargs): 27 | zero_depth = torch.zeros_like(left["depths"]) 28 | ones_depth = torch.ones_like(left["depths"]) 29 | 30 | a0 = unproject(zero_depth, left["points"], left["camera_idxs"]) 31 | b0 = unproject(zero_depth, right["points"], right["camera_idxs"]) 32 | 33 | a1 = unproject(ones_depth, left["points"], left["camera_idxs"]) 34 | b1 = unproject(ones_depth, right["points"], right["camera_idxs"]) 35 | 36 | a = a1 - a0 37 | b = b1 - b0 38 | a = a / torch.norm(a, dim=-1, keepdim=True) 39 | b = b / torch.norm(b, dim=-1, keepdim=True) 40 | 41 | dr, left_est_depth, right_est_depth = ray_distance(a0, a, b0, b) 42 | 43 | c = torch.cross(a, b) 44 | norm_c = torch.norm(c, dim=-1) 45 | 46 | mask = (left_est_depth > 0) & (right_est_depth > 0) & (left_est_depth < max_depth) \ 47 | & (right_est_depth < max_depth) & (norm_c > parallel_eps) 48 | 49 | dr = dr[mask] 50 | scaled_right_depth = right["depths"] / depths_scale 51 | scaled_left_depth = left["depths"] / depths_scale 52 | 53 | zeros = torch.zeros_like(dr) 54 | ray_term = distance_function(zeros, dr) 55 | depths_distance = distance_function(scaled_right_depth[mask], right_est_depth[mask]) 56 | depths_distance += distance_function(scaled_left_depth[mask], left_est_depth[mask]) 57 | 58 | return ray_term + depths_weight * depths_distance 59 | -------------------------------------------------------------------------------- /facap/geometry/torch.py: -------------------------------------------------------------------------------- 1 | import torch 2 | 3 | 4 | def unproject_points_rotvec(depths, yxs, f, pp, rotvec, translation, scale=1): 5 | """ 6 | Unprojects a given depth map into a point cloud with inverse extrinsics. 7 | 8 | Parameters 9 | ---------- 10 | depths : torch.Tensor 11 | Array of source depths. 12 | yxs : torch.Tensor 13 | Coordinates of points which are corresponding to depths. 14 | f : torch.Tensor 15 | Focus with shape (2,) and (N, 2). 16 | pp : torch.Tensor 17 | The principal point with shape (2,) and (N, 2). 18 | rotvec : torch.Tensor 19 | Rotation vectors. 20 | translation : torch.Tensor 21 | Translations. 22 | scale : float, optional (default=1000) 23 | The scale for the map. 24 | 25 | Returns 26 | ------- 27 | torch.Tensor 28 | 3d coordinates of the given points. 29 | 30 | """ 31 | u, v = yxs[:, 0], yxs[:, 1] 32 | z_cam = torch.divide(depths, scale) 33 | if isinstance(f, tuple) or len(f.shape) == 1: 34 | x_cam = z_cam * (v - pp[0]) / f[0] 35 | y_cam = z_cam * (u - pp[1]) / f[1] 36 | else: 37 | x_cam = z_cam * (v - pp[:, 0]) / f[:, 0] 38 | y_cam = z_cam * (u - pp[:, 1]) / f[:, 1] 39 | 40 | coords_cam = torch.stack((x_cam, y_cam, z_cam)).T 41 | coords_world = rotate(coords_cam, rotvec) + translation 42 | return coords_world 43 | 44 | 45 | def project_points_rotvec(coords_world, f, pp, rotvec, translation): 46 | """ 47 | Unprojects a given depth map into a point cloud with inverse extrinsics. 48 | 49 | Parameters 50 | ---------- 51 | depths : torch.Tensor 52 | Array of source depths. 53 | yxs : torch.Tensor 54 | Coordinates of points which are corresponding to depths. 55 | f : torch.Tensor 56 | Focus with shape (2,) and (N, 2). 57 | pp : torch.Tensor 58 | The principal point with shape (2,) and (N, 2). 59 | rotvec : torch.Tensor 60 | Rotation vectors. 61 | translation : torch.Tensor 62 | Translations. 63 | scale : float, optional (default=1000) 64 | The scale for the map. 65 | 66 | Returns 67 | ------- 68 | torch.Tensor 69 | 3d coordinates of the given points. 70 | 71 | """ 72 | coords_cam = rotate(coords_world - translation, -rotvec) 73 | x_cam, y_cam, z_cam = coords_cam[:, 0], coords_cam[:, 1], coords_cam[:, 2] 74 | if isinstance(f, tuple) or len(f.shape) == 1: 75 | v = torch.divide(x_cam, z_cam) * f[0] + pp[0] 76 | u = torch.divide(y_cam, z_cam) * f[1] + pp[1] 77 | else: 78 | v = torch.divide(x_cam, z_cam) * f[:, 0] + pp[:, 0] 79 | u = torch.divide(y_cam, z_cam) * f[:, 1] + pp[:, 1] 80 | yxs = torch.stack((u, v)).T 81 | return yxs 82 | 83 | 84 | def rotate(points, rotvec): 85 | """ 86 | Rotates the given point cloud. 87 | 88 | Parameters 89 | ---------- 90 | 91 | rotvec : torch.Tensor 92 | Rotation vectors. 93 | points : torch.Tensor 94 | 3d point cloud. 95 | 96 | Returns 97 | ------- 98 | torch.Tensor 99 | Rotated point cloud. 100 | 101 | """ 102 | 103 | if len(rotvec.shape) == 1: 104 | rotvec = rotvec.repeat(len(points)).reshape(-1, 3) 105 | theta = torch.linalg.norm(rotvec, dim=1).unsqueeze(1) 106 | v = torch.divide(rotvec, theta) 107 | v = torch.where(torch.isnan(v), torch.zeros_like(v), v) 108 | dot = torch.sum(points * v, dim=1).unsqueeze(1) 109 | cos_theta = torch.cos(theta) 110 | sin_theta = torch.sin(theta) 111 | return cos_theta * points + sin_theta * torch.cross(v, points) + dot * (1 - cos_theta) * v 112 | -------------------------------------------------------------------------------- /facap/utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import torch 3 | import open3d as o3d 4 | 5 | from matplotlib import pyplot as plt 6 | from facap.geometry.numpy import unproject_points_rotvec 7 | from facap.geometry.open3d import make_pcd_from_numpy, make_line_set 8 | 9 | 10 | def read_npy(path): 11 | paths = np.load(path, allow_pickle=True) 12 | 13 | edges = [] 14 | 15 | for path in paths: 16 | ed = list(zip(path[1:].tolist(), path[:-1].tolist())) 17 | ed = [(tuple(i[0]), tuple(i[1])) for i in ed if i[0] != i[1]] 18 | edges.extend(ed) 19 | corners = [tuple(elem) for edge in edges for elem in edge] 20 | corners = set(corners) 21 | 22 | return corners, edges 23 | 24 | 25 | def plot_graph(edges, color, name, markersize=20): 26 | for j, (st, end) in enumerate(edges): 27 | if j == 0: 28 | plt.plot([st[0], end[0]], [st[1], end[1]], f"{color}.-", label=name, markersize=markersize) 29 | else: 30 | plt.plot([st[0], end[0]], [st[1], end[1]], f"{color}.-", markersize=markersize) 31 | 32 | 33 | def dicts_to_torch(dict_list, device): 34 | for dct in dict_list: 35 | for key in dct: 36 | if key != "camera_idxs": 37 | dct[key] = torch.from_numpy(dct[key]).to(device).float() 38 | 39 | 40 | def dicts_to_numpy(dict_list): 41 | for dct in dict_list: 42 | for key in dct: 43 | if key != "camera_idxs": 44 | dct[key] = dct[key].cpu().detach().numpy() 45 | 46 | 47 | def visualize_aligned_walls(wall_data, save_path, scale=1): 48 | segments = [tuple(i.tolist()) for i in wall_data["segments"]] 49 | #print('seg\n', len(segments), segments) 50 | unique_segments = list(set(segments)) 51 | 52 | plt.figure(figsize=(8, 8)) 53 | 54 | segment_to_color = { i: np.random.rand(3) for i in unique_segments} 55 | points_colors = [segment_to_color[i] for i in segments] 56 | 57 | pcd = unproject_points_rotvec(wall_data["depths"], wall_data["points"], 58 | wall_data["f"], wall_data["pp"], 59 | wall_data["rotvecs"], wall_data["translations"], scale=scale) 60 | 61 | for s, c in segment_to_color.items(): 62 | plt.plot([s[0], s[2]], [s[1], s[3]], c=c) 63 | plt.scatter(pcd[:, 0], pcd[:, 1], c=points_colors) #TODO 2 64 | plt.savefig(f"{save_path}/alligned_walls.png") 65 | 66 | 67 | def visualize_data(data, save_path, suffix="", scale=1): 68 | left, right, wall, floor, ceiling = data 69 | left_pcd = unproject_points_rotvec(left["depths"], left["points"], left["f"], 70 | left["pp"], left["rotvecs"], left["translations"], scale=scale) 71 | 72 | right_pcd = unproject_points_rotvec(right["depths"], right["points"], right["f"], 73 | right["pp"], right["rotvecs"], right["translations"], scale=scale) 74 | 75 | floor_pcd = unproject_points_rotvec(floor["depths"], floor["points"], floor["f"],floor["pp"], floor["rotvecs"], floor["translations"], scale=scale) 76 | 77 | wall_pcd = unproject_points_rotvec(wall["depths"], wall["points"], wall["f"],wall["pp"], wall["rotvecs"], wall["translations"], scale=scale) 78 | 79 | ceiling_pcd = unproject_points_rotvec(ceiling["depths"], ceiling["points"], ceiling["f"],ceiling["pp"], ceiling["rotvecs"], ceiling["translations"], scale=scale) 80 | 81 | 82 | if "segments" in wall: 83 | visualize_aligned_walls(wall, save_path, scale=scale) 84 | 85 | 86 | red = np.array([1, 0, 0]) 87 | green = np.array([0, 1, 0]) 88 | blue = np.array([0, 0, 1]) 89 | left_pcd = make_pcd_from_numpy(left_pcd, red) 90 | right_pcd = make_pcd_from_numpy(right_pcd, green) 91 | wall_pcd = make_pcd_from_numpy(wall_pcd, red) 92 | floor_pcd = make_pcd_from_numpy(floor_pcd, blue) 93 | ceiling_pcd = make_pcd_from_numpy(ceiling_pcd, blue) 94 | line_set = make_line_set(left_pcd, right_pcd, green) 95 | 96 | 97 | o3d.io.write_line_set(f"{save_path}/keypoints"+suffix+".ply", line_set) 98 | o3d.io.write_point_cloud(f"{save_path}/wall"+suffix+".ply", wall_pcd) 99 | o3d.io.write_point_cloud(f"{save_path}/floor"+suffix+".ply", floor_pcd) 100 | o3d.io.write_point_cloud(f"{save_path}/ceiling"+suffix+".ply", ceiling_pcd) 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | -------------------------------------------------------------------------------- /facap/geometry/open3d.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | import open3d as o3d 4 | from open3d import visualization 5 | 6 | 7 | def project_points(pcd, extrinsic, f, pp, width, height): 8 | """ 9 | Projects a given depth map into a depth map with Open3D. 10 | 11 | Parameters 12 | ---------- 13 | pcd : open3d.geometry.PointCloud 14 | Source point cloud. 15 | f : tuple 16 | Focus (fx, fy). 17 | pp : tuple 18 | The principal point (cx, cy). 19 | extrinsic : numpy.ndarray 20 | The 4x4 extrinsic matrix. 21 | width : int 22 | Width of the map. 23 | height : int 24 | Height of the map. 25 | 26 | Returns 27 | ------- 28 | numpy.ndarray 29 | Depth map. 30 | 31 | """ 32 | camera = o3d.camera.PinholeCameraParameters() 33 | camera.intrinsic.set_intrinsics(width, height, f[0], f[1], pp[0] - 0.5, pp[1] - 0.5) 34 | camera.extrinsic = extrinsic 35 | viewer = visualization.Visualizer() 36 | viewer.create_window(width=width, height=height) 37 | viewer.add_geometry(pcd) 38 | viewer.get_view_control().convert_from_pinhole_camera_parameters(camera) 39 | viewer.poll_events() 40 | viewer.update_renderer() 41 | depth = viewer.capture_depth_float_buffer() 42 | viewer.destroy_window() 43 | depth = (np.asarray(depth).astype(np.float32)).astype(np.uint16) 44 | return depth 45 | 46 | 47 | def unproject_points(depth_map, color_map, extrinsic, f, pp, width, height): 48 | """ 49 | Unprojects a given depth map into a point cloud using Open3D. 50 | 51 | Parameters 52 | ---------- 53 | depth_map : numpy.ndarray 54 | Source depth map. 55 | color_map : numpy.ndarray 56 | Source rgb image. 57 | f : tuple 58 | Focus (fx, fy). 59 | pp : tuple 60 | The principal point (cx, cy). 61 | extrinsic : numpy.ndarray 62 | The 4x4 extrinsic matrix. 63 | width : int 64 | Width of the map. 65 | height : int 66 | Height of the map. 67 | 68 | Returns 69 | ------- 70 | open3d.geometry.PointCloud 71 | The point cloud constructed from the given data. 72 | 73 | """ 74 | 75 | camera = o3d.camera.PinholeCameraIntrinsic() 76 | camera.set_intrinsics(width, height, f[0], f[1], pp[0], pp[1]) 77 | color = o3d.geometry.Image(color_map) 78 | depth = o3d.geometry.Image(depth_map.astype(np.float32)) 79 | rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(color, depth, 80 | convert_rgb_to_intensity=False, depth_scale=1) 81 | pcd = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd, camera, extrinsic) 82 | return pcd 83 | 84 | 85 | def sample_points_from_pcd(pcd, num_points): 86 | new_pcd = o3d.geometry.PointCloud() 87 | xyz = np.asarray(pcd.points) 88 | color = np.asarray(pcd.colors) 89 | 90 | indexes = np.random.randint(0, len(xyz), num_points) 91 | 92 | xyz = xyz[indexes] 93 | color = color[indexes] 94 | 95 | new_pcd.points = o3d.utility.Vector3dVector(xyz) 96 | new_pcd.colors = o3d.utility.Vector3dVector(color) 97 | return new_pcd 98 | 99 | 100 | def make_pcd_from_numpy(xyz, colors): 101 | pcd = o3d.geometry.PointCloud() 102 | pcd.points = o3d.utility.Vector3dVector(xyz) 103 | 104 | if len(colors.shape) == 1: 105 | colors = np.ones_like(xyz) * colors 106 | pcd.colors = o3d.utility.Vector3dVector(colors) 107 | pcd.estimate_normals() 108 | return pcd 109 | 110 | 111 | def make_line_set(pcd1, pcd2, colors=np.array([1, 0, 0]), num_points=None): 112 | points1 = np.asarray(pcd1.points) 113 | points2 = np.asarray(pcd2.points) 114 | 115 | if num_points is not None: 116 | indexes = np.random.randint(0, len(points1), num_points) 117 | 118 | points1 = points1[indexes] 119 | points2 = points2[indexes] 120 | 121 | lines = o3d.geometry.LineSet() 122 | points = np.r_[points1, points2] 123 | lines.points = o3d.utility.Vector3dVector(points) 124 | 125 | size = len(points) // 2 126 | l = np.c_[np.arange(size), np.arange(size) + size] 127 | lines.lines = o3d.utility.Vector2iVector(l) 128 | if len(colors.shape) == 1: 129 | colors = np.ones((size, 3)) * colors 130 | lines.colors = o3d.utility.Vector3dVector(colors) 131 | 132 | return lines 133 | -------------------------------------------------------------------------------- /facap/geometry/numpy.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | def project_points(pcd, f, pp, extrinsic, height, width, scale=1): 5 | """ 6 | Projects a given point cloud to 2d according to camera's pose. 7 | 8 | Parameters 9 | ---------- 10 | pcd : numpy.ndarray 11 | Source point cloud. 12 | f : tuple 13 | Focus (fx, fy). 14 | pp : tuple 15 | The principal point (cx, cy). 16 | extrinsic : numpy.ndarray 17 | The 4x4 extrinsic matrix. 18 | scale : float, optional (default=1000) 19 | The scale for the map. 20 | width : int 21 | Width of the map. 22 | height : int 23 | Height of the map. 24 | 25 | Returns 26 | ------- 27 | numpy.ndarray 28 | 2d depth map. 29 | 30 | """ 31 | 32 | depth_map = np.zeros((height, width)) 33 | intrinsic = np.array([[f[0], 0, pp[0]], 34 | [0, f[1], pp[1]], 35 | [0, 0, 1]]) 36 | proj_mat = np.dot(intrinsic, extrinsic[:3]) 37 | pcd_xyz1 = np.pad(pcd, [[0, 0], [0, 1]], 'constant', constant_values=1).T 38 | p_uv_1 = np.dot(proj_mat, pcd_xyz1) 39 | p_uv = np.divide(p_uv_1, p_uv_1[2])[:2] 40 | pos_x = np.array(list(map(lambda x: round(x), p_uv[0]))) 41 | pos_y = np.array(list(map(lambda y: round(y), p_uv[1]))) 42 | valid_ids = [] 43 | for i, (x, y) in enumerate(zip(pos_x, pos_y)): 44 | if x < 0 or y < 0 or x >= width or y >= height: 45 | continue 46 | if p_uv_1[2][i] < 0 or p_uv_1[2][i] > 3.: 47 | continue 48 | valid_ids.append(i) 49 | valid_ids = np.array(valid_ids) 50 | if len(valid_ids) > 0: 51 | depth_map[pos_y[valid_ids], pos_x[valid_ids]] = np.maximum(0, p_uv_1[2][valid_ids]) * ( 52 | p_uv_1[2][valid_ids] < 3.) 53 | depth_map *= scale 54 | return depth_map, valid_ids 55 | 56 | 57 | def unproject_points(depth_map, coords_2d, f, pp, inv_extrinsic, scale=1): 58 | """ 59 | Unprojects a given depth map into a point cloud with inverse extrinsics. 60 | 61 | Parameters 62 | ---------- 63 | depth_map : numpy.ndarray 64 | Source depth map. 65 | coords_2d : numpy.ndarray 66 | Coordinates of using points on the map. 67 | f : tuple 68 | Focus (fx, fy). 69 | pp : tuple 70 | The principal point (cx, cy). 71 | inv_extrinsic : numpy.ndarray 72 | The 4x4 matrix with the pose of the camera. 73 | scale : float, optional (default=1000) 74 | The scale for the map. 75 | 76 | Returns 77 | ------- 78 | numpy.ndarray 79 | 3d coordinates of the given points. 80 | 81 | """ 82 | u, v = np.array(list(zip(coords_2d)))[:, 0, :] 83 | z_cam = depth_map / scale 84 | if isinstance(f, tuple) or len(f.shape) == 1: 85 | x_cam = z_cam * (v - pp[0]) / f[0] 86 | y_cam = z_cam * (u - pp[1]) / f[1] 87 | else: 88 | x_cam = z_cam * (v - pp[:, 0]) / f[:, 0] 89 | y_cam = z_cam * (u - pp[:, 1]) / f[:, 1] 90 | # x_cam = z_cam * (v - pp[0]) / f[0] 91 | # y_cam = z_cam * (u - pp[1]) / f[1] 92 | 93 | coords_cam = np.stack((x_cam, y_cam, z_cam, np.ones_like(z_cam))) 94 | coords_world = inv_extrinsic @ coords_cam.T[..., None] 95 | 96 | return coords_world[:, :3, 0] 97 | 98 | 99 | def unproject_points_rotvec(depths, yxs, f, pp, rotvec, translation, scale=1): 100 | """ 101 | Unprojects a given depth map into a point cloud with inverse extrinsics. 102 | 103 | Parameters 104 | ---------- 105 | depths : numpy.ndarray 106 | Array of source depths. 107 | yxs : numpy.ndarray 108 | Coordinates of points which are corresponding to depths. 109 | f : tuple 110 | Focus (fx, fy). 111 | pp : tuole 112 | The principal point (cx, cy). 113 | rotvec : numpy.ndarray 114 | Rotation vectors. 115 | translation : numpy.ndarray 116 | Translations. 117 | scale : float, optional (default=1000) 118 | The scale for the map. 119 | 120 | Returns 121 | ------- 122 | numpy.ndarray 123 | 3d coordinates of the given points. 124 | 125 | """ 126 | 127 | u, v = yxs[:, 0], yxs[:, 1] 128 | z_cam = depths / scale 129 | if isinstance(f, tuple) or len(f.shape) == 1: 130 | x_cam = z_cam * (v - pp[0]) / f[0] 131 | y_cam = z_cam * (u - pp[1]) / f[1] 132 | else: 133 | x_cam = z_cam * (v - pp[:, 0]) / f[:, 0] 134 | y_cam = z_cam * (u - pp[:, 1]) / f[:, 1] 135 | # x_cam = z_cam * (v - pp[0]) / f[0] 136 | # y_cam = z_cam * (u - pp[1]) / f[1] 137 | 138 | coords_cam = np.stack((x_cam, y_cam, z_cam)).T 139 | if len(rotvec.shape) == 1: 140 | coords_world = rotate(coords_cam, rotvec[None]) + translation 141 | else: 142 | coords_world = rotate(coords_cam, rotvec) + translation 143 | 144 | return coords_world 145 | 146 | 147 | def rotate(points, rotvec): 148 | """ 149 | Rotates the given point cloud. 150 | 151 | Parameters 152 | ---------- 153 | 154 | rotvec : numpy.ndarray 155 | Rotation vectors. 156 | points : numpy.ndarray 157 | 3d point cloud. 158 | 159 | Returns 160 | ------- 161 | numpy.ndarray 162 | Rotated point cloud. 163 | 164 | """ 165 | 166 | theta = np.linalg.norm(rotvec, axis=1)[:, np.newaxis] 167 | with np.errstate(invalid='ignore'): 168 | v = rotvec / theta 169 | v = np.nan_to_num(v) 170 | dot = np.sum(points * v, axis=1)[:, np.newaxis] 171 | cos_theta = np.cos(theta) 172 | sin_theta = np.sin(theta) 173 | return cos_theta * points + sin_theta * np.cross(v, points) + dot * (1 - cos_theta) * v 174 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 |

2 | 3 | BIMCaP 4 | 5 |

6 |

7 | BIM-based AI-supported LiDAR-Camera Pose Refinement. 8 |

9 | 10 | 11 | 12 |

13 | ArXiv Paper • 14 | BIM model Data • 15 | Construcion Site Semantic Segmentation Data 16 |

17 | 18 | This repository will contain the implementation of BIM-based AI-supported LiDAR-Camera Pose Refinement [paper](https://mediatum.ub.tum.de/node?id=1748534). 19 | This project is an extension of the [FACaP](https://github.com/SamsungLabs/FACaP) project. 20 | 21 | ## Introduction 22 | 23 | This paper introduces a method conceived to overcome the challenge of integrating mobile 3D sparse LiDAR data and camera measurements with pre-existing building information models (BIM models). This enhancement is crucial for fast and accurate mapping conducted with affordable sensors within indoor environments. 24 | Our proposed framework, termed BIMCaP, presents a novel approach for automatic sensor pose refinement, leveraging a reference 3D BIM model. 25 | Central to our methodology is the application of a bundle adjustment technique, which seamlessly aligns semantically enhanced measurements from the real world with a BIM model. 26 | Through experimentation on real-world open-access data, we demonstrate the superior accuracy of our framework, surpassing current state-of-the-art methods. 27 | BIMCaP holds significant promise for enhancing the efficiency and cost-effectiveness of 3D mapping methodologies. 28 | This advancement carries substantial advantages across various domains, such as in construction site management and emergency response, where up-to-date digital maps facilitate better decision-making and enhanced productivity. 29 | 30 | ![](imgs/BIMCaP_v2.png) 31 | 32 | We enhance the original FACaP pipeline by leveraging a 3D BIM model, fusioning sparse LiDAR and Camera sensors and implementing semantic segmentation on challenging construction site environments. 33 | 34 | ![](imgs/bimfloor.png) 35 | 36 | 37 | 38 | ## Installation 39 | 40 | We recommend using `Dockerfile` to build a container for the project. 41 | All required libraries are described in the environment file `env.yaml`. In the final version 42 | we drop all of the `pytorch3d` dependencies, so you do not have to install it. 43 | 44 | ## Data structure 45 | All scans should be preprocessed to the next structure: 46 | ``` 47 | scan 48 | │ floorplan.npy 49 | | floorplane.ply 50 | | ceiling.ply 51 | │ 52 | └───db 53 | │ └───sparse 54 | │ └───0 55 | | | cameras.bin 56 | | | images.bin 57 | | | points3D.bin 58 | │ 59 | └───arcore 60 | │ │ cam_params.txt 61 | │ │ ... 62 | │ │ depth-0001.png 63 | │ │ ... 64 | │ │ frame-0001.png 65 | │ │ ... 66 | │ │ pose-0001.txt 67 | │ │ ... 68 | │ │ 69 | │ 70 | └───segmentation 71 | │ │ frame-0001_wall.png 72 | │ │ ... 73 | │ │ frame-0001_floor.png 74 | │ │ ... 75 | │ │ frame-0001_ceiling.png 76 | │ │ ... 77 | │ │ frame-0001_columns.png 78 | │ 79 | 80 | ``` 81 | 82 | Here: 83 | - `floorplan.npy` is an array with the shape `n x 4`. Each element is a segment of the floorplan. 84 | - `floorplane.ply` is a pointcloud of the floor in the BIM model. 85 | - `ceiling.ply` is a pointcloud of the ceiling in the BIM model. 86 | - `db` features a database in COLMAP format, which is used to map covisible points. This can be computed from the image frames using the [COLMAP](https://colmap.github.io/cli.html) automatic reconstructor. 87 | - `cam_params.txt` intrinsics of the corresponding camera (w, h, f1, f1, p1, p2). 88 | - `pose-0001.txt` extrinsic matrix of the corresponding camera 89 | - `depth-0001.npy` depth map 90 | - `frame-0001.png` RGB frame 91 | - `frame-0001_wall.png` mask of walls for the corresponding frame 92 | - `frame-0001_floor.png` mask of the floor for the corresponding frame 93 | - `frame-0001_ceiling.png` mask of the ceiling for the corresponding frame 94 | - `frame-0001_columns.png` rotated mask of the columns for the corresponding frame 95 | 96 | 97 | For more details please see the file `facap/data/scan.py`. 98 | 99 | ## Usage 100 | 101 | To run an experiment you should create a config file run experiment. 102 | 103 | ```python 104 | python scripts/run_experimnt.py --config path_to_config --device "cuda:0" 105 | ``` 106 | 107 | The example of the config can be found in the path `experiments/config.yaml`. 108 | 109 | ## Citation 110 | 111 | ```BibTeX 112 | 113 | @inproceedings{Vega:2024:BIMCaP, 114 | author = {Vega-Torres, Miguel A. and Ribic, Anna and García de Soto, Borja and Borrmann, André}, 115 | title = {BIMCaP: BIM-based AI-supported LiDAR-Camera Pose Refinement}, 116 | booktitle = { Proc. of the 31th Int. Conference on Intelligent Computing in Engineering (EG-ICE)}, 117 | year = {2024}, 118 | month = {Jul}, 119 | doi = {https://doi.org/10.48550/arXiv.2412.03434}, 120 | keywords = {Construction Robotics; SLAM; BIM; Bundle Adjustment; Long-term Mapping}, 121 | url = {https://github.com/MigVega/BIMCaP} 122 | } 123 | 124 | ``` 125 | 126 | ## Acknowledgements 127 | This project is an extension of [FACaP](https://github.com/SamsungLabs/FACaP) by Sokolova, Anna and Nikitin, Filipp and Vorontsova, Anna and Konushin, Anton, which is licensed under the MIT License. 128 | 129 | ## License 130 | For academic usage, the code is released under the [GPLv3 license](https://www.gnu.org/licenses/gpl-3.0.en.html). For any commercial purpose, please contact the authors. 131 | -------------------------------------------------------------------------------- /facap/geometry/allign_walls.py: -------------------------------------------------------------------------------- 1 | from copy import deepcopy 2 | 3 | import numpy as np 4 | import open3d as o3d 5 | from scipy.signal import find_peaks 6 | from tqdm import tqdm 7 | 8 | from facap.geometry.numpy import unproject_points_rotvec 9 | from facap.geometry.open3d import make_pcd_from_numpy 10 | 11 | 12 | def find_nearest_segment(floorplan, wall): 13 | biases = np.stack([i[:2] for i in floorplan]) 14 | directions = np.stack([i[2:] - i[:2] for i in floorplan]) 15 | 16 | distances = [] 17 | 18 | for a, b in zip(directions, biases): 19 | c = wall - b 20 | dot_product = np.sum(c * a, axis=-1) / a.dot(a) 21 | 22 | par_dist = np.zeros_like(dot_product) 23 | par_dist[dot_product < 0] = - dot_product[dot_product < 0] * np.linalg.norm(a) 24 | par_dist[dot_product > 1.] = (dot_product[dot_product > 1.] - 1) * np.linalg.norm(a) 25 | ort_dst = np.abs(c[:, 0] * a[1] - c[:, 1] * a[0]) / np.linalg.norm(a) 26 | distances.append(np.sqrt(par_dist ** 2 + ort_dst ** 2)) 27 | 28 | distances = np.stack(distances) 29 | distances = np.mean(distances, axis=1) 30 | segment_idx = np.argmin(distances, axis=0) 31 | 32 | return segment_idx, distances[segment_idx] 33 | 34 | 35 | def get_distinct_walls(pcd, indexes, alpha, 36 | max_vertical_component=.3, 37 | min_num_points=500, 38 | num_ransac_tries=20000, 39 | max_collinearity_rate=.2): 40 | walls = [] 41 | cur_pcd = deepcopy(pcd) 42 | 43 | for i in tqdm(range(num_ransac_tries)): 44 | if len(cur_pcd.points) >= min_num_points: 45 | plane_model, inliers = cur_pcd.segment_plane(distance_threshold=0.01, 46 | ransac_n=10, 47 | num_iterations=100) 48 | a, b, c, _ = plane_model 49 | 50 | if (b <= max_vertical_component) and (abs(alpha - np.arctan(c / a)) <= max_collinearity_rate): 51 | wall_pcd = cur_pcd.select_by_index(inliers) 52 | wall = {"plane_model": plane_model, 53 | "pcd": wall_pcd, 54 | "indexes": indexes[inliers]} 55 | 56 | cur_pcd = cur_pcd.select_by_index(inliers, invert=True) 57 | indexes = np.delete(indexes, inliers) 58 | walls.append(wall) 59 | return walls 60 | 61 | 62 | def join_walls(walls): 63 | wall = o3d.geometry.PointCloud() 64 | plane_models = [] 65 | indexes = [] 66 | for w in walls: 67 | wall += w["pcd"] 68 | plane_models.append(w["plane_model"]) 69 | indexes.append(w["indexes"]) 70 | plane_model = np.array(plane_models).mean(axis=0).tolist() 71 | indexes = np.concatenate(indexes) 72 | wall = {"pcd": wall, "plane_model": plane_model, "indexes": indexes} 73 | return wall 74 | 75 | 76 | def unite_walls(walls, iou_thr=0.8, colin_thr=0.8): 77 | united_walls = {} 78 | for i, wall in tqdm(enumerate(walls)): 79 | new_id = -1 80 | for wall_id in united_walls: 81 | for w in united_walls[wall_id]: 82 | dist_1 = np.array(wall["pcd"].compute_point_cloud_distance(w["pcd"])) 83 | dist_2 = np.array(w["pcd"].compute_point_cloud_distance(wall["pcd"])) 84 | plane_model_1 = wall["plane_model"] 85 | plane_model_2 = w["plane_model"] 86 | if ((dist_1 < .2).mean() > iou_thr or (dist_2 < .2).mean() > iou_thr) and abs( 87 | plane_model_1[:3].dot(plane_model_2[:3])) > colin_thr: 88 | new_id = wall_id 89 | 90 | if len(united_walls) == 0: 91 | new_id = 0 92 | elif new_id == -1: 93 | new_id = max(list(united_walls.keys())) + 1 94 | if new_id not in united_walls: 95 | united_walls[new_id] = [] 96 | united_walls[new_id].append(wall) 97 | 98 | walls = [] 99 | for wall_id in united_walls: 100 | wall = join_walls(united_walls[wall_id]) 101 | walls.append(wall) 102 | return walls 103 | 104 | #TODO look here 105 | def reorder_wall(wall, segment2wall, floorplan): 106 | indexes = [] 107 | segments = [] 108 | print('wallreoder',wall) 109 | for segment_id, w in segment2wall.items(): 110 | indexes.append(w["indexes"]) 111 | segments.append(np.stack([floorplan[segment_id]] * len(w["indexes"]))) 112 | indexes = np.concatenate(indexes) 113 | segments = np.concatenate(segments) 114 | 115 | for key in wall: 116 | print('keyindex',key, indexes) 117 | wall[key] = wall[key][indexes] 118 | wall["segments"] = segments 119 | return wall 120 | 121 | 122 | def get_aligned_walls(walls, floorplan): 123 | plane2walls = {} 124 | for i, w in enumerate(walls): 125 | wall_pcd = np.asarray(w["pcd"].points)[:, [0, 1]] #TODO 0,2 126 | idx, distance = find_nearest_segment(floorplan, wall_pcd) 127 | if idx not in plane2walls: 128 | plane2walls[idx] = [w, ] 129 | else: 130 | plane2walls[idx].append(w) 131 | 132 | for segment_id in plane2walls: 133 | plane2walls[segment_id] = join_walls(plane2walls[segment_id]) 134 | return plane2walls 135 | 136 | 137 | def align_walls(wall_data, floorplan, scale=1): 138 | walls_pcd = unproject_points_rotvec(wall_data["depths"], wall_data["points"], wall_data["f"], wall_data["pp"], 139 | wall_data["rotvecs"], wall_data["translations"], scale=scale) 140 | walls_pcd = make_pcd_from_numpy(walls_pcd, np.array([0, 1, 0])) 141 | 142 | indexes = np.arange(len(walls_pcd.points)) 143 | normals = np.asarray(walls_pcd.normals) 144 | vertical_component_condition = np.abs(normals[..., 2]) < 0.3 # TODO normal 1 145 | norm_proj = normals[..., [0, 1]][vertical_component_condition] #TODO normals 2 146 | 147 | angles = np.arctan(norm_proj[..., 1] / (1e-10 + norm_proj[..., 0])) 148 | counts, bins = np.histogram(angles, bins=360) 149 | peaks = find_peaks(counts, height=300, distance=50, prominence=30) 150 | 151 | walls = [] 152 | for peak in peaks[0]: 153 | alpha = (bins[peak] + bins[peak + 1]) / 2 154 | 155 | angle_condition = np.abs((np.arctan(normals[..., 1] / (normals[..., 0] + 1e-10))) - alpha) < .1 #TODO normals 2 156 | condition = vertical_component_condition & angle_condition 157 | mask = np.where(condition)[0] 158 | filtered_by_normals = walls_pcd.select_by_index(mask) 159 | filtered_indexes = indexes[mask] 160 | walls.extend(get_distinct_walls(filtered_by_normals, filtered_indexes, alpha)) 161 | 162 | walls = unite_walls(walls, iou_thr=0.7) 163 | plane2walls = get_aligned_walls(walls, floorplan) 164 | #print('plane2walls', len(plane2walls), plane2walls) 165 | wall_data = reorder_wall(wall_data, plane2walls, floorplan) 166 | return wall_data 167 | -------------------------------------------------------------------------------- /scripts/run_experiment.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import torch 3 | import os 4 | import open3d as o3d 5 | import numpy as np 6 | 7 | import yaml 8 | from torch import nn 9 | from torch import optim 10 | import torch.nn.functional as F 11 | from torch.linalg import norm 12 | 13 | from facap import feature_errors 14 | from facap.data.scan import Scan 15 | from facap.optimization import Project, Unproject, CameraParameters, FloorTerm, WallTerm, WallSegmentTerm 16 | from facap.utils import dicts_to_torch, visualize_data 17 | from facap.geometry.allign_walls import align_walls 18 | from scipy.spatial.transform import Rotation as R 19 | 20 | #todo get floorplan from BIM 21 | 22 | def compute_pose_rmse(translations_gt, rotations_gt, translations_est, rotations_est): 23 | # Compute RMSE for translations 24 | translation_errors = np.linalg.norm(translations_gt - translations_est, axis=1) 25 | translation_err = np.mean(translation_errors) 26 | 27 | # Compute RMSE for rotations 28 | degree_errors = [] 29 | for rotvec_gt, rotvec_est in zip(rotations_gt, rotations_est): 30 | rotation_gt = R.from_rotvec(rotvec_gt) 31 | rotation_est = R.from_rotvec(rotvec_est) 32 | 33 | cosine_similarity = np.trace(np.dot(np.transpose(rotation_est.as_matrix()), rotation_gt.as_matrix())) 34 | angular_distance_radians = np.arccos((cosine_similarity - 1) / 2) 35 | 36 | # Convert angular distance from radians to degrees 37 | angular_distance_degrees = angular_distance_radians * (180 / np.pi) 38 | 39 | angular_distance_degrees = min(angular_distance_degrees, 180-angular_distance_degrees) 40 | 41 | print("Angular distance between estimated and ground truth rotations:", angular_distance_degrees, "degrees") 42 | 43 | degree_errors.append(angular_distance_degrees) 44 | 45 | rotation_err = np.mean(degree_errors) 46 | 47 | return translation_err, rotation_err 48 | 49 | 50 | if __name__ == '__main__': 51 | print('Started experiment') 52 | parser = argparse.ArgumentParser() 53 | parser.add_argument("--config", help="YAML configuration file") 54 | parser.add_argument("--device", default='cuda:0', help="Device to run") 55 | args = parser.parse_args() 56 | with open(args.config, 'r') as ymlfile: 57 | cfg = yaml.load(ymlfile, Loader=yaml.FullLoader) 58 | 59 | #Scan should have camera poses 60 | scan_path = cfg["paths"]["scan_path"] 61 | scan = Scan(scan_path, scale=cfg["data"]["depths_scale"])# , cut_frames=70) 62 | 63 | 64 | save_path = cfg["paths"]["save_path"] 65 | os.makedirs(save_path, exist_ok=True) 66 | 67 | mesh, pcd = scan.make_mesh_pcd() 68 | o3d.io.write_triangle_mesh(f"{save_path}/source_mesh.ply", mesh) 69 | o3d.io.write_point_cloud(f"{save_path}/source_pcd.ply", pcd) 70 | 71 | print('Written Source Mesh and PointCloud') 72 | 73 | #Extract scan data 74 | data = scan.generate_ba_data(min_frame_difference=cfg["data"]["min_frame_difference"], 75 | max_initial_distance=cfg["data"]["max_initial_distance"], 76 | floor_percentiles=cfg["data"]["floor_percentiles"], 77 | wall_sparsity=cfg["data"]["wall_sparsity"], 78 | floor_sparsity=cfg["data"]["floor_sparsity"], 79 | include_columns=cfg["error"]["include_columns"] 80 | ) 81 | 82 | print('Generated data') 83 | if "wall_term_type" in cfg["error"] and cfg["error"]["wall_term_type"] == "segment": 84 | floorplan = torch.from_numpy(np.load(f"{scan_path}/floorplan.npy")) 85 | alligned_walls = align_walls(data[2], floorplan, scale=scan.scale) 86 | data = (data[0], data[1], alligned_walls, data[3], data[4]) 87 | 88 | visualize_data(data, save_path=save_path, scale=scan.scale) 89 | dicts_to_torch(data, args.device) 90 | 91 | #Extract left? 92 | left, right, wall, floor, ceiling = data 93 | 94 | #Get camere parameters 95 | camera_parameters = CameraParameters(scan.cameras).to(args.device).float() 96 | unproject = Unproject(camera_parameters, scale=scan.scale) 97 | project = Project(camera_parameters) 98 | cost_function = nn.MSELoss() 99 | 100 | use_bim = cfg["data"]["use_bim"] 101 | 102 | if cfg["error"]["floor_term"]: 103 | floor_plane = None 104 | if use_bim: 105 | floor_plane = torch.Tensor(np.asarray(o3d.io.read_point_cloud(f"{scan_path}/floorplane.ply").points).astype(np.float32)) 106 | floor_function = FloorTerm(floor, unproject, cost_function, floor_plane) 107 | 108 | if cfg["error"]["ceil_term"]: 109 | ceil_plane = None 110 | if use_bim: 111 | ceil_plane = torch.Tensor(np.asarray(o3d.io.read_point_cloud(f"{scan_path}/ceiling.ply").points).astype(np.float32)) 112 | ceil_function = FloorTerm(ceiling, unproject, cost_function, ceil_plane) 113 | 114 | if cfg["error"]["wall_term"]: 115 | floorplan = torch.from_numpy(np.load(f"{scan_path}/floorplan.npy")) 116 | if cfg["error"]["wall_term_type"] == "point": 117 | wall_function = WallTerm(wall, unproject, cost_function, floorplan).to(args.device).float() 118 | else: 119 | wall_function = WallSegmentTerm(wall, unproject, cost_function, floorplan).to(args.device).float() 120 | 121 | params = [] 122 | 123 | #Add Camera Parameters as parameters to be optimized 124 | fixed_cameras = [scan._frames[i] for i in cfg["optimization"]["fixed_cameras_idx"]] 125 | for name, param in camera_parameters.named_parameters(): 126 | if name.split(".")[-1] not in fixed_cameras: 127 | params.append(param) 128 | 129 | optimizer = optim.SGD(params, lr=cfg["optimization"]["lr"], momentum=cfg["optimization"]["momentum"]) 130 | 131 | torch.save(camera_parameters.state_dict(), f"{save_path}/source_cameras.pth") 132 | 133 | scan_gt = Scan("../camera_refinement_BIM/scan_full/", scale=1)#, cut_frames=70) 134 | 135 | gt_mesh, gt_pcd = scan_gt.make_mesh_pcd() 136 | o3d.io.write_triangle_mesh(f"{save_path}/gt_mesh.ply", gt_mesh) 137 | o3d.io.write_point_cloud(f"{save_path}/gt_pcd.ply", gt_pcd) 138 | 139 | print('Written gt mesh and pointcloud') 140 | 141 | camera_parameters_gt = CameraParameters(scan_gt.cameras).to(args.device).float() 142 | torch.save(camera_parameters_gt.state_dict(), f"{save_path}/gt_cameras.pth") 143 | rotvec_gt = camera_parameters_gt.state_dict()['rotvecs'] 144 | trans_gt = camera_parameters_gt.state_dict()['translations'] 145 | 146 | for epoch in range(cfg["optimization"]["num_epoches"]): 147 | 148 | 149 | if epoch % 50 == 0: 150 | print(f"Epoch {epoch}") 151 | rotvec_est = camera_parameters.state_dict()['rotvecs'] 152 | trans_est = camera_parameters.state_dict()['translations'] 153 | translation_rmse, rotation_rmse = compute_pose_rmse(trans_gt.cpu().detach().numpy(), 154 | rotvec_gt.cpu().detach().numpy(), 155 | trans_est.cpu().detach().numpy(), 156 | rotvec_est.cpu().detach().numpy()) 157 | print("Translation Err:", translation_rmse) 158 | print("Rotation Err:", rotation_rmse) 159 | 160 | 161 | optimizer.zero_grad() 162 | error_args = {"unproject": unproject, 163 | "project": project, 164 | "scale": scan.scale, 165 | "distance_function": cost_function, 166 | **cfg["error"]} 167 | ba_function = getattr(feature_errors, cfg["error"]["error_type"]) 168 | ba_term = ba_function(left, right, **error_args) 169 | 170 | floor_term = 0. 171 | wall_term = 0. 172 | ceil_term = 0. 173 | print(f"The value of the loss function on the {epoch}-iteration") 174 | print(f"\t\t feature-based BA term - {float(ba_term)}") 175 | 176 | if cfg["error"]["floor_term"]: 177 | floor_term = floor_function() * cfg["error"]["floor_weight"] 178 | print(f"\t\t floor term - {float(floor_term)}") 179 | 180 | if cfg["error"]["ceil_term"]: 181 | 182 | ceil_term = ceil_function() * cfg["error"]["ceil_weight"] 183 | print(f"\t\t ceil term - {float(ceil_term)}") 184 | 185 | if cfg["error"]["wall_term"]: 186 | wall_term = wall_function() * cfg["error"]["wall_weight"] 187 | print(f"\t\t wall term - {float(wall_term)}") 188 | 189 | loss = ba_term + wall_term + floor_term + ceil_term 190 | 191 | loss.backward() 192 | 193 | for name, param in camera_parameters.named_parameters(): 194 | if torch.isnan(param.grad).any(): 195 | param.grad[torch.isnan(param.grad)] = 0.0 196 | optimizer.step() 197 | 198 | 199 | torch.save(camera_parameters.state_dict(), f"{save_path}/cameras.pth") 200 | cameras = camera_parameters.get_cameras() 201 | scan.set_cameras(cameras) 202 | 203 | p_mesh, p_pcd = scan.make_mesh_pcd() 204 | o3d.io.write_triangle_mesh(f"{save_path}/processed_mesh.ply", p_mesh) 205 | o3d.io.write_point_cloud(f"{save_path}/processed_pcd.ply", p_pcd) 206 | print('Written processed mesh and pointcloud') 207 | 208 | 209 | 210 | 211 | -------------------------------------------------------------------------------- /facap/data/scan.py: -------------------------------------------------------------------------------- 1 | from copy import deepcopy 2 | from glob import glob 3 | from open3d.pipelines import integration 4 | 5 | import cv2 6 | import numpy as np 7 | import open3d as o3d 8 | from scipy.spatial.transform import Rotation as R 9 | 10 | from facap.geometry.open3d import unproject_points, sample_points_from_pcd 11 | from facap.geometry.numpy import unproject_points_rotvec 12 | from facap.colmap_scripts.read_write_model import read_model 13 | import matplotlib.pyplot as plt 14 | 15 | 16 | def read_data(scan_path, frame_id, include_columns=False): 17 | color = cv2.imread(f'{scan_path}/arcore/frame-{frame_id}.png') 18 | wall = cv2.imread(f'{scan_path}/segmentation/frame-{frame_id}_wall.png', cv2.IMREAD_GRAYSCALE) 19 | wall = np.rot90(wall, k=1) 20 | floor = cv2.imread(f'{scan_path}/segmentation/frame-{frame_id}_floor.png', cv2.IMREAD_GRAYSCALE) 21 | floor = np.rot90(floor, k=1) #floor > floor.min() 22 | ceiling = cv2.imread(f'{scan_path}/segmentation/frame-{frame_id}_ceiling.png', cv2.IMREAD_GRAYSCALE) 23 | ceiling = np.rot90(ceiling, k=1) 24 | columns = cv2.imread(f'{scan_path}/segmentation/frame-{frame_id}_columns.png', cv2.IMREAD_GRAYSCALE) 25 | columns = np.rot90(columns, k=1) # floor > floor.min() 26 | 27 | if include_columns: 28 | print('includecolumsn', include_columns) 29 | wall = cv2.bitwise_or(wall, columns) 30 | 31 | depth = np.load(f'{scan_path}/arcore/depth-{frame_id}.npy') #cv2.imread(f'{scan_path}/arcore/depth-{frame_id}.png', -1) 32 | 33 | pose = np.loadtxt(f'{scan_path}/arcore/pose-{frame_id}.txt') 34 | 35 | depth = np.ascontiguousarray(np.rot90(depth, k=1)) 36 | color = np.ascontiguousarray(np.rot90(color, k=1)) 37 | 38 | rotation_matrix = np.array([[0, 0, 1], 39 | [0, 1, 0], 40 | [-1, 0, 0]]) 41 | 42 | # Extract the original translation part 43 | translation = pose[:3, 3] 44 | new_pose = np.eye(4) 45 | new_pose[:3, :3] = np.dot(pose[:3, :3], rotation_matrix) 46 | new_pose[:3, 3] = translation 47 | 48 | camera_params = np.loadtxt(f'{scan_path}/arcore/cam_params.txt') #-{frame_id}.txt') 49 | camera_params[[0, 1]]= camera_params[[1,0]] 50 | camera_params[[2, 3]]= camera_params[[3,2]] 51 | camera_params[[4, 5]]= camera_params[[5,4]] 52 | 53 | return color, wall, floor, ceiling, columns, depth, new_pose, camera_params 54 | 55 | 56 | def get_yxds(scan_path, frame_ids, max_depth=3000, min_depth=0): 57 | yxds = {} 58 | for frame_id in frame_ids: 59 | color, _, _, _, _, depth, _, _ = read_data(scan_path, frame_id) 60 | depth_mask = np.where((depth > min_depth) * (depth < max_depth)) 61 | yxds[frame_id] = get_index_value_dict(depth, depth_mask) 62 | return yxds 63 | 64 | 65 | def get_segmentation(scan_path, frame_ids, part="floor", sparsity=30, max_depth=10, min_depth=0, include_columns=False): 66 | result = {} 67 | for frame_id in frame_ids: 68 | color, wall, floor, ceiling, columns, depth, _, _ = read_data(scan_path, frame_id, include_columns=include_columns) 69 | if part == "floor": 70 | parts = floor 71 | elif part == "wall": 72 | parts = wall 73 | elif part == "ceiling": 74 | parts = ceiling 75 | else: 76 | parts = columns 77 | 78 | part_mask = np.where((depth > min_depth) * (depth < max_depth) * (parts > 0)) 79 | part_dict = get_index_value_dict(depth, part_mask, sparsity=sparsity) 80 | result[frame_id] = list(part_dict.items()) 81 | return result 82 | 83 | 84 | def read_features(scan_path, xyds, frame_ids, min_freq=2): 85 | scan_path = f"{scan_path}/db" #glob(f'{scan_path}/db/1/triangulated/feat*/')[0] 86 | cameras, images, points_3d = read_model(f'{scan_path}/sparse/0/')#read_model(f'{scan_path}/sparse/models/triangulated') 87 | result = {} 88 | for point in points_3d: 89 | result[point] = {} 90 | 91 | for img_id in points_3d[point].image_ids: 92 | img = images[img_id] 93 | xy = tuple((img.xys[img.point3D_ids == point] + 0.5).astype(int)[0]) 94 | yx = (xy[1], xy[0]) 95 | frame_id = img.name[6:-4] 96 | if frame_id in frame_ids: 97 | if yx in xyds[frame_id]: 98 | d = xyds[frame_id][yx] 99 | result[point][img.name[6:-4]] = (yx, d) 100 | filtered_result = {} 101 | for point in result: 102 | if len(result[point]) > min_freq: 103 | filtered_result[point] = result[point] 104 | return filtered_result 105 | 106 | 107 | def get_index_value_dict(array_2d, mask, sparsity=1): 108 | y, x = np.array(mask).astype(int) 109 | d = array_2d[mask].astype(float) 110 | x, y, d = x[::sparsity], y[::sparsity], d[::sparsity] 111 | yxd = dict(zip(list(zip(y, x)), d)) 112 | return yxd 113 | 114 | 115 | class Camera: 116 | def __init__(self, f, pp, rotvec, translation): 117 | self.f = f 118 | self.pp = pp 119 | self.rotvec = rotvec.astype(float) 120 | self.translation = translation.astype(float) 121 | 122 | @classmethod 123 | def read_camera(cls, scan_path, frame_id): 124 | pose = np.loadtxt(f'{scan_path}/arcore/pose-{frame_id}.txt') 125 | rotation_matrix = np.array([[0, 0, 1], 126 | [0, 1, 0], 127 | [-1, 0, 0]]) 128 | 129 | # Extract the original translation part 130 | translation = pose[:3, 3] 131 | new_pose = np.eye(4) 132 | new_pose[:3, :3] = np.dot(pose[:3, :3], rotation_matrix) 133 | new_pose[:3, 3] = translation 134 | pose = new_pose 135 | 136 | camera_params = np.loadtxt(f'{scan_path}/arcore/cam_params.txt') #-{frame_id}.txt') 137 | camera_params[[0, 1]] = camera_params[[1, 0]] 138 | camera_params[[2, 3]] = camera_params[[3, 2]] 139 | camera_params[[4, 5]] = camera_params[[5, 4]] 140 | 141 | rotvec = R.from_matrix(pose[:3, :3]).as_rotvec() 142 | translation = pose[:3, 3] 143 | f = (camera_params[2], camera_params[3]) 144 | pp = (camera_params[4], camera_params[5]) 145 | camera = cls(f, pp, rotvec, translation) 146 | return camera 147 | 148 | 149 | class Scan: 150 | def __init__(self, scan_path, sparsity=1, cut_frames=None, scale=1): 151 | frames = sorted(glob(f"{scan_path}/segmentation/frame*_floor*")) 152 | frame_ids = [i.split("/")[-1][6:-10] for i in frames] 153 | frame_ids = frame_ids[::sparsity] 154 | 155 | if cut_frames is not None: 156 | frame_ids = frame_ids[:cut_frames] 157 | self.scan_path = scan_path 158 | self._frames = frame_ids 159 | self.scale = scale 160 | self.cameras = {frame_id: Camera.read_camera(scan_path, frame_id) for frame_id in frame_ids} 161 | self.include_columns = False 162 | 163 | def get_data(self, cam_id): 164 | color, wall, floor, ceiling, columns, depth, pose, camera_params = read_data(self.scan_path, cam_id, include_columns=self.include_columns) 165 | return color, wall, floor, depth, pose, camera_params 166 | 167 | def make_pcd(self, num_points=9000000): 168 | pcds = [] 169 | for frame_id in self._frames: 170 | color_map, _, _, depth_map, _, _ = self.get_data(frame_id) 171 | camera = self.cameras[frame_id] 172 | extrinsic = np.eye(4) 173 | extrinsic[:3, :3] = R.from_rotvec(camera.rotvec).as_matrix() 174 | extrinsic[:3, 3] = camera.translation 175 | 176 | pcd = unproject_points(depth_map, color_map, np.linalg.inv(extrinsic), 177 | camera.f, camera.pp, *depth_map.shape, scale=self.scale) 178 | pcds.append(pcd) 179 | 180 | pcd_combined = o3d.geometry.PointCloud() 181 | 182 | for pcd in pcds: 183 | pcd_combined += pcd 184 | 185 | if num_points is not None: 186 | pcd_combined = sample_points_from_pcd(pcd_combined, num_points) 187 | 188 | return pcd_combined 189 | 190 | def make_mesh_pcd(self, vox_length=0.05): 191 | 192 | volume = integration.ScalableTSDFVolume( 193 | voxel_length=vox_length, 194 | sdf_trunc=vox_length * 4, 195 | color_type=integration.TSDFVolumeColorType.RGB8) 196 | 197 | camera = o3d.camera.PinholeCameraIntrinsic() 198 | #Bug cannot use original pose 199 | for cam_id in self._frames: 200 | color_map, wall, floor, depth_map, pose, camera_params = self.get_data(cam_id) 201 | depth_map = depth_map.astype(np.float32) 202 | 203 | camera.set_intrinsics(int(camera_params[0]), int(camera_params[1]), *camera_params[2:]) 204 | color = o3d.geometry.Image(color_map) 205 | depth = o3d.geometry.Image(depth_map) 206 | 207 | rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth( 208 | color, depth, depth_scale=1, depth_trunc=8, convert_rgb_to_intensity=False) #TODO changed depth trunc from 6 to 100 and depth scale 209 | 210 | rotvec = self.cameras[cam_id].rotvec 211 | translation = self.cameras[cam_id].translation 212 | 213 | rotation_matrix = R.from_rotvec(rotvec).as_matrix() 214 | pose_matrix = np.eye(4) # Initialize 4x4 identity matrix 215 | pose_matrix[:3, :3] = rotation_matrix # Assign rotation 216 | pose_matrix[:3, 3] = translation 217 | 218 | volume.integrate(rgbd, camera, np.linalg.inv(pose_matrix)) 219 | 220 | mesh = volume.extract_triangle_mesh() 221 | mesh.compute_vertex_normals() 222 | 223 | pcd = volume.extract_point_cloud() 224 | return mesh, pcd 225 | 226 | def set_cameras(self, cameras): 227 | self.cameras = cameras 228 | 229 | def generate_ba_data(self, min_frame_difference=3, 230 | floor_percentiles=(2, 90), 231 | max_initial_distance=0.4, 232 | wall_sparsity=30, 233 | floor_sparsity=30, 234 | include_columns=False): 235 | self.include_columns = include_columns 236 | left = {"points": [], 237 | "depths": [], 238 | "camera_idxs": [], 239 | "rotvecs": [], 240 | "translations": [], 241 | "f": [], 242 | "pp": []} 243 | right = deepcopy(left) 244 | floor = deepcopy(left) 245 | wall = deepcopy(left) 246 | ceiling = deepcopy(left) 247 | 248 | yxds = get_yxds(self.scan_path, self._frames) 249 | features = read_features(self.scan_path, yxds, self._frames) 250 | 251 | for point_id in features: 252 | point = features[point_id] 253 | cams = list(point.keys()) 254 | cam_params = [self.cameras[i] for i in cams] 255 | rotvecs = [i.rotvec for i in cam_params] 256 | translations = [i.translation for i in cam_params] 257 | 258 | for i, cam_i in enumerate(cams): 259 | for j in range(i + 1, len(cams)): 260 | if int(cam_i) - int(cams[j]) >= min_frame_difference: 261 | for part, idx, cam_idx in zip([left, right], [i, j], [cam_i, cams[j]]): 262 | part["points"].append(point[cam_idx][0]) 263 | part["depths"].append(point[cam_idx][1]) 264 | part["rotvecs"].append(rotvecs[idx]) 265 | part["translations"].append(translations[idx]) 266 | part["f"].append(cam_params[idx].f) 267 | part["pp"].append(cam_params[idx].pp) 268 | part["camera_idxs"].append(cam_idx) 269 | del yxds 270 | wall_data = get_segmentation(self.scan_path, self._frames, part="wall", sparsity=wall_sparsity, include_columns=self.include_columns) 271 | floor_data = get_segmentation(self.scan_path, self._frames, part="floor", sparsity=floor_sparsity ,include_columns=self.include_columns) 272 | ceiling_data = get_segmentation(self.scan_path, self._frames, part="ceiling", sparsity=floor_sparsity, include_columns=self.include_columns) 273 | 274 | for source, target in zip([wall_data, floor_data, ceiling_data], [wall, floor, ceiling]): 275 | for cam_id in source: 276 | camera = self.cameras[cam_id] 277 | rotvec = camera.rotvec 278 | translation = camera.translation 279 | f = camera.f 280 | pp = camera.pp 281 | for point in source[cam_id]: 282 | target["points"].append(point[0]) 283 | target["depths"].append(point[1]) 284 | target["rotvecs"].extend([rotvec] * len(source[cam_id])) 285 | target["translations"].extend([translation] * len(source[cam_id])) 286 | target["f"].extend([f] * len(source[cam_id])) 287 | target["pp"].extend([pp] * len(source[cam_id])) 288 | target["camera_idxs"].extend([cam_id] * len(source[cam_id])) 289 | del wall_data, floor_data, ceiling_data 290 | 291 | for part in [left, right, wall, floor, ceiling]: 292 | for key in part: 293 | part[key] = np.array(part[key]) 294 | 295 | def apply_mask(dct, mask): 296 | for key in dct: 297 | dct[key] = dct[key][mask] 298 | 299 | def unproject(part): 300 | return unproject_points_rotvec(part["depths"], part["points"], part["f"], 301 | part["pp"], part["rotvecs"], part["translations"], scale=self.scale) 302 | 303 | keypoint_mask = np.linalg.norm(unproject(left) - unproject(right), axis=-1) < max_initial_distance 304 | 305 | apply_mask(left, keypoint_mask) 306 | apply_mask(right, keypoint_mask) 307 | 308 | floor_pcd_vert = unproject(floor)[:, 2] 309 | floor_mask = (floor_pcd_vert > np.percentile(floor_pcd_vert, floor_percentiles[0])) & \ 310 | (floor_pcd_vert < np.percentile(floor_pcd_vert, floor_percentiles[1])) 311 | apply_mask(floor, floor_mask) 312 | 313 | ceiling_pcd_vert = unproject(ceiling)[:, 2] 314 | ceiling_mask = (ceiling_pcd_vert > np.percentile(ceiling_pcd_vert, floor_percentiles[0])) & \ 315 | (ceiling_pcd_vert < np.percentile(ceiling_pcd_vert, floor_percentiles[1])) 316 | apply_mask(ceiling, ceiling_mask) 317 | 318 | return left, right, wall, floor, ceiling 319 | -------------------------------------------------------------------------------- /facap/colmap_scripts/read_write_model.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2022, ETH Zurich and UNC Chapel Hill. 2 | # All rights reserved. 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # 7 | # * Redistributions of source code must retain the above copyright 8 | # notice, this list of conditions and the following disclaimer. 9 | # 10 | # * Redistributions in binary form must reproduce the above copyright 11 | # notice, this list of conditions and the following disclaimer in the 12 | # documentation and/or other materials provided with the distribution. 13 | # 14 | # * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of 15 | # its contributors may be used to endorse or promote products derived 16 | # from this software without specific prior written permission. 17 | # 18 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE 22 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | # POSSIBILITY OF SUCH DAMAGE. 29 | # 30 | # Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de) 31 | 32 | import os 33 | import collections 34 | import numpy as np 35 | import struct 36 | import argparse 37 | 38 | 39 | CameraModel = collections.namedtuple( 40 | "CameraModel", ["model_id", "model_name", "num_params"]) 41 | Camera = collections.namedtuple( 42 | "Camera", ["id", "model", "width", "height", "params"]) 43 | BaseImage = collections.namedtuple( 44 | "Image", ["id", "qvec", "tvec", "camera_id", "name", "xys", "point3D_ids"]) 45 | Point3D = collections.namedtuple( 46 | "Point3D", ["id", "xyz", "rgb", "error", "image_ids", "point2D_idxs"]) 47 | 48 | 49 | class Image(BaseImage): 50 | def qvec2rotmat(self): 51 | return qvec2rotmat(self.qvec) 52 | 53 | 54 | CAMERA_MODELS = { 55 | CameraModel(model_id=0, model_name="SIMPLE_PINHOLE", num_params=3), 56 | CameraModel(model_id=1, model_name="PINHOLE", num_params=4), 57 | CameraModel(model_id=2, model_name="SIMPLE_RADIAL", num_params=4), 58 | CameraModel(model_id=3, model_name="RADIAL", num_params=5), 59 | CameraModel(model_id=4, model_name="OPENCV", num_params=8), 60 | CameraModel(model_id=5, model_name="OPENCV_FISHEYE", num_params=8), 61 | CameraModel(model_id=6, model_name="FULL_OPENCV", num_params=12), 62 | CameraModel(model_id=7, model_name="FOV", num_params=5), 63 | CameraModel(model_id=8, model_name="SIMPLE_RADIAL_FISHEYE", num_params=4), 64 | CameraModel(model_id=9, model_name="RADIAL_FISHEYE", num_params=5), 65 | CameraModel(model_id=10, model_name="THIN_PRISM_FISHEYE", num_params=12) 66 | } 67 | CAMERA_MODEL_IDS = dict([(camera_model.model_id, camera_model) 68 | for camera_model in CAMERA_MODELS]) 69 | CAMERA_MODEL_NAMES = dict([(camera_model.model_name, camera_model) 70 | for camera_model in CAMERA_MODELS]) 71 | 72 | 73 | def read_next_bytes(fid, num_bytes, format_char_sequence, endian_character="<"): 74 | """Read and unpack the next bytes from a binary file. 75 | :param fid: 76 | :param num_bytes: Sum of combination of {2, 4, 8}, e.g. 2, 6, 16, 30, etc. 77 | :param format_char_sequence: List of {c, e, f, d, h, H, i, I, l, L, q, Q}. 78 | :param endian_character: Any of {@, =, <, >, !} 79 | :return: Tuple of read and unpacked values. 80 | """ 81 | data = fid.read(num_bytes) 82 | return struct.unpack(endian_character + format_char_sequence, data) 83 | 84 | 85 | def write_next_bytes(fid, data, format_char_sequence, endian_character="<"): 86 | """pack and write to a binary file. 87 | :param fid: 88 | :param data: data to send, if multiple elements are sent at the same time, 89 | they should be encapsuled either in a list or a tuple 90 | :param format_char_sequence: List of {c, e, f, d, h, H, i, I, l, L, q, Q}. 91 | should be the same length as the data list or tuple 92 | :param endian_character: Any of {@, =, <, >, !} 93 | """ 94 | if isinstance(data, (list, tuple)): 95 | bytes = struct.pack(endian_character + format_char_sequence, *data) 96 | else: 97 | bytes = struct.pack(endian_character + format_char_sequence, data) 98 | fid.write(bytes) 99 | 100 | 101 | def read_cameras_text(path): 102 | """ 103 | see: src/base/reconstruction.cc 104 | void Reconstruction::WriteCamerasText(const std::string& path) 105 | void Reconstruction::ReadCamerasText(const std::string& path) 106 | """ 107 | cameras = {} 108 | with open(path, "r") as fid: 109 | while True: 110 | line = fid.readline() 111 | if not line: 112 | break 113 | line = line.strip() 114 | if len(line) > 0 and line[0] != "#": 115 | elems = line.split() 116 | camera_id = int(elems[0]) 117 | model = elems[1] 118 | width = int(elems[2]) 119 | height = int(elems[3]) 120 | params = np.array(tuple(map(float, elems[4:]))) 121 | cameras[camera_id] = Camera(id=camera_id, model=model, 122 | width=width, height=height, 123 | params=params) 124 | return cameras 125 | 126 | 127 | def read_cameras_binary(path_to_model_file): 128 | """ 129 | see: src/base/reconstruction.cc 130 | void Reconstruction::WriteCamerasBinary(const std::string& path) 131 | void Reconstruction::ReadCamerasBinary(const std::string& path) 132 | """ 133 | cameras = {} 134 | with open(path_to_model_file, "rb") as fid: 135 | num_cameras = read_next_bytes(fid, 8, "Q")[0] 136 | for _ in range(num_cameras): 137 | camera_properties = read_next_bytes( 138 | fid, num_bytes=24, format_char_sequence="iiQQ") 139 | camera_id = camera_properties[0] 140 | model_id = camera_properties[1] 141 | model_name = CAMERA_MODEL_IDS[camera_properties[1]].model_name 142 | width = camera_properties[2] 143 | height = camera_properties[3] 144 | num_params = CAMERA_MODEL_IDS[model_id].num_params 145 | params = read_next_bytes(fid, num_bytes=8*num_params, 146 | format_char_sequence="d"*num_params) 147 | cameras[camera_id] = Camera(id=camera_id, 148 | model=model_name, 149 | width=width, 150 | height=height, 151 | params=np.array(params)) 152 | assert len(cameras) == num_cameras 153 | return cameras 154 | 155 | 156 | def write_cameras_text(cameras, path): 157 | """ 158 | see: src/base/reconstruction.cc 159 | void Reconstruction::WriteCamerasText(const std::string& path) 160 | void Reconstruction::ReadCamerasText(const std::string& path) 161 | """ 162 | HEADER = "# Camera list with one line of data per camera:\n" + \ 163 | "# CAMERA_ID, MODEL, WIDTH, HEIGHT, PARAMS[]\n" + \ 164 | "# Number of cameras: {}\n".format(len(cameras)) 165 | with open(path, "w") as fid: 166 | fid.write(HEADER) 167 | for _, cam in cameras.items(): 168 | to_write = [cam.id, cam.model, cam.width, cam.height, *cam.params] 169 | line = " ".join([str(elem) for elem in to_write]) 170 | fid.write(line + "\n") 171 | 172 | 173 | def write_cameras_binary(cameras, path_to_model_file): 174 | """ 175 | see: src/base/reconstruction.cc 176 | void Reconstruction::WriteCamerasBinary(const std::string& path) 177 | void Reconstruction::ReadCamerasBinary(const std::string& path) 178 | """ 179 | with open(path_to_model_file, "wb") as fid: 180 | write_next_bytes(fid, len(cameras), "Q") 181 | for _, cam in cameras.items(): 182 | model_id = CAMERA_MODEL_NAMES[cam.model].model_id 183 | camera_properties = [cam.id, 184 | model_id, 185 | cam.width, 186 | cam.height] 187 | write_next_bytes(fid, camera_properties, "iiQQ") 188 | for p in cam.params: 189 | write_next_bytes(fid, float(p), "d") 190 | return cameras 191 | 192 | 193 | def read_images_text(path): 194 | """ 195 | see: src/base/reconstruction.cc 196 | void Reconstruction::ReadImagesText(const std::string& path) 197 | void Reconstruction::WriteImagesText(const std::string& path) 198 | """ 199 | images = {} 200 | with open(path, "r") as fid: 201 | while True: 202 | line = fid.readline() 203 | if not line: 204 | break 205 | line = line.strip() 206 | if len(line) > 0 and line[0] != "#": 207 | elems = line.split() 208 | image_id = int(elems[0]) 209 | qvec = np.array(tuple(map(float, elems[1:5]))) 210 | tvec = np.array(tuple(map(float, elems[5:8]))) 211 | camera_id = int(elems[8]) 212 | image_name = elems[9] 213 | elems = fid.readline().split() 214 | xys = np.column_stack([tuple(map(float, elems[0::3])), 215 | tuple(map(float, elems[1::3]))]) 216 | point3D_ids = np.array(tuple(map(int, elems[2::3]))) 217 | images[image_id] = Image( 218 | id=image_id, qvec=qvec, tvec=tvec, 219 | camera_id=camera_id, name=image_name, 220 | xys=xys, point3D_ids=point3D_ids) 221 | return images 222 | 223 | 224 | def read_images_binary(path_to_model_file): 225 | """ 226 | see: src/base/reconstruction.cc 227 | void Reconstruction::ReadImagesBinary(const std::string& path) 228 | void Reconstruction::WriteImagesBinary(const std::string& path) 229 | """ 230 | images = {} 231 | with open(path_to_model_file, "rb") as fid: 232 | num_reg_images = read_next_bytes(fid, 8, "Q")[0] 233 | for _ in range(num_reg_images): 234 | binary_image_properties = read_next_bytes( 235 | fid, num_bytes=64, format_char_sequence="idddddddi") 236 | image_id = binary_image_properties[0] 237 | qvec = np.array(binary_image_properties[1:5]) 238 | tvec = np.array(binary_image_properties[5:8]) 239 | camera_id = binary_image_properties[8] 240 | image_name = "" 241 | current_char = read_next_bytes(fid, 1, "c")[0] 242 | while current_char != b"\x00": # look for the ASCII 0 entry 243 | image_name += current_char.decode("utf-8") 244 | current_char = read_next_bytes(fid, 1, "c")[0] 245 | num_points2D = read_next_bytes(fid, num_bytes=8, 246 | format_char_sequence="Q")[0] 247 | x_y_id_s = read_next_bytes(fid, num_bytes=24*num_points2D, 248 | format_char_sequence="ddq"*num_points2D) 249 | xys = np.column_stack([tuple(map(float, x_y_id_s[0::3])), 250 | tuple(map(float, x_y_id_s[1::3]))]) 251 | point3D_ids = np.array(tuple(map(int, x_y_id_s[2::3]))) 252 | images[image_id] = Image( 253 | id=image_id, qvec=qvec, tvec=tvec, 254 | camera_id=camera_id, name=image_name, 255 | xys=xys, point3D_ids=point3D_ids) 256 | return images 257 | 258 | 259 | def write_images_text(images, path): 260 | """ 261 | see: src/base/reconstruction.cc 262 | void Reconstruction::ReadImagesText(const std::string& path) 263 | void Reconstruction::WriteImagesText(const std::string& path) 264 | """ 265 | if len(images) == 0: 266 | mean_observations = 0 267 | else: 268 | mean_observations = sum((len(img.point3D_ids) for _, img in images.items()))/len(images) 269 | HEADER = "# Image list with two lines of data per image:\n" + \ 270 | "# IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME\n" + \ 271 | "# POINTS2D[] as (X, Y, POINT3D_ID)\n" + \ 272 | "# Number of images: {}, mean observations per image: {}\n".format(len(images), mean_observations) 273 | 274 | with open(path, "w") as fid: 275 | fid.write(HEADER) 276 | for _, img in images.items(): 277 | image_header = [img.id, *img.qvec, *img.tvec, img.camera_id, img.name] 278 | first_line = " ".join(map(str, image_header)) 279 | fid.write(first_line + "\n") 280 | 281 | points_strings = [] 282 | for xy, point3D_id in zip(img.xys, img.point3D_ids): 283 | points_strings.append(" ".join(map(str, [*xy, point3D_id]))) 284 | fid.write(" ".join(points_strings) + "\n") 285 | 286 | 287 | def write_images_binary(images, path_to_model_file): 288 | """ 289 | see: src/base/reconstruction.cc 290 | void Reconstruction::ReadImagesBinary(const std::string& path) 291 | void Reconstruction::WriteImagesBinary(const std::string& path) 292 | """ 293 | with open(path_to_model_file, "wb") as fid: 294 | write_next_bytes(fid, len(images), "Q") 295 | for _, img in images.items(): 296 | write_next_bytes(fid, img.id, "i") 297 | write_next_bytes(fid, img.qvec.tolist(), "dddd") 298 | write_next_bytes(fid, img.tvec.tolist(), "ddd") 299 | write_next_bytes(fid, img.camera_id, "i") 300 | for char in img.name: 301 | write_next_bytes(fid, char.encode("utf-8"), "c") 302 | write_next_bytes(fid, b"\x00", "c") 303 | write_next_bytes(fid, len(img.point3D_ids), "Q") 304 | for xy, p3d_id in zip(img.xys, img.point3D_ids): 305 | write_next_bytes(fid, [*xy, p3d_id], "ddq") 306 | 307 | 308 | def read_points3D_text(path): 309 | """ 310 | see: src/base/reconstruction.cc 311 | void Reconstruction::ReadPoints3DText(const std::string& path) 312 | void Reconstruction::WritePoints3DText(const std::string& path) 313 | """ 314 | points3D = {} 315 | with open(path, "r") as fid: 316 | while True: 317 | line = fid.readline() 318 | if not line: 319 | break 320 | line = line.strip() 321 | if len(line) > 0 and line[0] != "#": 322 | elems = line.split() 323 | point3D_id = int(elems[0]) 324 | xyz = np.array(tuple(map(float, elems[1:4]))) 325 | rgb = np.array(tuple(map(int, elems[4:7]))) 326 | error = float(elems[7]) 327 | image_ids = np.array(tuple(map(int, elems[8::2]))) 328 | point2D_idxs = np.array(tuple(map(int, elems[9::2]))) 329 | points3D[point3D_id] = Point3D(id=point3D_id, xyz=xyz, rgb=rgb, 330 | error=error, image_ids=image_ids, 331 | point2D_idxs=point2D_idxs) 332 | return points3D 333 | 334 | 335 | def read_points3D_binary(path_to_model_file): 336 | """ 337 | see: src/base/reconstruction.cc 338 | void Reconstruction::ReadPoints3DBinary(const std::string& path) 339 | void Reconstruction::WritePoints3DBinary(const std::string& path) 340 | """ 341 | points3D = {} 342 | with open(path_to_model_file, "rb") as fid: 343 | num_points = read_next_bytes(fid, 8, "Q")[0] 344 | for _ in range(num_points): 345 | binary_point_line_properties = read_next_bytes( 346 | fid, num_bytes=43, format_char_sequence="QdddBBBd") 347 | point3D_id = binary_point_line_properties[0] 348 | xyz = np.array(binary_point_line_properties[1:4]) 349 | rgb = np.array(binary_point_line_properties[4:7]) 350 | error = np.array(binary_point_line_properties[7]) 351 | track_length = read_next_bytes( 352 | fid, num_bytes=8, format_char_sequence="Q")[0] 353 | track_elems = read_next_bytes( 354 | fid, num_bytes=8*track_length, 355 | format_char_sequence="ii"*track_length) 356 | image_ids = np.array(tuple(map(int, track_elems[0::2]))) 357 | point2D_idxs = np.array(tuple(map(int, track_elems[1::2]))) 358 | points3D[point3D_id] = Point3D( 359 | id=point3D_id, xyz=xyz, rgb=rgb, 360 | error=error, image_ids=image_ids, 361 | point2D_idxs=point2D_idxs) 362 | return points3D 363 | 364 | 365 | def write_points3D_text(points3D, path): 366 | """ 367 | see: src/base/reconstruction.cc 368 | void Reconstruction::ReadPoints3DText(const std::string& path) 369 | void Reconstruction::WritePoints3DText(const std::string& path) 370 | """ 371 | if len(points3D) == 0: 372 | mean_track_length = 0 373 | else: 374 | mean_track_length = sum((len(pt.image_ids) for _, pt in points3D.items()))/len(points3D) 375 | HEADER = "# 3D point list with one line of data per point:\n" + \ 376 | "# POINT3D_ID, X, Y, Z, R, G, B, ERROR, TRACK[] as (IMAGE_ID, POINT2D_IDX)\n" + \ 377 | "# Number of points: {}, mean track length: {}\n".format(len(points3D), mean_track_length) 378 | 379 | with open(path, "w") as fid: 380 | fid.write(HEADER) 381 | for _, pt in points3D.items(): 382 | point_header = [pt.id, *pt.xyz, *pt.rgb, pt.error] 383 | fid.write(" ".join(map(str, point_header)) + " ") 384 | track_strings = [] 385 | for image_id, point2D in zip(pt.image_ids, pt.point2D_idxs): 386 | track_strings.append(" ".join(map(str, [image_id, point2D]))) 387 | fid.write(" ".join(track_strings) + "\n") 388 | 389 | 390 | def write_points3D_binary(points3D, path_to_model_file): 391 | """ 392 | see: src/base/reconstruction.cc 393 | void Reconstruction::ReadPoints3DBinary(const std::string& path) 394 | void Reconstruction::WritePoints3DBinary(const std::string& path) 395 | """ 396 | with open(path_to_model_file, "wb") as fid: 397 | write_next_bytes(fid, len(points3D), "Q") 398 | for _, pt in points3D.items(): 399 | write_next_bytes(fid, pt.id, "Q") 400 | write_next_bytes(fid, pt.xyz.tolist(), "ddd") 401 | write_next_bytes(fid, pt.rgb.tolist(), "BBB") 402 | write_next_bytes(fid, pt.error, "d") 403 | track_length = pt.image_ids.shape[0] 404 | write_next_bytes(fid, track_length, "Q") 405 | for image_id, point2D_id in zip(pt.image_ids, pt.point2D_idxs): 406 | write_next_bytes(fid, [image_id, point2D_id], "ii") 407 | 408 | 409 | def detect_model_format(path, ext): 410 | if os.path.isfile(os.path.join(path, "cameras" + ext)) and \ 411 | os.path.isfile(os.path.join(path, "images" + ext)) and \ 412 | os.path.isfile(os.path.join(path, "points3D" + ext)): 413 | print("Detected model format: '" + ext + "'") 414 | return True 415 | 416 | return False 417 | 418 | 419 | def read_model(path, ext=""): 420 | # try to detect the extension automatically 421 | if ext == "": 422 | if detect_model_format(path, ".bin"): 423 | ext = ".bin" 424 | elif detect_model_format(path, ".txt"): 425 | ext = ".txt" 426 | else: 427 | print("Provide model format: '.bin' or '.txt'") 428 | return 429 | 430 | if ext == ".txt": 431 | cameras = read_cameras_text(os.path.join(path, "cameras" + ext)) 432 | images = read_images_text(os.path.join(path, "images" + ext)) 433 | points3D = read_points3D_text(os.path.join(path, "points3D") + ext) 434 | else: 435 | cameras = read_cameras_binary(os.path.join(path, "cameras" + ext)) 436 | images = read_images_binary(os.path.join(path, "images" + ext)) 437 | points3D = read_points3D_binary(os.path.join(path, "points3D") + ext) 438 | print('colmappoints', len(points3D)) 439 | return cameras, images, points3D 440 | 441 | 442 | def write_model(cameras, images, points3D, path, ext=".bin"): 443 | if ext == ".txt": 444 | write_cameras_text(cameras, os.path.join(path, "cameras" + ext)) 445 | write_images_text(images, os.path.join(path, "images" + ext)) 446 | write_points3D_text(points3D, os.path.join(path, "points3D") + ext) 447 | else: 448 | write_cameras_binary(cameras, os.path.join(path, "cameras" + ext)) 449 | write_images_binary(images, os.path.join(path, "images" + ext)) 450 | write_points3D_binary(points3D, os.path.join(path, "points3D") + ext) 451 | return cameras, images, points3D 452 | 453 | 454 | def qvec2rotmat(qvec): 455 | return np.array([ 456 | [1 - 2 * qvec[2]**2 - 2 * qvec[3]**2, 457 | 2 * qvec[1] * qvec[2] - 2 * qvec[0] * qvec[3], 458 | 2 * qvec[3] * qvec[1] + 2 * qvec[0] * qvec[2]], 459 | [2 * qvec[1] * qvec[2] + 2 * qvec[0] * qvec[3], 460 | 1 - 2 * qvec[1]**2 - 2 * qvec[3]**2, 461 | 2 * qvec[2] * qvec[3] - 2 * qvec[0] * qvec[1]], 462 | [2 * qvec[3] * qvec[1] - 2 * qvec[0] * qvec[2], 463 | 2 * qvec[2] * qvec[3] + 2 * qvec[0] * qvec[1], 464 | 1 - 2 * qvec[1]**2 - 2 * qvec[2]**2]]) 465 | 466 | 467 | def rotmat2qvec(R): 468 | Rxx, Ryx, Rzx, Rxy, Ryy, Rzy, Rxz, Ryz, Rzz = R.flat 469 | K = np.array([ 470 | [Rxx - Ryy - Rzz, 0, 0, 0], 471 | [Ryx + Rxy, Ryy - Rxx - Rzz, 0, 0], 472 | [Rzx + Rxz, Rzy + Ryz, Rzz - Rxx - Ryy, 0], 473 | [Ryz - Rzy, Rzx - Rxz, Rxy - Ryx, Rxx + Ryy + Rzz]]) / 3.0 474 | eigvals, eigvecs = np.linalg.eigh(K) 475 | qvec = eigvecs[[3, 0, 1, 2], np.argmax(eigvals)] 476 | if qvec[0] < 0: 477 | qvec *= -1 478 | return qvec 479 | 480 | 481 | def main(): 482 | parser = argparse.ArgumentParser(description="Read and write COLMAP binary and text models") 483 | parser.add_argument("--input_model", help="path to input model folder") 484 | parser.add_argument("--input_format", choices=[".bin", ".txt"], 485 | help="input model format", default="") 486 | parser.add_argument("--output_model", 487 | help="path to output model folder") 488 | parser.add_argument("--output_format", choices=[".bin", ".txt"], 489 | help="outut model format", default=".txt") 490 | args = parser.parse_args() 491 | 492 | cameras, images, points3D = read_model(path=args.input_model, ext=args.input_format) 493 | 494 | print("num_cameras:", len(cameras)) 495 | print("num_images:", len(images)) 496 | print("num_points3D:", len(points3D)) 497 | 498 | if args.output_model is not None: 499 | write_model(cameras, images, points3D, path=args.output_model, ext=args.output_format) 500 | 501 | 502 | if __name__ == "__main__": 503 | main() 504 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | Preamble 9 | 10 | The GNU General Public License is a free, copyleft license for 11 | software and other kinds of works. 12 | 13 | The licenses for most software and other practical works are designed 14 | to take away your freedom to share and change the works. By contrast, 15 | the GNU General Public License is intended to guarantee your freedom to 16 | share and change all versions of a program--to make sure it remains free 17 | software for all its users. We, the Free Software Foundation, use the 18 | GNU General Public License for most of our software; it applies also to 19 | any other work released this way by its authors. You can apply it to 20 | your programs, too. 21 | 22 | When we speak of free software, we are referring to freedom, not 23 | price. Our General Public Licenses are designed to make sure that you 24 | have the freedom to distribute copies of free software (and charge for 25 | them if you wish), that you receive source code or can get it if you 26 | want it, that you can change the software or use pieces of it in new 27 | free programs, and that you know you can do these things. 28 | 29 | To protect your rights, we need to prevent others from denying you 30 | these rights or asking you to surrender the rights. Therefore, you have 31 | certain responsibilities if you distribute copies of the software, or if 32 | you modify it: responsibilities to respect the freedom of others. 33 | 34 | For example, if you distribute copies of such a program, whether 35 | gratis or for a fee, you must pass on to the recipients the same 36 | freedoms that you received. You must make sure that they, too, receive 37 | or can get the source code. And you must show them these terms so they 38 | know their rights. 39 | 40 | Developers that use the GNU GPL protect your rights with two steps: 41 | (1) assert copyright on the software, and (2) offer you this License 42 | giving you legal permission to copy, distribute and/or modify it. 43 | 44 | For the developers' and authors' protection, the GPL clearly explains 45 | that there is no warranty for this free software. For both users' and 46 | authors' sake, the GPL requires that modified versions be marked as 47 | changed, so that their problems will not be attributed erroneously to 48 | authors of previous versions. 49 | 50 | Some devices are designed to deny users access to install or run 51 | modified versions of the software inside them, although the manufacturer 52 | can do so. This is fundamentally incompatible with the aim of 53 | protecting users' freedom to change the software. The systematic 54 | pattern of such abuse occurs in the area of products for individuals to 55 | use, which is precisely where it is most unacceptable. Therefore, we 56 | have designed this version of the GPL to prohibit the practice for those 57 | products. If such problems arise substantially in other domains, we 58 | stand ready to extend this provision to those domains in future versions 59 | of the GPL, as needed to protect the freedom of users. 60 | 61 | Finally, every program is threatened constantly by software patents. 62 | States should not allow patents to restrict development and use of 63 | software on general-purpose computers, but in those that do, we wish to 64 | avoid the special danger that patents applied to a free program could 65 | make it effectively proprietary. To prevent this, the GPL assures that 66 | patents cannot be used to render the program non-free. 67 | 68 | The precise terms and conditions for copying, distribution and 69 | modification follow. 70 | 71 | TERMS AND CONDITIONS 72 | 73 | 0. Definitions. 74 | 75 | "This License" refers to version 3 of the GNU General Public License. 76 | 77 | "Copyright" also means copyright-like laws that apply to other kinds of 78 | works, such as semiconductor masks. 79 | 80 | "The Program" refers to any copyrightable work licensed under this 81 | License. Each licensee is addressed as "you". "Licensees" and 82 | "recipients" may be individuals or organizations. 83 | 84 | To "modify" a work means to copy from or adapt all or part of the work 85 | in a fashion requiring copyright permission, other than the making of an 86 | exact copy. The resulting work is called a "modified version" of the 87 | earlier work or a work "based on" the earlier work. 88 | 89 | A "covered work" means either the unmodified Program or a work based 90 | on the Program. 91 | 92 | To "propagate" a work means to do anything with it that, without 93 | permission, would make you directly or secondarily liable for 94 | infringement under applicable copyright law, except executing it on a 95 | computer or modifying a private copy. Propagation includes copying, 96 | distribution (with or without modification), making available to the 97 | public, and in some countries other activities as well. 98 | 99 | To "convey" a work means any kind of propagation that enables other 100 | parties to make or receive copies. Mere interaction with a user through 101 | a computer network, with no transfer of a copy, is not conveying. 102 | 103 | An interactive user interface displays "Appropriate Legal Notices" 104 | to the extent that it includes a convenient and prominently visible 105 | feature that (1) displays an appropriate copyright notice, and (2) 106 | tells the user that there is no warranty for the work (except to the 107 | extent that warranties are provided), that licensees may convey the 108 | work under this License, and how to view a copy of this License. If 109 | the interface presents a list of user commands or options, such as a 110 | menu, a prominent item in the list meets this criterion. 111 | 112 | 1. Source Code. 113 | 114 | The "source code" for a work means the preferred form of the work 115 | for making modifications to it. "Object code" means any non-source 116 | form of a work. 117 | 118 | A "Standard Interface" means an interface that either is an official 119 | standard defined by a recognized standards body, or, in the case of 120 | interfaces specified for a particular programming language, one that 121 | is widely used among developers working in that language. 122 | 123 | The "System Libraries" of an executable work include anything, other 124 | than the work as a whole, that (a) is included in the normal form of 125 | packaging a Major Component, but which is not part of that Major 126 | Component, and (b) serves only to enable use of the work with that 127 | Major Component, or to implement a Standard Interface for which an 128 | implementation is available to the public in source code form. A 129 | "Major Component", in this context, means a major essential component 130 | (kernel, window system, and so on) of the specific operating system 131 | (if any) on which the executable work runs, or a compiler used to 132 | produce the work, or an object code interpreter used to run it. 133 | 134 | The "Corresponding Source" for a work in object code form means all 135 | the source code needed to generate, install, and (for an executable 136 | work) run the object code and to modify the work, including scripts to 137 | control those activities. However, it does not include the work's 138 | System Libraries, or general-purpose tools or generally available free 139 | programs which are used unmodified in performing those activities but 140 | which are not part of the work. For example, Corresponding Source 141 | includes interface definition files associated with source files for 142 | the work, and the source code for shared libraries and dynamically 143 | linked subprograms that the work is specifically designed to require, 144 | such as by intimate data communication or control flow between those 145 | subprograms and other parts of the work. 146 | 147 | The Corresponding Source need not include anything that users 148 | can regenerate automatically from other parts of the Corresponding 149 | Source. 150 | 151 | The Corresponding Source for a work in source code form is that 152 | same work. 153 | 154 | 2. Basic Permissions. 155 | 156 | All rights granted under this License are granted for the term of 157 | copyright on the Program, and are irrevocable provided the stated 158 | conditions are met. This License explicitly affirms your unlimited 159 | permission to run the unmodified Program. The output from running a 160 | covered work is covered by this License only if the output, given its 161 | content, constitutes a covered work. This License acknowledges your 162 | rights of fair use or other equivalent, as provided by copyright law. 163 | 164 | You may make, run and propagate covered works that you do not 165 | convey, without conditions so long as your license otherwise remains 166 | in force. You may convey covered works to others for the sole purpose 167 | of having them make modifications exclusively for you, or provide you 168 | with facilities for running those works, provided that you comply with 169 | the terms of this License in conveying all material for which you do 170 | not control copyright. Those thus making or running the covered works 171 | for you must do so exclusively on your behalf, under your direction 172 | and control, on terms that prohibit them from making any copies of 173 | your copyrighted material outside their relationship with you. 174 | 175 | Conveying under any other circumstances is permitted solely under 176 | the conditions stated below. Sublicensing is not allowed; section 10 177 | makes it unnecessary. 178 | 179 | 3. Protecting Users' Legal Rights From Anti-Circumvention Law. 180 | 181 | No covered work shall be deemed part of an effective technological 182 | measure under any applicable law fulfilling obligations under article 183 | 11 of the WIPO copyright treaty adopted on 20 December 1996, or 184 | similar laws prohibiting or restricting circumvention of such 185 | measures. 186 | 187 | When you convey a covered work, you waive any legal power to forbid 188 | circumvention of technological measures to the extent such circumvention 189 | is effected by exercising rights under this License with respect to 190 | the covered work, and you disclaim any intention to limit operation or 191 | modification of the work as a means of enforcing, against the work's 192 | users, your or third parties' legal rights to forbid circumvention of 193 | technological measures. 194 | 195 | 4. Conveying Verbatim Copies. 196 | 197 | You may convey verbatim copies of the Program's source code as you 198 | receive it, in any medium, provided that you conspicuously and 199 | appropriately publish on each copy an appropriate copyright notice; 200 | keep intact all notices stating that this License and any 201 | non-permissive terms added in accord with section 7 apply to the code; 202 | keep intact all notices of the absence of any warranty; and give all 203 | recipients a copy of this License along with the Program. 204 | 205 | You may charge any price or no price for each copy that you convey, 206 | and you may offer support or warranty protection for a fee. 207 | 208 | 5. Conveying Modified Source Versions. 209 | 210 | You may convey a work based on the Program, or the modifications to 211 | produce it from the Program, in the form of source code under the 212 | terms of section 4, provided that you also meet all of these conditions: 213 | 214 | a) The work must carry prominent notices stating that you modified 215 | it, and giving a relevant date. 216 | 217 | b) The work must carry prominent notices stating that it is 218 | released under this License and any conditions added under section 219 | 7. This requirement modifies the requirement in section 4 to 220 | "keep intact all notices". 221 | 222 | c) You must license the entire work, as a whole, under this 223 | License to anyone who comes into possession of a copy. This 224 | License will therefore apply, along with any applicable section 7 225 | additional terms, to the whole of the work, and all its parts, 226 | regardless of how they are packaged. This License gives no 227 | permission to license the work in any other way, but it does not 228 | invalidate such permission if you have separately received it. 229 | 230 | d) If the work has interactive user interfaces, each must display 231 | Appropriate Legal Notices; however, if the Program has interactive 232 | interfaces that do not display Appropriate Legal Notices, your 233 | work need not make them do so. 234 | 235 | A compilation of a covered work with other separate and independent 236 | works, which are not by their nature extensions of the covered work, 237 | and which are not combined with it such as to form a larger program, 238 | in or on a volume of a storage or distribution medium, is called an 239 | "aggregate" if the compilation and its resulting copyright are not 240 | used to limit the access or legal rights of the compilation's users 241 | beyond what the individual works permit. Inclusion of a covered work 242 | in an aggregate does not cause this License to apply to the other 243 | parts of the aggregate. 244 | 245 | 6. Conveying Non-Source Forms. 246 | 247 | You may convey a covered work in object code form under the terms 248 | of sections 4 and 5, provided that you also convey the 249 | machine-readable Corresponding Source under the terms of this License, 250 | in one of these ways: 251 | 252 | a) Convey the object code in, or embodied in, a physical product 253 | (including a physical distribution medium), accompanied by the 254 | Corresponding Source fixed on a durable physical medium 255 | customarily used for software interchange. 256 | 257 | b) Convey the object code in, or embodied in, a physical product 258 | (including a physical distribution medium), accompanied by a 259 | written offer, valid for at least three years and valid for as 260 | long as you offer spare parts or customer support for that product 261 | model, to give anyone who possesses the object code either (1) a 262 | copy of the Corresponding Source for all the software in the 263 | product that is covered by this License, on a durable physical 264 | medium customarily used for software interchange, for a price no 265 | more than your reasonable cost of physically performing this 266 | conveying of source, or (2) access to copy the 267 | Corresponding Source from a network server at no charge. 268 | 269 | c) Convey individual copies of the object code with a copy of the 270 | written offer to provide the Corresponding Source. This 271 | alternative is allowed only occasionally and noncommercially, and 272 | only if you received the object code with such an offer, in accord 273 | with subsection 6b. 274 | 275 | d) Convey the object code by offering access from a designated 276 | place (gratis or for a charge), and offer equivalent access to the 277 | Corresponding Source in the same way through the same place at no 278 | further charge. You need not require recipients to copy the 279 | Corresponding Source along with the object code. If the place to 280 | copy the object code is a network server, the Corresponding Source 281 | may be on a different server (operated by you or a third party) 282 | that supports equivalent copying facilities, provided you maintain 283 | clear directions next to the object code saying where to find the 284 | Corresponding Source. Regardless of what server hosts the 285 | Corresponding Source, you remain obligated to ensure that it is 286 | available for as long as needed to satisfy these requirements. 287 | 288 | e) Convey the object code using peer-to-peer transmission, provided 289 | you inform other peers where the object code and Corresponding 290 | Source of the work are being offered to the general public at no 291 | charge under subsection 6d. 292 | 293 | A separable portion of the object code, whose source code is excluded 294 | from the Corresponding Source as a System Library, need not be 295 | included in conveying the object code work. 296 | 297 | A "User Product" is either (1) a "consumer product", which means any 298 | tangible personal property which is normally used for personal, family, 299 | or household purposes, or (2) anything designed or sold for incorporation 300 | into a dwelling. In determining whether a product is a consumer product, 301 | doubtful cases shall be resolved in favor of coverage. For a particular 302 | product received by a particular user, "normally used" refers to a 303 | typical or common use of that class of product, regardless of the status 304 | of the particular user or of the way in which the particular user 305 | actually uses, or expects or is expected to use, the product. A product 306 | is a consumer product regardless of whether the product has substantial 307 | commercial, industrial or non-consumer uses, unless such uses represent 308 | the only significant mode of use of the product. 309 | 310 | "Installation Information" for a User Product means any methods, 311 | procedures, authorization keys, or other information required to install 312 | and execute modified versions of a covered work in that User Product from 313 | a modified version of its Corresponding Source. The information must 314 | suffice to ensure that the continued functioning of the modified object 315 | code is in no case prevented or interfered with solely because 316 | modification has been made. 317 | 318 | If you convey an object code work under this section in, or with, or 319 | specifically for use in, a User Product, and the conveying occurs as 320 | part of a transaction in which the right of possession and use of the 321 | User Product is transferred to the recipient in perpetuity or for a 322 | fixed term (regardless of how the transaction is characterized), the 323 | Corresponding Source conveyed under this section must be accompanied 324 | by the Installation Information. But this requirement does not apply 325 | if neither you nor any third party retains the ability to install 326 | modified object code on the User Product (for example, the work has 327 | been installed in ROM). 328 | 329 | The requirement to provide Installation Information does not include a 330 | requirement to continue to provide support service, warranty, or updates 331 | for a work that has been modified or installed by the recipient, or for 332 | the User Product in which it has been modified or installed. Access to a 333 | network may be denied when the modification itself materially and 334 | adversely affects the operation of the network or violates the rules and 335 | protocols for communication across the network. 336 | 337 | Corresponding Source conveyed, and Installation Information provided, 338 | in accord with this section must be in a format that is publicly 339 | documented (and with an implementation available to the public in 340 | source code form), and must require no special password or key for 341 | unpacking, reading or copying. 342 | 343 | 7. Additional Terms. 344 | 345 | "Additional permissions" are terms that supplement the terms of this 346 | License by making exceptions from one or more of its conditions. 347 | Additional permissions that are applicable to the entire Program shall 348 | be treated as though they were included in this License, to the extent 349 | that they are valid under applicable law. If additional permissions 350 | apply only to part of the Program, that part may be used separately 351 | under those permissions, but the entire Program remains governed by 352 | this License without regard to the additional permissions. 353 | 354 | When you convey a copy of a covered work, you may at your option 355 | remove any additional permissions from that copy, or from any part of 356 | it. (Additional permissions may be written to require their own 357 | removal in certain cases when you modify the work.) You may place 358 | additional permissions on material, added by you to a covered work, 359 | for which you have or can give appropriate copyright permission. 360 | 361 | Notwithstanding any other provision of this License, for material you 362 | add to a covered work, you may (if authorized by the copyright holders of 363 | that material) supplement the terms of this License with terms: 364 | 365 | a) Disclaiming warranty or limiting liability differently from the 366 | terms of sections 15 and 16 of this License; or 367 | 368 | b) Requiring preservation of specified reasonable legal notices or 369 | author attributions in that material or in the Appropriate Legal 370 | Notices displayed by works containing it; or 371 | 372 | c) Prohibiting misrepresentation of the origin of that material, or 373 | requiring that modified versions of such material be marked in 374 | reasonable ways as different from the original version; or 375 | 376 | d) Limiting the use for publicity purposes of names of licensors or 377 | authors of the material; or 378 | 379 | e) Declining to grant rights under trademark law for use of some 380 | trade names, trademarks, or service marks; or 381 | 382 | f) Requiring indemnification of licensors and authors of that 383 | material by anyone who conveys the material (or modified versions of 384 | it) with contractual assumptions of liability to the recipient, for 385 | any liability that these contractual assumptions directly impose on 386 | those licensors and authors. 387 | 388 | All other non-permissive additional terms are considered "further 389 | restrictions" within the meaning of section 10. If the Program as you 390 | received it, or any part of it, contains a notice stating that it is 391 | governed by this License along with a term that is a further 392 | restriction, you may remove that term. If a license document contains 393 | a further restriction but permits relicensing or conveying under this 394 | License, you may add to a covered work material governed by the terms 395 | of that license document, provided that the further restriction does 396 | not survive such relicensing or conveying. 397 | 398 | If you add terms to a covered work in accord with this section, you 399 | must place, in the relevant source files, a statement of the 400 | additional terms that apply to those files, or a notice indicating 401 | where to find the applicable terms. 402 | 403 | Additional terms, permissive or non-permissive, may be stated in the 404 | form of a separately written license, or stated as exceptions; 405 | the above requirements apply either way. 406 | 407 | 8. Termination. 408 | 409 | You may not propagate or modify a covered work except as expressly 410 | provided under this License. Any attempt otherwise to propagate or 411 | modify it is void, and will automatically terminate your rights under 412 | this License (including any patent licenses granted under the third 413 | paragraph of section 11). 414 | 415 | However, if you cease all violation of this License, then your 416 | license from a particular copyright holder is reinstated (a) 417 | provisionally, unless and until the copyright holder explicitly and 418 | finally terminates your license, and (b) permanently, if the copyright 419 | holder fails to notify you of the violation by some reasonable means 420 | prior to 60 days after the cessation. 421 | 422 | Moreover, your license from a particular copyright holder is 423 | reinstated permanently if the copyright holder notifies you of the 424 | violation by some reasonable means, this is the first time you have 425 | received notice of violation of this License (for any work) from that 426 | copyright holder, and you cure the violation prior to 30 days after 427 | your receipt of the notice. 428 | 429 | Termination of your rights under this section does not terminate the 430 | licenses of parties who have received copies or rights from you under 431 | this License. If your rights have been terminated and not permanently 432 | reinstated, you do not qualify to receive new licenses for the same 433 | material under section 10. 434 | 435 | 9. Acceptance Not Required for Having Copies. 436 | 437 | You are not required to accept this License in order to receive or 438 | run a copy of the Program. Ancillary propagation of a covered work 439 | occurring solely as a consequence of using peer-to-peer transmission 440 | to receive a copy likewise does not require acceptance. However, 441 | nothing other than this License grants you permission to propagate or 442 | modify any covered work. These actions infringe copyright if you do 443 | not accept this License. Therefore, by modifying or propagating a 444 | covered work, you indicate your acceptance of this License to do so. 445 | 446 | 10. Automatic Licensing of Downstream Recipients. 447 | 448 | Each time you convey a covered work, the recipient automatically 449 | receives a license from the original licensors, to run, modify and 450 | propagate that work, subject to this License. You are not responsible 451 | for enforcing compliance by third parties with this License. 452 | 453 | An "entity transaction" is a transaction transferring control of an 454 | organization, or substantially all assets of one, or subdividing an 455 | organization, or merging organizations. If propagation of a covered 456 | work results from an entity transaction, each party to that 457 | transaction who receives a copy of the work also receives whatever 458 | licenses to the work the party's predecessor in interest had or could 459 | give under the previous paragraph, plus a right to possession of the 460 | Corresponding Source of the work from the predecessor in interest, if 461 | the predecessor has it or can get it with reasonable efforts. 462 | 463 | You may not impose any further restrictions on the exercise of the 464 | rights granted or affirmed under this License. For example, you may 465 | not impose a license fee, royalty, or other charge for exercise of 466 | rights granted under this License, and you may not initiate litigation 467 | (including a cross-claim or counterclaim in a lawsuit) alleging that 468 | any patent claim is infringed by making, using, selling, offering for 469 | sale, or importing the Program or any portion of it. 470 | 471 | 11. Patents. 472 | 473 | A "contributor" is a copyright holder who authorizes use under this 474 | License of the Program or a work on which the Program is based. The 475 | work thus licensed is called the contributor's "contributor version". 476 | 477 | A contributor's "essential patent claims" are all patent claims 478 | owned or controlled by the contributor, whether already acquired or 479 | hereafter acquired, that would be infringed by some manner, permitted 480 | by this License, of making, using, or selling its contributor version, 481 | but do not include claims that would be infringed only as a 482 | consequence of further modification of the contributor version. For 483 | purposes of this definition, "control" includes the right to grant 484 | patent sublicenses in a manner consistent with the requirements of 485 | this License. 486 | 487 | Each contributor grants you a non-exclusive, worldwide, royalty-free 488 | patent license under the contributor's essential patent claims, to 489 | make, use, sell, offer for sale, import and otherwise run, modify and 490 | propagate the contents of its contributor version. 491 | 492 | In the following three paragraphs, a "patent license" is any express 493 | agreement or commitment, however denominated, not to enforce a patent 494 | (such as an express permission to practice a patent or covenant not to 495 | sue for patent infringement). To "grant" such a patent license to a 496 | party means to make such an agreement or commitment not to enforce a 497 | patent against the party. 498 | 499 | If you convey a covered work, knowingly relying on a patent license, 500 | and the Corresponding Source of the work is not available for anyone 501 | to copy, free of charge and under the terms of this License, through a 502 | publicly available network server or other readily accessible means, 503 | then you must either (1) cause the Corresponding Source to be so 504 | available, or (2) arrange to deprive yourself of the benefit of the 505 | patent license for this particular work, or (3) arrange, in a manner 506 | consistent with the requirements of this License, to extend the patent 507 | license to downstream recipients. "Knowingly relying" means you have 508 | actual knowledge that, but for the patent license, your conveying the 509 | covered work in a country, or your recipient's use of the covered work 510 | in a country, would infringe one or more identifiable patents in that 511 | country that you have reason to believe are valid. 512 | 513 | If, pursuant to or in connection with a single transaction or 514 | arrangement, you convey, or propagate by procuring conveyance of, a 515 | covered work, and grant a patent license to some of the parties 516 | receiving the covered work authorizing them to use, propagate, modify 517 | or convey a specific copy of the covered work, then the patent license 518 | you grant is automatically extended to all recipients of the covered 519 | work and works based on it. 520 | 521 | A patent license is "discriminatory" if it does not include within 522 | the scope of its coverage, prohibits the exercise of, or is 523 | conditioned on the non-exercise of one or more of the rights that are 524 | specifically granted under this License. You may not convey a covered 525 | work if you are a party to an arrangement with a third party that is 526 | in the business of distributing software, under which you make payment 527 | to the third party based on the extent of your activity of conveying 528 | the work, and under which the third party grants, to any of the 529 | parties who would receive the covered work from you, a discriminatory 530 | patent license (a) in connection with copies of the covered work 531 | conveyed by you (or copies made from those copies), or (b) primarily 532 | for and in connection with specific products or compilations that 533 | contain the covered work, unless you entered into that arrangement, 534 | or that patent license was granted, prior to 28 March 2007. 535 | 536 | Nothing in this License shall be construed as excluding or limiting 537 | any implied license or other defenses to infringement that may 538 | otherwise be available to you under applicable patent law. 539 | 540 | 12. No Surrender of Others' Freedom. 541 | 542 | If conditions are imposed on you (whether by court order, agreement or 543 | otherwise) that contradict the conditions of this License, they do not 544 | excuse you from the conditions of this License. If you cannot convey a 545 | covered work so as to satisfy simultaneously your obligations under this 546 | License and any other pertinent obligations, then as a consequence you may 547 | not convey it at all. For example, if you agree to terms that obligate you 548 | to collect a royalty for further conveying from those to whom you convey 549 | the Program, the only way you could satisfy both those terms and this 550 | License would be to refrain entirely from conveying the Program. 551 | 552 | 13. Use with the GNU Affero General Public License. 553 | 554 | Notwithstanding any other provision of this License, you have 555 | permission to link or combine any covered work with a work licensed 556 | under version 3 of the GNU Affero General Public License into a single 557 | combined work, and to convey the resulting work. The terms of this 558 | License will continue to apply to the part which is the covered work, 559 | but the special requirements of the GNU Affero General Public License, 560 | section 13, concerning interaction through a network will apply to the 561 | combination as such. 562 | 563 | 14. Revised Versions of this License. 564 | 565 | The Free Software Foundation may publish revised and/or new versions of 566 | the GNU General Public License from time to time. Such new versions will 567 | be similar in spirit to the present version, but may differ in detail to 568 | address new problems or concerns. 569 | 570 | Each version is given a distinguishing version number. If the 571 | Program specifies that a certain numbered version of the GNU General 572 | Public License "or any later version" applies to it, you have the 573 | option of following the terms and conditions either of that numbered 574 | version or of any later version published by the Free Software 575 | Foundation. If the Program does not specify a version number of the 576 | GNU General Public License, you may choose any version ever published 577 | by the Free Software Foundation. 578 | 579 | If the Program specifies that a proxy can decide which future 580 | versions of the GNU General Public License can be used, that proxy's 581 | public statement of acceptance of a version permanently authorizes you 582 | to choose that version for the Program. 583 | 584 | Later license versions may give you additional or different 585 | permissions. However, no additional obligations are imposed on any 586 | author or copyright holder as a result of your choosing to follow a 587 | later version. 588 | 589 | 15. Disclaimer of Warranty. 590 | 591 | THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY 592 | APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT 593 | HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY 594 | OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, 595 | THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 596 | PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM 597 | IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF 598 | ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 599 | 600 | 16. Limitation of Liability. 601 | 602 | IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 603 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS 604 | THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY 605 | GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE 606 | USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF 607 | DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD 608 | PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), 609 | EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF 610 | SUCH DAMAGES. 611 | 612 | 17. Interpretation of Sections 15 and 16. 613 | 614 | If the disclaimer of warranty and limitation of liability provided 615 | above cannot be given local legal effect according to their terms, 616 | reviewing courts shall apply local law that most closely approximates 617 | an absolute waiver of all civil liability in connection with the 618 | Program, unless a warranty or assumption of liability accompanies a 619 | copy of the Program in return for a fee. 620 | 621 | END OF TERMS AND CONDITIONS 622 | 623 | How to Apply These Terms to Your New Programs 624 | 625 | If you develop a new program, and you want it to be of the greatest 626 | possible use to the public, the best way to achieve this is to make it 627 | free software which everyone can redistribute and change under these terms. 628 | 629 | To do so, attach the following notices to the program. It is safest 630 | to attach them to the start of each source file to most effectively 631 | state the exclusion of warranty; and each file should have at least 632 | the "copyright" line and a pointer to where the full notice is found. 633 | 634 | 635 | Copyright (C) 636 | 637 | This program is free software: you can redistribute it and/or modify 638 | it under the terms of the GNU General Public License as published by 639 | the Free Software Foundation, either version 3 of the License, or 640 | (at your option) any later version. 641 | 642 | This program is distributed in the hope that it will be useful, 643 | but WITHOUT ANY WARRANTY; without even the implied warranty of 644 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 645 | GNU General Public License for more details. 646 | 647 | You should have received a copy of the GNU General Public License 648 | along with this program. If not, see . 649 | 650 | Also add information on how to contact you by electronic and paper mail. 651 | 652 | If the program does terminal interaction, make it output a short 653 | notice like this when it starts in an interactive mode: 654 | 655 | Copyright (C) 656 | This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 657 | This is free software, and you are welcome to redistribute it 658 | under certain conditions; type `show c' for details. 659 | 660 | The hypothetical commands `show w' and `show c' should show the appropriate 661 | parts of the General Public License. Of course, your program's commands 662 | might be different; for a GUI interface, you would use an "about box". 663 | 664 | You should also get your employer (if you work as a programmer) or school, 665 | if any, to sign a "copyright disclaimer" for the program, if necessary. 666 | For more information on this, and how to apply and follow the GNU GPL, see 667 | . 668 | 669 | The GNU General Public License does not permit incorporating your program 670 | into proprietary programs. If your program is a subroutine library, you 671 | may consider it more useful to permit linking proprietary applications with 672 | the library. If this is what you want to do, use the GNU Lesser General 673 | Public License instead of this License. But first, please read 674 | . 675 | --------------------------------------------------------------------------------