From b0404aa109eb463c0764b4edded82fd3ab8f513a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 20 Jun 2023 14:36:45 -0400 Subject: [PATCH 01/15] 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 9f12d670e..3d24f784b 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 7e326117b..9db2a34f0 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/15] 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 dbb744d3e..50065b2dd 100644 --- a/gtsam_unstable/linear/LP.h +++ b/gtsam_unstable/linear/LP.h @@ -81,9 +81,23 @@ public: 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/15] add todo for error in SmartStereoProjectionFactorPP.h --- .../slam/SmartStereoProjectionFactorPP.h | 42 ++++++++++--------- 1 file changed, 22 insertions(+), 20 deletions(-) diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h index f9db90953..b022ce16f 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_); + //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 < RegularHessianFactor - > (keys_, augmentedHessianUniqueKeys); + // 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/15] undo changes --- gtsam_unstable/linear/LP.h | 20 +++---------------- .../slam/SmartStereoProjectionFactorPP.h | 14 ++++++------- 2 files changed, 9 insertions(+), 25 deletions(-) diff --git a/gtsam_unstable/linear/LP.h b/gtsam_unstable/linear/LP.h index 50065b2dd..dbb744d3e 100644 --- a/gtsam_unstable/linear/LP.h +++ b/gtsam_unstable/linear/LP.h @@ -81,23 +81,9 @@ public: 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 b022ce16f..189c501bb 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_); + SymmetricBlockMatrix augmentedHessianUniqueKeys = + Base::Cameras::template SchurComplementAndRearrangeBlocks<3, DimBlock, + DimPose>( + Fs, E, P, b, nonuniqueKeys, keys_); - // return std::make_shared>( - // keys_, augmentedHessianUniqueKeys); - return nullptr; + 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/15] 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 23a4b467e..5f77ebf58 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 dbb744d3e..53247be4c 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/15] 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 0434577c1..339f10a0a 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/15] 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 884a93f0d..0a3b25441 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -77,9 +77,11 @@ public: * @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/15] 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 f1b38c5a6..1a62fa938 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 @@ public: /// @{ /** 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 @@ public: 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 @@ public: /// @{ ///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 @@ public: /** * 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 @@ public: } /// 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 @@ public: /// @{ /** 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 @@ public: 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 @@ public: } //// 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 @@ public: * @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 @@ public: * @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 @@ public: * @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/15] 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 5f77ebf58..8621d77ad 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 ab3e3773ec0ecae3905655d11c1eb82302ceeb57 Mon Sep 17 00:00:00 2001 From: Adam Rutkowski Date: Wed, 21 Jun 2023 13:25:36 -0500 Subject: [PATCH 10/15] removed boost from TableFactor and added guards to testSerializationSlam --- gtsam/discrete/TableFactor.cpp | 3 +-- tests/testSerializationSlam.cpp | 4 ++++ 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index acb59a8be..5fe3cd9d1 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -21,7 +21,6 @@ #include #include -#include #include using namespace std; @@ -203,7 +202,7 @@ void TableFactor::print(const string& s, const KeyFormatter& formatter) const { cout << s; cout << " f["; for (auto&& key : keys()) - cout << boost::format(" (%1%,%2%),") % formatter(key) % cardinality(key); + cout << " (" << formatter(key) << "," << cardinality(key) << "),"; cout << " ]" << endl; for (SparseIt it(sparse_table_); it; ++it) { DiscreteValues assignment = findAssignments(it.index()); diff --git a/tests/testSerializationSlam.cpp b/tests/testSerializationSlam.cpp index c4170b108..6b504a810 100644 --- a/tests/testSerializationSlam.cpp +++ b/tests/testSerializationSlam.cpp @@ -52,6 +52,8 @@ #include #include +#ifdef GTSAM_USE_BOOST_FEATURES + #include #include @@ -674,6 +676,8 @@ TEST(SubgraphSolver, Solves) { } } +#endif + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From 5fa889b035c238b95c84de1e7e04cc50a275edcb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 21 Jun 2023 17:32:26 -0400 Subject: [PATCH 11/15] 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 e6b402e44..e005da14a 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 bd4254d0f..e751f34d2 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 12/15] 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 339f10a0a..e85064d08 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 13/15] 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 e005da14a..e6b402e44 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 475184cb3ccd7a3109da42fabd33306ed01cea12 Mon Sep 17 00:00:00 2001 From: Adam Rutkowski Date: Thu, 22 Jun 2023 09:46:21 -0500 Subject: [PATCH 14/15] Removed boost guards from testSerializationSlam and fixed CMakeLists --- tests/CMakeLists.txt | 2 +- tests/testSerializationSlam.cpp | 4 ---- 2 files changed, 1 insertion(+), 5 deletions(-) diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index d7b68c4ec..44b7505fc 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -13,7 +13,7 @@ if (NOT GTSAM_USE_BOOST_FEATURES) endif() if (NOT GTSAM_ENABLE_BOOST_SERIALIZATION) - list(APPEND excluded_tests "testSerializationSLAM.cpp") + list(APPEND excluded_tests "testSerializationSlam.cpp") endif() # Build tests diff --git a/tests/testSerializationSlam.cpp b/tests/testSerializationSlam.cpp index 6b504a810..c4170b108 100644 --- a/tests/testSerializationSlam.cpp +++ b/tests/testSerializationSlam.cpp @@ -52,8 +52,6 @@ #include #include -#ifdef GTSAM_USE_BOOST_FEATURES - #include #include @@ -676,8 +674,6 @@ TEST(SubgraphSolver, Solves) { } } -#endif - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From 1de26020b3c50e6e26d64c821f00230d140f1656 Mon Sep 17 00:00:00 2001 From: IshitaTakeshi Date: Wed, 28 Jun 2023 20:21:55 +0900 Subject: [PATCH 15/15] Add explanation of the StereoPoint constructor --- gtsam/geometry/StereoPoint2.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index bd335dfbb..4383e212e 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -46,7 +46,9 @@ public: uL_(0), uR_(0), v_(0) { } - /** constructor */ + /** uL and uR represent the x-axis value of left and right frame coordinates respectively. + v represents the y coordinate value. The y-axis value should be the same under the + stereo constraint. */ StereoPoint2(double uL, double uR, double v) : uL_(uL), uR_(uR), v_(v) { }