Skip to content

Commit

Permalink
Fixed GTSAM inconsistent arguments error
Browse files Browse the repository at this point in the history
  • Loading branch information
@travisdriver
travisdriver committed Oct 16, 2024
1 parent 2800239 commit b8ac520
Show file tree
Hide file tree
Showing 2 changed files with 64 additions and 82 deletions.
24 changes: 11 additions & 13 deletions gtsfm/utils/io.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()

Expand Down
122 changes: 53 additions & 69 deletions gtsfm/view_graph_estimator/cycle_consistent_rotation_estimator.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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)]
Expand Down Expand Up @@ -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

0 comments on commit b8ac520

Please sign in to comment.