Merge pull request #1553 from borglab/fix/windows-tests

.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,60 +112,37 @@ 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: |
           # Run GTSAM tests
           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

gtsam/geometry/CameraSet.h

@@ -131,12 +131,15 @@ class CameraSet : public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA>> {
     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 <class POINT, class... OptArgs>
-  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)...);
   }

gtsam/geometry/PinholePose.h

@@ -332,7 +332,8 @@ class PinholePose: public PinholeBaseK<CALIBRATION> {
   }
 
   /// 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";

gtsam/geometry/Similarity2.h

@@ -76,7 +76,8 @@ class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> {
   /// 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

gtsam/geometry/Similarity3.h

@@ -77,7 +77,8 @@ class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
   /// 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

gtsam/geometry/SphericalCamera.h

@@ -238,9 +238,9 @@ class GTSAM_EXPORT SphericalCamera {
 // end of class SphericalCamera
 
 template <>
-struct traits<SphericalCamera> : public internal::LieGroup<Pose3> {};
+struct traits<SphericalCamera> : public internal::Manifold<SphericalCamera> {};
 
 template <>
-struct traits<const SphericalCamera> : public internal::LieGroup<Pose3> {};
+struct traits<const SphericalCamera> : public internal::Manifold<SphericalCamera> {};
 
 }  // namespace gtsam

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;

gtsam/geometry/triangulation.h

@@ -665,8 +665,8 @@ class TriangulationResult : public std::optional<Point3> {
     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

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);

gtsam/nonlinear/ISAM2.h

@@ -345,7 +345,7 @@ class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-      ar & boost::serialization::base_object<BayesTree<ISAM2Clique> >(*this);
+      ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
       ar & BOOST_SERIALIZATION_NVP(theta_);
       ar & BOOST_SERIALIZATION_NVP(variableIndex_);
       ar & BOOST_SERIALIZATION_NVP(delta_);

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(...) {

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<Vector3>(key1, v);
   Vector3 expected(5.0, 6.0, 7.0);
   Vector3 actual = values.at<Vector3>(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<Matrix13>(key1, v);
   Vector3 expected(5.0, 6.0, 7.0);
   CHECK(assert_equal((Vector)expected, values.at<Matrix13>(key1)));
   CHECK_EXCEPTION(values.at<Matrix23>(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<double, 1, 3, 1, 1, 3>)\n[\n	5, 6, 7\n]\n\n";
+#ifdef __GNUG__
+  string expected =
+      "Values with 1 values:\nValue v1: (Eigen::Matrix<double, 1, 3, 1, 1, "
+      "3>)\n[\n	5, 6, 7\n]\n\n";
+#elif _WIN32
+  string expected =
+      "Values with 1 values:\nValue v1: "
+      "(class Eigen::Matrix<double,1,3,1,1,3>)\n[\n	5, 6, 7\n]\n\n";
+#endif
 
   EXPECT(assert_print_equal(expected, values));
 }

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<CAMERA>(k));
+    }
     return cameras;
   }
 

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<Key, size_t>;
+using KeyDimMap = std::map<Key, uint32_t>;
 /*
  * Iterates through every factor in a linear graph and generates a
  * mapping between every factor key and it's corresponding dimensionality.

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)));
   }

gtsam_unstable/slam/SmartStereoProjectionFactorPP.h

@@ -201,34 +201,32 @@ class GTSAM_UNSTABLE_EXPORT SmartStereoProjectionFactorPP
   }
 
   /// linearize and return a Hessianfactor that is an approximation of error(p)
-  std::shared_ptr<RegularHessianFactor<DimPose> > createHessianFactor(
-      const Values& values, const double lambda = 0.0, bool diagonalDamping =
-          false) const {
-
+  std::shared_ptr<RegularHessianFactor<DimPose>> 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)
     size_t nrUniqueKeys = keys_.size();
 
     // Create structures for Hessian Factors
     KeyVector js;
-    std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
+    std::vector<Matrix> Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
     std::vector<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<DimPose>
-          > (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<RegularHessianFactor<DimPose>>(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<DimPose>
-        > (keys_, augmentedHessianUniqueKeys);
+    return std::make_shared<RegularHessianFactor<DimPose>>(
+        keys_, augmentedHessianUniqueKeys);
   }
 
   /**