diff --git a/Dockerfile b/Dockerfile index 75de8b3cf..95919c97c 100644 --- a/Dockerfile +++ b/Dockerfile @@ -28,4 +28,4 @@ COPY . /source/OpenSfM WORKDIR /source/OpenSfM RUN pip3 install -r requirements.txt && \ - python3 setup.py build + python3 setup.py build \ No newline at end of file diff --git a/opensfm/dataset.py b/opensfm/dataset.py index 12af6a344..7a748f32d 100644 --- a/opensfm/dataset.py +++ b/opensfm/dataset.py @@ -18,9 +18,12 @@ pymap, masking, rig, + pairs_selection_by_cones ) from opensfm.dataset_base import DataSetBase from PIL.PngImagePlugin import PngImageFile +import rasterio +import trimesh logger: logging.Logger = logging.getLogger(__name__) @@ -112,6 +115,66 @@ def load_image( anydepth=anydepth, grayscale=grayscale, ) + + def load_demmesh(self, dem_name, dem_hight, reference=None): + dem_path = os.path.join(self.data_path, dem_name) + # Create DEM model + with rasterio.open(dem_path) as src: + elevation = src.read(1) # Read the first band + height = elevation.shape[0] + width = elevation.shape[1] + cols, rows = np.meshgrid(np.arange(width), np.arange(height)) + xs, ys = rasterio.transform.xy(src.transform, rows, cols) + xs, ys = np.array(xs).flatten(), np.array(ys).flatten() + + # Converting TIF to topocentric coordinated + if reference is None: + reference = self.load_reference() + tx, ty, tz = [], [], [] + for lon, lat, alt in zip(xs, ys, elevation.flatten()): + x, y, z = reference.to_topocentric(lat, lon, alt) + tx.append(x) + ty.append(y) + tz.append(z) + + tx = np.array(tx).reshape(width, height) + ty = np.array(ty).reshape(width, height) + tz = np.array(tz).reshape(width, height) + + vertices = np.column_stack([tx.flatten(), ty.flatten(), tz.flatten()]) + + # Create triangular faces (mesh grid-based) + faces = [] + for i in range(height - 1): + for j in range(width - 1): + # Define two triangles per grid cell + v1 = i * width + j + v2 = i * width + (j + 1) + v3 = (i + 1) * width + j + v4 = (i + 1) * width + (j + 1) + + faces.append([v1, v2, v3]) # Triangle 1 + faces.append([v2, v4, v3]) # Triangle 2 + + faces = np.array(faces) + + surface_mesh = trimesh.Trimesh(vertices=vertices, faces=faces) + surface_mesh.invert() + + # Extrude model + model_faces = np.array(trimesh.creation.extrude_triangulation(vertices=vertices[:, :2], faces=faces, height=dem_hight).faces) + model_bottom_vertices = vertices.copy() + model_top_vertices = vertices.copy() + model_top_vertices[:, -1] = model_top_vertices[:, -1] + dem_hight + model_vertices = np.concatenate([model_bottom_vertices, model_top_vertices]) + + volume_mesh = trimesh.Trimesh(vertices=model_vertices, faces=model_faces) + return volume_mesh, surface_mesh + + + def save_cones_projection_as_gpkg(self, mesh_list, reference, output_name='cones_projections.gpkg'): + output_path = os.path.join(self.data_path, output_name) + pairs_selection_by_cones.save_cones_projection_as_gpkg(mesh_list, reference, output_path) def image_size(self, image: str) -> Tuple[int, int]: """Height and width of the image.""" diff --git a/opensfm/exif.py b/opensfm/exif.py index 6b4e76462..c758c4a02 100644 --- a/opensfm/exif.py +++ b/opensfm/exif.py @@ -464,7 +464,8 @@ def extract_capture_time(self) -> float: return 0.0 def extract_opk(self, geo) -> Optional[Dict[str, Any]]: - opk = None + opk_dict = None + ypr_dict = None if self.has_xmp() and geo and "latitude" in geo and "longitude" in geo: ypr = np.array([None, None, None]) @@ -503,6 +504,7 @@ def extract_opk(self, geo) -> Optional[Dict[str, Any]]: ] ) ypr[1] += 90 # DJI's values need to be offset + ypr_dict = {'yaw':ypr[0], 'pitch': ypr[1], 'roll': ypr[2]} except ValueError: logger.debug( 'Invalid yaw/pitch/roll tag in image file "{0:s}"'.format( @@ -567,7 +569,7 @@ def extract_opk(self, geo) -> Optional[Dict[str, Any]]: if m == 0: logger.debug("Cannot compute OPK angles, divider = 0") - return opk + return opk_dict, ypr_dict # Unit vector pointing north xnp /= m @@ -580,12 +582,12 @@ def extract_opk(self, geo) -> Optional[Dict[str, Any]]: # OPK rotation matrix ceb = cen.dot(cnb).dot(cbb) - opk = {} - opk["omega"] = np.degrees(np.arctan2(-ceb[1][2], ceb[2][2])) - opk["phi"] = np.degrees(np.arcsin(ceb[0][2])) - opk["kappa"] = np.degrees(np.arctan2(-ceb[0][1], ceb[0][0])) + opk_dict = {} + opk_dict["omega"] = np.degrees(np.arctan2(-ceb[1][2], ceb[2][2])) + opk_dict["phi"] = np.degrees(np.arcsin(ceb[0][2])) + opk_dict["kappa"] = np.degrees(np.arctan2(-ceb[0][1], ceb[0][0])) - return opk + return opk_dict, ypr_dict def extract_exif(self) -> Dict[str, Any]: width, height = self.extract_image_size() @@ -595,7 +597,7 @@ def extract_exif(self) -> Dict[str, Any]: orientation = self.extract_orientation() geo = self.extract_geo() capture_time = self.extract_capture_time() - opk = self.extract_opk(geo) + opk, ypr = self.extract_opk(geo) d = { "make": make, "model": model, @@ -609,6 +611,7 @@ def extract_exif(self) -> Dict[str, Any]: } if opk: d["opk"] = opk + d["ypr"] = ypr d["camera"] = camera_id(d) return d diff --git a/opensfm/matching.py b/opensfm/matching.py index a96eee3f0..674a88cd2 100644 --- a/opensfm/matching.py +++ b/opensfm/matching.py @@ -11,6 +11,7 @@ log, multiview, pairs_selection, + pairs_selection_by_cones, pyfeatures, pygeometry, ) @@ -43,14 +44,28 @@ def match_images( all_images = list(set(ref_images + cand_images)) exifs = {im: data.load_exif(im) for im in all_images} - # Generate pairs for matching - pairs, preport = pairs_selection.match_candidates_from_metadata( - ref_images, - cand_images, - exifs, - data, - config_override, - ) + overriden_config = data.config.copy() + overriden_config.update(config_override) + + by_cones = overriden_config.get('pair_selection_by_cones', False) + if by_cones: + pairs, preport = pairs_selection_by_cones.pairing_by_cones_from_dataset( + ref_images, + cand_images, + exifs, + data, + config_override, + ) + + else: + # Generate pairs for matching + pairs, preport = pairs_selection.match_candidates_from_metadata( + ref_images, + cand_images, + exifs, + data, + config_override, + ) # Match them ! return ( diff --git a/opensfm/pairs_selection_by_cones.py b/opensfm/pairs_selection_by_cones.py new file mode 100644 index 000000000..6a26f0cc0 --- /dev/null +++ b/opensfm/pairs_selection_by_cones.py @@ -0,0 +1,184 @@ +import numpy as np +import math +import trimesh +from scipy.spatial.transform import Rotation as R +from shapely import MultiPoint, convex_hull +import shapely +import fiona +from fiona.crs import from_epsg + +import logging +logger: logging.Logger = logging.getLogger(__name__) + + +def extract_metadata(images, exifs, reference, camera_pairing_fov): + + # Extract all the metadata for cone matching + metadata = [] + for i in images: + image_exif = exifs[i] + if not 'gps' in image_exif: + logger.warning(f'Image {i} no GPS -> skip') + continue + if not 'ypr' in image_exif: + logger.warning(f'Image {i} no YPR -> skip') + continue + + annotation = { + 'fn': i, + 'lon': image_exif['gps']['longitude'], + 'lat': image_exif['gps']['latitude'], + 'alt': image_exif['gps']['altitude'], + 'yaw': image_exif['ypr']['yaw'], + 'pitch': image_exif['ypr']['pitch'], + 'roll': image_exif['ypr']['roll'], + 'fov': camera_pairing_fov, + } + metadata.append(annotation) + + # Add relative positions + for i in metadata: + x, y, z = reference.to_topocentric(i['lat'], i['lon'], i['alt']) + i['x'] = x + i['y'] = y + i['z'] = z + + return metadata + + +def save_cones_projection_as_gpkg(mesh_list, reference, output_file): + schema = { + 'geometry': 'Polygon', + 'properties': {'name': 'str'} + } + + with fiona.open(output_file, 'w', driver='GPKG', crs=from_epsg(4326), schema=schema) as layer: + for cn, cc in mesh_list: + vertex_array = trimesh.convex.hull_points(cc) + ll_point_list = [] + for x, y, z in vertex_array: + lat, lon, _ = reference.to_lla(x, y, z) + ll_point_list.append((lon, lat)) + extent = convex_hull(MultiPoint(ll_point_list)) + + layer.write({ + 'geometry': shapely.geometry.mapping(extent), + 'properties': {'name': cn} + }) + + +def create_fov_cone(point, yaw, pitch, roll, fov, cone_height, cone_sections=32): + """ + Point: x, y, z cone tip + Yaw 0 degrees -> top direction, increased clockwise + Pitch 0 degrees -> top to bottom direction + FOV - cone angle, degrees + """ + + # Create a 3D cone + cone_radius = cone_height * np.tan(math.radians(fov / 2)) + cone = trimesh.creation.cone(radius=cone_radius, height=cone_height, sections=cone_sections) + + # Moving cone tip to 0,0 + cone.apply_translation([0, 0, -cone_height]) + + # Create transformation matrix + transform = np.eye(4) + + # Rotate cone + euler_angles_xyz = (pitch, roll, -1 * yaw) + transform[:3, :3] = R.from_euler('xyz', euler_angles_xyz, degrees=True).as_matrix() # Rotation + + # Shift cone + transform[:3, 3] = point # Translation + cone.apply_transform(transform) + + return cone + + +def match_candidaes_by_fov_cones(metadata, volume_mesh, images_ref=None, images_cand=None): + all_images = [i['fn'] for i in metadata] + if images_ref is None: + images_ref = all_images + if images_cand is None: + images_cand = all_images + + + # Create cones + model_diagonal = math.dist(*volume_mesh.bounds) + cone_list = [] + for i in metadata: + fn = i['fn'] + x = i['x'] + y = i['y'] + z = i['z'] + yaw = i['yaw'] + pitch = i['pitch'] + roll = i['roll'] + fov = i['fov'] + + cone = create_fov_cone(point=(x, y, z), + yaw=yaw, + pitch=pitch, + roll=roll, + fov=fov, + cone_height=model_diagonal) + cone_list.append((fn, cone)) + + clipped_cone_list = [] + + for fn, cone in cone_list: + clipped_cone_list.append((fn, trimesh.boolean.intersection([cone, volume_mesh]))) + + + # Find overlapping cones + pairing_map = dict() + unique_pairs = set() + for c_fn, c_cone in clipped_cone_list: + if c_fn not in images_ref: + continue + pairing_map[c_fn] = [] + for t_fn, t_cone in clipped_cone_list: + if t_fn not in images_cand: + continue + if c_fn == t_fn: + continue + intersection_volume = trimesh.boolean.intersection([c_cone, t_cone]).volume + if intersection_volume > 0: + pairing_map[c_fn].append(t_fn) + unique_pairs.add(tuple(sorted((t_fn, c_fn)))) + + return pairing_map, unique_pairs, clipped_cone_list + + +def pairing_by_cones_from_dataset(ref_images, + cand_images, + exifs, + data, + config_override): + data.init_reference() + reference = data.load_reference() + + overriden_config = data.config.copy() + overriden_config.update(config_override) + + c = set() + dem_name = str(overriden_config["dem_path"]) + dem_hight = float(overriden_config["dem_hight"]) + pairing_fov = float(overriden_config["pairing_fov"]) + + metadata = extract_metadata(set(ref_images + cand_images), + exifs, + reference, + pairing_fov) + volume_mesh, _ = data.load_demmesh(dem_name, dem_hight, reference=reference) + + _, c, cone_list = match_candidaes_by_fov_cones( + metadata, + volume_mesh, + images_ref=ref_images, + images_cand=cand_images) + + data.save_cones_projection_as_gpkg(cone_list, reference) + + return c, {"num_pairs_cones": len(c),} diff --git a/requirements.txt b/requirements.txt index 0d50b21ef..bc85d3c7c 100644 --- a/requirements.txt +++ b/requirements.txt @@ -5,16 +5,21 @@ fpdf2==2.4.6 joblib==0.14.1 matplotlib networkx==2.5 -numpy>=1.19 +numpy==1.21 Pillow>=8.1.1 pyproj>=1.9.5.1 pytest==3.0.7 python-dateutil>=2.7 pyyaml==5.4 -scipy>=1.10.0 +scipy>=1.10.1 Sphinx==4.2.0 six xmltodict==0.10.2 wheel opencv-python==4.5.1.48 ; sys_platform == "win32" opencv-python ; sys_platform == "linux" +fiona==1.10.1 +trimesh==4.6.3 +rasterio==1.3.11 +manifold3d==3.0.1 +shapely==2.0.7