diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index f4442bdde..670d87b3f 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -34,6 +34,7 @@ jobs: Debug, Release ] + build_unstable: [ON] include: - name: windows-2019-cl @@ -93,10 +94,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 -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 @@ -106,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: | @@ -116,50 +119,30 @@ 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.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.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 - - # Compilation error - # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam - + 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 - # Compile. Fail with exception # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.geometry_unstable - # Compile. Fail with exception # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.linear_unstable - # Compile. Fail with exception # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.discrete_unstable - # Compile. Fail with exception # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.dynamics_unstable - # Compile. Fail with exception # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.nonlinear_unstable - # Compilation error # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.slam_unstable - # Compilation error - # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.partition + # cmake --build build -j4 --config ${{ matrix.build_type }} --target check.partition diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 23a4b467e..950747102 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)...); } diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 5cad3b6e7..df6ec5c08 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -332,7 +332,8 @@ public: } /// 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 47281b383..3e04aa3a1 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 aa0f3a62a..69b401620 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/SphericalCamera.h b/gtsam/geometry/SphericalCamera.h index a9c38cd69..ef20aa7fe 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/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 72dd49c29..18bc5d9f0 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -105,7 +105,8 @@ public: /// @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 3a8398804..68795a646 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 c4a719436..0441cd9da 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -72,8 +72,8 @@ public: 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); diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 50c5058c2..d3abd95fd 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 f402656c1..d6f693a23 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(...) { diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 2a7bede30..a2b265a56 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)); } diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index e0540cc41..5d6ec4445 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -162,8 +162,9 @@ protected: /// 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; } 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/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); } /**