hook up opensfm.reconstructor and depth_anything_v2

Summary:
This diff adds a depth prior to several "observation/feature" data structs. These fields are populated by the depth value read from respective pixels of the Depth-Anything-V2-estimated depth maps.

These depth priors are eventually used in OpenSfM as an additional constraint in BA. It can produce a map with absolute scale. It can also be helpful for scale drift.

Reviewed By: YanNoun

Differential Revision: D66050326

fbshipit-source-id: 4fd32a11cc5424a30b683934ee24f770b98e54b2

opensfm/actions/create_tracks.py

@@ -10,7 +10,7 @@ def run_dataset(data: DataSetBase) -> None:
     """Link matches pair-wise matches into tracks."""
 
     start = timer()
-    features, colors, segmentations, instances = tracking.load_features(
+    features, colors, segmentations, instances, depths = tracking.load_features(
         data, data.images()
     )
     features_end = timer()
@@ -23,6 +23,9 @@ def run_dataset(data: DataSetBase) -> None:
         instances,
         matches,
         data.config["min_track_length"],
+        depths,
+        data.config["depth_is_radial"],
+        data.config["depth_std_deviation_m_default"],
     )
     tracks_end = timer()
     data.save_tracks_manager(tracks_manager)

opensfm/config.py

@@ -217,6 +217,14 @@ class OpenSfMConfig:
     ##################################
     # Minimum number of features/images per track
     min_track_length: int = 2
+    # Whether use depth prior during BA
+    use_depth_prior: bool = False
+    # Depth prior default std deviation
+    depth_std_deviation_m_default: float = 1.0
+    # Whether depth is radial (distance to camera center) or Z value
+    depth_is_radial: bool = False
+    # Whether depth is stored as inverted depth
+    depth_is_inverted: bool = False
 
     ##################################
     # Params for bundle adjustment

opensfm/features.py

@@ -51,6 +51,7 @@ class FeaturesData:
     descriptors: Optional[np.ndarray]
     colors: np.ndarray
     semantic: Optional[SemanticData]
+    depths: np.ndarray | None  # New field. This field is not serialized yet
 
     FEATURES_VERSION: int = 3
     FEATURES_HEADER: str = "OPENSFM_FEATURES_VERSION"
@@ -61,11 +62,13 @@ def __init__(
         descriptors: Optional[np.ndarray],
         colors: np.ndarray,
         semantic: Optional[SemanticData],
+        depths: np.ndarray | None = None,
     ):
         self.points = points
         self.descriptors = descriptors
         self.colors = colors
         self.semantic = semantic
+        self.depths = depths
 
     def get_segmentation(self) -> Optional[np.ndarray]:
         semantic = self.semantic

opensfm/src/bundle/src/bundle_adjuster.cc

@@ -507,6 +507,9 @@ struct AddRelativeDepthError {
       return;
     }
     const auto& depth = obs.depth_prior.value();
+    if (!ceres::isfinite(depth.value)) {
+      throw std::runtime_error(obs.shot->GetID() + " has non-finite depth prior");
+    }
 
     const bool is_rig_camera_useful =
         IsRigCameraUseful(*obs.shot->GetRigCamera());

opensfm/tracking.py

@@ -20,12 +20,14 @@ def load_features(
     t.Dict[str, np.ndarray],
     t.Dict[str, np.ndarray],
     t.Dict[str, np.ndarray],
+    t.Dict[str, np.ndarray],
 ]:
     logging.info("reading features")
     features = {}
     colors = {}
     segmentations = {}
     instances = {}
+    depths = {}
     for im in images:
         features_data = dataset.load_features(im)
 
@@ -41,7 +43,11 @@ def load_features(
             if semantic_data.has_instances():
                 instances[im] = semantic_data.instances
 
-    return features, colors, segmentations, instances
+        depth_data = features_data.depths
+        if depth_data is not None:
+            depths[im] = depth_data
+
+    return features, colors, segmentations, instances, depths
 
 
 def load_matches(
@@ -66,6 +72,9 @@ def create_tracks_manager(
     instances: t.Dict[str, np.ndarray],
     matches: t.Dict[t.Tuple[str, str], t.List[t.Tuple[int, int]]],
     min_length: int,
+    depths: t.Dict[str, np.ndarray],
+    depth_is_radial: bool = True,
+    depth_std_deviation: float = 1.0,
 ) -> TracksManager:
     """Link matches into tracks."""
     logger.debug("Merging features onto tracks")
@@ -83,10 +92,11 @@ def create_tracks_manager(
             sets[p] = [i]
 
     tracks = [t for t in sets.values() if _good_track(t, min_length)]
-    logger.debug("Good tracks: {}".format(len(tracks)))
 
     NO_VALUE = pymap.Observation.NO_SEMANTIC_VALUE
     tracks_manager = pymap.TracksManager()
+    num_observations = 0
+    num_depth_priors = 0
     for track_id, track in enumerate(tracks):
         for image, featureid in track:
             if image not in features:
@@ -104,7 +114,21 @@ def create_tracks_manager(
                 #  `Union[ndarray[typing.Any, typing.Any], int]`.
                 x, y, s, int(r), int(g), int(b), featureid, segmentation, instance
             )
+            if image in depths:
+                depth_value = depths[image][featureid]
+                if not np.isnan(depth_value) and not np.isinf(depth_value):
+                    std = max(depth_std_deviation * depth_value, depth_std_deviation)  # pyre-ignore
+                    obs.depth_prior = pymap.Depth(
+                        value=depth_value,   # pyre-ignore
+                        std_deviation=std,
+                        is_radial=depth_is_radial,
+                    )
+                    num_depth_priors += 1
             tracks_manager.add_observation(image, str(track_id), obs)
+            num_observations += 1
+    logger.info(
+        f"{len(tracks)} tracks, {num_observations} observations,"
+        f" {num_depth_priors} depth priors added to TracksManager")
     return tracks_manager