diff --git a/conda.yml b/conda.yml index 1e163ceff..39500686e 100644 --- a/conda.yml +++ b/conda.yml @@ -6,7 +6,9 @@ dependencies: - libxcrypt - libopencv *=headless* - py-opencv - - conda-forge::libvorbis - - ceres-solver=2.1 + - anaconda::ceres-solver=2.2 - conda-forge::llvm-openmp - conda-forge::cxx-compiler + - anaconda::suitesparse + - conda-forge::openblas + - anaconda::metis \ No newline at end of file diff --git a/opensfm/actions/create_tracks.py b/opensfm/actions/create_tracks.py index b57b810d4..ae7b456f8 100644 --- a/opensfm/actions/create_tracks.py +++ b/opensfm/actions/create_tracks.py @@ -13,9 +13,10 @@ def run_dataset(data: DataSetBase) -> None: data, data.images() ) features_end = timer() - matches = tracking.load_matches(data, data.images()) + matches = lambda: tracking.load_matches(data, data.images()) matches_end = timer() - tracks_manager = tracking.create_tracks_manager( + + tracks_manager = tracking.create_tracks_manager_from_matches_iter( features, colors, segmentations, diff --git a/opensfm/actions/reconstruct.py b/opensfm/actions/reconstruct.py index 0034b0355..0e03a182a 100644 --- a/opensfm/actions/reconstruct.py +++ b/opensfm/actions/reconstruct.py @@ -1,4 +1,5 @@ # pyre-strict +import gc from opensfm import io, reconstruction from opensfm.dataset_base import DataSetBase @@ -21,5 +22,8 @@ def run_dataset( else: raise RuntimeError(f"Unsupported algorithm for reconstruction {algorithm}") - data.save_reconstruction(reconstructions) + del tracks_manager + gc.collect() + data.save_report(io.json_dumps(report), "reconstruction.json") + data.save_reconstruction(reconstructions) diff --git a/opensfm/config.py b/opensfm/config.py index 4eb19c200..4476f3abf 100644 --- a/opensfm/config.py +++ b/opensfm/config.py @@ -283,6 +283,8 @@ class OpenSfMConfig: # Maximum optimizer iterations. bundle_max_iterations: int = 100 + # Ratio of (resection candidates / total tracks) of a given image so that it is culled at resection and resected later + resect_redundancy_threshold: float = 0.7 # Retriangulate all points from time to time retriangulation: bool = True # Retriangulate when the number of points grows by this ratio @@ -299,6 +301,10 @@ class OpenSfMConfig: local_bundle_min_common_points: int = 20 # Max number of shots to optimize during local bundle adjustment local_bundle_max_shots: int = 30 + # Number of grid division for seleccting tracks in local bundle adjustment + local_bundle_grid: int = 8 + # Number of grid division for selecting tracks in final bundle adjustment + final_bundle_grid: int = 32 # Remove uncertain and isolated points from the final point cloud filter_final_point_cloud: bool = False diff --git a/opensfm/dense.py b/opensfm/dense.py index 9f4042cb4..0f4b69ec0 100644 --- a/opensfm/dense.py +++ b/opensfm/dense.py @@ -30,7 +30,7 @@ def compute_depthmaps( num_neighbors = config["depthmap_num_neighbors"] neighbors = {} - common_tracks = common_tracks_double_dict(graph) + common_tracks = common_tracks_double_dict(graph, processes) for shot in reconstruction.shots.values(): neighbors[shot.id] = find_neighboring_images( shot, common_tracks, reconstruction, num_neighbors @@ -394,13 +394,14 @@ def compute_depth_range( def common_tracks_double_dict( tracks_manager: pymap.TracksManager, + processes: int, ) -> Dict[str, Dict[str, List[str]]]: """List of track ids observed by each image pair. Return a dict, ``res``, such that ``res[im1][im2]`` is the list of common tracks between ``im1`` and ``im2``. """ - common_tracks_per_pair = tracking.all_common_tracks_without_features(tracks_manager) + common_tracks_per_pair = tracking.all_common_tracks_without_features(tracks_manager, processes=processes) res = {image: {} for image in tracks_manager.get_shot_ids()} for (im1, im2), v in common_tracks_per_pair.items(): res[im1][im2] = v diff --git a/opensfm/exif.py b/opensfm/exif.py index 2205007a8..228caf229 100644 --- a/opensfm/exif.py +++ b/opensfm/exif.py @@ -3,6 +3,7 @@ import datetime import logging from codecs import decode, encode +import math from typing import Any, BinaryIO, Callable, Dict, List, Optional, Tuple, Union import exifread @@ -60,14 +61,42 @@ def get_tag_as_float(tags: Dict[str, Any], key: str, index: int = 0) -> Optional return None +def focal35_to_focal_ratio( + focal35_or_ratio: float, width: int, height: int, inverse=False +) -> float: + """ + Convert focal length in 35mm film equivalent to focal ratio (and vice versa). + We follow https://en.wikipedia.org/wiki/35_mm_equivalent_focal_length + """ + image_ratio = float(max(width, height)) / min(width, height) + is_32 = math.fabs(image_ratio - 3.0 / 2.0) + is_43 = math.fabs(image_ratio - 4.0 / 3.0) + if is_32 < is_43: + # 3:2 aspect ratio : use 36mm for 35mm film + film_width = 36.0 + if inverse: + return focal35_or_ratio * film_width + else: + return focal35_or_ratio / film_width + else: + # 4:3 aspect ratio : use 34mm for 35mm film + film_width = 34 + if inverse: + return focal35_or_ratio * film_width + else: + return focal35_or_ratio / film_width + + def compute_focal( + pixel_width: int, + pixel_height: int, focal_35: Optional[float], focal: Optional[float], sensor_width: Optional[float], sensor_string: Optional[str], ) -> Tuple[float, float]: if focal_35 is not None and focal_35 > 0: - focal_ratio = focal_35 / 36.0 # 35mm film produces 36x24mm pictures. + focal_ratio = focal35_to_focal_ratio(focal_35, pixel_width, pixel_height) else: if not sensor_width: sensor_width = ( @@ -75,7 +104,9 @@ def compute_focal( ) if sensor_width and focal: focal_ratio = focal / sensor_width - focal_35 = 36.0 * focal_ratio + focal_35 = focal35_to_focal_ratio( + focal, pixel_width, pixel_height, inverse=True + ) else: focal_35 = 0.0 focal_ratio = 0.0 @@ -248,7 +279,10 @@ def extract_projection_type(self) -> str: def extract_focal(self) -> Tuple[float, float]: make, model = self.extract_make(), self.extract_model() + width, height = self.extract_image_size() focal_35, focal_ratio = compute_focal( + width, + height, get_tag_as_float(self.tags, "EXIF FocalLengthIn35mmFilm"), get_tag_as_float(self.tags, "EXIF FocalLength"), self.extract_sensor_width(), @@ -636,7 +670,13 @@ def extract_exif(self) -> Dict[str, Any]: def hard_coded_calibration(exif: Dict[str, Any]) -> Optional[Dict[str, Any]]: focal = exif["focal_ratio"] - fmm35 = int(round(focal * 36.0)) + fmm35 = int( + round( + focal35_to_focal_ratio( + focal, int(exif["width"]), int(exif["height"]), inverse=True + ) + ) + ) make = exif["make"].strip().lower() model = exif["model"].strip().lower() raw_calibrations = camera_calibration()[0] diff --git a/opensfm/reconstruction.py b/opensfm/reconstruction.py index 071e4a0f8..382958435 100644 --- a/opensfm/reconstruction.py +++ b/opensfm/reconstruction.py @@ -51,7 +51,7 @@ def _get_camera_from_bundle( def log_bundle_stats(bundle_type: str, bundle_report: Dict[str, Any]) -> None: times = bundle_report["wall_times"] - time_secs = times["run"] + times["setup"] + times["teardown"] + time_secs = times["run"] + times["setup"] + times["teardown"] + times["triangulate"] num_images, num_points, num_reprojections = ( bundle_report["num_images"], bundle_report["num_points"], @@ -71,6 +71,7 @@ def bundle( camera_priors: Dict[str, pygeometry.Camera], rig_camera_priors: Dict[str, pymap.RigCamera], gcp: Optional[List[pymap.GroundControlPoint]], + grid_size: int, config: Dict[str, Any], ) -> Dict[str, Any]: """Bundle adjust a reconstruction.""" @@ -79,6 +80,7 @@ def bundle( dict(camera_priors), dict(rig_camera_priors), gcp if gcp is not None else [], + grid_size, config, ) log_bundle_stats("GLOBAL", report) @@ -109,6 +111,7 @@ def bundle_local( camera_priors: Dict[str, pygeometry.Camera], rig_camera_priors: Dict[str, pymap.RigCamera], gcp: Optional[List[pymap.GroundControlPoint]], + grid_size: int, central_shot_id: str, config: Dict[str, Any], ) -> Tuple[Dict[str, Any], List[int]]: @@ -119,6 +122,7 @@ def bundle_local( dict(rig_camera_priors), gcp if gcp is not None else [], central_shot_id, + grid_size, config, ) log_bundle_stats("LOCAL", report) @@ -235,7 +239,6 @@ def _pair_reconstructability_arguments( def _compute_pair_reconstructability(args: TPairArguments) -> Tuple[str, str, float]: - log.setup() im1, im2, p1, p2, camera1, camera2, threshold = args R, inliers = two_view_reconstruction_rotation_only( p1, p2, camera1, camera2, threshold @@ -706,7 +709,7 @@ def resect( bs = np.array(bs) Xs = np.array(Xs) if len(bs) < 5: - return False, set(), {"num_common_points": len(bs)} + return False, set(), set(), {"num_common_points": len(bs)} T = multiview.absolute_pose_ransac(bs, Xs, threshold, 1000, 0.999) @@ -737,15 +740,17 @@ def resect( triangulate_shot_features( tracks_manager, reconstruction, new_shots, data.config ) + tracks = set() for i, succeed in enumerate(inliers): if succeed: add_observation_to_reconstruction( tracks_manager, reconstruction, shot_id, ids[i] ) + tracks.add(ids[i]) report["shots"] = list(new_shots) - return True, new_shots, report + return True, new_shots, tracks, report else: - return False, set(), report + return False, set(), set(), report def corresponding_tracks( @@ -912,7 +917,7 @@ def triangulate_robust( min_ray_angle_degrees: float, min_depth: float, iterations: int, - ) -> None: + ) -> bool: """Triangulate track in a RANSAC way and add point to reconstruction.""" os, bs, ids = [], [], [] for shot_id, obs in self.tracks_handler.get_observations(track).items(): @@ -924,7 +929,7 @@ def triangulate_robust( ids.append(shot_id) if len(ids) < 2: - return + return False os = np.array(os) bs = np.array(bs) @@ -1004,6 +1009,7 @@ def triangulate_robust( self.tracks_handler.store_track_coordinates(track, best_point) for i in best_inliers: self.tracks_handler.store_inliers_observation(track, ids[i]) + return len(best_inliers) > 1 def triangulate( self, @@ -1012,7 +1018,7 @@ def triangulate( min_ray_angle_degrees: float, min_depth: float, iterations: int, - ) -> None: + ) -> bool: """Triangulate track and add point to reconstruction.""" os, bs, ids = [], [], [] for shot_id, obs in self.tracks_handler.get_observations(track).items(): @@ -1040,6 +1046,9 @@ def triangulate( self.tracks_handler.store_track_coordinates(track, X.tolist()) for shot_id in ids: self.tracks_handler.store_inliers_observation(track, shot_id) + return True + + return False def triangulate_dlt( self, @@ -1048,7 +1057,7 @@ def triangulate_dlt( min_ray_angle_degrees: float, min_depth: float, iterations: int, - ) -> None: + ) -> bool: """Triangulate track using DLT and add point to reconstruction.""" Rts, bs, os, ids = [], [], [], [] for shot_id, obs in self.tracks_handler.get_observations(track).items(): @@ -1076,6 +1085,8 @@ def triangulate_dlt( self.tracks_handler.store_track_coordinates(track, X.tolist()) for shot_id in ids: self.tracks_handler.store_inliers_observation(track, shot_id) + return True + return False def _shot_origin(self, shot: pymap.Shot) -> NDArray: if shot.id in self.origins: @@ -1107,7 +1118,7 @@ def triangulate_shot_features( reconstruction: types.Reconstruction, shot_ids: Set[str], config: Dict[str, Any], -) -> None: +) -> List[str]: """Reconstruct as many tracks seen in shot_id as possible.""" reproj_threshold = config["triangulation_threshold"] min_ray_angle = config["triangulation_min_ray_angle"] @@ -1125,24 +1136,29 @@ def triangulate_shot_features( if s in all_shots_ids for t in tracks_manager.get_shot_observations(s) } + + created_tracks = [] for track in tracks_ids: if track not in reconstruction.points: if config["triangulation_type"] == "ROBUST": - triangulator.triangulate_robust( + if(triangulator.triangulate_robust( track, reproj_threshold, min_ray_angle, min_depth, refinement_iterations, - ) + )): + created_tracks.append(track) elif config["triangulation_type"] == "FULL": - triangulator.triangulate( + if(triangulator.triangulate( track, reproj_threshold, min_ray_angle, min_depth, refinement_iterations, - ) + )): + created_tracks.append(track) + return created_tracks def retriangulate( @@ -1215,7 +1231,7 @@ def remove_outliers( reconstruction: types.Reconstruction, config: Dict[str, Any], points: Optional[Dict[str, pymap.Landmark]] = None, -) -> int: +) -> Tuple[List[Tuple[str, str]], Set[str]]: """Remove points with large reprojection error. A list of point ids to be processed can be given in ``points``. @@ -1237,14 +1253,16 @@ def remove_outliers( reconstruction.map.remove_observation(shot_id, track) track_ids.add(track) + removed_tracks = set() for track in track_ids: if track in reconstruction.points: lm = reconstruction.points[track] if lm.number_of_observations() < 2: reconstruction.map.remove_landmark(lm) + removed_tracks.add(track) logger.info("Removed outliers: {}".format(len(outliers))) - return len(outliers) + return outliers, removed_tracks def shot_lla_and_compass( @@ -1408,6 +1426,75 @@ def should(self) -> bool: def done(self) -> None: self.num_points_last = len(self.reconstruction.points) +class ResectionCandidates: + """Helper to keep track of resection candidates.""" + + def __init__(self, tracks_manager: pymap.TracksManager, reconstruction: types.Reconstruction) -> None: + self.tracks_manager = tracks_manager + self.reconstruction = reconstruction + self.candidates: Dict[str, Set[str]] = {} + self.tracks = set() + + def update(self, reconstruction: types.Reconstruction) -> None: + """Update candidates based on a new reconstruction.""" + self.reconstruction = reconstruction + rec_tracks = set(reconstruction.points.keys()) + + # add new tracks + for track_id in rec_tracks: + if track_id not in self.tracks: + self.tracks.add(track_id) + for shot_id in self.tracks_manager.get_track_observations(track_id): + if shot_id not in self.candidates: + self.candidates[shot_id] = set() + self.candidates[shot_id].add(track_id) + + # remove tracks that are not in the reconstruction + to_remove = set() + for track_id in self.tracks: + if track_id not in rec_tracks: + for shot_id, shot_candidates in self.candidates.items(): + if track_id in shot_candidates: + shot_candidates.remove(track_id) + to_remove.add(track_id) + + for track_id in to_remove: + self.tracks.remove(track_id) + + def add(self, shots: List[str], tracks: Set[str]) -> None: + """Add newly resected tracks and the shots they belong to.""" + for shot_id in shots: + if shot_id in self.candidates: + del self.candidates[shot_id] + + for track_id in tracks: + self.tracks.add(track_id) + for shot_id in self.tracks_manager.get_track_observations(track_id): + if shot_id in self.reconstruction.shots: + continue + if shot_id not in self.candidates: + self.candidates[shot_id] = set() + self.candidates[shot_id].add(track_id) + + def remove(self, outliers: List[Tuple[str, str]], removed_tracks: List[str]) -> None: + """Remove outliers from candidates.""" + for shot_id, track_id in outliers: + if shot_id in self.candidates and track_id in self.candidates[shot_id]: + self.candidates[shot_id].remove(track_id) + if not self.candidates[shot_id]: + del self.candidates[shot_id] + + for track_id in removed_tracks: + for shot_id, shot_candidates in self.candidates.items(): + if track_id in shot_candidates: + shot_candidates.remove(track_id) + self.tracks.remove(track_id) + + + def get_candidates(self, images: Set[str]) -> List[Tuple[str, int]]: + """Get sorted candidates for resection.""" + return [(k, len(v)) for k,v in filter(lambda x: x[0] in images, sorted(self.candidates.items(), key=lambda x: -len(x[1])))] + def grow_reconstruction( data: DataSetBase, @@ -1426,12 +1513,25 @@ def grow_reconstruction( paint_reconstruction(data, tracks_manager, reconstruction) align_reconstruction(reconstruction, gcp, config) - bundle(reconstruction, camera_priors, rig_camera_priors, None, config) + bundle(reconstruction, camera_priors, rig_camera_priors, None, 0, config) remove_outliers(reconstruction, config) paint_reconstruction(data, tracks_manager, reconstruction) + resection_candidates = ResectionCandidates(tracks_manager, reconstruction) should_bundle = ShouldBundle(data, reconstruction) should_retriangulate = ShouldRetriangulate(data, reconstruction) + + local_ba_radius = config["local_bundle_radius"] + local_ba_grid = config["local_bundle_grid"] + final_bundle_grid = config["final_bundle_grid"] + + resection_candidates.add( + list(reconstruction.shots.keys()), + list(reconstruction.points.keys()), + ) + redundant_shots = set() + ratio_redundant = config["resect_redundancy_threshold"] + while True: if config["save_partial_reconstructions"]: paint_reconstruction(data, tracks_manager, reconstruction) @@ -1442,17 +1542,25 @@ def grow_reconstruction( ), ) - candidates = reconstructed_points_for_images( - tracks_manager, reconstruction, images - ) + candidates = resection_candidates.get_candidates(images) if not candidates: break logger.info("-------------------------------------------------------") threshold = data.config["resection_threshold"] min_inliers = data.config["resection_min_inliers"] - for image, _ in candidates: - ok, new_shots, resrep = resect( + + for image, resect_points in candidates: + + new_tracks = {t for t in tracks_manager.get_shot_observations(image)} + ratio_existing = resect_points / len(new_tracks) if new_tracks else 1.0 + logger.info("Ratio of resected tracks in {}: {:.2f}".format(image, ratio_existing)) + if ratio_existing > ratio_redundant: + redundant_shots.add(image) + images.remove(image) + continue + + ok, new_shots, resected_tracks, resrep = resect( data, tracks_manager, reconstruction, @@ -1464,6 +1572,7 @@ def grow_reconstruction( continue images -= new_shots + redundant_shots -= new_shots bundle_shot_poses( reconstruction, new_shots, @@ -1471,6 +1580,7 @@ def grow_reconstruction( rig_camera_priors, data.config, ) + resection_candidates.add(new_shots, resected_tracks) logger.info(f"Adding {' and '.join(new_shots)} to the reconstruction") step: Dict[str, Union[List[int], List[str], int, List[int], Any]] = { @@ -1481,7 +1591,8 @@ def grow_reconstruction( report["steps"].append(step) np_before = len(reconstruction.points) - triangulate_shot_features(tracks_manager, reconstruction, new_shots, config) + new_tracks = triangulate_shot_features(tracks_manager, reconstruction, new_shots, config) + resection_candidates.add([], new_tracks) np_after = len(reconstruction.points) step["triangulated_points"] = np_after - np_before @@ -1489,13 +1600,14 @@ def grow_reconstruction( logger.info("Re-triangulating") align_reconstruction(reconstruction, gcp, config) b1rep = bundle( - reconstruction, camera_priors, rig_camera_priors, None, config + reconstruction, camera_priors, rig_camera_priors, None, local_ba_grid, config ) rrep = retriangulate(tracks_manager, reconstruction, config) + resection_candidates.update(reconstruction) b2rep = bundle( - reconstruction, camera_priors, rig_camera_priors, None, config + reconstruction, camera_priors, rig_camera_priors, None, local_ba_grid, config ) - remove_outliers(reconstruction, config) + resection_candidates.remove(*remove_outliers(reconstruction, config)) step["bundle"] = b1rep step["retriangulation"] = rrep step["bundle_after_retriangulation"] = b2rep @@ -1504,29 +1616,36 @@ def grow_reconstruction( elif should_bundle.should(): align_reconstruction(reconstruction, gcp, config) brep = bundle( - reconstruction, camera_priors, rig_camera_priors, None, config + reconstruction, camera_priors, rig_camera_priors, None, local_ba_grid, config ) - remove_outliers(reconstruction, config) + resection_candidates.remove(*remove_outliers(reconstruction, config)) step["bundle"] = brep should_bundle.done() - elif config["local_bundle_radius"] > 0: + elif local_ba_radius > 0: bundled_points, brep = bundle_local( reconstruction, camera_priors, rig_camera_priors, None, + local_ba_grid, image, config, ) - remove_outliers(reconstruction, config, bundled_points) + resection_candidates.remove(*remove_outliers(reconstruction, config, bundled_points)) step["local_bundle"] = brep logger.info(f"Reconstruction now has {len(reconstruction.shots)} shots.") break else: - logger.info("Some images can not be added") - break + if redundant_shots: + images.update(redundant_shots) + redundant_shots.clear() + ratio_redundant = 1 + local_ba_radius = 0 + else: + logger.info("Some images can not be added") + break logger.info("-------------------------------------------------------") @@ -1536,8 +1655,8 @@ def grow_reconstruction( overidden_config["bundle_compensate_gps_bias"] = False config = overidden_config - bundle(reconstruction, camera_priors, rig_camera_priors, gcp, config) - remove_outliers(reconstruction, config) + bundle(reconstruction, camera_priors, rig_camera_priors, gcp, final_bundle_grid, config) + resection_candidates.remove(*remove_outliers(reconstruction, config)) if config["filter_final_point_cloud"]: bad_condition = pysfm.filter_badly_conditioned_points( @@ -1546,7 +1665,7 @@ def grow_reconstruction( logger.info("Removed bad-condition: {}".format(bad_condition)) isolated = pysfm.remove_isolated_points(reconstruction.map) logger.info("Removed isolated: {}".format(isolated)) - + paint_reconstruction(data, tracks_manager, reconstruction) return reconstruction, report @@ -1684,7 +1803,7 @@ def incremental_reconstruction( # pairs of image. Features of an image can be stored # repeatedly in multiple image pairs. # Pros: fast; Cons: high memory usage - common_tracks = tracking.all_common_tracks_with_features(tracks_manager) + common_tracks = tracking.all_common_tracks_with_features(tracks_manager, processes=data.config["processes"]) pairs = compute_image_pairs(common_tracks, data) else: # Get pairs sequentially, load features lazily. diff --git a/opensfm/src/CMakeLists.txt b/opensfm/src/CMakeLists.txt index 88a7dd84f..3cf994f02 100644 --- a/opensfm/src/CMakeLists.txt +++ b/opensfm/src/CMakeLists.txt @@ -40,6 +40,9 @@ endif() # For compiling VLFeat add_definitions(-DVL_DISABLE_AVX) +# We're not using an inplace Glog +add_definitions(-DGLOG_USE_GLOG_EXPORT) + # Use the version of vlfeat in ./src/third_party/vlfeat add_definitions(-DINPLACE_VLFEAT) if(NOT USE_SSE2) diff --git a/opensfm/src/bundle/src/bundle_adjuster.cc b/opensfm/src/bundle/src/bundle_adjuster.cc index a8185354d..5df2fa6ba 100644 --- a/opensfm/src/bundle/src/bundle_adjuster.cc +++ b/opensfm/src/bundle/src/bundle_adjuster.cc @@ -1109,6 +1109,7 @@ void BundleAdjuster::Run() { } options.num_threads = num_threads_; options.max_num_iterations = max_num_iterations_; + options.sparse_linear_algebra_library_type = ceres::SUITE_SPARSE; ceres::Solve(options, &problem, &last_run_summary_); diff --git a/opensfm/src/bundle/test/reprojection_errors_test.cc b/opensfm/src/bundle/test/reprojection_errors_test.cc index 3a920de0f..7c9093c19 100644 --- a/opensfm/src/bundle/test/reprojection_errors_test.cc +++ b/opensfm/src/bundle/test/reprojection_errors_test.cc @@ -1,8 +1,24 @@ -#include #include #include #include +#include + +using TEigenDiff = Eigen::AutoDiffScalar>; + +namespace std { +int fpclassify(const TEigenDiff& x) { + return std::fpclassify(x.value()); +} + +inline TEigenDiff hypot(const TEigenDiff& x, const TEigenDiff& y, + const TEigenDiff& z) { + return sqrt(x * x + y * y + z * z); +} +} // namespace std + +// Has to be included AFTER std:: maths overloads for Eigen Autodiff above +#include class ReprojectionError2DFixtureBase : public ::testing::Test { public: diff --git a/opensfm/src/map/python/pybind.cc b/opensfm/src/map/python/pybind.cc index 8307b7001..4a74b4ce8 100644 --- a/opensfm/src/map/python/pybind.cc +++ b/opensfm/src/map/python/pybind.cc @@ -305,6 +305,9 @@ PYBIND11_MODULE(pymap, m) { .def("get_all_common_observations", &map::TracksManager::GetAllCommonObservations, py::call_guard()) + .def("get_all_common_observations_arrays", + &map::TracksManager::GetAllCommonObservationsArrays, + py::call_guard()) .def("get_all_pairs_connectivity", &map::TracksManager::GetAllPairsConnectivity, py::arg("shots") = std::vector(), diff --git a/opensfm/src/map/shot.h b/opensfm/src/map/shot.h index 45846fca9..89ce77264 100644 --- a/opensfm/src/map/shot.h +++ b/opensfm/src/map/shot.h @@ -141,6 +141,7 @@ class Shot { MatX2d ProjectMany(const MatX3d& points) const; Vec3d Bearing(const Vec2d& point) const; MatX3d BearingMany(const MatX2d& points) const; + Vec3d LandmarkBearing(const Landmark* landmark) const; // Covariance MatXd GetCovariance() const { return covariance_.Value(); } diff --git a/opensfm/src/map/src/shot.cc b/opensfm/src/map/src/shot.cc index 2356d97b1..0b13520da 100644 --- a/opensfm/src/map/src/shot.cc +++ b/opensfm/src/map/src/shot.cc @@ -185,6 +185,15 @@ Vec3d Shot::Bearing(const Vec2d& point) const { return GetPose()->RotationCameraToWorld() * shot_camera_->Bearing(point); } +Vec3d Shot::LandmarkBearing(const Landmark* landmark) const { + auto it = landmark_observations_.find(const_cast(landmark)); + if (it == landmark_observations_.end()) { + throw std::runtime_error("Landmark not observed in this shot"); + } + const Observation& obs = it->second; + return Bearing(obs.point); +} + MatX3d Shot::BearingMany(const MatX2d& points) const { MatX3d bearings(points.rows(), 3); for (int i = 0; i < points.rows(); ++i) { diff --git a/opensfm/src/map/src/tracks_manager.cc b/opensfm/src/map/src/tracks_manager.cc index b3ca2ed41..881dfa2e0 100644 --- a/opensfm/src/map/src/tracks_manager.cc +++ b/opensfm/src/map/src/tracks_manager.cc @@ -1,4 +1,5 @@ #include +#include #include #include @@ -305,6 +306,23 @@ TracksManager::GetAllCommonObservations(const ShotId& shot1, return tuples; } +std::tuple, MatX2f, MatX2f> +TracksManager::GetAllCommonObservationsArrays(const ShotId& shot1, + const ShotId& shot2) const { + const auto tuples = GetAllCommonObservations(shot1, shot2); + + std::vector track_ids(tuples.size()); + MatX2f points1(tuples.size(), 2); + MatX2f points2(tuples.size(), 2); + for (int i = 0; i < tuples.size(); ++i) { + const auto& [track_id, obs1, obs2] = tuples[i]; + track_ids[i] = track_id; + points1.row(i) = obs1.point.cast(); + points2.row(i) = obs2.point.cast(); + } + return {track_ids, points1, points2}; +} + std::unordered_map TracksManager::GetAllPairsConnectivity( const std::vector& shots, diff --git a/opensfm/src/map/tracks_manager.h b/opensfm/src/map/tracks_manager.h index ab17d3189..6a9cfd446 100644 --- a/opensfm/src/map/tracks_manager.h +++ b/opensfm/src/map/tracks_manager.h @@ -33,6 +33,9 @@ class TracksManager { using KeyPointTuple = std::tuple; std::vector GetAllCommonObservations( const ShotId& shot1, const ShotId& shot2) const; + std::tuple, MatX2f, MatX2f> GetAllCommonObservationsArrays( + const ShotId& shot1, + const ShotId& shot2) const; using ShotPair = std::pair; std::unordered_map GetAllPairsConnectivity( diff --git a/opensfm/src/sfm/CMakeLists.txt b/opensfm/src/sfm/CMakeLists.txt index ffc8d6e56..e587f674d 100644 --- a/opensfm/src/sfm/CMakeLists.txt +++ b/opensfm/src/sfm/CMakeLists.txt @@ -15,6 +15,7 @@ target_link_libraries(sfm PRIVATE foundation map + geometry bundle vl ) diff --git a/opensfm/src/sfm/ba_helpers.h b/opensfm/src/sfm/ba_helpers.h index e0c8552d1..4b0ad37a5 100644 --- a/opensfm/src/sfm/ba_helpers.h +++ b/opensfm/src/sfm/ba_helpers.h @@ -18,6 +18,7 @@ class BAHelpers { const std::unordered_map& rig_camera_priors, const AlignedVector& gcp, + int grid_size, const py::dict& config); static py::tuple BundleLocal( @@ -26,7 +27,9 @@ class BAHelpers { const std::unordered_map& rig_camera_priors, const AlignedVector& gcp, - const map::ShotId& central_shot_id, const py::dict& config); + const map::ShotId& central_shot_id, + int grid_size, + const py::dict& config); static py::dict BundleShotPoses( map::Map& map, const std::unordered_set& shot_ids, @@ -57,6 +60,11 @@ class BAHelpers { const AlignedVector& gcp, const py::dict& config); + static std::unordered_set SelectTracksGrid( + map::Map& map, + const std::unordered_set& shot_ids, + size_t grid_size); + private: static std::unordered_set DirectShotNeighbors( map::Map& map, const std::unordered_set& shot_ids, diff --git a/opensfm/src/sfm/retriangulation.h b/opensfm/src/sfm/retriangulation.h index ffa56bca0..58b39b436 100644 --- a/opensfm/src/sfm/retriangulation.h +++ b/opensfm/src/sfm/retriangulation.h @@ -1,8 +1,11 @@ #pragma once +#include + #include #include namespace sfm::retriangulation { void RealignMaps(const map::Map& reference, map::Map& to_align, bool update_points); +int Triangulate(map::Map& map, const std::unordered_set& track_ids, float min_angle, float min_depth, int processing_threads); } // namespace sfm::retriangulation diff --git a/opensfm/src/sfm/src/ba_helpers.cc b/opensfm/src/sfm/src/ba_helpers.cc index 1f1d55e95..264241aca 100644 --- a/opensfm/src/sfm/src/ba_helpers.cc +++ b/opensfm/src/sfm/src/ba_helpers.cc @@ -4,6 +4,7 @@ #include #include #include +#include #include #include @@ -116,10 +117,11 @@ std::unordered_set BAHelpers::DirectShotNeighbors( py::tuple BAHelpers::BundleLocal( map::Map& map, const std::unordered_map& camera_priors, - const std::unordered_map& - rig_camera_priors, + const std::unordered_map& rig_camera_priors, const AlignedVector& gcp, - const map::ShotId& central_shot_id, const py::dict& config) { + const map::ShotId& central_shot_id, + int grid_size, + const py::dict& config) { py::dict report; const auto start = std::chrono::high_resolution_clock::now(); auto neighborhood = ShotNeighborhood( @@ -129,6 +131,16 @@ py::tuple BAHelpers::BundleLocal( auto& interior = neighborhood.first; auto& boundary = neighborhood.second; + // Convert subset to set for fast lookup + std::unordered_set all_shots; + for (auto* shot : interior) { + all_shots.insert(shot->GetId()); + } + for (auto* shot : boundary) { + all_shots.insert(shot->GetId()); + } + const auto subset = SelectTracksGrid(map, all_shots, grid_size); + // set up BA auto ba = bundle::BundleAdjuster(); ba.SetUseAnalyticDerivatives( @@ -141,8 +153,7 @@ py::tuple BAHelpers::BundleLocal( ba.AddCamera(cam.id, cam, cam_prior, fix_cameras); } // combine the sets - std::unordered_set int_and_bound(interior.cbegin(), - interior.cend()); + std::unordered_set int_and_bound(interior.cbegin(), interior.cend()); int_and_bound.insert(boundary.cbegin(), boundary.cend()); std::unordered_set points; py::list pt_ids; @@ -179,7 +190,7 @@ py::tuple BAHelpers::BundleLocal( int gps_count = 0; // if any instance's shot is in boundary - // then the entire instance will be fixed + // then the entire instance will be fixed bool fix_instance = false; for (const auto& shot_n_rig_camera : instance.GetRigCameras()) { const auto shot_id = shot_n_rig_camera.first; @@ -216,12 +227,18 @@ py::tuple BAHelpers::BundleLocal( } } + std::unordered_set to_retriangulate; size_t added_landmarks = 0; size_t added_reprojections = 0; for (auto* shot : interior) { // Add all points of the shots that are in the interior for (const auto& lm_obs : shot->GetLandmarkObservations()) { auto* lm = lm_obs.first; + // Only add points in the subset + if (!subset.empty() && subset.count(lm->id_) == 0) { + to_retriangulate.insert(lm->id_); + continue; + } if (points.count(lm) == 0) { points.insert(lm); pt_ids.append(lm->id_); @@ -237,6 +254,11 @@ py::tuple BAHelpers::BundleLocal( for (auto* shot : boundary) { for (const auto& lm_obs : shot->GetLandmarkObservations()) { auto* lm = lm_obs.first; + // Only add points in the subset + if (!subset.empty() && subset.count(lm->id_) == 0) { + to_retriangulate.insert(lm->id_); + continue; + } if (points.count(lm) > 0) { const auto& obs = lm_obs.second; ba.AddPointProjectionObservation(shot->id_, lm_obs.first->id_, @@ -288,6 +310,12 @@ py::tuple BAHelpers::BundleLocal( point->SetGlobalPos(pt.GetValue()); point->SetReprojectionErrors(pt.reprojection_errors); } + + const auto timer_triangulate = std::chrono::high_resolution_clock::now(); + sfm::retriangulation::Triangulate(map, to_retriangulate, + config["triangulation_min_ray_angle"].cast(), + config["triangulation_min_depth"].cast(), + config["processes"].cast()); const auto timer_teardown = std::chrono::high_resolution_clock::now(); report["brief_report"] = ba.BriefReport(); report["wall_times"] = py::dict(); @@ -302,6 +330,11 @@ py::tuple BAHelpers::BundleLocal( 1000000.0; report["wall_times"]["teardown"] = std::chrono::duration_cast(timer_teardown - + timer_triangulate) + .count() / + 1000000.0; + report["wall_times"]["triangulate"] = + std::chrono::duration_cast(timer_triangulate - timer_run) .count() / 1000000.0; @@ -410,11 +443,69 @@ size_t BAHelpers::AddGCPToBundle( return added_gcp_observations; } + +std::unordered_set BAHelpers::SelectTracksGrid( + map::Map& map, + const std::unordered_set& shot_ids, + size_t grid_size) { + std::unordered_set set_selected_tracks; + if (shot_ids.empty() || grid_size <= 1) { + return set_selected_tracks; + } + + // For each shot (image) + for (const auto& shot_id : shot_ids) { + const auto& shot = map.GetShot(shot_id); + const int width = shot.GetCamera()->width; + const int height = shot.GetCamera()->height; + if (width <= 0 || height <= 0) { + continue; + } + + // Prepare grid cells: each cell holds the longest track (by observation count) + std::vector> grid(grid_size*grid_size, {"", 0}); + + // For each observation in the shot + for (const auto& lm_obs : shot.GetLandmarkObservations()) { + auto* lm = lm_obs.first; + const auto& obs = lm_obs.second; + // Get normalized coordinates [0,1] + const auto normalize = std::max(width, height); + double x = (obs.point(0) * normalize + width * 0.5) / width; + double y = (obs.point(1) * normalize + height * 0.5) / height; + // Clamp to [0,1] + x = std::max(0.0, std::min(1.0, x)); + y = std::max(0.0, std::min(1.0, y)); + // Compute grid cell + const int gx = std::min(static_cast(x * grid_size), static_cast(grid_size - 1)); + const int gy = std::min(static_cast(y * grid_size), static_cast(grid_size - 1)); + // Track length = number of observations + const size_t track_len = lm->GetObservations().size(); + // Keep the longest track in this cell + if (set_selected_tracks.count(lm->id_) == 0 && track_len > grid[gx+gy*grid_size].second) { + grid[gx+gy*grid_size] = {lm->id_, track_len}; + } + } + + // Add selected tracks for this shot + for (int i = 0; i < static_cast(grid_size*grid_size); ++i) { + const auto& track_id = grid[i].first; + if (!track_id.empty()) { + set_selected_tracks.insert(track_id); + } + } + } + + return set_selected_tracks; +} + + + py::dict BAHelpers::BundleShotPoses( map::Map& map, const std::unordered_set& shot_ids, const std::unordered_map& camera_priors, const std::unordered_map& - rig_camera_priors, + rig_camera_priors, const py::dict& config) { py::dict report; @@ -591,9 +682,19 @@ py::dict BAHelpers::Bundle( const std::unordered_map& camera_priors, const std::unordered_map& rig_camera_priors, - const AlignedVector& gcp, const py::dict& config) { + const AlignedVector& gcp, + int grid_size, + const py::dict& config) { py::dict report; + // Get shot ids from the map + const auto& all_shots = map.GetShots(); + std::unordered_set shot_ids; + for (const auto& shot_pair : all_shots) { + shot_ids.insert(shot_pair.first); + } + const auto subset = SelectTracksGrid(map, shot_ids, grid_size); + auto ba = bundle::BundleAdjuster(); const bool fix_cameras = !config["optimize_camera_parameters"].cast(); ba.SetUseAnalyticDerivatives( @@ -607,8 +708,14 @@ py::dict BAHelpers::Bundle( ba.AddCamera(cam.id, cam, cam_prior, fix_cameras); } + // Only add points in the subset + std::unordered_set to_retriangulate; for (const auto& pt_pair : map.GetLandmarks()) { const auto& pt = pt_pair.second; + if (!subset.empty() && subset.count(pt.id_) == 0) { + to_retriangulate.insert(pt.id_); + continue; + } ba.AddPoint(pt.id_, pt.GetGlobalPos(), false); } @@ -707,6 +814,10 @@ py::dict BAHelpers::Bundle( // setup observations for any shot type for (const auto& lm_obs : shot.GetLandmarkObservations()) { + if (!subset.empty() && subset.count(lm_obs.first->id_) == 0) { + to_retriangulate.insert(lm_obs.first->id_); + continue; + } const auto& obs = lm_obs.second; ba.AddPointProjectionObservation(shot.id_, lm_obs.first->id_, obs.point, obs.scale, obs.depth_prior); @@ -754,6 +865,13 @@ py::dict BAHelpers::Bundle( const auto timer_run = std::chrono::high_resolution_clock::now(); BundleToMap(ba, map, !fix_cameras); + if(!subset.empty()){ + sfm::retriangulation::Triangulate(map, to_retriangulate, + config["triangulation_min_ray_angle"].cast(), + config["triangulation_min_depth"].cast(), + config["processes"].cast()); + } + const auto timer_triangulate = std::chrono::high_resolution_clock::now(); const auto timer_teardown = std::chrono::high_resolution_clock::now(); report["brief_report"] = ba.BriefReport(); @@ -767,13 +885,18 @@ py::dict BAHelpers::Bundle( timer_setup) .count() / 1000000.0; + report["wall_times"]["triangulate"] = + std::chrono::duration_cast(timer_triangulate - + timer_run) + .count() / + 1000000.0; report["wall_times"]["teardown"] = std::chrono::duration_cast(timer_teardown - - timer_run) + timer_triangulate) .count() / 1000000.0; report["num_images"] = map.GetShots().size(); - report["num_points"] = map.GetLandmarks().size(); + report["num_points"] = subset.empty() ? map.GetLandmarks().size() : subset.size(); report["num_reprojections"] = added_reprojections; return report; } @@ -824,10 +947,15 @@ void BAHelpers::BundleToMap(const bundle::BundleAdjuster& bundle_adjuster, // Update points for (auto& point : output_map.GetLandmarks()) { - const auto& pt = bundle_adjuster.GetPoint(point.first); + if(!bundle_adjuster.HasPoint(point.first)) { + continue; + } + auto pt = bundle_adjuster.GetPoint(point.first); if (!pt.GetValue().allFinite()) { - throw std::runtime_error("Point " + point.first + - " has either NaN or INF values."); + // set large reprojection errors + for(auto& proj_error : pt.reprojection_errors) { + proj_error.second.setConstant(1.0); + } } point.second.SetGlobalPos(pt.GetValue()); point.second.SetReprojectionErrors(pt.reprojection_errors); diff --git a/opensfm/src/sfm/src/retriangulation.cc b/opensfm/src/sfm/src/retriangulation.cc index 65ff8073a..2173a5d39 100644 --- a/opensfm/src/sfm/src/retriangulation.cc +++ b/opensfm/src/sfm/src/retriangulation.cc @@ -2,8 +2,10 @@ #include #include +#include +#include + #include -#include namespace sfm::retriangulation { void RealignMaps(const map::Map& map_from, map::Map& map_to, @@ -113,4 +115,80 @@ void RealignMaps(const map::Map& map_from, map::Map& map_to, map_to.RemoveShot(shot_id); } } + +int Triangulate(map::Map& map, const std::unordered_set& track_ids, float min_angle, float min_depth, int processing_threads) { + constexpr size_t kDefaultObservations = 10; + const float min_angle_rad = min_angle * M_PI / 180.0; + + int count = 0; + std::vector track_ids_vec(track_ids.begin(), track_ids.end()); + std::vector to_delete; +#pragma omp parallel num_threads(processing_threads) + { + std::vector thread_to_delete; + + std::vector threshold_list(kDefaultObservations, 1.0); + MatX3d origins(kDefaultObservations, 3); + MatX3d bearings(kDefaultObservations, 3); +#pragma omp for schedule(static) + for (int i = 0; i < static_cast(track_ids_vec.size()); ++i) { + const auto& track_id = track_ids_vec.at(i); + if (!map.HasLandmark(track_id)) { + continue; + } + auto& landmark = map.GetLandmark(track_id); + const auto& observations = landmark.GetObservations(); + const size_t num_observations = observations.size(); + if (num_observations < 2) { + thread_to_delete.push_back(track_id); + continue; + } + + if(threshold_list.size() != num_observations) { + threshold_list.resize(num_observations, 1.0); + } + if(origins.rows() != num_observations) { + origins.resize(num_observations, 3); + } + if(bearings.rows() != num_observations) { + bearings.resize(num_observations, 3); + } + + int idx = 0; + for (const auto& obs_pair : observations) { + const map::Shot* shot = obs_pair.first; + origins.row(idx) = shot->GetPose()->GetOrigin(); + bearings.row(idx) = shot->LandmarkBearing(&landmark); + ++idx; + } + + // Triangulate using midpoint method + const auto ok_pos = geometry::TriangulateBearingsMidpoint(origins, bearings, threshold_list, + min_angle_rad, min_depth); + if (!ok_pos.first) { + thread_to_delete.push_back(track_id); + } + else { + landmark.SetGlobalPos(ok_pos.second); + + // Refine the triangulated point + const auto pos_refined = geometry::PointRefinement(origins, bearings, landmark.GetGlobalPos(), 20); + landmark.SetGlobalPos(pos_refined); + } + } +#pragma omp critical + { + to_delete.insert(to_delete.end(), thread_to_delete.begin(), thread_to_delete.end()); + } + } + + // Remove landmarks that were not triangulated + for (const auto& track_id : to_delete) { + if (map.HasLandmark(track_id)) { + map.RemoveLandmark(track_id); + } + } + return count; +} + } // namespace sfm::retriangulation diff --git a/opensfm/src/sfm/src/tracks_helpers.cc b/opensfm/src/sfm/src/tracks_helpers.cc index d3aa710dd..4b25cd84b 100644 --- a/opensfm/src/sfm/src/tracks_helpers.cc +++ b/opensfm/src/sfm/src/tracks_helpers.cc @@ -11,8 +11,18 @@ std::unordered_map CountTracksPerShot( for (const auto& track : tracks) { tracks_set.insert(track); } + std::unordered_map counts; - for (const auto& shot : shots) { + for (int i = 0; i < shots.size(); ++i) { + counts[shots[i]] = 0; + } + +#pragma omp parallel +{ + std::vector thread_counts(shots.size(), 0); +#pragma omp for + for (int i = 0; i < shots.size(); ++i) { + const auto& shot = shots[i]; const auto& observations = manager.GetShotObservations(shot); int sum = 0; @@ -23,8 +33,19 @@ std::unordered_map CountTracksPerShot( } ++sum; } - counts[shot] = sum; + thread_counts[i] = sum; + } + +#pragma omp critical + { + for (int i = 0; i < shots.size(); ++i) { + if (thread_counts[i] > 0) { + counts[shots[i]] = thread_counts[i]; + } + } } +} + return counts; } diff --git a/opensfm/tracking.py b/opensfm/tracking.py index 04e5da48c..2fbc0e196 100644 --- a/opensfm/tracking.py +++ b/opensfm/tracking.py @@ -1,12 +1,12 @@ # pyre-strict import logging import typing as t -from typing import cast, Dict, List, Tuple +from typing import Callable, cast, Dict, List, Tuple import networkx as nx import numpy as np from numpy.typing import NDArray -from opensfm import pymap +from opensfm import context, pymap from opensfm.dataset_base import DataSetBase from opensfm.pymap import TracksManager from opensfm.unionfind import UnionFind @@ -24,38 +24,49 @@ def load_features( Dict[str, NDArray[np.int32]], Dict[str, NDArray[np.float64]], ]: - logging.info("reading features") + logging.info("Reading features") + + def load_one(im): + features_data = dataset.load_features(im) + if not features_data: + return im, None, None, None, None, None + features = features_data.points[:, :3] + colors = features_data.colors + segmentations = None + instances = None + if features_data.semantic: + segmentations = features_data.semantic.segmentation + if features_data.semantic.has_instances(): + instances = features_data.semantic.instances + depths = features_data.depths if features_data.depths is not None else None + return im, features, colors, segmentations, instances, depths + + # Use context.parallel_map for parallel loading + results = context.parallel_map(load_one, images, dataset.config.get("processes", 1)) + features = {} colors = {} segmentations = {} instances = {} depths = {} - for im in images: - features_data = dataset.load_features(im) - - if not features_data: - continue - - features[im] = features_data.points[:, :3] - colors[im] = features_data.colors - - semantic_data = features_data.semantic - if semantic_data: - segmentations[im] = semantic_data.segmentation - if semantic_data.has_instances(): - instances[im] = semantic_data.instances - - depth_data = features_data.depths - if depth_data is not None: - depths[im] = depth_data + for im, feat, color, seg, inst, depth in results: + if feat is not None: + features[im] = feat + colors[im] = color + if seg is not None: + segmentations[im] = seg + if inst is not None: + instances[im] = inst + if depth is not None: + depths[im] = depth return features, colors, segmentations, instances, depths def load_matches( dataset: DataSetBase, images: List[str] -) -> Dict[Tuple[str, str], List[Tuple[int, int]]]: - matches = {} +) -> t.Iterator[Tuple[Tuple[str, str], List[Tuple[int, int]]]]: + """Yield matches for each image pair""" for im1 in images: try: im1_matches = dataset.load_matches(im1) @@ -63,16 +74,15 @@ def load_matches( continue for im2 in im1_matches: if im2 in images: - matches[im1, im2] = im1_matches[im2] - return matches + yield (im1, im2), im1_matches[im2] -def create_tracks_manager( +def create_tracks_manager_from_matches_iter( features: Dict[str, NDArray[np.float64]], colors: Dict[str, NDArray[np.int32]], segmentations: Dict[str, NDArray[np.int32]], instances: Dict[str, NDArray[np.int32]], - matches: Dict[Tuple[str, str], List[Tuple[int, int]]], + matches: Callable[[], t.Iterator[Tuple[Tuple[str, str], List[Tuple[int, int]]]]], min_length: int, depths: Dict[str, NDArray[np.float64]], depth_is_radial: bool = True, @@ -80,9 +90,11 @@ def create_tracks_manager( ) -> TracksManager: """Link matches into tracks.""" logger.debug("Merging features onto tracks") + + logger.debug("Running union-find to find aggregated tracks") uf = UnionFind() - for im1, im2 in matches: - for f1, f2 in matches[im1, im2]: + for (im1, im2), pairs in matches(): + for f1, f2 in pairs: uf.union((im1, f1), (im2, f2)) sets = {} @@ -95,6 +107,7 @@ def create_tracks_manager( tracks = [t for t in sets.values() if _good_track(t, min_length)] + logger.debug("Constructing TracksManager from tracks") NO_VALUE = pymap.Observation.NO_SEMANTIC_VALUE tracks_manager = pymap.TracksManager() num_observations = 0 @@ -147,6 +160,44 @@ def create_tracks_manager( return tracks_manager +def create_tracks_manager( + features: Dict[str, NDArray[np.float64]], + colors: Dict[str, NDArray[np.int32]], + segmentations: Dict[str, NDArray[np.int32]], + instances: Dict[str, NDArray[np.int32]], + matches: t.Union[ + Dict[Tuple[str, str], List[Tuple[int, int]]], + Callable[[], t.Iterator[Tuple[Tuple[str, str], List[Tuple[int, int]]]]] + ], + min_length: int, + depths: Dict[str, NDArray[np.float64]], + depth_is_radial: bool = True, + depth_std_deviation: float = 1.0, +) -> TracksManager: + """ + Link matches into tracks. + + If matches is a dict, it will be wrapped as an iterator. + If matches is a callable, it will be used directly. + """ + if callable(matches): + matches_iter = matches + else: + def matches_iter(): + return iter(matches.items()) + return create_tracks_manager_from_matches_iter( + features, + colors, + segmentations, + instances, + matches_iter, + min_length, + depths, + depth_is_radial, + depth_std_deviation, + ) + + def common_tracks( tracks_manager: pymap.TracksManager, im1: str, im2: str ) -> Tuple[List[str], NDArray[np.float64], NDArray[np.float64]]: @@ -179,9 +230,10 @@ def common_tracks( def all_common_tracks_with_features( tracks_manager: pymap.TracksManager, min_common: int = 50, + processes: int = 1, ) -> Dict[Tuple[str, str], TPairTracks]: tracks = all_common_tracks( - tracks_manager, include_features=True, min_common=min_common + tracks_manager, include_features=True, min_common=min_common, processes=processes ) return cast(Dict[Tuple[str, str], TPairTracks], tracks) @@ -189,9 +241,10 @@ def all_common_tracks_with_features( def all_common_tracks_without_features( tracks_manager: pymap.TracksManager, min_common: int = 50, + processes: int = 1, ) -> Dict[Tuple[str, str], List[str]]: tracks = all_common_tracks( - tracks_manager, include_features=False, min_common=min_common + tracks_manager, include_features=False, min_common=min_common, processes=processes ) return cast(Dict[Tuple[str, str], List[str]], tracks) @@ -200,6 +253,7 @@ def all_common_tracks( tracks_manager: pymap.TracksManager, include_features: bool = True, min_common: int = 50, + processes: int = 1, ) -> Dict[Tuple[str, str], t.Union[TPairTracks, List[str]]]: """List of tracks observed by each image pair. @@ -213,20 +267,31 @@ def all_common_tracks( tuple: im1, im2 -> tuple: tracks, features from first image, features from second image """ - common_tracks = {} - for (im1, im2), size in tracks_manager.get_all_pairs_connectivity().items(): + def process_pair(pair): + im1, im2, size = pair if size < min_common: - continue - - tuples = tracks_manager.get_all_common_observations(im1, im2) + return None + track_ids, points1, points2 = tracks_manager.get_all_common_observations_arrays(im1, im2) if include_features: - common_tracks[im1, im2] = ( - [v for v, _, _ in tuples], - np.array([p.point for _, p, _ in tuples]), - np.array([p.point for _, _, p in tuples]), + return ( + (im1, im2), + (track_ids, points1, points2), ) else: - common_tracks[im1, im2] = [v for v, _, _ in tuples] + return ((im1, im2), track_ids,) + + logger.debug("Computing pairwise connectivity of images") + pairs = [(k[0], k[1], v) for k, v in tracks_manager.get_all_pairs_connectivity().items()] + + logger.debug(f"Gathering pairwise tracks with {processes} processes") + batch_size = max(1, len(pairs) // (2 * processes)) + results = context.parallel_map(process_pair, pairs, processes, batch_size) + + common_tracks = {} + for result in results: + if result is not None: + k, v = result + common_tracks[k] = v return common_tracks diff --git a/opensfm/vlad.py b/opensfm/vlad.py index 64d7cd18c..d6de6b510 100644 --- a/opensfm/vlad.py +++ b/opensfm/vlad.py @@ -41,8 +41,14 @@ def vlad_distances( Returns the image, the order of the other images, and the other images. """ + + # avoid passing a gigantic VLAD dictionary in case of preemption : copy instead + ratio_copy = 0.5 + need_copy = len(other_images) < ratio_copy*len(histograms) + candidates = {k: histograms[k] for k in other_images + [image]} if need_copy else histograms + distances, others = pyfeatures.compute_vlad_distances( - histograms, image, set(other_images) + candidates, image, set(other_images) ) return image, distances, others