From 169d42f313bd81a1dedfffd05fa24fe95c34cb90 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 21 Sep 2020 12:08:18 -0400 Subject: [PATCH 1/9] use cleaner checkmark symbol in readme --- python/gtsam/examples/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/gtsam/examples/README.md b/python/gtsam/examples/README.md index e998e4dcd8..d70867dda7 100644 --- a/python/gtsam/examples/README.md +++ b/python/gtsam/examples/README.md @@ -7,7 +7,7 @@ | DiscreteBayesNet_FG | none of the required discrete functionality is exposed through Python | | easyPoint2KalmanFilter | ExtendedKalmanFilter not yet exposed through Python | | elaboratePoint2KalmanFilter | GaussianSequentialSolver not yet exposed through Python | -| ImuFactorExample2 | X | +| ImuFactorExample2 | :heavy_check_mark: | | ImuFactorsExample | | | ISAM2Example_SmartFactor | | | ISAM2_SmartFactorStereo_IMU | | @@ -49,4 +49,4 @@ Extra Examples (with no C++ equivalent) - PlanarManipulatorExample -- SFMData \ No newline at end of file +- SFMData From 7478c19aad268ff8b5e677dad2f405b20b9ea285 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 21 Sep 2020 12:16:20 -0400 Subject: [PATCH 2/9] update example list --- python/gtsam/examples/README.md | 31 ++++++++++++++++++++----------- 1 file changed, 20 insertions(+), 11 deletions(-) diff --git a/python/gtsam/examples/README.md b/python/gtsam/examples/README.md index d70867dda7..3a325aa305 100644 --- a/python/gtsam/examples/README.md +++ b/python/gtsam/examples/README.md @@ -3,49 +3,58 @@ | C++ Example Name | Ported | |-------------------------------------------------------|--------| | CameraResectioning | | +| CombinedImuFactorsExample | | | CreateSFMExampleData | | +| DiscreteBayesNetExample | | | DiscreteBayesNet_FG | none of the required discrete functionality is exposed through Python | | easyPoint2KalmanFilter | ExtendedKalmanFilter not yet exposed through Python | | elaboratePoint2KalmanFilter | GaussianSequentialSolver not yet exposed through Python | -| ImuFactorExample2 | :heavy_check_mark: | +| FisheyeExample | | +| HMMExample | | +| ImuFactorsExample2 | :heavy_check_mark: | | ImuFactorsExample | | +| IMUKittiExampleGPS.cpp | | +| InverseKinematicsExampleExpressions.cpp | | | ISAM2Example_SmartFactor | | | ISAM2_SmartFactorStereo_IMU | | | LocalizationExample | impossible? | | METISOrderingExample | | -| OdometryExample | X | -| PlanarSLAMExample | X | -| Pose2SLAMExample | X | +| OdometryExample | :heavy_check_mark: | +| PlanarSLAMExample | :heavy_check_mark: | +| Pose2SLAMExample | :heavy_check_mark: | | Pose2SLAMExampleExpressions | | -| Pose2SLAMExample_g2o | X | +| Pose2SLAMExample_g2o | :heavy_check_mark: | | Pose2SLAMExample_graph | | | Pose2SLAMExample_graphviz | | | Pose2SLAMExample_lago | lago not yet exposed through Python | | Pose2SLAMStressTest | | | Pose2SLAMwSPCG | | +| Pose3Localization.cpp | | | Pose3SLAMExample_changeKeys | | | Pose3SLAMExampleExpressions_BearingRangeWithTransform | | -| Pose3SLAMExample_g2o | X | +| Pose3SLAMExample_g2o | :heavy_check_mark: | | Pose3SLAMExample_initializePose3Chordal | | | Pose3SLAMExample_initializePose3Gradient | | | RangeISAMExample_plaza2 | | | SelfCalibrationExample | | +| SFMdata | | | SFMExample_bal_COLAMD_METIS | | -| SFMExample_bal | | -| SFMExample | X | +| SFMExample_bal | :heavy_check_mark: | +| SFMExample | :heavy_check_mark: | | SFMExampleExpressions_bal | | | SFMExampleExpressions | | | SFMExample_SmartFactor | | | SFMExample_SmartFactorPCG | | -| SimpleRotation | X | +| ShonanAveragingCLI.cpp | | +| SimpleRotation | :heavy_check_mark: | | SolverComparer | | | StereoVOExample | | | StereoVOExample_large | | | TimeTBB | | | UGM_chain | discrete functionality not yet exposed | | UGM_small | discrete functionality not yet exposed | -| VisualISAM2Example | X | -| VisualISAMExample | X | +| VisualISAM2Example | :heavy_check_mark: | +| VisualISAMExample | :heavy_check_mark: | Extra Examples (with no C++ equivalent) - PlanarManipulatorExample From 0e7719ae9893ef04f13adbf8fa797c11d167a293 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 21 Sep 2020 12:19:33 -0400 Subject: [PATCH 3/9] update python examples list --- python/gtsam/examples/README.md | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/python/gtsam/examples/README.md b/python/gtsam/examples/README.md index 3a325aa305..84a41990fe 100644 --- a/python/gtsam/examples/README.md +++ b/python/gtsam/examples/README.md @@ -13,7 +13,7 @@ | HMMExample | | | ImuFactorsExample2 | :heavy_check_mark: | | ImuFactorsExample | | -| IMUKittiExampleGPS.cpp | | +| IMUKittiExampleGPS | | | InverseKinematicsExampleExpressions.cpp | | | ISAM2Example_SmartFactor | | | ISAM2_SmartFactorStereo_IMU | | @@ -29,11 +29,11 @@ | Pose2SLAMExample_lago | lago not yet exposed through Python | | Pose2SLAMStressTest | | | Pose2SLAMwSPCG | | -| Pose3Localization.cpp | | +| Pose3Localization | | | Pose3SLAMExample_changeKeys | | | Pose3SLAMExampleExpressions_BearingRangeWithTransform | | | Pose3SLAMExample_g2o | :heavy_check_mark: | -| Pose3SLAMExample_initializePose3Chordal | | +| Pose3SLAMExample_initializePose3Chordal | :heavy_check_mark: | | Pose3SLAMExample_initializePose3Gradient | | | RangeISAMExample_plaza2 | | | SelfCalibrationExample | | @@ -45,7 +45,7 @@ | SFMExampleExpressions | | | SFMExample_SmartFactor | | | SFMExample_SmartFactorPCG | | -| ShonanAveragingCLI.cpp | | +| ShonanAveragingCLI | :heavy_check_mark: | | SimpleRotation | :heavy_check_mark: | | SolverComparer | | | StereoVOExample | | @@ -57,5 +57,8 @@ | VisualISAMExample | :heavy_check_mark: | Extra Examples (with no C++ equivalent) +- DogLegOptimizerExample +- GPSFactorExample - PlanarManipulatorExample +- PreintegrationExample - SFMData From 2af56303fc4f05c9ea841398974715ee3c8fe3f8 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 21 Sep 2020 12:28:16 -0400 Subject: [PATCH 4/9] add p --- python/gtsam/examples/SFMExample_bal.py | 111 ++++++++++++++++++++++++ 1 file changed, 111 insertions(+) create mode 100644 python/gtsam/examples/SFMExample_bal.py diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py new file mode 100644 index 0000000000..50e450fea6 --- /dev/null +++ b/python/gtsam/examples/SFMExample_bal.py @@ -0,0 +1,111 @@ +""" + 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: John Lambert +""" + +import argparse +import logging + +import gtsam +import matplotlib.pyplot as plt +import numpy as np + +from gtsam import symbol_shorthand +from gtsam import readBal + +L = symbol_shorthand.L +X = symbol_shorthand.X + +import pdb + +#include +#include +#include +#include +#include // for loading BAL datasets ! + + +# using symbol_shorthand::C; +# using symbol_shorthand::P; + +# We will be using a projection factor that ties a SFM_Camera to a 3D point. +# An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration +# and has a total of 9 free parameters +#typedef GeneralSFMFactor MyFactor; + + +def run(args): + """ Run LM optimization with BAL input data and report resulting error """ + # Find default file, but if an argument is given, try loading a 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 + mydata = readBal(input_file) + logging.info(f"read {mydata.number_tracks()} tracks on {mydata.number_cameras()} cameras\n") + + # # 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 + + # Add measurements to the factor graph + j = 0 + pdb.set_trace() + for t_idx in range(mydata.number_tracks()): + track = mydata.track(t_idx) # SfmTrack + for m_idx in range(track.number_measurements()): + # retrieve the SfmMeasurement + # i represents the camera index, and uv is the 2d measurement + i, uv = track.measurement(0) # + #graph.emplace_shared(uv, noise, C(i), P(j)) # note use of shorthand symbols C and P + #graph.add + j += 1 + pdb.set_trace() + + # # Add a prior on pose x1. This indirectly specifies where the origin is. + # # and a prior on the position of the first landmark to fix the scale + # graph.addPrior(C(0), mydata.cameras[0], gtsam.noiseModel.Isotropic.Sigma(9, 0.1)) + # graph.addPrior(P(0), mydata.tracks[0].p, gtsam.noiseModel.Isotropic.Sigma(3, 0.1)) + + # # Create initial estimate + initial = gtsam.Values() + # i = 0 + # j = 0; + # for(const SfmCamera& camera: mydata.cameras) initial.insert(C(i++), camera) + # for(const SfmTrack& track: mydata.tracks) initial.insert(P(j++), track.p) + + # Optimize the graph and print results + # Values result; + try: + params = gtsam.LevenbergMarquardtParams() + params.setVerbosityLM("ERROR") + lm = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) + result = lm.optimize() + except Exception as e: + logging.exception("LM Optimization failed") + return + + logging.info(f"final error: {graph.error(result)}") + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument('-i', '--input_file', type=str, default="", + help='Read SFM data from the specified BAL file') + run(parser.parse_args()) + + + + From 0d19859f8259c81bad5cf97b736b7f4d84e6ced1 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 21 Sep 2020 12:28:46 -0400 Subject: [PATCH 5/9] add python equivalent of c++ sfm data calls --- python/gtsam/examples/SFMExample_bal.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py index 50e450fea6..b70b8057b1 100644 --- a/python/gtsam/examples/SFMExample_bal.py +++ b/python/gtsam/examples/SFMExample_bal.py @@ -81,8 +81,8 @@ def run(args): # # Create initial estimate initial = gtsam.Values() - # i = 0 - # j = 0; + i = 0 + j = 0 # for(const SfmCamera& camera: mydata.cameras) initial.insert(C(i++), camera) # for(const SfmTrack& track: mydata.tracks) initial.insert(P(j++), track.p) From afa74c4f575a5cd92442823c6acad41ff31efde7 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 21 Sep 2020 14:21:33 -0400 Subject: [PATCH 6/9] cannot retrieve p attribute --- python/gtsam/examples/SFMExample_bal.py | 40 ++++++++++++++----------- 1 file changed, 23 insertions(+), 17 deletions(-) diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py index b70b8057b1..ab3baa26c6 100644 --- a/python/gtsam/examples/SFMExample_bal.py +++ b/python/gtsam/examples/SFMExample_bal.py @@ -4,7 +4,6 @@ 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 @@ -21,20 +20,12 @@ from gtsam import symbol_shorthand from gtsam import readBal -L = symbol_shorthand.L -X = symbol_shorthand.X +C = symbol_shorthand.C +P = symbol_shorthand.P import pdb -#include -#include -#include #include -#include // for loading BAL datasets ! - - -# using symbol_shorthand::C; -# using symbol_shorthand::P; # We will be using a projection factor that ties a SFM_Camera to a 3D point. # An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration @@ -65,8 +56,8 @@ def run(args): pdb.set_trace() for t_idx in range(mydata.number_tracks()): track = mydata.track(t_idx) # SfmTrack + # retrieve the SfmMeasurement objects for m_idx in range(track.number_measurements()): - # retrieve the SfmMeasurement # i represents the camera index, and uv is the 2d measurement i, uv = track.measurement(0) # #graph.emplace_shared(uv, noise, C(i), P(j)) # note use of shorthand symbols C and P @@ -76,18 +67,33 @@ def run(args): # # Add a prior on pose x1. This indirectly specifies where the origin is. # # and a prior on the position of the first landmark to fix the scale - # graph.addPrior(C(0), mydata.cameras[0], gtsam.noiseModel.Isotropic.Sigma(9, 0.1)) - # graph.addPrior(P(0), mydata.tracks[0].p, gtsam.noiseModel.Isotropic.Sigma(3, 0.1)) + + + graph.push_back(gtsam.PriorFactorVector( + C(0), mydata.camera(0), gtsam.noiseModel.Isotropic.Sigma(9, 0.1))) + # # graph.addPrior(P(0), mydata.track(0).p, + # equivalent of addPrior + graph.push_back(gtsam.PriorFactorVector( + P(0), mydata.track(0)[1], gtsam.noiseModel.Isotropic.Sigma(3, 0.1))) # # Create initial estimate initial = gtsam.Values() + i = 0 + # add each SfmCamera + for cam_idx in range(mydata.number_cameras()): + camera = mydata.camera(0) + initial.insert(C(i), camera) + i += 1 + j = 0 - # for(const SfmCamera& camera: mydata.cameras) initial.insert(C(i++), camera) - # for(const SfmTrack& track: mydata.tracks) initial.insert(P(j++), track.p) + # add each SfmTrack + for t_idx in range(mydata.number_tracks()): + track = mydata.track(0) + initial.insert(P(j), track.p) + j += 1 # Optimize the graph and print results - # Values result; try: params = gtsam.LevenbergMarquardtParams() params.setVerbosityLM("ERROR") From 34f670e9d5ba6225b7ea8705d4c7ad310429169f Mon Sep 17 00:00:00 2001 From: John Lambert Date: Tue, 22 Sep 2020 15:44:31 -0400 Subject: [PATCH 7/9] remove BAL for now, and add get3dPoint() for p access --- gtsam/gtsam.i | 1 + gtsam/slam/dataset.h | 3 + python/gtsam/examples/SFMExample_bal.py | 117 ------------------------ 3 files changed, 4 insertions(+), 117 deletions(-) delete mode 100644 python/gtsam/examples/SFMExample_bal.py diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 52f5901ee4..78e0043903 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2847,6 +2847,7 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { #include class SfmTrack { + Point3 get3dPoint() const; size_t number_measurements() const; pair measurement(size_t idx) const; pair siftIndex(size_t idx) const; diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 91ceaa5fd2..5bb8063bd5 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -233,6 +233,9 @@ struct SfmTrack { SiftIndex siftIndex(size_t idx) const { return siftIndices[idx]; } + Point3 get3dPoint() const { + return p; + } }; /// Define the structure for the camera poses diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py deleted file mode 100644 index ab3baa26c6..0000000000 --- a/python/gtsam/examples/SFMExample_bal.py +++ /dev/null @@ -1,117 +0,0 @@ -""" - 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: John Lambert -""" - -import argparse -import logging - -import gtsam -import matplotlib.pyplot as plt -import numpy as np - -from gtsam import symbol_shorthand -from gtsam import readBal - -C = symbol_shorthand.C -P = symbol_shorthand.P - -import pdb - -#include - -# We will be using a projection factor that ties a SFM_Camera to a 3D point. -# An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration -# and has a total of 9 free parameters -#typedef GeneralSFMFactor MyFactor; - - -def run(args): - """ Run LM optimization with BAL input data and report resulting error """ - # Find default file, but if an argument is given, try loading a 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 - mydata = readBal(input_file) - logging.info(f"read {mydata.number_tracks()} tracks on {mydata.number_cameras()} cameras\n") - - # # 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 - - # Add measurements to the factor graph - j = 0 - pdb.set_trace() - for t_idx in range(mydata.number_tracks()): - track = mydata.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 - i, uv = track.measurement(0) # - #graph.emplace_shared(uv, noise, C(i), P(j)) # note use of shorthand symbols C and P - #graph.add - j += 1 - pdb.set_trace() - - # # Add a prior on pose x1. This indirectly specifies where the origin is. - # # and a prior on the position of the first landmark to fix the scale - - - graph.push_back(gtsam.PriorFactorVector( - C(0), mydata.camera(0), gtsam.noiseModel.Isotropic.Sigma(9, 0.1))) - # # graph.addPrior(P(0), mydata.track(0).p, - # equivalent of addPrior - graph.push_back(gtsam.PriorFactorVector( - P(0), mydata.track(0)[1], gtsam.noiseModel.Isotropic.Sigma(3, 0.1))) - - # # Create initial estimate - initial = gtsam.Values() - - i = 0 - # add each SfmCamera - for cam_idx in range(mydata.number_cameras()): - camera = mydata.camera(0) - initial.insert(C(i), camera) - i += 1 - - j = 0 - # add each SfmTrack - for t_idx in range(mydata.number_tracks()): - track = mydata.track(0) - initial.insert(P(j), track.p) - j += 1 - - # Optimize the graph and print results - try: - params = gtsam.LevenbergMarquardtParams() - params.setVerbosityLM("ERROR") - lm = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) - result = lm.optimize() - except Exception as e: - logging.exception("LM Optimization failed") - return - - logging.info(f"final error: {graph.error(result)}") - - -if __name__ == "__main__": - parser = argparse.ArgumentParser() - parser.add_argument('-i', '--input_file', type=str, default="", - help='Read SFM data from the specified BAL file') - run(parser.parse_args()) - - - - From 8e0b0c1641fa6baf897250ccb33d3959f1d79ae5 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Tue, 22 Sep 2020 15:49:05 -0400 Subject: [PATCH 8/9] mark SFMExample_bal as still in progress --- python/gtsam/examples/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/examples/README.md b/python/gtsam/examples/README.md index 84a41990fe..e05a0c7e1e 100644 --- a/python/gtsam/examples/README.md +++ b/python/gtsam/examples/README.md @@ -39,7 +39,7 @@ | SelfCalibrationExample | | | SFMdata | | | SFMExample_bal_COLAMD_METIS | | -| SFMExample_bal | :heavy_check_mark: | +| SFMExample_bal | | | SFMExample | :heavy_check_mark: | | SFMExampleExpressions_bal | | | SFMExampleExpressions | | From e6057fc4faf5f44f223ca3f0d62018231c5d099a Mon Sep 17 00:00:00 2001 From: John Lambert Date: Tue, 22 Sep 2020 23:20:36 -0400 Subject: [PATCH 9/9] rename get3dPoint() to point3() --- gtsam/gtsam.i | 2 +- gtsam/slam/dataset.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 78e0043903..1764ecd82f 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2847,7 +2847,7 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { #include class SfmTrack { - Point3 get3dPoint() const; + Point3 point3() const; size_t number_measurements() const; pair measurement(size_t idx) const; pair siftIndex(size_t idx) const; diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 5bb8063bd5..a8fddcfe4c 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -233,7 +233,7 @@ struct SfmTrack { SiftIndex siftIndex(size_t idx) const { return siftIndices[idx]; } - Point3 get3dPoint() const { + Point3 point3() const { return p; } };