diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index 1223ef13c..80479eb78 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -76,9 +76,18 @@ gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db); gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db); #include +#include +template +virtual class TransferFactor : gtsam::NoiseModelFactor { + TransferFactor(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2, + const std::vector>& triplets + ); +}; + #include +#include #include -template +template virtual class EssentialTransferFactor : gtsam::NoiseModelFactor { EssentialTransferFactor(gtsam::EdgeKey edge1, gtsam::EdgeKey edge2, const std::vector>& triplets diff --git a/python/gtsam/examples/EssentialViewGraphExample.py b/python/gtsam/examples/EssentialViewGraphExample.py index 2d3df815c..a3c313674 100644 --- a/python/gtsam/examples/EssentialViewGraphExample.py +++ b/python/gtsam/examples/EssentialViewGraphExample.py @@ -24,25 +24,20 @@ import gtsam from gtsam import Cal3_S2, EdgeKey, EssentialMatrix from gtsam import EssentialTransferFactorCal3_S2 as Factor from gtsam import (LevenbergMarquardtOptimizer, LevenbergMarquardtParams, - NonlinearFactorGraph, PinholeCameraCal3_S2, Point2, Point3, - Pose3, Values, symbol_shorthand) + NonlinearFactorGraph, PinholeCameraCal3_S2, Values) # For symbol shorthand (e.g., X(0), L(1)) -K = symbol_shorthand.K +K = gtsam.symbol_shorthand.K # Formatter function for printing keys def formatter(key): sym = gtsam.Symbol(key) - if sym.chr() == ord("K"): - return str(sym) - elif sym.chr() == ord("E"): - idx = sym.index() - a = idx // 10 - b = idx % 10 - return f"E({a},{b})" + if sym.chr() == ord("k"): + return f"k{sym.index()}" else: - return str(sym) + edge = EdgeKey(key) + return f"({edge.i()},{edge.j()})" def main(): @@ -88,6 +83,8 @@ def main(): graph.add(Factor(EdgeKey(a, b), EdgeKey(b, c), tuples2)) graph.add(Factor(EdgeKey(a, c), EdgeKey(a, b), tuples3)) + graph.print("graph", formatter) + # Create a delta vector to perturb the ground truth (small perturbation) delta = np.ones(5) * 1e-2 diff --git a/python/gtsam/examples/ViewGraphExample.py b/python/gtsam/examples/ViewGraphExample.py new file mode 100644 index 000000000..fe4d78e3c --- /dev/null +++ b/python/gtsam/examples/ViewGraphExample.py @@ -0,0 +1,114 @@ +""" + GTSAM Copyright 2010, Georgia Tech Research Corporation, + Atlanta, Georgia 30332-0415 + All Rights Reserved + Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + See LICENSE for the license information + + Solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file + Author: Frank Dellaert (Python: Akshay Krishnan, John Lambert, Varun Agrawal) +""" + +""" +Python version of ViewGraphExample.cpp +View-graph calibration on a simulated dataset, a la Sweeney 2015 +Author: Frank Dellaert +Date: October 2024 +""" + +import numpy as np +from gtsam.examples import SFMdata + +from gtsam import (Cal3_S2, EdgeKey, FundamentalMatrix, + LevenbergMarquardtOptimizer, LevenbergMarquardtParams, + NonlinearFactorGraph, PinholeCameraCal3_S2) +from gtsam import TransferFactorFundamentalMatrix as Factor +from gtsam import Values + + +# Formatter function for printing keys +def formatter(key): + edge = EdgeKey(key) + return f"({edge.i()},{edge.j()})" + + +def main(): + # Define the camera calibration parameters + K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0) + + # Create the set of 8 ground-truth landmarks + points = SFMdata.createPoints() + + # Create the set of 4 ground-truth poses + poses = SFMdata.posesOnCircle(4, 30) + + # Calculate ground truth fundamental matrices, 1 and 2 poses apart + F1 = FundamentalMatrix(K, poses[0].between(poses[1]), K) + F2 = FundamentalMatrix(K, poses[0].between(poses[2]), K) + + # Simulate measurements from each camera pose + p = [[None for _ in range(8)] for _ in range(4)] + for i in range(4): + camera = PinholeCameraCal3_S2(poses[i], K) + for j in range(8): + p[i][j] = camera.project(points[j]) + + # Create the factor graph + graph = NonlinearFactorGraph() + + for a in range(4): + b = (a + 1) % 4 # Next camera + c = (a + 2) % 4 # Camera after next + + # Vectors to collect tuples for each factor + tuples1 = [] + tuples2 = [] + tuples3 = [] + + # Collect data for the three factors + for j in range(8): + tuples1.append((p[a][j], p[b][j], p[c][j])) + tuples2.append((p[a][j], p[c][j], p[b][j])) + tuples3.append((p[c][j], p[b][j], p[a][j])) + + # Add transfer factors between views a, b, and c. + graph.add(Factor(EdgeKey(a, c), EdgeKey(b, c), tuples1)) + graph.add(Factor(EdgeKey(a, b), EdgeKey(b, c), tuples2)) + graph.add(Factor(EdgeKey(a, c), EdgeKey(a, b), tuples3)) + + # Print the factor graph + graph.print("Factor Graph:\n", formatter) + + # Create a delta vector to perturb the ground truth + delta = np.array([1, 2, 3, 4, 5, 6, 7]) * 1e-5 + + # Create the data structure to hold the initial estimate to the solution + initialEstimate = Values() + for a in range(4): + b = (a + 1) % 4 # Next camera + c = (a + 2) % 4 # Camera after next + initialEstimate.insert(EdgeKey(a, b).key(), F1.retract(delta)) + initialEstimate.insert(EdgeKey(a, c).key(), F2.retract(delta)) + + initialEstimate.print("Initial Estimates:\n", formatter) + graph.printErrors(initialEstimate, "Initial Errors:\n", formatter) + + # Optimize the graph and print results + params = LevenbergMarquardtParams() + params.setLambdaInitial(1000.0) # Initialize lambda to a high value + params.setVerbosityLM("SUMMARY") + optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate, params) + result = optimizer.optimize() + + print(f"Initial error = {graph.error(initialEstimate)}") + print(f"Final error = {graph.error(result)}") + + result.print("Final Results:\n", formatter) + + print("Ground Truth F1:\n", F1.matrix()) + print("Ground Truth F2:\n", F2.matrix()) + + +if __name__ == "__main__": + main()