diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index f493fe8f6..80f694e97 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -537,9 +537,8 @@ class ISAM2 { gtsam::Values calculateEstimate() const; template , gtsam::PinholeCamera, gtsam::PinholeCamera, diff --git a/gtsam/nonlinear/values.i b/gtsam/nonlinear/values.i index 3582a3249..a75fe5ae1 100644 --- a/gtsam/nonlinear/values.i +++ b/gtsam/nonlinear/values.i @@ -4,13 +4,14 @@ namespace gtsam { +#include #include #include +#include #include #include -#include -#include #include +#include #include #include #include @@ -80,17 +81,24 @@ class Values { void insert(size_t j, const gtsam::Rot3& rot3); void insert(size_t j, const gtsam::Pose3& pose3); void insert(size_t j, const gtsam::Unit3& unit3); + void insert(size_t j, const gtsam::Cal3f& cal3f); void insert(size_t j, const gtsam::Cal3_S2& cal3_s2); void insert(size_t j, const gtsam::Cal3DS2& cal3ds2); void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler); void insert(size_t j, const gtsam::Cal3Fisheye& cal3fisheye); void insert(size_t j, const gtsam::Cal3Unified& cal3unified); - void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix); + void insert(size_t j, const gtsam::EssentialMatrix& E); + void insert(size_t j, const gtsam::FundamentalMatrix& F); + void insert(size_t j, const gtsam::SimpleFundamentalMatrix& F); + void insert(size_t j, const gtsam::PinholeCamera& camera); void insert(size_t j, const gtsam::PinholeCamera& camera); + void insert(size_t j, const gtsam::PinholeCamera& camera); void insert(size_t j, const gtsam::PinholeCamera& camera); void insert(size_t j, const gtsam::PinholeCamera& camera); void insert(size_t j, const gtsam::PinholeCamera& camera); + void insert(size_t j, const gtsam::PinholePose& camera); void insert(size_t j, const gtsam::PinholePose& camera); + void insert(size_t j, const gtsam::PinholePose& camera); void insert(size_t j, const gtsam::PinholePose& camera); void insert(size_t j, const gtsam::PinholePose& camera); void insert(size_t j, const gtsam::PinholePose& camera); @@ -115,17 +123,24 @@ class Values { void update(size_t j, const gtsam::Rot3& rot3); void update(size_t j, const gtsam::Pose3& pose3); void update(size_t j, const gtsam::Unit3& unit3); + void update(size_t j, const gtsam::Cal3f& cal3f); void update(size_t j, const gtsam::Cal3_S2& cal3_s2); void update(size_t j, const gtsam::Cal3DS2& cal3ds2); void update(size_t j, const gtsam::Cal3Bundler& cal3bundler); void update(size_t j, const gtsam::Cal3Fisheye& cal3fisheye); void update(size_t j, const gtsam::Cal3Unified& cal3unified); - void update(size_t j, const gtsam::EssentialMatrix& essential_matrix); + void update(size_t j, const gtsam::EssentialMatrix& E); + void update(size_t j, const gtsam::FundamentalMatrix& F); + void update(size_t j, const gtsam::SimpleFundamentalMatrix& F); + void update(size_t j, const gtsam::PinholeCamera& camera); void update(size_t j, const gtsam::PinholeCamera& camera); + void update(size_t j, const gtsam::PinholeCamera& camera); void update(size_t j, const gtsam::PinholeCamera& camera); void update(size_t j, const gtsam::PinholeCamera& camera); void update(size_t j, const gtsam::PinholeCamera& camera); + void update(size_t j, const gtsam::PinholePose& camera); void update(size_t j, const gtsam::PinholePose& camera); + void update(size_t j, const gtsam::PinholePose& camera); void update(size_t j, const gtsam::PinholePose& camera); void update(size_t j, const gtsam::PinholePose& camera); void update(size_t j, const gtsam::PinholePose& camera); @@ -147,23 +162,33 @@ class Values { void insert_or_assign(size_t j, const gtsam::Rot3& rot3); void insert_or_assign(size_t j, const gtsam::Pose3& pose3); void insert_or_assign(size_t j, const gtsam::Unit3& unit3); + void insert_or_assign(size_t j, const gtsam::Cal3f& cal3f); void insert_or_assign(size_t j, const gtsam::Cal3_S2& cal3_s2); void insert_or_assign(size_t j, const gtsam::Cal3DS2& cal3ds2); void insert_or_assign(size_t j, const gtsam::Cal3Bundler& cal3bundler); void insert_or_assign(size_t j, const gtsam::Cal3Fisheye& cal3fisheye); void insert_or_assign(size_t j, const gtsam::Cal3Unified& cal3unified); + void insert_or_assign(size_t j, const gtsam::EssentialMatrix& E); + void insert_or_assign(size_t j, const gtsam::FundamentalMatrix& F); + void insert_or_assign(size_t j, const gtsam::SimpleFundamentalMatrix& F); void insert_or_assign(size_t j, - const gtsam::EssentialMatrix& essential_matrix); + const gtsam::PinholeCamera& camera); void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); + void insert_or_assign(size_t j, + const gtsam::PinholeCamera& camera); void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); + void insert_or_assign(size_t j, + const gtsam::PinholePose& camera); void insert_or_assign(size_t j, const gtsam::PinholePose& camera); + void insert_or_assign(size_t j, + const gtsam::PinholePose& camera); void insert_or_assign(size_t j, const gtsam::PinholePose& camera); void insert_or_assign(size_t j, @@ -185,17 +210,24 @@ class Values { gtsam::Rot3, gtsam::Pose3, gtsam::Unit3, + gtsam::Cal3f, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::Cal3Fisheye, gtsam::Cal3Unified, - gtsam::EssentialMatrix, + gtsam::EssentialMatrix, + gtsam::FundamentalMatrix, + gtsam::SimpleFundamentalMatrix, + gtsam::PinholeCamera, gtsam::PinholeCamera, + gtsam::PinholeCamera, gtsam::PinholeCamera, gtsam::PinholeCamera, gtsam::PinholeCamera, + gtsam::PinholePose, gtsam::PinholePose, + gtsam::PinholePose, gtsam::PinholePose, gtsam::PinholePose, gtsam::PinholePose, diff --git a/python/gtsam/examples/EssentialViewGraphExample.py b/python/gtsam/examples/EssentialViewGraphExample.py index a3c313674..d7d2cda38 100644 --- a/python/gtsam/examples/EssentialViewGraphExample.py +++ b/python/gtsam/examples/EssentialViewGraphExample.py @@ -5,9 +5,6 @@ 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) """ """ diff --git a/python/gtsam/examples/ViewGraphExample.py b/python/gtsam/examples/ViewGraphExample.py index fe4d78e3c..0ba41762e 100644 --- a/python/gtsam/examples/ViewGraphExample.py +++ b/python/gtsam/examples/ViewGraphExample.py @@ -5,9 +5,6 @@ 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) """ """ @@ -96,7 +93,7 @@ def main(): # Optimize the graph and print results params = LevenbergMarquardtParams() - params.setLambdaInitial(1000.0) # Initialize lambda to a high value + params.setlambdaInitial(1000.0) # Initialize lambda to a high value params.setVerbosityLM("SUMMARY") optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate, params) result = optimizer.optimize()