From b8ac52022a03c62ee6a84c8ef0da52240270f6c6 Mon Sep 17 00:00:00 2001 From: Travis Driver Date: Wed, 16 Oct 2024 17:48:50 -0400 Subject: [PATCH] Fixed GTSAM inconsistent arguments error --- gtsfm/utils/io.py | 24 ++-- .../cycle_consistent_rotation_estimator.py | 122 ++++++++---------- 2 files changed, 64 insertions(+), 82 deletions(-) diff --git a/gtsfm/utils/io.py b/gtsfm/utils/io.py index 1c8ee211b..027d02bb7 100644 --- a/gtsfm/utils/io.py +++ b/gtsfm/utils/io.py @@ -11,27 +11,25 @@ from typing import Any, Dict, List, Optional, Tuple, Union import gtsam +import h5py +import numpy as np +import open3d +import simplejson as json +from gtsam import Cal3Bundler, Point3, Pose3, Rot3, SfmTrack +from PIL import Image as PILImage +from PIL.ExifTags import GPSTAGS, TAGS + import gtsfm.utils.images as image_utils import gtsfm.utils.logger as logger_utils import gtsfm.utils.reprojection as reproj_utils import gtsfm.visualization.open3d_vis_utils as open3d_vis_utils -import h5py -import numpy as np -#import open3d -import simplejson as json import thirdparty.colmap.scripts.python.read_write_model as colmap_io -from gtsam import Cal3Bundler, Point3, Pose3, Rot3, SfmTrack from gtsfm.common.gtsfm_data import GtsfmData from gtsfm.common.image import Image from gtsfm.common.sfm_track import SfmTrack2d -from PIL import Image as PILImage -from PIL.ExifTags import GPSTAGS, TAGS -from thirdparty.colmap.scripts.python.read_write_model import \ - Camera as ColmapCamera -from thirdparty.colmap.scripts.python.read_write_model import \ - Image as ColmapImage -from thirdparty.colmap.scripts.python.read_write_model import \ - Point3D as ColmapPoint3D +from thirdparty.colmap.scripts.python.read_write_model import Camera as ColmapCamera +from thirdparty.colmap.scripts.python.read_write_model import Image as ColmapImage +from thirdparty.colmap.scripts.python.read_write_model import Point3D as ColmapPoint3D logger = logger_utils.get_logger() diff --git a/gtsfm/view_graph_estimator/cycle_consistent_rotation_estimator.py b/gtsfm/view_graph_estimator/cycle_consistent_rotation_estimator.py index 682527e9d..16ab5a597 100644 --- a/gtsfm/view_graph_estimator/cycle_consistent_rotation_estimator.py +++ b/gtsfm/view_graph_estimator/cycle_consistent_rotation_estimator.py @@ -23,10 +23,10 @@ logger = logger_utils.get_logger() -# threshold for cycle consistency inference +# Threshold for cycle consistency inference ERROR_THRESHOLD = 7.0 -# threshold for evaluation w.r.t. GT +# Threshold for evaluation w.r.t. GT MAX_INLIER_MEASUREMENT_ERROR_DEG = 5.0 E = gtsam.symbol_shorthand.E # essential matrix @@ -108,18 +108,17 @@ def run( logger.info("Input number of edges: %d" % len(i2Ri1_dict)) input_edges: List[Tuple[int, int]] = i2Ri1_dict.keys() triplets: List[Tuple[int, int, int]] = graph_utils.extract_cyclic_triplets_from_edges(input_edges) - logger.info("Number of triplets: %d" % len(triplets)) - i2Ri1_dict, i2Ui1_dict = self.optimize_essential_matrices( - triplets, i2Ri1_dict, i2Ui1_dict, calibrations, corr_idxs_i1i2, keypoints - ) + # # Optimize essential matrices. + # i2Ri1_dict, i2Ui1_dict = self.optimize_essential_matrices( + # triplets, i2Ri1_dict, i2Ui1_dict, calibrations, corr_idxs_i1i2, keypoints + # ) + # Compute the cycle error for each triplet, and add it to its edges for aggregation. per_edge_errors = defaultdict(list) cycle_errors: List[float] = [] max_gt_error_in_cycle = [] - - # Compute the cycle error for each triplet, and add it to its edges for aggregation. for i0, i1, i2 in triplets: # sort order guaranteed error = comp_utils.compute_cyclic_rotation_error( i1Ri0=i2Ri1_dict[(i0, i1)], i2Ri1=i2Ri1_dict[(i1, i2)], i2Ri0=i2Ri1_dict[(i0, i2)] @@ -249,79 +248,64 @@ def optimize_essential_matrices( calibrations: List[Cal3Bundler], corr_idxs_i1i2: Dict[Tuple[int, int], np.ndarray], keypoints: List[Keypoints], - ): - # Create a factor graph container. + use_robust_loss: bool = True, + ) -> Tuple[Dict[Tuple[int, int], Rot3], Dict[Tuple[int, int], Unit3]]: + """Jointly optimize over all essential matrices for edges contained in triplets. + + Args: + triplets: List of triplets in view graph. + i2Ri1_dict: Dict from (i1, i2) to relative rotation of i1 with respect to i2. + i2Ui1_dict: Dict from (i1, i2) to relative translation direction of i1 with respect to i2. + calibrations: List of calibrations for each image. + corr_idxs_i1i2: Dict from (i1, i2) to indices of verified correspondences from i1 to i2. + keypoints: keypoints for each images. + use_robust_loss: whether to use Huber loss. + + Returns: + Optimized relative rotations and directions. + """ + # Create GTSAM objects. graph = gtsam.NonlinearFactorGraph() + initial = gtsam.Values() + noise_model=gtsam.noiseModel.Isotropic.Sigma(1, 1e-2) + if use_robust_loss: + huber_loss = gtsam.noiseModel.mEstimator.Huber.Create(1.345) + noise_model = gtsam.noiseModel.Robust.Create(huber_loss, noise_model) - # Add essential matrix factors. - noise_model = gtsam.noiseModel.Isotropic.Sigma(1, 1.0) - pair_to_key = {} + # Loop through triplets and edges. + edge_to_key = {} for i0, i1, i2 in triplets: - - # i0 -> i1 - pair = (i0, i1) - corr01 = corr_idxs_i1i2[pair] - mkps0, mkps1 = keypoints[pair[0]].coordinates[corr01[:, 0]], keypoints[pair[1]].coordinates[corr01[:, 1]] - if pair in pair_to_key: - key = pair_to_key[pair] - else: - key = E(len(pair_to_key)) - pair_to_key[pair] = key - for kp0, kp1 in zip(mkps0, mkps1): - graph.add( - gtsam.EssentialMatrixFactor( - key, calibrations[pair[1]].calibrate(kp1), calibrations[pair[0]].calibrate(kp0), noise_model - ) - ) - - # i0 -> i2 - pair = (i0, i2) - corr01 = corr_idxs_i1i2[pair] - mkps0, mkps1 = keypoints[pair[0]].coordinates[corr01[:, 0]], keypoints[pair[1]].coordinates[corr01[:, 1]] - if pair in pair_to_key: - key = pair_to_key[pair] - else: - key = E(len(pair_to_key)) - pair_to_key[pair] = key - for kp0, kp1 in zip(mkps0, mkps1): - graph.add( - gtsam.EssentialMatrixFactor( - key, calibrations[pair[1]].calibrate(kp1), calibrations[pair[0]].calibrate(kp0), noise_model + for edge in [(i0, i1), (i0, i2), (i1, i2)]: + if edge not in edge_to_key and corr_idxs_i1i2[edge].shape[0] > 0: + # Add graph key to dictionary for later. + key = len(edge_to_key) + edge_to_key[edge] = key + + # Add essential matrix factors. + mkps0 = keypoints[edge[0]].coordinates[corr_idxs_i1i2[edge][:, 0]] + mkps1 = keypoints[edge[1]].coordinates[corr_idxs_i1i2[edge][:, 1]] + initial.insert(key, gtsam.EssentialMatrix(i2Ri1_dict[edge], i2Ui1_dict[edge])) + for kp0, kp1 in zip(mkps0, mkps1): + graph.add( + gtsam.EssentialMatrixFactor( + key, + calibrations[edge[1]].calibrate(kp1), + calibrations[edge[0]].calibrate(kp0), + noise_model, + ) ) - ) - - # i1 -> i2 - pair = (i1, i2) - corr01 = corr_idxs_i1i2[pair] - mkps0, mkps1 = keypoints[pair[0]].coordinates[corr01[:, 0]], keypoints[pair[1]].coordinates[corr01[:, 1]] - if pair in pair_to_key: - key = pair_to_key[pair] - else: - key = E(len(pair_to_key)) - pair_to_key[pair] = key - for kp0, kp1 in zip(mkps0, mkps1): - graph.add( - gtsam.EssentialMatrixFactor( - key, calibrations[pair[1]].calibrate(kp1), calibrations[pair[0]].calibrate(kp0), noise_model - ) - ) - print("Num edges in E opt:", len(pair_to_key)) - - # Create initial estimate. - initial = gtsam.Values() - for pair, key in pair_to_key.items(): - initial.insert(key, gtsam.EssentialMatrix(i2Ri1_dict[pair], i2Ui1_dict[pair])) # Optimize! params = gtsam.LevenbergMarquardtParams() params.setVerbosity("ERROR") + params.setMaxIterations(10) optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) result = optimizer.optimize() # Add optimized rotations and translations to the dictionary. - for pair, key in pair_to_key.items(): + for edge, key in edge_to_key.items(): E_opt = result.atEssentialMatrix(key) - i2Ri1_dict[pair] = E_opt.rotation() - i2Ui1_dict[pair] = E_opt.direction() + i2Ri1_dict[edge] = E_opt.rotation() + i2Ui1_dict[edge] = E_opt.direction() return i2Ri1_dict, i2Ui1_dict