From 5cdbacfd4408db583c1726dd301b64fe60a659ca Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 7 Jan 2024 20:23:58 -0500 Subject: [PATCH 01/21] test and fix for issue 301 --- gtsam/nonlinear/ISAM2.cpp | 15 +++-- tests/testDoglegOptimizer.cpp | 122 ++++++++++++++++++++++++++++++++-- 2 files changed, 126 insertions(+), 11 deletions(-) diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 0b5449b58..47857a2a2 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -424,6 +424,11 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, ISAM2Result result(params_.enableDetailedResults); UpdateImpl update(params_, updateParams); + // Initialize any new variables \Theta_{new} and add + // \Theta:=\Theta\cup\Theta_{new}. + // Needed before delta update if using Dogleg optimizer. + addVariables(newTheta, result.details()); + // Update delta if we need it to check relinearization later if (update.relinarizationNeeded(update_count_)) updateDelta(updateParams.forceFullSolve); @@ -435,9 +440,7 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, update.computeUnusedKeys(newFactors, variableIndex_, result.keysWithRemovedFactors, &result.unusedKeys); - // 2. Initialize any new variables \Theta_{new} and add - // \Theta:=\Theta\cup\Theta_{new}. - addVariables(newTheta, result.details()); + // 2. Compute new error to check for relinearization if (params_.evaluateNonlinearError) update.error(nonlinearFactors_, calculateEstimate(), &result.errorBefore); @@ -731,6 +734,7 @@ void ISAM2::updateDelta(bool forceFullSolve) const { effectiveWildfireThreshold, &delta_); deltaReplacedMask_.clear(); gttoc(Wildfire_update); + } else if (std::holds_alternative(params_.optimizationParams)) { // If using Dogleg, do a Dogleg step const ISAM2DoglegParams& doglegParams = @@ -769,9 +773,8 @@ void ISAM2::updateDelta(bool forceFullSolve) const { gttic(Copy_dx_d); // Update Delta and linear step doglegDelta_ = doglegResult.delta; - delta_ = - doglegResult - .dx_d; // Copy the VectorValues containing with the linear solution + // Copy the VectorValues containing with the linear solution + delta_ = doglegResult.dx_d; gttoc(Copy_dx_d); } else { throw std::runtime_error("iSAM2: unknown ISAM2Params type"); diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index e64e5ab7a..d7de63d3d 100644 --- a/tests/testDoglegOptimizer.cpp +++ b/tests/testDoglegOptimizer.cpp @@ -24,10 +24,9 @@ #include #include #include -#include -#include -#include -#include +#include +#include +#include "examples/SFMdata.h" #include @@ -36,7 +35,6 @@ using namespace gtsam; // Convenience for named keys using symbol_shorthand::X; -using symbol_shorthand::L; /* ************************************************************************* */ TEST(DoglegOptimizer, ComputeBlend) { @@ -185,6 +183,120 @@ TEST(DoglegOptimizer, Constraint) { #endif } +/* ************************************************************************* */ +TEST(DogLegOptimizer, VariableUpdate) { + // Make the typename short so it looks much cleaner + typedef SmartProjectionPoseFactor SmartFactor; + + // create a typedef to the camera type + typedef PinholePose Camera; + // Define the camera calibration parameters + Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); + + // Define the camera observation noise model + noiseModel::Isotropic::shared_ptr measurementNoise = + noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + + // Create the set of ground-truth landmarks and poses + vector points = createPoints(); + vector poses = createPoses(); + + // Create a factor graph + NonlinearFactorGraph graph; + + ISAM2DoglegParams doglegparams = ISAM2DoglegParams(); + doglegparams.verbose = false; + ISAM2Params isam2_params; + isam2_params.evaluateNonlinearError = true; + isam2_params.relinearizeThreshold = 0.0; + isam2_params.enableRelinearization = true; + isam2_params.optimizationParams = doglegparams; + isam2_params.relinearizeSkip = 1; + ISAM2 isam2(isam2_params); + + // Simulated measurements from each camera pose, adding them to the factor + // graph + unordered_map smart_factors; + for (size_t j = 0; j < points.size(); ++j) { + // every landmark represent a single landmark, we use shared pointer to init + // the factor, and then insert measurements. + SmartFactor::shared_ptr smartfactor(new SmartFactor(measurementNoise, K)); + + for (size_t i = 0; i < poses.size(); ++i) { + // generate the 2D measurement + Camera camera(poses[i], K); + Point2 measurement = camera.project(points[j]); + + // call add() function to add measurement into a single factor, here we + // need to add: + // 1. the 2D measurement + // 2. the corresponding camera's key + // 3. camera noise model + // 4. camera calibration + + // add only first 3 measurements and update the later measurements + // incrementally + if (i < 3) smartfactor->add(measurement, i); + } + + // insert the smart factor in the graph + smart_factors[j] = smartfactor; + graph.push_back(smartfactor); + } + + // Add a prior on pose x0. This indirectly specifies where the origin is. + // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas( + (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); + graph.emplace_shared >(0, poses[0], noise); + + // Because the structure-from-motion problem has a scale ambiguity, the + // problem is still under-constrained. Here we add a prior on the second pose + // x1, so this will fix the scale by indicating the distance between x0 and + // x1. Because these two are fixed, the rest of the poses will be also be + // fixed. + graph.emplace_shared >(1, poses[1], + noise); // add directly to graph + + // Create the initial estimate to the solution + // Intentionally initialize the variables off from the ground truth + Values initialEstimate; + Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); + for (size_t i = 0; i < 3; ++i) + initialEstimate.insert(i, poses[i].compose(delta)); + // initialEstimate.print("Initial Estimates:\n"); + + // Optimize the graph and print results + isam2.update(graph, initialEstimate); + Values result = isam2.calculateEstimate(); + // result.print("Results:\n"); + + // we add new measurements from this pose + size_t pose_idx = 3; + + // Now update existing smart factors with new observations + for (size_t j = 0; j < points.size(); ++j) { + SmartFactor::shared_ptr smartfactor = smart_factors[j]; + + // add the 4th measurement + Camera camera(poses[pose_idx], K); + Point2 measurement = camera.project(points[j]); + smartfactor->add(measurement, pose_idx); + } + + graph.resize(0); + initialEstimate.clear(); + + // update initial estimate for the new pose + initialEstimate.insert(pose_idx, poses[pose_idx].compose(delta)); + + // this should break the system + isam2.update(graph, initialEstimate); + result = isam2.calculateEstimate(); + EXPECT(std::find(result.keys().begin(), result.keys().end(), pose_idx) != + result.keys().end()); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From 4c8e4973bf00d6d01379ab0b01bc481a653ea86a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 8 Jan 2024 08:52:47 -0500 Subject: [PATCH 02/21] update Rot2::fromCosSin docstring --- gtsam/geometry/Rot2.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 63b914434..6bb97ff6c 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -68,7 +68,7 @@ namespace gtsam { return fromAngle(theta * degree); } - /// Named constructor from cos(theta),sin(theta) pair, will *not* normalize! + /// Named constructor from cos(theta),sin(theta) pair static Rot2 fromCosSin(double c, double s); /** From 6dfd5671b1303785f28717388f76e3578fee45df Mon Sep 17 00:00:00 2001 From: Gary Date: Wed, 7 Aug 2024 15:46:13 -0400 Subject: [PATCH 03/21] Adding test and fix for issue #1596 A Non-active constraint returns a `nullptr`, which needs to be checked for when gathering the keys in `EliminateSymbolic`. --- gtsam/symbolic/SymbolicFactor-inst.h | 4 ++- tests/testGaussianISAM2.cpp | 50 ++++++++++++++++++++++++++++ 2 files changed, 53 insertions(+), 1 deletion(-) diff --git a/gtsam/symbolic/SymbolicFactor-inst.h b/gtsam/symbolic/SymbolicFactor-inst.h index db0eb05ca..a5410aad5 100644 --- a/gtsam/symbolic/SymbolicFactor-inst.h +++ b/gtsam/symbolic/SymbolicFactor-inst.h @@ -42,7 +42,9 @@ namespace gtsam // Gather all keys KeySet allKeys; for(const std::shared_ptr& factor: factors) { - allKeys.insert(factor->begin(), factor->end()); + // Non-active factors are nullptr + if (factor) + allKeys.insert(factor->begin(), factor->end()); } // Check keys diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 1ea60dac9..721ed51c0 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -994,6 +994,56 @@ TEST(ISAM2, calculate_nnz) EXPECT_LONGS_EQUAL(expected, actual); } +class FixActiveFactor : public NoiseModelFactorN { + using Base = NoiseModelFactorN; + bool is_active_; + +public: + FixActiveFactor(const gtsam::Key& key, const bool active) + : Base(nullptr, key), is_active_(active) {} + + virtual bool active(const gtsam::Values &values) const override { + return is_active_; + } + + virtual Vector + evaluateError(const Vector2& x, + Base::OptionalMatrixTypeT H = nullptr) const override { + if (H) { + *H = Vector2::Identity(); + } + return Vector2::Zero(); + } +}; + +TEST(ActiveFactorTesting, Issue1596) { + // Issue1596: When a derived Nonlinear Factor is not active, the linearization returns a nullptr (NonlinearFactor.cpp:156), which + // causes an error when `EliminateSymbolic` is called (SymbolicFactor-inst.h:45) due to not checking if the factor is nullptr. + const gtsam::Key key{Symbol('x', 0)}; + + ISAM2 isam; + Values values; + NonlinearFactorGraph graph; + + // Insert an active factor + values.insert(key, Vector2::Zero()); + graph.emplace_shared(key, true); + + // No problem here + isam.update(graph, values); + + graph = NonlinearFactorGraph(); + + // Inserting a factor that is never active + graph.emplace_shared(key, false); + + // This call throws the error if the pointer is not validated on (SymbolicFactor-inst.h:45) + isam.update(graph); + + // If the bug is fixed, this line is reached. + EXPECT(isam.getFactorsUnsafe().size() == 2); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ From 7765314638432c267eaf1631c61786c3b55f79d6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 24 Aug 2024 05:23:52 -0400 Subject: [PATCH 04/21] fix return type of EliminateQR --- gtsam/linear/linear.i | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index 3a629f349..ed19eaa3b 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -375,7 +375,8 @@ virtual class JacobianFactor : gtsam::GaussianFactor { void serialize() const; }; -pair EliminateQR(const gtsam::GaussianFactorGraph& factors, const gtsam::Ordering& keys); +pair EliminateQR( + const gtsam::GaussianFactorGraph& factors, const gtsam::Ordering& keys); #include virtual class HessianFactor : gtsam::GaussianFactor { From bf70087fff9d5454a311039375b3c8512d2d47d7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Aug 2024 04:35:02 -0400 Subject: [PATCH 05/21] Make default PartialPriorFactor constructor public --- gtsam_unstable/slam/PartialPriorFactor.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index b70fe8b19..7722e5d82 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -50,9 +50,6 @@ namespace gtsam { Vector prior_; ///< Measurement on tangent space parameters, in compressed form. std::vector indices_; ///< Indices of the measured tangent space parameters. - /** default constructor - only use for serialization */ - PartialPriorFactor() {} - /** * constructor with just minimum requirements for a factor - allows more * computation in the constructor. This should only be used by subclasses @@ -65,7 +62,8 @@ namespace gtsam { // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; - ~PartialPriorFactor() override {} + /** default constructor - only use for serialization */ + PartialPriorFactor() {} /** Single Element Constructor: Prior on a single parameter at index 'idx' in the tangent vector.*/ PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel& model) : @@ -85,6 +83,8 @@ namespace gtsam { assert(model->dim() == (size_t)prior.size()); } + ~PartialPriorFactor() override {} + /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { return std::static_pointer_cast( From 2e39fef721f8e1dd9a824d6c0e3fed55080e7bd8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Aug 2024 04:35:16 -0400 Subject: [PATCH 06/21] wrap PartialPriorFactor --- gtsam_unstable/gtsam_unstable.i | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/gtsam_unstable/gtsam_unstable.i b/gtsam_unstable/gtsam_unstable.i index 8566934dd..3beb845fb 100644 --- a/gtsam_unstable/gtsam_unstable.i +++ b/gtsam_unstable/gtsam_unstable.i @@ -671,6 +671,21 @@ class AHRS { //void print(string s) const; }; +#include +template +virtual class PartialPriorFactor : gtsam::NoiseModelFactor { + PartialPriorFactor(gtsam::Key key, size_t idx, double prior, + const gtsam::noiseModel::Base* model); + PartialPriorFactor(gtsam::Key key, const std::vector& indices, + const gtsam::Vector& prior, + const gtsam::noiseModel::Base* model); + + // enabling serialization functionality + void serialize() const; + + const gtsam::Vector& prior(); +}; + // Tectonic SAM Factors #include From 95da15a61abac9b6bf66e25e0193841964d86870 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Aug 2024 04:35:33 -0400 Subject: [PATCH 07/21] allow stl binding for pybind11 in gtsam_unstable --- python/CMakeLists.txt | 2 ++ python/gtsam_unstable/gtsam_unstable.tpl | 1 + 2 files changed, 3 insertions(+) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index ba55ac2af..0c9dd6967 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -189,6 +189,8 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) gtsam::BinaryMeasurementsPoint3 gtsam::BinaryMeasurementsUnit3 gtsam::BinaryMeasurementsRot3 + gtsam::SimWall2DVector + gtsam::SimPolygon2DVector gtsam::CameraSetCal3_S2 gtsam::CameraSetCal3Bundler gtsam::CameraSetCal3Unified diff --git a/python/gtsam_unstable/gtsam_unstable.tpl b/python/gtsam_unstable/gtsam_unstable.tpl index b98872c46..006ca7fc8 100644 --- a/python/gtsam_unstable/gtsam_unstable.tpl +++ b/python/gtsam_unstable/gtsam_unstable.tpl @@ -9,6 +9,7 @@ #include #include +#include #include #include #include From b94ab31e1f058d3fac9596922a6bda01fb1423a5 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Aug 2024 04:36:00 -0400 Subject: [PATCH 08/21] localization example script which gives exact same result as C++ version --- .../examples/LocalizationExample.py | 68 +++++++++++++++++++ 1 file changed, 68 insertions(+) create mode 100644 python/gtsam_unstable/examples/LocalizationExample.py diff --git a/python/gtsam_unstable/examples/LocalizationExample.py b/python/gtsam_unstable/examples/LocalizationExample.py new file mode 100644 index 000000000..ad8266d6d --- /dev/null +++ b/python/gtsam_unstable/examples/LocalizationExample.py @@ -0,0 +1,68 @@ +""" +A simple 2D pose slam example with "GPS" measurements + - The robot moves forward 2 meter each iteration + - The robot initially faces along the X axis (horizontal, to the right in 2D) + - We have full odometry between pose + - We have "GPS-like" measurements implemented with a custom factor +""" +import numpy as np + +import gtsam +from gtsam import BetweenFactorPose2, Pose2, noiseModel +from gtsam_unstable import PartialPriorFactorPose2 + + +def main(): + # 1. Create a factor graph container and add factors to it. + graph = gtsam.NonlinearFactorGraph() + + # 2a. Add odometry factors + # For simplicity, we will use the same noise model for each odometry factor + odometryNoise = noiseModel.Diagonal.Sigmas(np.asarray([0.2, 0.2, 0.1])) + + # Create odometry (Between) factors between consecutive poses + graph.push_back( + BetweenFactorPose2(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise)) + graph.push_back( + BetweenFactorPose2(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise)) + + # 2b. Add "GPS-like" measurements + # We will use PartialPrior factor for this. + unaryNoise = noiseModel.Diagonal.Sigmas(np.array([0.1, + 0.1])) # 10cm std on x,y + + graph.push_back( + PartialPriorFactorPose2(1, [0, 1], np.asarray([0.0, 0.0]), unaryNoise)) + graph.push_back( + PartialPriorFactorPose2(2, [0, 1], np.asarray([2.0, 0.0]), unaryNoise)) + graph.push_back( + PartialPriorFactorPose2(3, [0, 1], np.asarray([4.0, 0.0]), unaryNoise)) + graph.print("\nFactor Graph:\n") + + # 3. Create the data structure to hold the initialEstimate estimate to the solution + # For illustrative purposes, these have been deliberately set to incorrect values + initialEstimate = gtsam.Values() + initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)) + initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2)) + initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1)) + initialEstimate.print("\nInitial Estimate:\n") + + # 4. Optimize using Levenberg-Marquardt optimization. The optimizer + # accepts an optional set of configuration parameters, controlling + # things like convergence criteria, the type of linear system solver + # to use, and the amount of information displayed during optimization. + # Here we will use the default set of parameters. See the + # documentation for the full set of parameters. + optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) + result = optimizer.optimize() + result.print("Final Result:\n") + + # 5. Calculate and print marginal covariances for all variables + marginals = gtsam.Marginals(graph, result) + print("x1 covariance:\n", marginals.marginalCovariance(1)) + print("x2 covariance:\n", marginals.marginalCovariance(2)) + print("x3 covariance:\n", marginals.marginalCovariance(3)) + + +if __name__ == "__main__": + main() From c57d5a0f7052b9a3b4c34b95b1d795bebe312632 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Aug 2024 07:23:31 -0400 Subject: [PATCH 09/21] Updated porting 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 2a2c9f2ef..70f6cfcb0 100644 --- a/python/gtsam/examples/README.md +++ b/python/gtsam/examples/README.md @@ -17,7 +17,7 @@ | InverseKinematicsExampleExpressions.cpp | | | ISAM2Example_SmartFactor | | | ISAM2_SmartFactorStereo_IMU | | -| LocalizationExample | impossible? | +| LocalizationExample | :heavy_check_mark: | | METISOrderingExample | | | OdometryExample | :heavy_check_mark: | | PlanarSLAMExample | :heavy_check_mark: | From 353ff644fcdf51ac3c9e94f99b9e2574dd1fe686 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 24 Jul 2022 12:40:11 -0400 Subject: [PATCH 10/21] generate python type hints during pip build --- python/setup.py.in | 1 + 1 file changed, 1 insertion(+) diff --git a/python/setup.py.in b/python/setup.py.in index 824a6656e..b9d7392c7 100644 --- a/python/setup.py.in +++ b/python/setup.py.in @@ -13,6 +13,7 @@ package_data = { "./*.so", "./*.dll", "./*.pyd", + "*.pyi", "**/*.pyi", # Add the type hints ] } From 51aadbda3411411597d7f0d53bede212e898dc08 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 24 Jul 2022 22:25:56 -0400 Subject: [PATCH 11/21] add type hints and make command to test gtsam_unstable for python --- python/CMakeLists.txt | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index ba55ac2af..2068ee8dc 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -266,7 +266,9 @@ endif() # Add custom target so we can install with `make python-install` set(GTSAM_PYTHON_INSTALL_TARGET python-install) add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET} - COMMAND ${PYTHON_EXECUTABLE} -m pip install --user . + COMMAND stubgen -q -p gtsam && + rsync -a "out/gtsam/" gtsam && + ${PYTHON_EXECUTABLE} -m pip install --user . DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}) @@ -279,3 +281,12 @@ add_custom_target( ${PYTHON_EXECUTABLE} -m unittest discover -v -s . DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} ${GTSAM_PYTHON_TEST_FILES} WORKING_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam/tests") + +add_custom_target( + python-test-unstable + COMMAND + ${CMAKE_COMMAND} -E env # add package to python path so no need to install + "PYTHONPATH=${GTSAM_PYTHON_BUILD_DIRECTORY}/$ENV{PYTHONPATH}" + ${PYTHON_EXECUTABLE} -m unittest discover -v -s . + DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} ${GTSAM_PYTHON_TEST_FILES} + WORKING_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable/tests") From 48215b9063045921bba4268db36e3ac8085687e3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Aug 2024 14:03:43 -0400 Subject: [PATCH 12/21] remove duplicate target --- python/CMakeLists.txt | 9 --------- 1 file changed, 9 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 2068ee8dc..20f2eab4b 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -281,12 +281,3 @@ add_custom_target( ${PYTHON_EXECUTABLE} -m unittest discover -v -s . DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} ${GTSAM_PYTHON_TEST_FILES} WORKING_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam/tests") - -add_custom_target( - python-test-unstable - COMMAND - ${CMAKE_COMMAND} -E env # add package to python path so no need to install - "PYTHONPATH=${GTSAM_PYTHON_BUILD_DIRECTORY}/$ENV{PYTHONPATH}" - ${PYTHON_EXECUTABLE} -m unittest discover -v -s . - DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} ${GTSAM_PYTHON_TEST_FILES} - WORKING_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable/tests") From f2d69ed697b047e8fa5c89cc4613ffcee89c3c88 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Aug 2024 14:47:14 -0400 Subject: [PATCH 13/21] always use bundled pybind11 inside wrap --- python/CMakeLists.txt | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index ba55ac2af..12d8a9b4d 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -29,11 +29,8 @@ if(POLICY CMP0057) cmake_policy(SET CMP0057 NEW) endif() -# Prefer system pybind11 first, if not found, rely on bundled version: -find_package(pybind11 CONFIG QUIET) -if (NOT pybind11_FOUND) - add_subdirectory(${PROJECT_SOURCE_DIR}/wrap/pybind11 pybind11) -endif() +# Use bundled pybind11 version +find_package(pybind11 CONFIG QUIET PATHS "${PROJECT_SOURCE_DIR}/wrap/pybind11") # Set the wrapping script variable set(PYBIND_WRAP_SCRIPT "${PROJECT_SOURCE_DIR}/wrap/scripts/pybind_wrap.py") From e58a5c4cac810db86c3f1a960e61db81446126b4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Aug 2024 15:41:55 -0400 Subject: [PATCH 14/21] directly add pybind11 subdirectory so files are generated correctly --- python/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 12d8a9b4d..b8c2c718f 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -30,7 +30,7 @@ if(POLICY CMP0057) endif() # Use bundled pybind11 version -find_package(pybind11 CONFIG QUIET PATHS "${PROJECT_SOURCE_DIR}/wrap/pybind11") +add_subdirectory(${PROJECT_SOURCE_DIR}/wrap/pybind11 pybind11) # Set the wrapping script variable set(PYBIND_WRAP_SCRIPT "${PROJECT_SOURCE_DIR}/wrap/scripts/pybind_wrap.py") From bde9285211b1882865a59f870eb3a8f1bb6d2395 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Aug 2024 16:42:23 -0400 Subject: [PATCH 15/21] better test documentation --- tests/testDoglegOptimizer.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index d7de63d3d..6fbc522d3 100644 --- a/tests/testDoglegOptimizer.cpp +++ b/tests/testDoglegOptimizer.cpp @@ -184,6 +184,14 @@ TEST(DoglegOptimizer, Constraint) { } /* ************************************************************************* */ +/** + * Test created to fix issue in ISAM2 when using the DogLegOptimizer. + * Originally reported by kvmanohar22 in issue #301 + * https://github.com/borglab/gtsam/issues/301 + * + * This test is based on a script provided by kvmanohar22 + * to help reproduce the issue. + */ TEST(DogLegOptimizer, VariableUpdate) { // Make the typename short so it looks much cleaner typedef SmartProjectionPoseFactor SmartFactor; From 5f43b69c4371ca8d784378d11256f25412874a81 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Aug 2024 16:45:23 -0400 Subject: [PATCH 16/21] add mypy to the dev_requirements so we can get stubgen --- python/dev_requirements.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/python/dev_requirements.txt b/python/dev_requirements.txt index 6970ee613..c76d3bf1b 100644 --- a/python/dev_requirements.txt +++ b/python/dev_requirements.txt @@ -1,2 +1,3 @@ -r requirements.txt -pyparsing>=2.4.2 \ No newline at end of file +pyparsing>=2.4.2 +mypy>=1.11.2 \ No newline at end of file From 0e73367345f50f9c3a0a7f6e3beb7be124d99f23 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Aug 2024 18:57:03 -0400 Subject: [PATCH 17/21] stubgen only for Windows until we can find the rsync equivalent --- python/CMakeLists.txt | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 20f2eab4b..b32d51c68 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -265,12 +265,18 @@ endif() # Add custom target so we can install with `make python-install` set(GTSAM_PYTHON_INSTALL_TARGET python-install) -add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET} - COMMAND stubgen -q -p gtsam && - rsync -a "out/gtsam/" gtsam && - ${PYTHON_EXECUTABLE} -m pip install --user . +if (NOT WIN32) + add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET} + COMMAND stubgen -q -p gtsam && rsync -a \"out/gtsam/\" gtsam && ${PYTHON_EXECUTABLE} -m pip install --user . DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}) +elseif() + #TODO(Varun) Find rsync equivalent on Windows + add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET} + COMMAND ${PYTHON_EXECUTABLE} -m pip install --user . + DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} + WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}) +endif() # Custom make command to run all GTSAM Python tests add_custom_target( From bfcd5eb08a9feea9f84b079878396afae63e5b54 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 26 Aug 2024 05:48:18 -0400 Subject: [PATCH 18/21] use older version of mypy --- python/dev_requirements.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/dev_requirements.txt b/python/dev_requirements.txt index c76d3bf1b..6a5e7f924 100644 --- a/python/dev_requirements.txt +++ b/python/dev_requirements.txt @@ -1,3 +1,3 @@ -r requirements.txt pyparsing>=2.4.2 -mypy>=1.11.2 \ No newline at end of file +mypy==1.4.1 #TODO(Varun) A bug in mypy>=1.5.0 causes an unresolved placeholder error when importing numpy>=2.0.0 (https://github.com/python/mypy/issues/17396) \ No newline at end of file From da99cf0e192ca4033d34575714b7591f343258ff Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 26 Aug 2024 06:25:17 -0400 Subject: [PATCH 19/21] remove extraneous variable --- python/CMakeLists.txt | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index b32d51c68..677ee72ce 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -264,15 +264,14 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) endif() # Add custom target so we can install with `make python-install` -set(GTSAM_PYTHON_INSTALL_TARGET python-install) if (NOT WIN32) - add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET} + add_custom_target(python-install COMMAND stubgen -q -p gtsam && rsync -a \"out/gtsam/\" gtsam && ${PYTHON_EXECUTABLE} -m pip install --user . DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}) elseif() #TODO(Varun) Find rsync equivalent on Windows - add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET} + add_custom_target(python-install COMMAND ${PYTHON_EXECUTABLE} -m pip install --user . DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}) From 48f9d0b11664b5e69e4f8cd925851e3206ecb5af Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 26 Aug 2024 09:44:28 -0400 Subject: [PATCH 20/21] fix if-else mistype --- python/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 677ee72ce..1e4acf142 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -264,12 +264,12 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) endif() # Add custom target so we can install with `make python-install` -if (NOT WIN32) +if (NOT WIN32) # WIN32=1 is target platform is Windows add_custom_target(python-install COMMAND stubgen -q -p gtsam && rsync -a \"out/gtsam/\" gtsam && ${PYTHON_EXECUTABLE} -m pip install --user . DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}) -elseif() +else() #TODO(Varun) Find rsync equivalent on Windows add_custom_target(python-install COMMAND ${PYTHON_EXECUTABLE} -m pip install --user . From 1744c4aeb3b8875b8c17651905eebb4c7687d4ef Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 26 Aug 2024 10:07:24 -0400 Subject: [PATCH 21/21] save stubs to 'stubs' directory and use cp instead of rsync --- python/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 1e4acf142..bdab178ba 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -266,11 +266,11 @@ endif() # Add custom target so we can install with `make python-install` if (NOT WIN32) # WIN32=1 is target platform is Windows add_custom_target(python-install - COMMAND stubgen -q -p gtsam && rsync -a \"out/gtsam/\" gtsam && ${PYTHON_EXECUTABLE} -m pip install --user . + COMMAND stubgen -q -p gtsam -o ./stubs && cp -a stubs/gtsam/ gtsam && ${PYTHON_EXECUTABLE} -m pip install --user . DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}) else() - #TODO(Varun) Find rsync equivalent on Windows + #TODO(Varun) Find equivalent cp on Windows add_custom_target(python-install COMMAND ${PYTHON_EXECUTABLE} -m pip install --user . DEPENDS ${GTSAM_PYTHON_DEPENDENCIES}