From 4d275a45e64dc3c39b975e582bcdf05b8c248aa3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 5 May 2022 11:23:51 -0400 Subject: [PATCH 1/4] remove key formatting from keys when defining vertices and edges to avoid syntax errors --- gtsam/inference/DotWriter.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/inference/DotWriter.cpp b/gtsam/inference/DotWriter.cpp index ad53305757..eac0c90f93 100644 --- a/gtsam/inference/DotWriter.cpp +++ b/gtsam/inference/DotWriter.cpp @@ -43,7 +43,7 @@ void DotWriter::drawVariable(Key key, const KeyFormatter& keyFormatter, const boost::optional& position, ostream* os) const { // Label the node with the label from the KeyFormatter - *os << " var" << keyFormatter(key) << "[label=\"" << keyFormatter(key) + *os << " var" << key << "[label=\"" << keyFormatter(key) << "\""; if (position) { *os << ", pos=\"" << position->x() << "," << position->y() << "!\""; @@ -65,13 +65,13 @@ void DotWriter::DrawFactor(size_t i, const boost::optional& position, static void ConnectVariables(Key key1, Key key2, const KeyFormatter& keyFormatter, ostream* os) { - *os << " var" << keyFormatter(key1) << "--" - << "var" << keyFormatter(key2) << ";\n"; + *os << " var" << key1 << "--" + << "var" << key2 << ";\n"; } static void ConnectVariableFactor(Key key, const KeyFormatter& keyFormatter, size_t i, ostream* os) { - *os << " var" << keyFormatter(key) << "--" + *os << " var" << key << "--" << "factor" << i << ";\n"; } From 51d1c27f2d8c829dfb4711739f2a0fd4d1cd3311 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 5 May 2022 11:24:26 -0400 Subject: [PATCH 2/4] add prior factor tests and remove TODO --- gtsam/nonlinear/Marginals.h | 2 +- gtsam/nonlinear/PriorFactor.h | 1 - gtsam/slam/tests/testPriorFactor.cpp | 38 +++++++++++++++++++++++++--- 3 files changed, 36 insertions(+), 5 deletions(-) diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h index 028545d019..3c5aa9cabc 100644 --- a/gtsam/nonlinear/Marginals.h +++ b/gtsam/nonlinear/Marginals.h @@ -121,7 +121,7 @@ class GTSAM_EXPORT Marginals { /** Optimize the bayes tree */ VectorValues optimize() const; - + protected: /** Compute the Bayes Tree as a helper function to the constructor */ diff --git a/gtsam/nonlinear/PriorFactor.h b/gtsam/nonlinear/PriorFactor.h index c745f7bd91..a490162ac3 100644 --- a/gtsam/nonlinear/PriorFactor.h +++ b/gtsam/nonlinear/PriorFactor.h @@ -94,7 +94,6 @@ namespace gtsam { Vector evaluateError(const T& x, boost::optional H = boost::none) const override { if (H) (*H) = Matrix::Identity(traits::GetDimension(x),traits::GetDimension(x)); // manifold equivalent of z-x -> Local(x,z) - // TODO(ASL) Add Jacobians. return -traits::Local(x, prior_); } diff --git a/gtsam/slam/tests/testPriorFactor.cpp b/gtsam/slam/tests/testPriorFactor.cpp index 2dc083cb23..d1a60e3461 100644 --- a/gtsam/slam/tests/testPriorFactor.cpp +++ b/gtsam/slam/tests/testPriorFactor.cpp @@ -5,12 +5,16 @@ * @date Nov 4, 2014 */ +#include #include +#include #include -#include +#include using namespace std; +using namespace std::placeholders; using namespace gtsam; +using namespace imuBias; /* ************************************************************************* */ @@ -23,16 +27,44 @@ TEST(PriorFactor, ConstructorScalar) { // Constructor vector3 TEST(PriorFactor, ConstructorVector3) { SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0); - PriorFactor factor(1, Vector3(1,2,3), model); + PriorFactor factor(1, Vector3(1, 2, 3), model); } // Constructor dynamic sized vector TEST(PriorFactor, ConstructorDynamicSizeVector) { - Vector v(5); v << 1, 2, 3, 4, 5; + Vector v(5); + v << 1, 2, 3, 4, 5; SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 1.0); PriorFactor factor(1, v, model); } +Vector callEvaluateError(const PriorFactor& factor, + const ConstantBias& bias) { + return factor.evaluateError(bias); +} + +// Test for imuBias::ConstantBias +TEST(PriorFactor, ConstantBias) { + Vector3 biasAcc(1, 2, 3); + Vector3 biasGyro(0.1, 0.2, 0.3); + ConstantBias bias(biasAcc, biasGyro); + + PriorFactor factor(1, bias, + noiseModel::Isotropic::Sigma(6, 0.1)); + Values values; + values.insert(1, bias); + + EXPECT_DOUBLES_EQUAL(0.0, factor.error(values), 1e-8); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); + + ConstantBias incorrectBias( + (Vector6() << 1.1, 2.1, 3.1, 0.2, 0.3, 0.4).finished()); + values.clear(); + values.insert(1, incorrectBias); + EXPECT_DOUBLES_EQUAL(3.0, factor.error(values), 1e-8); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +} + /* ************************************************************************* */ int main() { TestResult tr; From 0cfeb8621c5605b0d32fb469f4aa8d1600c4c8fd Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 5 May 2022 14:35:10 -0400 Subject: [PATCH 3/4] fix tests and remove additional keyformatter calls --- .../tests/testDiscreteFactorGraph.cpp | 14 +++---- gtsam/inference/BayesNet-inst.h | 5 ++- gtsam/symbolic/tests/testSymbolicBayesNet.cpp | 18 ++++----- tests/testNonlinearFactorGraph.cpp | 40 +++++++++---------- 4 files changed, 39 insertions(+), 38 deletions(-) diff --git a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp index 0a7d869ec5..3d9621affa 100644 --- a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp +++ b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp @@ -415,16 +415,16 @@ TEST(DiscreteFactorGraph, DotWithNames) { "graph {\n" " size=\"5,5\";\n" "\n" - " varC[label=\"C\"];\n" - " varA[label=\"A\"];\n" - " varB[label=\"B\"];\n" + " var0[label=\"C\"];\n" + " var1[label=\"A\"];\n" + " var2[label=\"B\"];\n" "\n" " factor0[label=\"\", shape=point];\n" - " varC--factor0;\n" - " varA--factor0;\n" + " var0--factor0;\n" + " var1--factor0;\n" " factor1[label=\"\", shape=point];\n" - " varC--factor1;\n" - " varB--factor1;\n" + " var0--factor1;\n" + " var2--factor1;\n" "}\n"; EXPECT(actual == expected); } diff --git a/gtsam/inference/BayesNet-inst.h b/gtsam/inference/BayesNet-inst.h index afde5498dc..e792b5c032 100644 --- a/gtsam/inference/BayesNet-inst.h +++ b/gtsam/inference/BayesNet-inst.h @@ -53,8 +53,9 @@ void BayesNet::dot(std::ostream& os, auto frontals = conditional->frontals(); const Key me = frontals.front(); auto parents = conditional->parents(); - for (const Key& p : parents) - os << " var" << keyFormatter(p) << "->var" << keyFormatter(me) << "\n"; + for (const Key& p : parents) { + os << " var" << p << "->var" << me << "\n"; + } } os << "}"; diff --git a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp index 2e13be10eb..7795d5b89b 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp @@ -104,16 +104,16 @@ TEST(SymbolicBayesNet, Dot) { "digraph {\n" " size=\"5,5\";\n" "\n" - " vara1[label=\"a1\", pos=\"1,2!\", shape=box];\n" - " vara2[label=\"a2\", pos=\"2,2!\", shape=box];\n" - " varx1[label=\"x1\", pos=\"1,1!\"];\n" - " varx2[label=\"x2\", pos=\"2,1!\"];\n" - " varx3[label=\"x3\", pos=\"3,1!\"];\n" + " var6989586621679009793[label=\"a1\", pos=\"1,2!\", shape=box];\n" + " var6989586621679009794[label=\"a2\", pos=\"2,2!\", shape=box];\n" + " var8646911284551352321[label=\"x1\", pos=\"1,1!\"];\n" + " var8646911284551352322[label=\"x2\", pos=\"2,1!\"];\n" + " var8646911284551352323[label=\"x3\", pos=\"3,1!\"];\n" "\n" - " varx1->varx2\n" - " vara1->varx2\n" - " varx2->varx3\n" - " vara2->varx3\n" + " var8646911284551352321->var8646911284551352322\n" + " var6989586621679009793->var8646911284551352322\n" + " var8646911284551352322->var8646911284551352323\n" + " var6989586621679009794->var8646911284551352323\n" "}"); } diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index 05a6e7f45e..e1a88d6169 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -335,21 +335,21 @@ TEST(NonlinearFactorGraph, dot) { "graph {\n" " size=\"5,5\";\n" "\n" - " varl1[label=\"l1\"];\n" - " varx1[label=\"x1\"];\n" - " varx2[label=\"x2\"];\n" + " var7782220156096217089[label=\"l1\"];\n" + " var8646911284551352321[label=\"x1\"];\n" + " var8646911284551352322[label=\"x2\"];\n" "\n" " factor0[label=\"\", shape=point];\n" - " varx1--factor0;\n" + " var8646911284551352321--factor0;\n" " factor1[label=\"\", shape=point];\n" - " varx1--factor1;\n" - " varx2--factor1;\n" + " var8646911284551352321--factor1;\n" + " var8646911284551352322--factor1;\n" " factor2[label=\"\", shape=point];\n" - " varx1--factor2;\n" - " varl1--factor2;\n" + " var8646911284551352321--factor2;\n" + " var7782220156096217089--factor2;\n" " factor3[label=\"\", shape=point];\n" - " varx2--factor3;\n" - " varl1--factor3;\n" + " var8646911284551352322--factor3;\n" + " var7782220156096217089--factor3;\n" "}\n"; const NonlinearFactorGraph fg = createNonlinearFactorGraph(); @@ -363,21 +363,21 @@ TEST(NonlinearFactorGraph, dot_extra) { "graph {\n" " size=\"5,5\";\n" "\n" - " varl1[label=\"l1\", pos=\"0,0!\"];\n" - " varx1[label=\"x1\", pos=\"1,0!\"];\n" - " varx2[label=\"x2\", pos=\"1,1.5!\"];\n" + " var7782220156096217089[label=\"l1\", pos=\"0,0!\"];\n" + " var8646911284551352321[label=\"x1\", pos=\"1,0!\"];\n" + " var8646911284551352322[label=\"x2\", pos=\"1,1.5!\"];\n" "\n" " factor0[label=\"\", shape=point];\n" - " varx1--factor0;\n" + " var8646911284551352321--factor0;\n" " factor1[label=\"\", shape=point];\n" - " varx1--factor1;\n" - " varx2--factor1;\n" + " var8646911284551352321--factor1;\n" + " var8646911284551352322--factor1;\n" " factor2[label=\"\", shape=point];\n" - " varx1--factor2;\n" - " varl1--factor2;\n" + " var8646911284551352321--factor2;\n" + " var7782220156096217089--factor2;\n" " factor3[label=\"\", shape=point];\n" - " varx2--factor3;\n" - " varl1--factor3;\n" + " var8646911284551352322--factor3;\n" + " var7782220156096217089--factor3;\n" "}\n"; const NonlinearFactorGraph fg = createNonlinearFactorGraph(); From 1d77ba55e400560e59dce39961d56ea59ccd240b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 5 May 2022 16:49:03 -0400 Subject: [PATCH 4/4] fix python test --- python/gtsam/tests/test_DiscreteBayesNet.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/python/gtsam/tests/test_DiscreteBayesNet.py b/python/gtsam/tests/test_DiscreteBayesNet.py index 74191dcc7a..10c5db612a 100644 --- a/python/gtsam/tests/test_DiscreteBayesNet.py +++ b/python/gtsam/tests/test_DiscreteBayesNet.py @@ -11,12 +11,12 @@ # pylint: disable=no-name-in-module, invalid-name -import unittest import textwrap +import unittest import gtsam -from gtsam import (DiscreteBayesNet, DiscreteConditional, DiscreteFactorGraph, - DiscreteKeys, DiscreteDistribution, DiscreteValues, Ordering) +from gtsam import (DiscreteBayesNet, DiscreteConditional, DiscreteDistribution, + DiscreteFactorGraph, DiscreteKeys, DiscreteValues, Ordering) from gtsam.utils.test_case import GtsamTestCase # Some keys: @@ -152,10 +152,10 @@ def test_dot(self): var4[label="4"]; var5[label="5"]; var6[label="6"]; - vara0[label="a0", pos="0,2!"]; + var6989586621679009792[label="a0", pos="0,2!"]; var4->var6 - vara0->var3 + var6989586621679009792->var3 var3->var5 var6->var5 }"""