Added functionality to pair images with Field Of View Cones

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

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."""

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

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 (

opensfm/pairs_selection_by_cones.py

@@ -0,0 +1,196 @@
+import numpy as np
+
+import numpy as np
+import math
+
+import numpy as np
+import trimesh
+
+import math
+from scipy.spatial.transform import Rotation as R
+
+from shapely import MultiPoint, convex_hull
+import shapely
+
+import trimesh
+import fiona
+from fiona.crs import from_epsg
+
+import numpy as np
+import trimesh
+
+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),}