From 7d468e98a04a581caeafb674e0ac4af024552f59 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 9 Nov 2021 14:50:48 -0500 Subject: [PATCH 1/7] fix warning --- gtsam/nonlinear/GncOptimizer.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index 3ddaf4820..3025d2468 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -142,8 +142,9 @@ class GTSAM_EXPORT GncOptimizer { * provides an extra interface for the user to initialize the weightst * */ void setWeights(const Vector w) { - if(w.size() != nfg_.size()){ - throw std::runtime_error("GncOptimizer::setWeights: the number of specified weights" + if (size_t(w.size()) != nfg_.size()) { + throw std::runtime_error( + "GncOptimizer::setWeights: the number of specified weights" " does not match the size of the factor graph."); } weights_ = w; From bfb21c2faad20e2731cd4d98c56c9b78d4e60ea1 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 9 Nov 2021 16:50:14 -0500 Subject: [PATCH 2/7] reduce call stack --- gtsam/inference/EliminateableFactorGraph-inst.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index 81f4047a1..4157336d1 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -36,17 +36,17 @@ namespace gtsam { // no Ordering is provided. When removing optional from VariableIndex, create VariableIndex // before creating ordering. VariableIndex computedVariableIndex(asDerived()); - return eliminateSequential(function, computedVariableIndex, orderingType); + return eliminateSequential(orderingType, function, computedVariableIndex); } else { // Compute an ordering and call this function again. We are guaranteed to have a // VariableIndex already here because we computed one if needed in the previous 'if' block. if (orderingType == Ordering::METIS) { Ordering computedOrdering = Ordering::Metis(asDerived()); - return eliminateSequential(computedOrdering, function, variableIndex, orderingType); + return eliminateSequential(computedOrdering, function, variableIndex); } else { Ordering computedOrdering = Ordering::Colamd(*variableIndex); - return eliminateSequential(computedOrdering, function, variableIndex, orderingType); + return eliminateSequential(computedOrdering, function, variableIndex); } } } From 916504129990b40683427b81453cacbc787a82da Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 9 Nov 2021 17:56:03 -0500 Subject: [PATCH 3/7] use safer eigen indexing syntax --- gtsam/nonlinear/factorTesting.h | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/gtsam/nonlinear/factorTesting.h b/gtsam/nonlinear/factorTesting.h index 74ef87737..3a9b6fb11 100644 --- a/gtsam/nonlinear/factorTesting.h +++ b/gtsam/nonlinear/factorTesting.h @@ -35,8 +35,7 @@ namespace gtsam { * zero errors anyway. However, it means that below will only be exact for the correct measurement. */ JacobianFactor linearizeNumerically(const NoiseModelFactor& factor, - const Values& values, double delta = 1e-5) { - + const Values& values, double delta = 1e-5) { // We will fill a vector of key/Jacobians pairs (a map would sort) std::vector > jacobians; @@ -46,24 +45,24 @@ JacobianFactor linearizeNumerically(const NoiseModelFactor& factor, // Loop over all variables const double one_over_2delta = 1.0 / (2.0 * delta); - for(Key key: factor) { + for (Key key : factor) { // Compute central differences using the values struct. VectorValues dX = values.zeroVectors(); const size_t cols = dX.dim(key); Matrix J = Matrix::Zero(rows, cols); for (size_t col = 0; col < cols; ++col) { Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols); - dx[col] = delta; + dx(col) = delta; dX[key] = dx; Values eval_values = values.retract(dX); const Eigen::VectorXd left = factor.whitenedError(eval_values); - dx[col] = -delta; + dx(col) = -delta; dX[key] = dx; eval_values = values.retract(dX); const Eigen::VectorXd right = factor.whitenedError(eval_values); J.col(col) = (left - right) * one_over_2delta; } - jacobians.push_back(std::make_pair(key,J)); + jacobians.push_back(std::make_pair(key, J)); } // Next step...return JacobianFactor From a634a91c1a7233d1afaa016d3e909ae7077d4b73 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 9 Nov 2021 18:14:01 -0500 Subject: [PATCH 4/7] wrap Colamd function --- gtsam/nonlinear/nonlinear.i | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index d068bd7ee..ecf63094d 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -115,6 +115,10 @@ class Ordering { Ordering(); Ordering(const gtsam::Ordering& other); + template + static gtsam::Ordering Colamd(const FACTOR_GRAPH& graph); + // Testable void print(string s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; From 1bcb44784a1dc7f5239001b1cec6a6383a058ed9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 9 Nov 2021 18:19:47 -0500 Subject: [PATCH 5/7] format and refactor the SFM BAL example --- python/gtsam/examples/SFMExample_bal.py | 107 +++++++++++++----------- 1 file changed, 59 insertions(+), 48 deletions(-) diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py index dfe8b523c..65b9e1334 100644 --- a/python/gtsam/examples/SFMExample_bal.py +++ b/python/gtsam/examples/SFMExample_bal.py @@ -7,49 +7,58 @@ 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) + Author: Frank Dellaert (Python: Akshay Krishnan, John Lambert, Varun Agrawal) """ import argparse import logging import sys -import matplotlib.pyplot as plt -import numpy as np - import gtsam -from gtsam import ( - GeneralSFMFactorCal3Bundler, - PinholeCameraCal3Bundler, - PriorFactorPinholeCameraCal3Bundler, - readBal, - symbol_shorthand -) +from gtsam import (GeneralSFMFactorCal3Bundler, + PriorFactorPinholeCameraCal3Bundler, PriorFactorPoint3, + readBal) +from gtsam.symbol_shorthand import C, P +from gtsam.utils.plot import plot_3d_points, plot_trajectory -C = symbol_shorthand.C -P = symbol_shorthand.P +logging.basicConfig(stream=sys.stdout, level=logging.INFO) -logging.basicConfig(stream=sys.stdout, level=logging.DEBUG) +def plot(scene_data: gtsam.SfmData, result: gtsam.Values): + """Plot the trajectory.""" + plot_vals = gtsam.Values() + for cam_idx in range(scene_data.number_cameras()): + plot_vals.insert(C(cam_idx), + result.atPinholeCameraCal3Bundler(C(cam_idx)).pose()) + for t_idx in range(scene_data.number_tracks()): + plot_vals.insert(P(t_idx), result.atPoint3(P(t_idx))) -def run(args): + plot_3d_points(0, plot_vals, linespec="g.") + plot_trajectory(0, plot_vals, show=True) + + +def run(args: argparse.Namespace): """ Run LM optimization with BAL input data and report resulting error """ - input_file = gtsam.findExampleDataFile(args.input_file) + if args.input_file: + input_file = args.input_file + else: + input_file = gtsam.findExampleDataFile("dubrovnik-3-7-pre") # Load the SfM data from file scene_data = readBal(input_file) - logging.info(f"read {scene_data.number_tracks()} tracks on {scene_data.number_cameras()} cameras\n") + logging.info("read %d tracks on %d cameras\n", scene_data.number_tracks(), + scene_data.number_cameras()) # Create a factor graph graph = gtsam.NonlinearFactorGraph() # We share *one* noiseModel between all projection factors - noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v + noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v # Add measurements to the factor graph j = 0 for t_idx in range(scene_data.number_tracks()): - track = scene_data.track(t_idx) # SfmTrack + track = scene_data.track(t_idx) # SfmTrack # retrieve the SfmMeasurement objects for m_idx in range(track.number_measurements()): # i represents the camera index, and uv is the 2d measurement @@ -60,20 +69,18 @@ def run(args): # Add a prior on pose x1. This indirectly specifies where the origin is. graph.push_back( - gtsam.PriorFactorPinholeCameraCal3Bundler( - C(0), scene_data.camera(0), gtsam.noiseModel.Isotropic.Sigma(9, 0.1) - ) - ) + PriorFactorPinholeCameraCal3Bundler( + C(0), scene_data.camera(0), + gtsam.noiseModel.Isotropic.Sigma(9, 0.1))) # Also add a prior on the position of the first landmark to fix the scale graph.push_back( - gtsam.PriorFactorPoint3( - P(0), scene_data.track(0).point3(), gtsam.noiseModel.Isotropic.Sigma(3, 0.1) - ) - ) + PriorFactorPoint3(P(0), + scene_data.track(0).point3(), + gtsam.noiseModel.Isotropic.Sigma(3, 0.1))) # Create initial estimate initial = gtsam.Values() - + i = 0 # add each PinholeCameraCal3Bundler for cam_idx in range(scene_data.number_cameras()): @@ -81,12 +88,10 @@ def run(args): initial.insert(C(i), camera) i += 1 - j = 0 # add each SfmTrack for t_idx in range(scene_data.number_tracks()): - track = scene_data.track(t_idx) - initial.insert(P(j), track.point3()) - j += 1 + track = scene_data.track(t_idx) + initial.insert(P(t_idx), track.point3()) # Optimize the graph and print results try: @@ -94,25 +99,31 @@ def run(args): params.setVerbosityLM("ERROR") lm = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) result = lm.optimize() - except Exception as e: + except RuntimeError: logging.exception("LM Optimization failed") return + # Error drops from ~2764.22 to ~0.046 - logging.info(f"final error: {graph.error(result)}") + logging.info("initial error: %f", graph.error(initial)) + logging.info("final error: %f", graph.error(result)) + + plot(scene_data, result) + + +def main(): + """Main runner.""" + parser = argparse.ArgumentParser() + parser.add_argument('-i', + '--input_file', + type=str, + default="", + help="""Read SFM data from the specified BAL file. + The data format is described here: https://grail.cs.washington.edu/projects/bal/. + BAL files contain (nrPoses, nrPoints, nrObservations), followed by (i,j,u,v) tuples, + then (wx,wy,wz,tx,ty,tz,f,k1,k1) as Bundler camera calibrations w/ Rodrigues vector + and (x,y,z) 3d point initializations.""") + run(parser.parse_args()) if __name__ == "__main__": - parser = argparse.ArgumentParser() - parser.add_argument( - '-i', - '--input_file', - type=str, - default="dubrovnik-3-7-pre", - help='Read SFM data from the specified BAL file' - 'The data format is described here: https://grail.cs.washington.edu/projects/bal/.' - 'BAL files contain (nrPoses, nrPoints, nrObservations), followed by (i,j,u,v) tuples, ' - 'then (wx,wy,wz,tx,ty,tz,f,k1,k1) as Bundler camera calibrations w/ Rodrigues vector' - 'and (x,y,z) 3d point initializations.' - ) - run(parser.parse_args()) - + main() From 5c9c60a0be9e0ac556e0b6801412814ecb7434bd Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 10 Nov 2021 13:57:31 -0500 Subject: [PATCH 6/7] address reviewer comments --- python/gtsam/examples/SFMExample_bal.py | 45 ++++++++++++------------- 1 file changed, 22 insertions(+), 23 deletions(-) diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py index 65b9e1334..77c186bd3 100644 --- a/python/gtsam/examples/SFMExample_bal.py +++ b/python/gtsam/examples/SFMExample_bal.py @@ -16,36 +16,37 @@ import sys import gtsam from gtsam import (GeneralSFMFactorCal3Bundler, - PriorFactorPinholeCameraCal3Bundler, PriorFactorPoint3, - readBal) + PriorFactorPinholeCameraCal3Bundler, PriorFactorPoint3) from gtsam.symbol_shorthand import C, P -from gtsam.utils.plot import plot_3d_points, plot_trajectory +from gtsam.utils import plot +from matplotlib import pyplot as plt logging.basicConfig(stream=sys.stdout, level=logging.INFO) +DEFAULT_BAL_DATASET = "dubrovnik-3-7-pre" -def plot(scene_data: gtsam.SfmData, result: gtsam.Values): - """Plot the trajectory.""" + +def plot_scene(scene_data: gtsam.SfmData, result: gtsam.Values): + """Plot the SFM results.""" plot_vals = gtsam.Values() for cam_idx in range(scene_data.number_cameras()): plot_vals.insert(C(cam_idx), result.atPinholeCameraCal3Bundler(C(cam_idx)).pose()) - for t_idx in range(scene_data.number_tracks()): - plot_vals.insert(P(t_idx), result.atPoint3(P(t_idx))) + for j in range(scene_data.number_tracks()): + plot_vals.insert(P(j), result.atPoint3(P(j))) - plot_3d_points(0, plot_vals, linespec="g.") - plot_trajectory(0, plot_vals, show=True) + plot.plot_3d_points(0, plot_vals, linespec="g.") + plot.plot_trajectory(0, plot_vals, title="SFM results") + + plt.show() def run(args: argparse.Namespace): """ Run LM optimization with BAL input data and report resulting error """ - if args.input_file: - input_file = args.input_file - else: - input_file = gtsam.findExampleDataFile("dubrovnik-3-7-pre") + input_file = args.input_file # Load the SfM data from file - scene_data = readBal(input_file) + scene_data = gtsam.readBal(input_file) logging.info("read %d tracks on %d cameras\n", scene_data.number_tracks(), scene_data.number_cameras()) @@ -56,16 +57,14 @@ def run(args: argparse.Namespace): noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v # Add measurements to the factor graph - j = 0 - for t_idx in range(scene_data.number_tracks()): - track = scene_data.track(t_idx) # SfmTrack + for j in range(scene_data.number_tracks()): + track = scene_data.track(j) # SfmTrack # retrieve the SfmMeasurement objects for m_idx in range(track.number_measurements()): # i represents the camera index, and uv is the 2d measurement i, uv = track.measurement(m_idx) # note use of shorthand symbols C and P graph.add(GeneralSFMFactorCal3Bundler(uv, noise, C(i), P(j))) - j += 1 # Add a prior on pose x1. This indirectly specifies where the origin is. graph.push_back( @@ -89,9 +88,9 @@ def run(args: argparse.Namespace): i += 1 # add each SfmTrack - for t_idx in range(scene_data.number_tracks()): - track = scene_data.track(t_idx) - initial.insert(P(t_idx), track.point3()) + for j in range(scene_data.number_tracks()): + track = scene_data.track(j) + initial.insert(P(j), track.point3()) # Optimize the graph and print results try: @@ -107,7 +106,7 @@ def run(args: argparse.Namespace): logging.info("initial error: %f", graph.error(initial)) logging.info("final error: %f", graph.error(result)) - plot(scene_data, result) + plot_scene(scene_data, result) def main(): @@ -116,7 +115,7 @@ def main(): parser.add_argument('-i', '--input_file', type=str, - default="", + default=gtsam.findExampleDataFile(DEFAULT_BAL_DATASET), help="""Read SFM data from the specified BAL file. The data format is described here: https://grail.cs.washington.edu/projects/bal/. BAL files contain (nrPoses, nrPoints, nrObservations), followed by (i,j,u,v) tuples, From 0ccb18b055074777593f6169e2c5110037cd2cab Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 10 Nov 2021 14:27:28 -0500 Subject: [PATCH 7/7] add return type definitions Because my time is more valuable than a reviewer's pedanticness --- python/gtsam/examples/SFMExample_bal.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py index 77c186bd3..f3e48c3c3 100644 --- a/python/gtsam/examples/SFMExample_bal.py +++ b/python/gtsam/examples/SFMExample_bal.py @@ -17,8 +17,8 @@ import sys import gtsam from gtsam import (GeneralSFMFactorCal3Bundler, PriorFactorPinholeCameraCal3Bundler, PriorFactorPoint3) -from gtsam.symbol_shorthand import C, P -from gtsam.utils import plot +from gtsam.symbol_shorthand import C, P # type: ignore +from gtsam.utils import plot # type: ignore from matplotlib import pyplot as plt logging.basicConfig(stream=sys.stdout, level=logging.INFO) @@ -26,7 +26,7 @@ logging.basicConfig(stream=sys.stdout, level=logging.INFO) DEFAULT_BAL_DATASET = "dubrovnik-3-7-pre" -def plot_scene(scene_data: gtsam.SfmData, result: gtsam.Values): +def plot_scene(scene_data: gtsam.SfmData, result: gtsam.Values) -> None: """Plot the SFM results.""" plot_vals = gtsam.Values() for cam_idx in range(scene_data.number_cameras()): @@ -41,7 +41,7 @@ def plot_scene(scene_data: gtsam.SfmData, result: gtsam.Values): plt.show() -def run(args: argparse.Namespace): +def run(args: argparse.Namespace) -> None: """ Run LM optimization with BAL input data and report resulting error """ input_file = args.input_file @@ -109,7 +109,7 @@ def run(args: argparse.Namespace): plot_scene(scene_data, result) -def main(): +def main() -> None: """Main runner.""" parser = argparse.ArgumentParser() parser.add_argument('-i',