From b0404aa109eb463c0764b4edded82fd3ab8f513a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 20 Jun 2023 14:36:45 -0400 Subject: [PATCH 01/29] small improvements --- gtsam_unstable/linear/LPInitSolver.cpp | 4 +--- gtsam_unstable/linear/LPInitSolver.h | 3 ++- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/gtsam_unstable/linear/LPInitSolver.cpp b/gtsam_unstable/linear/LPInitSolver.cpp index 9f12d670e9..3d24f784b3 100644 --- a/gtsam_unstable/linear/LPInitSolver.cpp +++ b/gtsam_unstable/linear/LPInitSolver.cpp @@ -65,9 +65,7 @@ GaussianFactorGraph::shared_ptr LPInitSolver::buildInitOfInitGraph() const { new GaussianFactorGraph(lp_.equalities)); // create factor ||x||^2 and add to the graph - const KeyDimMap& constrainedKeyDim = lp_.constrainedKeyDimMap(); - for (const auto& [key, _] : constrainedKeyDim) { - size_t dim = constrainedKeyDim.at(key); + for (const auto& [key, dim] : lp_.constrainedKeyDimMap()) { initGraph->push_back( JacobianFactor(key, Matrix::Identity(dim, dim), Vector::Zero(dim))); } diff --git a/gtsam_unstable/linear/LPInitSolver.h b/gtsam_unstable/linear/LPInitSolver.h index 7e326117b3..9db2a34f09 100644 --- a/gtsam_unstable/linear/LPInitSolver.h +++ b/gtsam_unstable/linear/LPInitSolver.h @@ -19,6 +19,7 @@ #pragma once +#include #include #include @@ -49,7 +50,7 @@ namespace gtsam { * inequality constraint, we can't conclude that the problem is infeasible. * However, whether it is infeasible or unbounded, we don't have a unique solution anyway. */ -class LPInitSolver { +class GTSAM_UNSTABLE_EXPORT LPInitSolver { private: const LP& lp_; From 254e3128e6d199508215ec71cb8626fdadafc45f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 20 Jun 2023 17:28:01 -0400 Subject: [PATCH 02/29] fix for multiply defined symbol error in LPInitSolver --- gtsam_unstable/linear/LP.h | 20 +++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/gtsam_unstable/linear/LP.h b/gtsam_unstable/linear/LP.h index dbb744d3ef..50065b2dda 100644 --- a/gtsam_unstable/linear/LP.h +++ b/gtsam_unstable/linear/LP.h @@ -81,9 +81,23 @@ struct LP { if (!cachedConstrainedKeyDimMap_.empty()) return cachedConstrainedKeyDimMap_; // Collect key-dim map of all variables in the constraints - cachedConstrainedKeyDimMap_ = collectKeyDim(equalities); - KeyDimMap keysDim2 = collectKeyDim(inequalities); - cachedConstrainedKeyDimMap_.insert(keysDim2.begin(), keysDim2.end()); + //TODO(Varun) seems like the templated function is causing the multiple symbols error on Windows + // cachedConstrainedKeyDimMap_ = collectKeyDim(equalities); + // KeyDimMap keysDim2 = collectKeyDim(inequalities); + // cachedConstrainedKeyDimMap_.insert(keysDim2.begin(), keysDim2.end()); + cachedConstrainedKeyDimMap_.clear(); + for (auto&& factor : equalities) { + if (!factor) continue; + for (Key key : factor->keys()) { + cachedConstrainedKeyDimMap_[key] = factor->getDim(factor->find(key)); + } + } + for (auto&& factor : inequalities) { + if (!factor) continue; + for (Key key : factor->keys()) { + cachedConstrainedKeyDimMap_[key] = factor->getDim(factor->find(key)); + } + } return cachedConstrainedKeyDimMap_; } From 62fe1a937979968d3b8f4349e0a39aa445d13358 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 20 Jun 2023 17:31:49 -0400 Subject: [PATCH 03/29] add todo for error in SmartStereoProjectionFactorPP.h --- .../slam/SmartStereoProjectionFactorPP.h | 44 ++++++++++--------- 1 file changed, 23 insertions(+), 21 deletions(-) diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h index f9db909533..b022ce16f3 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h @@ -201,10 +201,9 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP } /// linearize and return a Hessianfactor that is an approximation of error(p) - std::shared_ptr > createHessianFactor( - const Values& values, const double lambda = 0.0, bool diagonalDamping = - false) const { - + std::shared_ptr> createHessianFactor( + const Values& values, const double lambda = 0.0, + bool diagonalDamping = false) const { // we may have multiple cameras sharing the same extrinsic cals, hence the number // of keys may be smaller than 2 * nrMeasurements (which is the upper bound where we // have a body key and an extrinsic calibration key for each measurement) @@ -212,23 +211,22 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP // Create structures for Hessian Factors KeyVector js; - std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); + std::vector Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); std::vector gs(nrUniqueKeys); if (this->measured_.size() != cameras(values).size()) throw std::runtime_error("SmartStereoProjectionHessianFactor: this->" - "measured_.size() inconsistent with input"); + "measured_.size() inconsistent with input"); // triangulate 3D point at given linearization point triangulateSafe(cameras(values)); - if (!result_) { // failed: return "empty/zero" Hessian - for (Matrix& m : Gs) - m = Matrix::Zero(DimPose, DimPose); - for (Vector& v : gs) - v = Vector::Zero(DimPose); - return std::make_shared < RegularHessianFactor - > (keys_, Gs, gs, 0.0); + // failed: return "empty/zero" Hessian + if (!result_) { + for (Matrix& m : Gs) m = Matrix::Zero(DimPose, DimPose); + for (Vector& v : gs) v = Vector::Zero(DimPose); + return std::make_shared>(keys_, Gs, gs, + 0.0); } // compute Jacobian given triangulated 3D Point @@ -239,12 +237,13 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP // Whiten using noise model noiseModel_->WhitenSystem(E, b); - for (size_t i = 0; i < Fs.size(); i++) + for (size_t i = 0; i < Fs.size(); i++) { Fs[i] = noiseModel_->Whiten(Fs[i]); + } // build augmented Hessian (with last row/column being the information vector) Matrix3 P; - Cameras::ComputePointCovariance <3> (P, E, lambda, diagonalDamping); + Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping); // these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement) KeyVector nonuniqueKeys; @@ -253,12 +252,15 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP nonuniqueKeys.push_back(body_P_cam_keys_.at(i)); } // but we need to get the augumented hessian wrt the unique keys in key_ - SymmetricBlockMatrix augmentedHessianUniqueKeys = - Cameras::SchurComplementAndRearrangeBlocks<3,DimBlock,DimPose>(Fs,E,P,b, - nonuniqueKeys, keys_); - - return std::make_shared < RegularHessianFactor - > (keys_, augmentedHessianUniqueKeys); + //TODO(Varun) SchurComplementAndRearrangeBlocks is causing the multiply defined symbol error + // SymmetricBlockMatrix augmentedHessianUniqueKeys = + // Base::Cameras::template SchurComplementAndRearrangeBlocks<3, DimBlock, + // DimPose>( + // Fs, E, P, b, nonuniqueKeys, keys_); + + // return std::make_shared>( + // keys_, augmentedHessianUniqueKeys); + return nullptr; } /** From fbf155d91e4ccbf0590fe2f693f50c7da6e528a3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 21 Jun 2023 09:55:52 -0400 Subject: [PATCH 04/29] undo changes --- gtsam_unstable/linear/LP.h | 20 +++---------------- .../slam/SmartStereoProjectionFactorPP.h | 16 +++++++-------- 2 files changed, 10 insertions(+), 26 deletions(-) diff --git a/gtsam_unstable/linear/LP.h b/gtsam_unstable/linear/LP.h index 50065b2dda..dbb744d3ef 100644 --- a/gtsam_unstable/linear/LP.h +++ b/gtsam_unstable/linear/LP.h @@ -81,23 +81,9 @@ struct LP { if (!cachedConstrainedKeyDimMap_.empty()) return cachedConstrainedKeyDimMap_; // Collect key-dim map of all variables in the constraints - //TODO(Varun) seems like the templated function is causing the multiple symbols error on Windows - // cachedConstrainedKeyDimMap_ = collectKeyDim(equalities); - // KeyDimMap keysDim2 = collectKeyDim(inequalities); - // cachedConstrainedKeyDimMap_.insert(keysDim2.begin(), keysDim2.end()); - cachedConstrainedKeyDimMap_.clear(); - for (auto&& factor : equalities) { - if (!factor) continue; - for (Key key : factor->keys()) { - cachedConstrainedKeyDimMap_[key] = factor->getDim(factor->find(key)); - } - } - for (auto&& factor : inequalities) { - if (!factor) continue; - for (Key key : factor->keys()) { - cachedConstrainedKeyDimMap_[key] = factor->getDim(factor->find(key)); - } - } + cachedConstrainedKeyDimMap_ = collectKeyDim(equalities); + KeyDimMap keysDim2 = collectKeyDim(inequalities); + cachedConstrainedKeyDimMap_.insert(keysDim2.begin(), keysDim2.end()); return cachedConstrainedKeyDimMap_; } diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h index b022ce16f3..189c501bb3 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h @@ -252,15 +252,13 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP nonuniqueKeys.push_back(body_P_cam_keys_.at(i)); } // but we need to get the augumented hessian wrt the unique keys in key_ - //TODO(Varun) SchurComplementAndRearrangeBlocks is causing the multiply defined symbol error - // SymmetricBlockMatrix augmentedHessianUniqueKeys = - // Base::Cameras::template SchurComplementAndRearrangeBlocks<3, DimBlock, - // DimPose>( - // Fs, E, P, b, nonuniqueKeys, keys_); - - // return std::make_shared>( - // keys_, augmentedHessianUniqueKeys); - return nullptr; + SymmetricBlockMatrix augmentedHessianUniqueKeys = + Base::Cameras::template SchurComplementAndRearrangeBlocks<3, DimBlock, + DimPose>( + Fs, E, P, b, nonuniqueKeys, keys_); + + return std::make_shared>( + keys_, augmentedHessianUniqueKeys); } /** From 57311281fbea70ab639a5ad754484c55c0063a4b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 21 Jun 2023 09:56:16 -0400 Subject: [PATCH 05/29] use std::map for Key-Dim maps --- gtsam/geometry/CameraSet.h | 2 +- gtsam_unstable/linear/LP.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 23a4b467ef..5f77ebf58f 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -270,7 +270,7 @@ class CameraSet : public std::vector> { // Get map from key to location in the new augmented Hessian matrix (the // one including only unique keys). - std::map keyToSlotMap; + std::map keyToSlotMap; for (size_t k = 0; k < nrUniqueKeys; k++) { keyToSlotMap[hessianKeys[k]] = k; } diff --git a/gtsam_unstable/linear/LP.h b/gtsam_unstable/linear/LP.h index dbb744d3ef..53247be4c1 100644 --- a/gtsam_unstable/linear/LP.h +++ b/gtsam_unstable/linear/LP.h @@ -29,7 +29,7 @@ namespace gtsam { using namespace std; /// Mapping between variable's key and its corresponding dimensionality -using KeyDimMap = std::map; +using KeyDimMap = std::map; /* * Iterates through every factor in a linear graph and generates a * mapping between every factor key and it's corresponding dimensionality. From 429d5de601db382f3a5f025dc84a03ccc4e9b505 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 21 Jun 2023 12:30:56 -0400 Subject: [PATCH 06/29] Actions file formatting --- .github/workflows/build-windows.yml | 30 ++++++++++++++--------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 0434577c10..339f10a0a7 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -96,23 +96,23 @@ jobs: run: | # Since Visual Studio is a multi-generator, we need to use --config # https://stackoverflow.com/a/24470998/1236990 - cmake --build build -j 4 --config ${{ matrix.build_type }} --target gtsam - cmake --build build -j 4 --config ${{ matrix.build_type }} --target gtsam_unstable - cmake --build build -j 4 --config ${{ matrix.build_type }} --target wrap + 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 wrap # Run GTSAM tests - cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.base - cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.basis - cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.discrete - #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.geometry - cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.inference - cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.linear - cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.navigation - #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.nonlinear - #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.sam - cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.sfm - #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.slam - cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.symbolic + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.base + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.basis + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.discrete + #cmake --build build -j4 --config ${{ matrix.build_type }} --target check.geometry + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.inference + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.linear + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.navigation + #cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.sam + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.sfm + #cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.symbolic # Run GTSAM_UNSTABLE tests #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.base_unstable From f65414d7efc9c8a2a7cf45d1a877815e9184983f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 21 Jun 2023 13:03:54 -0400 Subject: [PATCH 07/29] add GTSAM_EXPORT to additional Ordering methods --- gtsam/inference/Ordering.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index 884a93f0d8..0a3b254419 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -77,9 +77,11 @@ class Ordering: public KeyVector { * @param keys The key vector to append to this ordering. * @return The ordering variable with appended keys. */ + GTSAM_EXPORT This& operator+=(KeyVector& keys); /// Check if key exists in ordering. + GTSAM_EXPORT bool contains(const Key& key) const; /** From 43a9fbf461ee29ec9478d21b4362d35498ee3550 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 21 Jun 2023 13:29:10 -0400 Subject: [PATCH 08/29] mark Pose2 as GTSAM_EXPORT at the class level --- gtsam/geometry/Pose2.h | 38 +++++++++++++++++++------------------- 1 file changed, 19 insertions(+), 19 deletions(-) diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index f1b38c5a6f..1a62fa9381 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -36,7 +36,7 @@ namespace gtsam { * @ingroup geometry * \nosubgrouping */ -class Pose2: public LieGroup { +class GTSAM_EXPORT Pose2: public LieGroup { public: @@ -112,10 +112,10 @@ class Pose2: public LieGroup { /// @{ /** print with optional string */ - GTSAM_EXPORT void print(const std::string& s = "") const; + void print(const std::string& s = "") const; /** assert equality up to a tolerance */ - GTSAM_EXPORT bool equals(const Pose2& pose, double tol = 1e-9) const; + bool equals(const Pose2& pose, double tol = 1e-9) const; /// @} /// @name Group @@ -125,7 +125,7 @@ class Pose2: public LieGroup { inline static Pose2 Identity() { return Pose2(); } /// inverse - GTSAM_EXPORT Pose2 inverse() const; + Pose2 inverse() const; /// compose syntactic sugar inline Pose2 operator*(const Pose2& p2) const { @@ -137,16 +137,16 @@ class Pose2: public LieGroup { /// @{ ///Exponential map at identity - create a rotation from canonical coordinates \f$ [T_x,T_y,\theta] \f$ - GTSAM_EXPORT static Pose2 Expmap(const Vector3& xi, ChartJacobian H = {}); + static Pose2 Expmap(const Vector3& xi, ChartJacobian H = {}); ///Log map at identity - return the canonical coordinates \f$ [T_x,T_y,\theta] \f$ of this rotation - GTSAM_EXPORT static Vector3 Logmap(const Pose2& p, ChartJacobian H = {}); + static Vector3 Logmap(const Pose2& p, ChartJacobian H = {}); /** * Calculate Adjoint map * Ad_pose is 3*3 matrix that when applied to twist xi \f$ [T_x,T_y,\theta] \f$, returns Ad_pose(xi) */ - GTSAM_EXPORT Matrix3 AdjointMap() const; + Matrix3 AdjointMap() const; /// Apply AdjointMap to twist xi inline Vector3 Adjoint(const Vector3& xi) const { @@ -156,7 +156,7 @@ class Pose2: public LieGroup { /** * Compute the [ad(w,v)] operator for SE2 as in [Kobilarov09siggraph], pg 19 */ - GTSAM_EXPORT static Matrix3 adjointMap(const Vector3& v); + static Matrix3 adjointMap(const Vector3& v); /** * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives @@ -192,15 +192,15 @@ class Pose2: public LieGroup { } /// Derivative of Expmap - GTSAM_EXPORT static Matrix3 ExpmapDerivative(const Vector3& v); + static Matrix3 ExpmapDerivative(const Vector3& v); /// Derivative of Logmap - GTSAM_EXPORT static Matrix3 LogmapDerivative(const Pose2& v); + static Matrix3 LogmapDerivative(const Pose2& v); // Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP struct ChartAtOrigin { - GTSAM_EXPORT static Pose2 Retract(const Vector3& v, ChartJacobian H = {}); - GTSAM_EXPORT static Vector3 Local(const Pose2& r, ChartJacobian H = {}); + static Pose2 Retract(const Vector3& v, ChartJacobian H = {}); + static Vector3 Local(const Pose2& r, ChartJacobian H = {}); }; using LieGroup::inverse; // version with derivative @@ -210,7 +210,7 @@ class Pose2: public LieGroup { /// @{ /** Return point coordinates in pose coordinate frame */ - GTSAM_EXPORT Point2 transformTo(const Point2& point, + Point2 transformTo(const Point2& point, OptionalJacobian<2, 3> Dpose = {}, OptionalJacobian<2, 2> Dpoint = {}) const; @@ -222,7 +222,7 @@ class Pose2: public LieGroup { Matrix transformTo(const Matrix& points) const; /** Return point coordinates in global frame */ - GTSAM_EXPORT Point2 transformFrom(const Point2& point, + Point2 transformFrom(const Point2& point, OptionalJacobian<2, 3> Dpose = {}, OptionalJacobian<2, 2> Dpoint = {}) const; @@ -273,14 +273,14 @@ class Pose2: public LieGroup { } //// return transformation matrix - GTSAM_EXPORT Matrix3 matrix() const; + Matrix3 matrix() const; /** * Calculate bearing to a landmark * @param point 2D location of landmark * @return 2D rotation \f$ \in SO(2) \f$ */ - GTSAM_EXPORT Rot2 bearing(const Point2& point, + Rot2 bearing(const Point2& point, OptionalJacobian<1, 3> H1={}, OptionalJacobian<1, 2> H2={}) const; /** @@ -288,7 +288,7 @@ class Pose2: public LieGroup { * @param point SO(2) location of other pose * @return 2D rotation \f$ \in SO(2) \f$ */ - GTSAM_EXPORT Rot2 bearing(const Pose2& pose, + Rot2 bearing(const Pose2& pose, OptionalJacobian<1, 3> H1={}, OptionalJacobian<1, 3> H2={}) const; /** @@ -296,7 +296,7 @@ class Pose2: public LieGroup { * @param point 2D location of landmark * @return range (double) */ - GTSAM_EXPORT double range(const Point2& point, + double range(const Point2& point, OptionalJacobian<1, 3> H1={}, OptionalJacobian<1, 2> H2={}) const; @@ -305,7 +305,7 @@ class Pose2: public LieGroup { * @param point 2D location of other pose * @return range (double) */ - GTSAM_EXPORT double range(const Pose2& point, + double range(const Pose2& point, OptionalJacobian<1, 3> H1={}, OptionalJacobian<1, 3> H2={}) const; From c9397c34fcd9c287c905fa091dc254a5d0f054ac Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 21 Jun 2023 13:29:32 -0400 Subject: [PATCH 09/29] CameraSet::project2 SFINAE to resolve overload ambiguity --- gtsam/geometry/CameraSet.h | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 5f77ebf58f..8621d77ad2 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -131,12 +131,15 @@ class CameraSet : public std::vector> { return z; } - /** An overload o the project2 function to accept + /** An overload of the project2 function to accept * full matrices and vectors and pass it to the pointer - * version of the function + * version of the function. + * + * Use SFINAE to resolve overload ambiguity. */ template - ZVector project2(const POINT& point, OptArgs&... args) const { + typename std::enable_if<(sizeof...(OptArgs) != 0), ZVector>::type project2( + const POINT& point, OptArgs&... args) const { // pass it to the pointer version of the function return project2(point, (&args)...); } From 5fa889b035c238b95c84de1e7e04cc50a275edcb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 21 Jun 2023 17:32:26 -0400 Subject: [PATCH 10/29] add GTSAM_EXPORT tags --- gtsam/discrete/SignatureParser.h | 2 +- gtsam_unstable/linear/QPSParser.h | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/gtsam/discrete/SignatureParser.h b/gtsam/discrete/SignatureParser.h index e6b402e440..e005da14a7 100644 --- a/gtsam/discrete/SignatureParser.h +++ b/gtsam/discrete/SignatureParser.h @@ -47,7 +47,7 @@ namespace gtsam { * * Also fails if the rows are not of the same size. */ -struct SignatureParser { +struct GTSAM_EXPORT SignatureParser { using Row = std::vector; using Table = std::vector; diff --git a/gtsam_unstable/linear/QPSParser.h b/gtsam_unstable/linear/QPSParser.h index bd4254d0f7..e751f34d27 100644 --- a/gtsam_unstable/linear/QPSParser.h +++ b/gtsam_unstable/linear/QPSParser.h @@ -18,12 +18,13 @@ #pragma once +#include #include #include namespace gtsam { -class QPSParser { +class GTSAM_UNSTABLE_EXPORT QPSParser { private: std::string fileName_; From c954e546ef4fb6614344d417a76ad922df45af29 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 21 Jun 2023 17:33:14 -0400 Subject: [PATCH 11/29] enable more Windows tests --- .github/workflows/build-windows.yml | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 339f10a0a7..e85064d08f 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -104,15 +104,23 @@ jobs: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.base cmake --build build -j4 --config ${{ matrix.build_type }} --target check.basis cmake --build build -j4 --config ${{ matrix.build_type }} --target check.discrete - #cmake --build build -j4 --config ${{ matrix.build_type }} --target check.geometry + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.geometry cmake --build build -j4 --config ${{ matrix.build_type }} --target check.inference cmake --build build -j4 --config ${{ matrix.build_type }} --target check.linear cmake --build build -j4 --config ${{ matrix.build_type }} --target check.navigation - #cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear cmake --build build -j4 --config ${{ matrix.build_type }} --target check.sam cmake --build build -j4 --config ${{ matrix.build_type }} --target check.sfm - #cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam cmake --build build -j4 --config ${{ matrix.build_type }} --target check.symbolic + # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.hybrid + # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear + # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam # Run GTSAM_UNSTABLE tests - #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.base_unstable + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.base_unstable + # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.geometry_unstable + # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.linear_unstable + # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.discrete_unstable + # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.dynamics_unstable + # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear_unstable + # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam_unstable + # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.partition From 0bc08b88bc7598eb5d8f6554a113235d3dd8a012 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 21 Jun 2023 17:49:15 -0400 Subject: [PATCH 12/29] remove GTSAM_EXPORT from SignatureParser struct --- gtsam/discrete/SignatureParser.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/discrete/SignatureParser.h b/gtsam/discrete/SignatureParser.h index e005da14a7..e6b402e440 100644 --- a/gtsam/discrete/SignatureParser.h +++ b/gtsam/discrete/SignatureParser.h @@ -47,7 +47,7 @@ namespace gtsam { * * Also fails if the rows are not of the same size. */ -struct GTSAM_EXPORT SignatureParser { +struct SignatureParser { using Row = std::vector; using Table = std::vector; From 2168d0f0867640b7eb2610e8a294a9bb4a04fa45 Mon Sep 17 00:00:00 2001 From: Tal Regev Date: Fri, 30 Jun 2023 19:43:34 +0300 Subject: [PATCH 13/29] Compile with ninja --- .github/workflows/build-windows.yml | 31 +++++++++++++++++------------ 1 file changed, 18 insertions(+), 13 deletions(-) diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 0434577c10..9326a81b0a 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -2,6 +2,10 @@ name: Windows CI on: [pull_request] +concurrency: + group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }} + cancel-in-progress: true + jobs: build: name: ${{ matrix.name }} ${{ matrix.build_type }} @@ -16,19 +20,15 @@ jobs: BOOST_EXE: boost_1_72_0-msvc-14.2 strategy: - fail-fast: true + fail-fast: false matrix: - # Github Actions requires a single row to be added to the build matrix. - # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. - name: [ - windows-2019-cl, - ] - - build_type: [ - Debug, - Release - ] - build_unstable: [ON] + build_type: + - Debug + # - Release + + build_unstable: + - ON + # - OFF include: - name: windows-2019-cl os: windows-2019 @@ -87,10 +87,15 @@ jobs: - name: Checkout uses: actions/checkout@v3 + - name: Setup msbuild + uses: ilammy/msvc-dev-cmd@v1 + with: + arch: x${{ matrix.platform }} + - name: Configuration run: | cmake -E remove_directory build - cmake -B build -S . -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DBOOST_ROOT="${env:BOOST_ROOT}" -DBOOST_INCLUDEDIR="${env:BOOST_ROOT}\boost\include" -DBOOST_LIBRARYDIR="${env:BOOST_ROOT}\lib" + cmake -G Ninja -B build -S . -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DBOOST_ROOT="${env:BOOST_ROOT}" -DBOOST_INCLUDEDIR="${env:BOOST_ROOT}\boost\include" -DBOOST_LIBRARYDIR="${env:BOOST_ROOT}\lib" - name: Build run: | From 92de2273a86bd316205b018d425449d1ca4d4a0e Mon Sep 17 00:00:00 2001 From: Tal Regev Date: Fri, 30 Jun 2023 20:21:11 +0300 Subject: [PATCH 14/29] Fix linkage errors: unresolved external symbol --- gtsam/basis/Basis.h | 2 +- gtsam/discrete/SignatureParser.h | 2 +- gtsam/geometry/Pose2.h | 6 +++--- gtsam/geometry/Pose3.h | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/gtsam/basis/Basis.h b/gtsam/basis/Basis.h index 41cdeeaaa0..0b2b3606b3 100644 --- a/gtsam/basis/Basis.h +++ b/gtsam/basis/Basis.h @@ -80,7 +80,7 @@ using Weights = Eigen::Matrix; /* 1xN vector */ * * @ingroup basis */ -Matrix kroneckerProductIdentity(size_t M, const Weights& w); +Matrix GTSAM_EXPORT kroneckerProductIdentity(size_t M, const Weights& w); /** * CRTP Base class for function bases diff --git a/gtsam/discrete/SignatureParser.h b/gtsam/discrete/SignatureParser.h index e6b402e440..e005da14a7 100644 --- a/gtsam/discrete/SignatureParser.h +++ b/gtsam/discrete/SignatureParser.h @@ -47,7 +47,7 @@ namespace gtsam { * * Also fails if the rows are not of the same size. */ -struct SignatureParser { +struct GTSAM_EXPORT SignatureParser { using Row = std::vector; using Table = std::vector; diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index f1b38c5a6f..b455a81c31 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -198,9 +198,9 @@ class Pose2: public LieGroup { GTSAM_EXPORT static Matrix3 LogmapDerivative(const Pose2& v); // Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP - struct ChartAtOrigin { - GTSAM_EXPORT static Pose2 Retract(const Vector3& v, ChartJacobian H = {}); - GTSAM_EXPORT static Vector3 Local(const Pose2& r, ChartJacobian H = {}); + struct GTSAM_EXPORT ChartAtOrigin { + static Pose2 Retract(const Vector3& v, ChartJacobian H = {}); + static Vector3 Local(const Pose2& r, ChartJacobian H = {}); }; using LieGroup::inverse; // version with derivative diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 6c91d74686..8a807cc23b 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -204,7 +204,7 @@ class GTSAM_EXPORT Pose3: public LieGroup { static Matrix6 LogmapDerivative(const Pose3& xi); // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP - struct ChartAtOrigin { + struct GTSAM_EXPORT ChartAtOrigin { static Pose3 Retract(const Vector6& xi, ChartJacobian Hxi = {}); static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = {}); }; From a499855eafe945f5559d4712054bd48289c3699d Mon Sep 17 00:00:00 2001 From: Tal Regev Date: Fri, 30 Jun 2023 21:56:24 +0300 Subject: [PATCH 15/29] Add release to windows tests --- .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 9326a81b0a..dabdff7f90 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -24,7 +24,7 @@ jobs: matrix: build_type: - Debug - # - Release + - Release build_unstable: - ON From 9aa67b5235f8cb11a6bad8ab08f422f6fe78a7c4 Mon Sep 17 00:00:00 2001 From: Tal Regev Date: Fri, 30 Jun 2023 22:14:43 +0300 Subject: [PATCH 16/29] Add #include --- gtsam/discrete/SignatureParser.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/discrete/SignatureParser.h b/gtsam/discrete/SignatureParser.h index e005da14a7..fec3b9af29 100644 --- a/gtsam/discrete/SignatureParser.h +++ b/gtsam/discrete/SignatureParser.h @@ -21,6 +21,7 @@ #include #include #include +#include namespace gtsam { /** From 7f46117666cfb35f7a4e90a8ef6cf72b14f0b17c Mon Sep 17 00:00:00 2001 From: Tal Regev Date: Fri, 30 Jun 2023 22:37:43 +0300 Subject: [PATCH 17/29] Add non concurrency to all workflows --- .github/workflows/build-linux.yml | 6 ++++++ .github/workflows/build-macos.yml | 6 ++++++ .github/workflows/build-python.yml | 6 ++++++ .github/workflows/build-special.yml | 6 ++++++ .github/workflows/build-windows.yml | 2 ++ 5 files changed, 26 insertions(+) diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index 129b5aaafa..e4937ce065 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -2,6 +2,12 @@ name: Linux CI on: [pull_request] +# Every time you make a push to your PR, it cancel immediately the previous checks, +# and start a new one. The other runner will be available more quickly to your PR. +concurrency: + group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }} + cancel-in-progress: true + jobs: build: name: ${{ matrix.name }} ${{ matrix.build_type }} diff --git a/.github/workflows/build-macos.yml b/.github/workflows/build-macos.yml index 3fa3c15dde..5417018365 100644 --- a/.github/workflows/build-macos.yml +++ b/.github/workflows/build-macos.yml @@ -2,6 +2,12 @@ name: macOS CI on: [pull_request] +# Every time you make a push to your PR, it cancel immediately the previous checks, +# and start a new one. The other runner will be available more quickly to your PR. +concurrency: + group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }} + cancel-in-progress: true + jobs: build: name: ${{ matrix.name }} ${{ matrix.build_type }} diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index f09d589dcf..ca4645a77d 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -2,6 +2,12 @@ name: Python CI on: [pull_request] +# Every time you make a push to your PR, it cancel immediately the previous checks, +# and start a new one. The other runner will be available more quickly to your PR. +concurrency: + group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }} + cancel-in-progress: true + jobs: build: name: ${{ matrix.name }} ${{ matrix.build_type }} Python ${{ matrix.python_version }} diff --git a/.github/workflows/build-special.yml b/.github/workflows/build-special.yml index f72dadbae0..72466ffd68 100644 --- a/.github/workflows/build-special.yml +++ b/.github/workflows/build-special.yml @@ -2,6 +2,12 @@ name: Special Cases CI on: [pull_request] +# Every time you make a push to your PR, it cancel immediately the previous checks, +# and start a new one. The other runner will be available more quickly to your PR. +concurrency: + group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }} + cancel-in-progress: true + jobs: build: name: ${{ matrix.name }} ${{ matrix.build_type }} diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 99b654b3a4..5d5342f3f3 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -2,6 +2,8 @@ name: Windows CI on: [pull_request] +# Every time you make a push to your PR, it cancel immediately the previous checks, +# and start a new one. The other runner will be available more quickly to your PR. concurrency: group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }} cancel-in-progress: true From d1bd76a0aadb06da0ff258e8091b1e8ba9c7b10d Mon Sep 17 00:00:00 2001 From: Tal Regev Date: Sat, 1 Jul 2023 17:02:03 +0300 Subject: [PATCH 18/29] Check more tests --- .github/workflows/build-windows.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 5d5342f3f3..7f8c56558d 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -118,9 +118,9 @@ jobs: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.sam cmake --build build -j4 --config ${{ matrix.build_type }} --target check.sfm cmake --build build -j4 --config ${{ matrix.build_type }} --target check.symbolic - # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.hybrid - # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear - # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.hybrid + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam # Run GTSAM_UNSTABLE tests cmake --build build -j4 --config ${{ matrix.build_type }} --target check.base_unstable From ac86415032ff54a3b1b488e4dc7060796675baf3 Mon Sep 17 00:00:00 2001 From: Tal Regev Date: Sat, 1 Jul 2023 21:20:56 +0300 Subject: [PATCH 19/29] Fix tests --- .github/workflows/build-windows.yml | 14 +++++++------- gtsam/hybrid/HybridFactorGraph.h | 2 +- gtsam/hybrid/HybridSmoother.h | 2 +- .../hybrid/tests/testHybridGaussianFactorGraph.cpp | 1 + 4 files changed, 10 insertions(+), 9 deletions(-) diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 7f8c56558d..e0f56aad22 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -123,11 +123,11 @@ jobs: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam # Run GTSAM_UNSTABLE tests + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.geometry_unstable + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.linear_unstable + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.discrete_unstable + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.dynamics_unstable + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear_unstable + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam_unstable + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.partition cmake --build build -j4 --config ${{ matrix.build_type }} --target check.base_unstable - # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.geometry_unstable - # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.linear_unstable - # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.discrete_unstable - # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.dynamics_unstable - # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear_unstable - # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam_unstable - # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.partition diff --git a/gtsam/hybrid/HybridFactorGraph.h b/gtsam/hybrid/HybridFactorGraph.h index b8e7ff5885..8b59fd4f93 100644 --- a/gtsam/hybrid/HybridFactorGraph.h +++ b/gtsam/hybrid/HybridFactorGraph.h @@ -35,7 +35,7 @@ using SharedFactor = std::shared_ptr; * Hybrid Factor Graph * Factor graph with utilities for hybrid factors. */ -class HybridFactorGraph : public FactorGraph { +class GTSAM_EXPORT HybridFactorGraph : public FactorGraph { public: using Base = FactorGraph; using This = HybridFactorGraph; ///< this class diff --git a/gtsam/hybrid/HybridSmoother.h b/gtsam/hybrid/HybridSmoother.h index 0767da12ff..5c2e4f546d 100644 --- a/gtsam/hybrid/HybridSmoother.h +++ b/gtsam/hybrid/HybridSmoother.h @@ -24,7 +24,7 @@ namespace gtsam { -class HybridSmoother { +class GTSAM_EXPORT HybridSmoother { private: HybridBayesNet hybridBayesNet_; HybridGaussianFactorGraph remainingFactorGraph_; diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index 8276264ae0..073f49c041 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -43,6 +43,7 @@ #include #include #include +#include #include "Switching.h" #include "TinyHybridExample.h" From e00e1236b5799c63d9c8a756d3e742a3c47caa67 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 3 Jul 2023 08:56:08 -0400 Subject: [PATCH 20/29] reorganize CI workflow file --- .github/workflows/build-windows.yml | 23 ++++++++++++++--------- 1 file changed, 14 insertions(+), 9 deletions(-) diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index e0f56aad22..7123d6eeaa 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -22,15 +22,20 @@ jobs: BOOST_EXE: boost_1_72_0-msvc-14.2 strategy: - fail-fast: false + fail-fast: true matrix: - build_type: - - Debug - - Release - - build_unstable: - - ON - # - OFF + # Github Actions requires a single row to be added to the build matrix. + # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. + name: [ + windows-2019-cl, + ] + + build_type: [ + Debug, + Release + ] + + build_unstable: [ON] include: - name: windows-2019-cl os: windows-2019 @@ -123,6 +128,7 @@ jobs: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam # Run GTSAM_UNSTABLE tests + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.base_unstable cmake --build build -j4 --config ${{ matrix.build_type }} --target check.geometry_unstable cmake --build build -j4 --config ${{ matrix.build_type }} --target check.linear_unstable cmake --build build -j4 --config ${{ matrix.build_type }} --target check.discrete_unstable @@ -130,4 +136,3 @@ jobs: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear_unstable cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam_unstable cmake --build build -j4 --config ${{ matrix.build_type }} --target check.partition - cmake --build build -j4 --config ${{ matrix.build_type }} --target check.base_unstable From 38e52810bc288ccf070cc308fd74e618267e1a98 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 27 Jul 2023 14:45:08 -0400 Subject: [PATCH 21/29] remove extra GTSAM_EXPORTs from Ordering.h --- gtsam/inference/Ordering.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index 5202a27491..c3df1b8a08 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -70,11 +70,9 @@ class GTSAM_EXPORT Ordering: public KeyVector { * @param keys The key vector to append to this ordering. * @return The ordering variable with appended keys. */ - GTSAM_EXPORT This& operator+=(KeyVector& keys); /// Check if key exists in ordering. - GTSAM_EXPORT bool contains(const Key& key) const; /** From e562d708a3db88e68341b1a0c17ce811ddbe4a8f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 30 Jul 2023 10:16:15 -0400 Subject: [PATCH 22/29] enable more tests --- .github/workflows/build-windows.yml | 13 +++---------- 1 file changed, 3 insertions(+), 10 deletions(-) diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index af629c7be4..9b54cb94b4 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -102,7 +102,7 @@ jobs: - name: Configuration run: | cmake -E remove_directory build - cmake -G Ninja -B build -S . -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DBOOST_ROOT="${env:BOOST_ROOT}" -DBOOST_INCLUDEDIR="${env:BOOST_ROOT}\boost\include" -DBOOST_LIBRARYDIR="${env:BOOST_ROOT}\lib" + cmake -G Ninja -B build -S . -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DGTSAM_ALLOW_DEPRECATED_SINCE_V43=OFF -DBOOST_ROOT="${env:BOOST_ROOT}" -DBOOST_INCLUDEDIR="${env:BOOST_ROOT}\boost\include" -DBOOST_LIBRARYDIR="${env:BOOST_ROOT}\lib" - name: Build shell: bash @@ -112,9 +112,6 @@ jobs: cmake --build build -j4 --config ${{ matrix.build_type }} --target gtsam cmake --build build -j4 --config ${{ matrix.build_type }} --target gtsam_unstable - # Target doesn't exist - # cmake --build build -j4 --config ${{ matrix.build_type }} --target wrap - - name: Test shell: bash run: | @@ -122,19 +119,15 @@ jobs: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.base cmake --build build -j4 --config ${{ matrix.build_type }} --target check.basis cmake --build build -j4 --config ${{ matrix.build_type }} --target check.discrete - # Compilation error - # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.geometry + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.geometry cmake --build build -j4 --config ${{ matrix.build_type }} --target check.inference - # Compile. Fail with exception cmake --build build -j4 --config ${{ matrix.build_type }} --target check.linear cmake --build build -j4 --config ${{ matrix.build_type }} --target check.navigation cmake --build build -j4 --config ${{ matrix.build_type }} --target check.sam cmake --build build -j4 --config ${{ matrix.build_type }} --target check.sfm cmake --build build -j4 --config ${{ matrix.build_type }} --target check.symbolic - # Compile. Fail with exception cmake --build build -j4 --config ${{ matrix.build_type }} --target check.hybrid - # Compile. Fail with exception - # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear # Compilation error # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam From 67b49d6a5cc57eec93b827a752522b349122b331 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 30 Jul 2023 13:57:00 -0400 Subject: [PATCH 23/29] fix tests in testValues --- gtsam/nonlinear/tests/testValues.cpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 2a7bede307..a2b265a56f 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -581,7 +581,7 @@ TEST(Values, std_move) { TEST(Values, VectorDynamicInsertFixedRead) { Values values; Vector v(3); v << 5.0, 6.0, 7.0; - values.insert(key1, v); + values.insert(key1, v); Vector3 expected(5.0, 6.0, 7.0); Vector3 actual = values.at(key1); CHECK(assert_equal(expected, actual)); @@ -629,7 +629,7 @@ TEST(Values, VectorFixedInsertFixedRead) { TEST(Values, MatrixDynamicInsertFixedRead) { Values values; Matrix v(1,3); v << 5.0, 6.0, 7.0; - values.insert(key1, v); + values.insert(key1, v); Vector3 expected(5.0, 6.0, 7.0); CHECK(assert_equal((Vector)expected, values.at(key1))); CHECK_EXCEPTION(values.at(key1), exception); @@ -639,7 +639,15 @@ TEST(Values, Demangle) { Values values; Matrix13 v; v << 5.0, 6.0, 7.0; values.insert(key1, v); - string expected = "Values with 1 values:\nValue v1: (Eigen::Matrix)\n[\n 5, 6, 7\n]\n\n"; +#ifdef __GNUG__ + string expected = + "Values with 1 values:\nValue v1: (Eigen::Matrix)\n[\n 5, 6, 7\n]\n\n"; +#elif _WIN32 + string expected = + "Values with 1 values:\nValue v1: " + "(class Eigen::Matrix)\n[\n 5, 6, 7\n]\n\n"; +#endif EXPECT(assert_print_equal(expected, values)); } From df3899e2e8bfc9e98dad4e493409ae11e17d1989 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 30 Jul 2023 14:11:05 -0400 Subject: [PATCH 24/29] comment out nonlinear check --- .github/workflows/build-windows.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 9b54cb94b4..3a2d3816a1 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -127,8 +127,8 @@ jobs: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.sfm cmake --build build -j4 --config ${{ matrix.build_type }} --target check.symbolic cmake --build build -j4 --config ${{ matrix.build_type }} --target check.hybrid - cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear - # Compilation error + # Compilation error or test failure + # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam # Run GTSAM_UNSTABLE tests From ddef6445699835557f269a5c52b700d1269d095d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 31 Jul 2023 16:58:06 -0400 Subject: [PATCH 25/29] fix nonlinear tests --- .github/workflows/build-windows.yml | 2 +- gtsam/nonlinear/ISAM2.h | 2 +- gtsam/nonlinear/tests/testSerializationNonlinear.cpp | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 3a2d3816a1..357c4ca089 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -127,8 +127,8 @@ jobs: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.sfm cmake --build build -j4 --config ${{ matrix.build_type }} --target check.symbolic cmake --build build -j4 --config ${{ matrix.build_type }} --target check.hybrid + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear # Compilation error or test failure - # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam # Run GTSAM_UNSTABLE tests diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 50c5058c2b..d3abd95fd2 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -345,7 +345,7 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & boost::serialization::base_object >(*this); + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(theta_); ar & BOOST_SERIALIZATION_NVP(variableIndex_); ar & BOOST_SERIALIZATION_NVP(delta_); diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp index f402656c12..d6f693a23f 100644 --- a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp +++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp @@ -183,7 +183,7 @@ TEST(Serialization, ISAM2) { std::string binaryPath = "saved_solver.dat"; try { - std::ofstream outputStream(binaryPath); + std::ofstream outputStream(binaryPath, std::ios::out | std::ios::binary); boost::archive::binary_oarchive outputArchive(outputStream); outputArchive << solver; } catch(...) { @@ -192,7 +192,7 @@ TEST(Serialization, ISAM2) { gtsam::ISAM2 solverFromDisk; try { - std::ifstream ifs(binaryPath); + std::ifstream ifs(binaryPath, std::ios::in | std::ios::binary); boost::archive::binary_iarchive inputArchive(ifs); inputArchive >> solverFromDisk; } catch(...) { From ced6c53e06e291618a80d039710f234d34593288 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 31 Jul 2023 23:06:11 -0400 Subject: [PATCH 26/29] add GTSAM_EXPORT to various ostream operator overloads --- gtsam/geometry/PinholePose.h | 3 ++- gtsam/geometry/Similarity2.h | 3 ++- gtsam/geometry/Similarity3.h | 3 ++- gtsam/geometry/Unit3.h | 3 ++- gtsam/geometry/triangulation.h | 4 ++-- gtsam/linear/IterativeSolver.h | 4 ++-- 6 files changed, 12 insertions(+), 8 deletions(-) diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 5cad3b6e72..df6ec5c08a 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -332,7 +332,8 @@ class PinholePose: public PinholeBaseK { } /// stream operator - friend std::ostream& operator<<(std::ostream &os, const PinholePose& camera) { + GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, + const PinholePose& camera) { os << "{R: " << camera.pose().rotation().rpy().transpose(); os << ", t: " << camera.pose().translation().transpose(); if (!camera.K_) os << ", K: none"; diff --git a/gtsam/geometry/Similarity2.h b/gtsam/geometry/Similarity2.h index 47281b3839..3e04aa3a10 100644 --- a/gtsam/geometry/Similarity2.h +++ b/gtsam/geometry/Similarity2.h @@ -76,7 +76,8 @@ class GTSAM_EXPORT Similarity2 : public LieGroup { /// Print with optional string void print(const std::string& s) const; - friend std::ostream& operator<<(std::ostream& os, const Similarity2& p); + GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, + const Similarity2& p); /// @} /// @name Group diff --git a/gtsam/geometry/Similarity3.h b/gtsam/geometry/Similarity3.h index aa0f3a62a1..69b4016208 100644 --- a/gtsam/geometry/Similarity3.h +++ b/gtsam/geometry/Similarity3.h @@ -77,7 +77,8 @@ class GTSAM_EXPORT Similarity3 : public LieGroup { /// Print with optional string void print(const std::string& s) const; - friend std::ostream& operator<<(std::ostream& os, const Similarity3& p); + GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, + const Similarity3& p); /// @} /// @name Group diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 72dd49c291..18bc5d9f04 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -105,7 +105,8 @@ class GTSAM_EXPORT Unit3 { /// @name Testable /// @{ - friend std::ostream& operator<<(std::ostream& os, const Unit3& pair); + GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, + const Unit3& pair); /// The print fuction void print(const std::string& s = std::string()) const; diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 3a8398804d..68795a6464 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -665,8 +665,8 @@ class TriangulationResult : public std::optional { return value(); } // stream to output - friend std::ostream& operator<<(std::ostream& os, - const TriangulationResult& result) { + GTSAM_EXPORT friend std::ostream& operator<<( + std::ostream& os, const TriangulationResult& result) { if (result) os << "point = " << *result << std::endl; else diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index c4a7194366..0441cd9da3 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -72,8 +72,8 @@ class IterativeOptimizationParameters { GTSAM_EXPORT virtual void print(std::ostream &os) const; /* for serialization */ - friend std::ostream& operator<<(std::ostream &os, - const IterativeOptimizationParameters &p); + GTSAM_EXPORT friend std::ostream &operator<<( + std::ostream &os, const IterativeOptimizationParameters &p); GTSAM_EXPORT static Verbosity verbosityTranslator(const std::string &s); GTSAM_EXPORT static std::string verbosityTranslator(Verbosity v); From c8184eb109877e40a503f09d842d88732896014c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 1 Aug 2023 09:55:00 -0400 Subject: [PATCH 27/29] fix SphericalCamera traits definition --- gtsam/geometry/SphericalCamera.h | 4 ++-- gtsam/slam/SmartFactorBase.h | 3 ++- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/SphericalCamera.h b/gtsam/geometry/SphericalCamera.h index a9c38cd696..ef20aa7fe7 100644 --- a/gtsam/geometry/SphericalCamera.h +++ b/gtsam/geometry/SphericalCamera.h @@ -238,9 +238,9 @@ class GTSAM_EXPORT SphericalCamera { // end of class SphericalCamera template <> -struct traits : public internal::LieGroup {}; +struct traits : public internal::Manifold {}; template <> -struct traits : public internal::LieGroup {}; +struct traits : public internal::Manifold {}; } // namespace gtsam diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index e0540cc41a..5d6ec44457 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -162,8 +162,9 @@ class SmartFactorBase: public NonlinearFactor { /// Collect all cameras: important that in key order. virtual Cameras cameras(const Values& values) const { Cameras cameras; - for(const Key& k: this->keys_) + for(const Key& k: this->keys_) { cameras.push_back(values.at(k)); + } return cameras; } From 2c569c1fdd58492d4afbab91abc34dc555d530b6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 1 Aug 2023 09:59:43 -0400 Subject: [PATCH 28/29] enable slam tests --- .github/workflows/build-windows.yml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 357c4ca089..670d87b3f1 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -128,8 +128,7 @@ jobs: cmake --build build -j4 --config ${{ matrix.build_type }} --target check.symbolic cmake --build build -j4 --config ${{ matrix.build_type }} --target check.hybrid cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear - # Compilation error or test failure - # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam + cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam # Run GTSAM_UNSTABLE tests cmake --build build -j4 --config ${{ matrix.build_type }} --target check.base_unstable From 8b4fbb1d7bdfd1b6e5c09c061f66e850d065bc0e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 1 Aug 2023 10:54:48 -0400 Subject: [PATCH 29/29] address PR comments --- gtsam/discrete/SignatureParser.h | 1 - gtsam/geometry/CameraSet.h | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/gtsam/discrete/SignatureParser.h b/gtsam/discrete/SignatureParser.h index 5de8b9a507..ddc548fc66 100644 --- a/gtsam/discrete/SignatureParser.h +++ b/gtsam/discrete/SignatureParser.h @@ -21,7 +21,6 @@ #include #include #include -#include #include diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 8621d77ad2..9507471025 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -273,7 +273,7 @@ class CameraSet : public std::vector> { // Get map from key to location in the new augmented Hessian matrix (the // one including only unique keys). - std::map keyToSlotMap; + std::map keyToSlotMap; for (size_t k = 0; k < nrUniqueKeys; k++) { keyToSlotMap[hessianKeys[k]] = k; }