diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index dabdff7f9..99b654b3a 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -101,23 +101,31 @@ 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.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 # 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 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/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 23a4b467e..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)...); } @@ -270,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; } diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index b455a81c3..aad81493f 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,10 +192,10 @@ 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 GTSAM_EXPORT ChartAtOrigin { @@ -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; 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) { } 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; /** 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. 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_; 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_; diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h index f9db90953..189c501bb 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; @@ -254,11 +253,12 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP } // 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_); + 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); } /** 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