From 760b799ed8111b4413f39784c932d566290dbb19 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 29 May 2024 15:26:55 -0400 Subject: [PATCH 1/4] Add EliminateQR to Python --- gtsam/linear/linear.i | 36 ++++++++++++++++++++++++++++++------ 1 file changed, 30 insertions(+), 6 deletions(-) diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index d4a045f097..640a1a1a41 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -127,7 +127,7 @@ virtual class Huber: gtsam::noiseModel::mEstimator::Base { }; virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { - Cauchy(double k); + Cauchy(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight); static gtsam::noiseModel::mEstimator::Cauchy* Create(double k); // enabling serialization functionality @@ -138,7 +138,7 @@ virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { }; virtual class Tukey: gtsam::noiseModel::mEstimator::Base { - Tukey(double k); + Tukey(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight); static gtsam::noiseModel::mEstimator::Tukey* Create(double k); // enabling serialization functionality @@ -149,7 +149,7 @@ virtual class Tukey: gtsam::noiseModel::mEstimator::Base { }; virtual class Welsch: gtsam::noiseModel::mEstimator::Base { - Welsch(double k); + Welsch(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight); static gtsam::noiseModel::mEstimator::Welsch* Create(double k); // enabling serialization functionality @@ -160,7 +160,7 @@ virtual class Welsch: gtsam::noiseModel::mEstimator::Base { }; virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { - GemanMcClure(double c); + GemanMcClure(double c, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight); static gtsam::noiseModel::mEstimator::GemanMcClure* Create(double c); // enabling serialization functionality @@ -171,7 +171,7 @@ virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { }; virtual class DCS: gtsam::noiseModel::mEstimator::Base { - DCS(double c); + DCS(double c, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight); static gtsam::noiseModel::mEstimator::DCS* Create(double c); // enabling serialization functionality @@ -182,7 +182,7 @@ virtual class DCS: gtsam::noiseModel::mEstimator::Base { }; virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base { - L2WithDeadZone(double k); + L2WithDeadZone(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight); static gtsam::noiseModel::mEstimator::L2WithDeadZone* Create(double k); // enabling serialization functionality @@ -203,6 +203,28 @@ virtual class AsymmetricTukey: gtsam::noiseModel::mEstimator::Base { double loss(double error) const; }; +virtual class AsymmetricCauchy: gtsam::noiseModel::mEstimator::Base { + AsymmetricCauchy(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight); + static gtsam::noiseModel::mEstimator::AsymmetricCauchy* Create(double k); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class TwoSidedHuberCauchy: gtsam::noiseModel::mEstimator::Base { + TwoSidedHuberCauchy(double k, double k_huber, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight); + static gtsam::noiseModel::mEstimator::TwoSidedHuberCauchy* Create(double k); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + virtual class Custom: gtsam::noiseModel::mEstimator::Base { Custom(gtsam::noiseModel::mEstimator::CustomWeightFunction weight, gtsam::noiseModel::mEstimator::CustomLossFunction loss, @@ -356,6 +378,8 @@ virtual class JacobianFactor : gtsam::GaussianFactor { void serialize() const; }; +pair EliminateQR(const gtsam::GaussianFactorGraph& factors, const gtsam::Ordering& keys); + #include virtual class HessianFactor : gtsam::GaussianFactor { //Constructors From cb30fddfc8aba930bd28c1d47c88effe4432ef55 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 29 May 2024 15:31:54 -0400 Subject: [PATCH 2/4] Try fixing Windows build --- .github/workflows/build-windows.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index a73842a982..766662a361 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -50,7 +50,7 @@ jobs: uses: ilammy/msvc-dev-cmd@v1 with: arch: x${{ matrix.platform }} - toolset: 14.38 + toolset: 14.2 - name: cl version shell: cmd From 02784de742858f1068b174071976b3759c8836b1 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 29 May 2024 19:26:20 -0400 Subject: [PATCH 3/4] Try fixing Windows build --- .github/workflows/build-python.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index ce3685f878..4ce6030233 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -109,7 +109,7 @@ jobs: uses: ilammy/msvc-dev-cmd@v1 with: arch: x${{matrix.platform}} - toolset: 14.38 + toolset: 14.2 - name: cl version (Windows) if: runner.os == 'Windows' From 6f408fe1c0fc5f33daa49740c98945cb9b4747be Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 29 May 2024 19:34:49 -0400 Subject: [PATCH 4/4] Remove local changes --- gtsam/linear/linear.i | 19 ++++++++----------- 1 file changed, 8 insertions(+), 11 deletions(-) diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index 640a1a1a41..2bf55bba10 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -127,6 +127,7 @@ virtual class Huber: gtsam::noiseModel::mEstimator::Base { }; virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { + Cauchy(double k); Cauchy(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight); static gtsam::noiseModel::mEstimator::Cauchy* Create(double k); @@ -138,6 +139,7 @@ virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { }; virtual class Tukey: gtsam::noiseModel::mEstimator::Base { + Tukey(double k); Tukey(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight); static gtsam::noiseModel::mEstimator::Tukey* Create(double k); @@ -149,6 +151,7 @@ virtual class Tukey: gtsam::noiseModel::mEstimator::Base { }; virtual class Welsch: gtsam::noiseModel::mEstimator::Base { + Welsch(double k); Welsch(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight); static gtsam::noiseModel::mEstimator::Welsch* Create(double k); @@ -160,6 +163,7 @@ virtual class Welsch: gtsam::noiseModel::mEstimator::Base { }; virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { + GemanMcClure(double c); GemanMcClure(double c, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight); static gtsam::noiseModel::mEstimator::GemanMcClure* Create(double c); @@ -171,6 +175,7 @@ virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { }; virtual class DCS: gtsam::noiseModel::mEstimator::Base { + DCS(double c); DCS(double c, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight); static gtsam::noiseModel::mEstimator::DCS* Create(double c); @@ -182,6 +187,7 @@ virtual class DCS: gtsam::noiseModel::mEstimator::Base { }; virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base { + L2WithDeadZone(double k); L2WithDeadZone(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight); static gtsam::noiseModel::mEstimator::L2WithDeadZone* Create(double k); @@ -193,6 +199,7 @@ virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base { }; virtual class AsymmetricTukey: gtsam::noiseModel::mEstimator::Base { + AsymmetricTukey(double k); AsymmetricTukey(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight); static gtsam::noiseModel::mEstimator::AsymmetricTukey* Create(double k); @@ -204,6 +211,7 @@ virtual class AsymmetricTukey: gtsam::noiseModel::mEstimator::Base { }; virtual class AsymmetricCauchy: gtsam::noiseModel::mEstimator::Base { + AsymmetricCauchy(double k); AsymmetricCauchy(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight); static gtsam::noiseModel::mEstimator::AsymmetricCauchy* Create(double k); @@ -214,17 +222,6 @@ virtual class AsymmetricCauchy: gtsam::noiseModel::mEstimator::Base { double loss(double error) const; }; -virtual class TwoSidedHuberCauchy: gtsam::noiseModel::mEstimator::Base { - TwoSidedHuberCauchy(double k, double k_huber, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight); - static gtsam::noiseModel::mEstimator::TwoSidedHuberCauchy* Create(double k); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; -}; - virtual class Custom: gtsam::noiseModel::mEstimator::Base { Custom(gtsam::noiseModel::mEstimator::CustomWeightFunction weight, gtsam::noiseModel::mEstimator::CustomLossFunction loss,