diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 0cc67ad5a..ef2500b46 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -48,7 +48,9 @@ jobs: - name: Install Dependencies shell: powershell run: | - Invoke-Expression (New-Object System.Net.WebClient).DownloadString('https://get.scoop.sh') + iwr -useb get.scoop.sh -outfile 'install_scoop.ps1' + .\install_scoop.ps1 -RunAsAdmin + scoop install cmake --global # So we don't get issues with CMP0074 policy scoop install ninja --global diff --git a/gtsam/discrete/DiscreteLookupDAG.h b/gtsam/discrete/DiscreteLookupDAG.h index 38a846f1f..15169a1dc 100644 --- a/gtsam/discrete/DiscreteLookupDAG.h +++ b/gtsam/discrete/DiscreteLookupDAG.h @@ -46,7 +46,7 @@ class GTSAM_EXPORT DiscreteLookupTable : public DiscreteConditional { * @brief Construct a new Discrete Lookup Table object * * @param nFrontals number of frontal variables - * @param keys a orted list of gtsam::Keys + * @param keys a sorted list of gtsam::Keys * @param potentials the algebraic decision tree with lookup values */ DiscreteLookupTable(size_t nFrontals, const DiscreteKeys& keys, diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 1e399ad18..88f128191 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -59,7 +59,7 @@ Matrix6 Pose3::AdjointMap() const { const Matrix3 R = R_.matrix(); Matrix3 A = skewSymmetric(t_.x(), t_.y(), t_.z()) * R; Matrix6 adj; - adj << R, Z_3x3, A, R; + adj << R, Z_3x3, A, R; // Gives [R 0; A R] return adj; } diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index a32b3ce22..5b053ebee 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -139,7 +139,7 @@ namespace gtsam { return nodes_.empty(); } - /** return nodes */ + /** Return nodes. Each node is a clique of variables obtained after elimination. */ const Nodes& nodes() const { return nodes_; } /** Access node by variable */ diff --git a/gtsam/navigation/PreintegrationParams.cpp b/gtsam/navigation/PreintegrationParams.cpp index 2298bb696..2548f95a6 100644 --- a/gtsam/navigation/PreintegrationParams.cpp +++ b/gtsam/navigation/PreintegrationParams.cpp @@ -34,7 +34,6 @@ void PreintegrationParams::print(const string& s) const { << endl; if (omegaCoriolis && use2ndOrderCoriolis) cout << "Using 2nd-order Coriolis" << endl; - if (body_P_sensor) body_P_sensor->print(" "); cout << "n_gravity = (" << n_gravity.transpose() << ")" << endl; } diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 92c2142a7..37feee837 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -295,6 +295,17 @@ class GTSAM_EXPORT ISAM2 : public BayesTree { const ISAM2UpdateParams& updateParams, const FastList& affectedKeys, const KeySet& relinKeys); + /** + * @brief Perform an incremental update of the factor graph to return a new + * Bayes Tree with affected keys. + * + * @param updateParams Parameters for the ISAM2 update. + * @param relinKeys Keys of variables to relinearize. + * @param affectedKeys The set of keys which are affected in the update. + * @param affectedKeysSet [output] Affected and contaminated keys. + * @param orphans [output] List of orphanes cliques after elimination. + * @param result [output] The result of the incremental update step. + */ void recalculateIncremental(const ISAM2UpdateParams& updateParams, const KeySet& relinKeys, const FastList& affectedKeys, diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 8a1ffdd72..f80462847 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -80,7 +80,9 @@ namespace gtsam { /// @{ /// print with optional string - void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { + void print( + const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "BetweenFactor(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << ")\n"; diff --git a/python/gtsam/tests/test_GaussianFactorGraph.py b/python/gtsam/tests/test_GaussianFactorGraph.py index a29b0f263..09ac4c564 100644 --- a/python/gtsam/tests/test_GaussianFactorGraph.py +++ b/python/gtsam/tests/test_GaussianFactorGraph.py @@ -23,23 +23,23 @@ from gtsam.utils.test_case import GtsamTestCase def create_graph(): """Create a basic linear factor graph for testing""" graph = gtsam.GaussianFactorGraph() - + x0 = X(0) x1 = X(1) x2 = X(2) - + BETWEEN_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.ones(1)) PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.ones(1)) graph.add(x1, np.eye(1), x0, -np.eye(1), np.ones(1), BETWEEN_NOISE) - graph.add(x2, np.eye(1), x1, -np.eye(1), 2*np.ones(1), BETWEEN_NOISE) + graph.add(x2, np.eye(1), x1, -np.eye(1), 2 * np.ones(1), BETWEEN_NOISE) graph.add(x0, np.eye(1), np.zeros(1), PRIOR_NOISE) return graph, (x0, x1, x2) + class TestGaussianFactorGraph(GtsamTestCase): """Tests for Gaussian Factor Graphs.""" - def test_fg(self): """Test solving a linear factor graph""" graph, X = create_graph() @@ -71,7 +71,7 @@ class TestGaussianFactorGraph(GtsamTestCase): self.assertAlmostEqual(EXPECTEDM[0], m[0], delta=1e-8) self.assertAlmostEqual(EXPECTEDM[1], m[1], delta=1e-8) self.assertAlmostEqual(EXPECTEDM[2], m[2], delta=1e-8) - + def test_linearMarginalization(self): """Marginalize a linear factor graph""" graph, X = create_graph() @@ -88,5 +88,23 @@ class TestGaussianFactorGraph(GtsamTestCase): self.assertAlmostEqual(EXPECTEDM[1], m[1], delta=1e-8) self.assertAlmostEqual(EXPECTEDM[2], m[2], delta=1e-8) + def test_ordering(self): + """Test ordering""" + gfg, keys = create_graph() + ordering = gtsam.Ordering() + for key in keys[::-1]: + ordering.push_back(key) + + bn = gfg.eliminateSequential(ordering) + self.assertEqual(bn.size(), 3) + + keyVector = gtsam.KeyVector() + keyVector.append(keys[2]) + #TODO(Varun) Below code causes segfault in Debug config + # ordering = gtsam.Ordering.ColamdConstrainedLastGaussianFactorGraph(gfg, keyVector) + # bn = gfg.eliminateSequential(ordering) + # self.assertEqual(bn.size(), 3) + + if __name__ == '__main__': unittest.main()