From 865288a4dd1b5fd8b9255f044c0f8d2195341f56 Mon Sep 17 00:00:00 2001 From: Michal Nowicki Date: Mon, 18 Mar 2024 08:47:11 +0100 Subject: [PATCH 01/15] Fixing strength of bias constraints in IMUKittiExampleGPS.cpp --- examples/IMUKittiExampleGPS.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/IMUKittiExampleGPS.cpp b/examples/IMUKittiExampleGPS.cpp index cb60b2516..66ebba8b2 100644 --- a/examples/IMUKittiExampleGPS.cpp +++ b/examples/IMUKittiExampleGPS.cpp @@ -241,7 +241,6 @@ int main(int argc, char* argv[]) { "-- Starting main loop: inference is performed at each time step, but we " "plot trajectory every 10 steps\n"); size_t j = 0; - size_t included_imu_measurement_count = 0; for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) { // At each non=IMU measurement we initialize a new node in the graph @@ -249,6 +248,7 @@ int main(int argc, char* argv[]) { auto current_vel_key = V(i); auto current_bias_key = B(i); double t = gps_measurements[i].time; + size_t included_imu_measurement_count = 0; if (i == first_gps_pose) { // Create initial estimate and prior on initial pose, velocity, and biases From bbdf4e1ef3edd976a5c34fdb05314c033c3a9561 Mon Sep 17 00:00:00 2001 From: Nikhil Khedekar Date: Fri, 22 Mar 2024 17:50:06 +0100 Subject: [PATCH 02/15] removed self include --- gtsam/navigation/PreintegrationCombinedParams.h | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/navigation/PreintegrationCombinedParams.h b/gtsam/navigation/PreintegrationCombinedParams.h index 7d775d9e4..6be05c082 100644 --- a/gtsam/navigation/PreintegrationCombinedParams.h +++ b/gtsam/navigation/PreintegrationCombinedParams.h @@ -25,7 +25,6 @@ /* GTSAM includes */ #include #include -#include #include #include From afd3330acb932b78ad5b2d55238d1f7418d804e0 Mon Sep 17 00:00:00 2001 From: Adam Rutkowski Date: Mon, 25 Mar 2024 16:05:47 -0500 Subject: [PATCH 03/15] added VectorValues::sorted() --- gtsam/linear/VectorValues.cpp | 22 ++++++++++++++-------- gtsam/linear/VectorValues.h | 3 +++ 2 files changed, 17 insertions(+), 8 deletions(-) diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 075e3b9ec..7440c0b2b 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -64,6 +64,13 @@ namespace gtsam { } } + /* ************************************************************************ */ + std::map VectorValues::sorted() const { + std::map ordered; + for (const auto& kv : *this) ordered.emplace(kv); + return ordered; + } + /* ************************************************************************ */ VectorValues VectorValues::Zero(const VectorValues& other) { @@ -130,11 +137,7 @@ namespace gtsam { GTSAM_EXPORT std::ostream& operator<<(std::ostream& os, const VectorValues& v) { // Change print depending on whether we are using TBB #ifdef GTSAM_USE_TBB - std::map sorted; - for (const auto& [key, value] : v) { - sorted.emplace(key, value); - } - for (const auto& [key, value] : sorted) + for (const auto& [key, value] : v.sorted()) #else for (const auto& [key,value] : v) #endif @@ -176,7 +179,12 @@ namespace gtsam { // Copy vectors Vector result(totalDim); DenseIndex pos = 0; +#ifdef GTSAM_USE_TBB + // TBB uses un-ordered map, so inefficiently order them: + for (const auto& [key, value] : sorted()) { +#else for (const auto& [key, value] : *this) { +#endif result.segment(pos, value.size()) = value; pos += value.size(); } @@ -392,9 +400,7 @@ namespace gtsam { // Print out all rows. #ifdef GTSAM_USE_TBB // TBB uses un-ordered map, so inefficiently order them: - std::map ordered; - for (const auto& kv : *this) ordered.emplace(kv); - for (const auto& kv : ordered) { + for (const auto& kv : sorted()) { #else for (const auto& kv : *this) { #endif diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 478cfd770..7fbd43ffc 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -77,6 +77,9 @@ namespace gtsam { typedef ConcurrentMap Values; ///< Collection of Vectors making up a VectorValues Values values_; ///< Vectors making up this VectorValues + /** Sort by key (primarily for use with TBB, which uses an unordered map)*/ + std::map sorted() const; + public: typedef Values::iterator iterator; ///< Iterator over vector values typedef Values::const_iterator const_iterator; ///< Const iterator over vector values From 1a591ace59541ea1a24b4b356d689afef5d465fc Mon Sep 17 00:00:00 2001 From: ShuangLiu1992 Date: Sat, 29 Apr 2023 22:10:36 +0100 Subject: [PATCH 04/15] Fix JacobianFactor and HessianFactor on platforms where Eigen::Index is not the same size as gtsam::Key Fix JacobianFactor and HessianFactor on platforms where Eigen::Index is not the same size as gtsam::Key Eigen::Index is defined as std::ptrdiff_t (size_t), which is not always the same size as gtsam::Key (uint64) --- gtsam/linear/JacobianFactor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 579f6cbc2..3cfb5ce7b 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -40,8 +40,8 @@ using namespace std; namespace gtsam { // Typedefs used in constructors below. -using Dims = std::vector; -using Pairs = std::vector>; +using Dims = std::vector; +using Pairs = std::vector>; /* ************************************************************************* */ JacobianFactor::JacobianFactor() : From 84a44eeca14c3a1a4d2d79516845ced603dd77ac Mon Sep 17 00:00:00 2001 From: ShuangLiu1992 Date: Sat, 29 Apr 2023 22:12:13 +0100 Subject: [PATCH 05/15] Update HessianFactor.cpp Fix JacobianFactor and HessianFactor on platforms where Eigen::Index is not the same size as gtsam::Key Eigen::Index is defined as std::ptrdiff_t (size_t), which is not always the same size as gtsam::Key (uint64) --- gtsam/linear/HessianFactor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 376509069..8980b5030 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -38,7 +38,7 @@ using namespace std; namespace gtsam { // Typedefs used in constructors below. -using Dims = std::vector; +using Dims = std::vector; /* ************************************************************************* */ void HessianFactor::Allocate(const Scatter& scatter) { From ac5934822e64f0b9ab4df959c39f3a20767fe564 Mon Sep 17 00:00:00 2001 From: ShuangLiu1992 Date: Sat, 29 Apr 2023 22:24:28 +0100 Subject: [PATCH 06/15] Update HessianFactor.cpp add static_cast to make compiler happy --- gtsam/linear/HessianFactor.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 8980b5030..3c3050f90 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -72,7 +72,7 @@ HessianFactor::HessianFactor() : /* ************************************************************************* */ HessianFactor::HessianFactor(Key j, const Matrix& G, const Vector& g, double f) - : GaussianFactor(KeyVector{j}), info_(Dims{G.cols(), 1}) { + : GaussianFactor(KeyVector{j}), info_(Dims{static_cast(G.cols()), 1}) { if (G.rows() != G.cols() || G.rows() != g.size()) throw invalid_argument( "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions"); @@ -85,7 +85,7 @@ HessianFactor::HessianFactor(Key j, const Matrix& G, const Vector& g, double f) // error is 0.5*(x-mu)'*inv(Sigma)*(x-mu) = 0.5*(x'*G*x - 2*x'*G*mu + mu'*G*mu) // where G = inv(Sigma), g = G*mu, f = mu'*G*mu = mu'*g HessianFactor::HessianFactor(Key j, const Vector& mu, const Matrix& Sigma) - : GaussianFactor(KeyVector{j}), info_(Dims{Sigma.cols(), 1}) { + : GaussianFactor(KeyVector{j}), info_(Dims{static_cast(Sigma.cols()), 1}) { if (Sigma.rows() != Sigma.cols() || Sigma.rows() != mu.size()) throw invalid_argument( "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions"); @@ -99,7 +99,7 @@ HessianFactor::HessianFactor(Key j1, Key j2, const Matrix& G11, const Matrix& G12, const Vector& g1, const Matrix& G22, const Vector& g2, double f) : GaussianFactor(KeyVector{j1,j2}), info_( - Dims{G11.cols(),G22.cols(),1}) { + Dims{static_cast(G11.cols()),static_cast(G22.cols()),1}) { info_.setDiagonalBlock(0, G11); info_.setOffDiagonalBlock(0, 1, G12); info_.setDiagonalBlock(1, G22); @@ -113,7 +113,7 @@ HessianFactor::HessianFactor(Key j1, Key j2, Key j3, const Matrix& G11, const Matrix& G23, const Vector& g2, const Matrix& G33, const Vector& g3, double f) : GaussianFactor(KeyVector{j1,j2,j3}), info_( - Dims{G11.cols(),G22.cols(),G33.cols(),1}) { + Dims{static_cast(G11.cols()),static_cast(G22.cols()),static_cast(G33.cols()),1}) { if (G11.rows() != G11.cols() || G11.rows() != G12.rows() || G11.rows() != G13.rows() || G11.rows() != g1.size() || G22.cols() != G12.cols() || G33.cols() != G13.cols() From 0fee5cb76e7a04b590ff0dc1051950da5b265340 Mon Sep 17 00:00:00 2001 From: zzodo Date: Thu, 25 Apr 2024 14:16:36 +0900 Subject: [PATCH 07/15] Add missing header in 3rdparty/cephes --- gtsam/3rdparty/cephes/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/3rdparty/cephes/CMakeLists.txt b/gtsam/3rdparty/cephes/CMakeLists.txt index 5940d39d2..190b73db9 100644 --- a/gtsam/3rdparty/cephes/CMakeLists.txt +++ b/gtsam/3rdparty/cephes/CMakeLists.txt @@ -16,7 +16,8 @@ set(CEPHES_HEADER_FILES cephes/lanczos.h cephes/mconf.h cephes/polevl.h - cephes/sf_error.h) + cephes/sf_error.h + dllexport.h) # Add header files install(FILES ${CEPHES_HEADER_FILES} DESTINATION include/gtsam/3rdparty/cephes) From 42b27e4484e9deea43a87b648d40b1b1db2d86e4 Mon Sep 17 00:00:00 2001 From: Vincent Richard Date: Mon, 13 May 2024 09:16:53 +0900 Subject: [PATCH 08/15] fix typo --- gtsam/navigation/PreintegrationBase.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index f573c4010..7f998987b 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -129,7 +129,7 @@ NavState PreintegrationBase::predict(const NavState& state_i, Matrix9 D_predict_state, D_predict_delta; NavState state_j = state_i.retract(xi, H1 ? &D_predict_state : nullptr, - H2 || H2 ? &D_predict_delta : nullptr); + H1 || H2 ? &D_predict_delta : nullptr); if (H1) *H1 = D_predict_state + D_predict_delta * D_delta_state; if (H2) From e116123ea58323b893d1a93041ec0dae15e6b892 Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Mon, 27 May 2024 13:31:43 -0400 Subject: [PATCH 09/15] Add a few functions to python wrapper --- gtsam/geometry/geometry.i | 10 ++++++++++ gtsam/navigation/navigation.i | 2 +- gtsam/nonlinear/nonlinear.i | 3 +++ gtsam/slam/slam.i | 3 ++- 4 files changed, 16 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 01161817b..3cf78989c 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -69,6 +69,7 @@ class StereoPoint2 { // Standard Constructors StereoPoint2(); StereoPoint2(double uL, double uR, double v); + StereoPoint2(const gtsam::Vector3 &v); // Testable void print(string s = "") const; @@ -836,6 +837,12 @@ class Cal3_S2Stereo { Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b); Cal3_S2Stereo(Vector v); + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Cal3_S2Stereo retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3_S2Stereo& c) const; + // Testable void print(string s = "") const; bool equals(const gtsam::Cal3_S2Stereo& K, double tol) const; @@ -846,8 +853,11 @@ class Cal3_S2Stereo { double skew() const; double px() const; double py() const; + Matrix K() const; gtsam::Point2 principalPoint() const; double baseline() const; + Vector6 vector() const; + Matrix inverse() const; }; #include diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 92864c18a..aad6f9851 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -200,7 +200,7 @@ class PreintegratedCombinedMeasurements { const gtsam::imuBias::ConstantBias& bias) const; }; -virtual class CombinedImuFactor: gtsam::NonlinearFactor { +virtual class CombinedImuFactor: gtsam::NoiseModelFactor { CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j, const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements); diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 3f5fb1dd5..c39cfd22a 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -130,6 +130,7 @@ virtual class NonlinearFactor : gtsam::Factor { virtual class NoiseModelFactor : gtsam::NonlinearFactor { bool equals(const gtsam::NoiseModelFactor& other, double tol) const; gtsam::noiseModel::Base* noiseModel() const; + gtsam::NoiseModelFactor* cloneWithNewNoiseModel(gtsam::noiseModel::Base* newNoise) const; Vector unwhitenedError(const gtsam::Values& x) const; Vector whitenedError(const gtsam::Values& x) const; }; @@ -320,6 +321,8 @@ virtual class GncParams { enum Verbosity { SILENT, SUMMARY, + MU, + WEIGHTS, VALUES }; }; diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 64977a2a5..8dc10e51c 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -170,7 +170,8 @@ template virtual class GenericStereoFactor : gtsam::NoiseModelFactor { GenericStereoFactor(const gtsam::StereoPoint2& measured, const gtsam::noiseModel::Base* noiseModel, size_t poseKey, - size_t landmarkKey, const gtsam::Cal3_S2Stereo* K); + size_t landmarkKey, const gtsam::Cal3_S2Stereo* K, + POSE body_P_sensor); gtsam::StereoPoint2 measured() const; gtsam::Cal3_S2Stereo* calibration() const; From d7e2e1cdf6e74cbf4e92b199713e797f084c8a2e Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Mon, 27 May 2024 15:58:05 -0400 Subject: [PATCH 10/15] Expanded stereofactor python constructor --- gtsam/slam/StereoFactor.h | 5 +++++ gtsam/slam/slam.i | 13 +++++++++++++ 2 files changed, 18 insertions(+) diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index a2d428d11..5adafcf3a 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -169,6 +169,11 @@ public: /** return flag for throwing cheirality exceptions */ inline bool throwCheirality() const { return throwCheirality_; } + /** return the (optional) sensor pose with respect to the vehicle frame */ + const std::optional& body_P_sensor() const { + return body_P_sensor_; + } + private: #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 8dc10e51c..97b198b2f 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -168,10 +168,23 @@ typedef gtsam::SmartProjectionPoseFactor #include template virtual class GenericStereoFactor : gtsam::NoiseModelFactor { + GenericStereoFactor(const gtsam::StereoPoint2& measured, + const gtsam::noiseModel::Base* noiseModel, size_t poseKey, + size_t landmarkKey, const gtsam::Cal3_S2Stereo* K); GenericStereoFactor(const gtsam::StereoPoint2& measured, const gtsam::noiseModel::Base* noiseModel, size_t poseKey, size_t landmarkKey, const gtsam::Cal3_S2Stereo* K, POSE body_P_sensor); + + GenericStereoFactor(const gtsam::StereoPoint2& measured, + const gtsam::noiseModel::Base* noiseModel, size_t poseKey, + size_t landmarkKey, const gtsam::Cal3_S2Stereo* K, + bool throwCheirality, bool verboseCheirality); + GenericStereoFactor(const gtsam::StereoPoint2& measured, + const gtsam::noiseModel::Base* noiseModel, size_t poseKey, + size_t landmarkKey, const gtsam::Cal3_S2Stereo* K, + bool throwCheirality, bool verboseCheirality, + POSE body_P_sensor); gtsam::StereoPoint2 measured() const; gtsam::Cal3_S2Stereo* calibration() const; From 760b799ed8111b4413f39784c932d566290dbb19 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 29 May 2024 15:26:55 -0400 Subject: [PATCH 11/15] 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 d4a045f09..640a1a1a4 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 12/15] 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 a73842a98..766662a36 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 13/15] 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 ce3685f87..4ce603023 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 14/15] 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 640a1a1a4..2bf55bba1 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, From 6419b0d749b08ce0b0a5a806ac6a55942a288d69 Mon Sep 17 00:00:00 2001 From: Tal Regev Date: Thu, 13 Jun 2024 14:20:20 +0300 Subject: [PATCH 15/15] Build all tests for windows --- .github/workflows/build-windows.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 766662a36..17306b66f 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -124,6 +124,7 @@ jobs: # https://stackoverflow.com/a/24470998/1236990 cmake --build build -j4 --config ${{ matrix.build_type }} --target gtsam cmake --build build -j4 --config ${{ matrix.build_type }} --target gtsam_unstable + cmake --build build -j4 --config ${{ matrix.build_type }} --target all.tests - name: Test shell: bash