Skip to content

Commit

Permalink
Merge pull request #1187 from borglab/dot-printer
Browse files Browse the repository at this point in the history
  • Loading branch information
@varunagrawal
varunagrawal authored May 8, 2022
2 parents 63ffeaf + 1d77ba5 commit fb1da00
Show file tree
Hide file tree
Showing 9 changed files with 84 additions and 52 deletions.
14 changes: 7 additions & 7 deletions gtsam/discrete/tests/testDiscreteFactorGraph.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
Expand Down
5 changes: 3 additions & 2 deletions gtsam/inference/BayesNet-inst.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,8 +53,9 @@ void BayesNet<CONDITIONAL>::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 << "}";
Expand Down
8 changes: 4 additions & 4 deletions gtsam/inference/DotWriter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ void DotWriter::drawVariable(Key key, const KeyFormatter& keyFormatter,
const boost::optional<Vector2>& 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() << "!\"";
Expand All @@ -65,13 +65,13 @@ void DotWriter::DrawFactor(size_t i, const boost::optional<Vector2>& 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";
}

Expand Down
2 changes: 1 addition & 1 deletion gtsam/nonlinear/Marginals.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 */
Expand Down
1 change: 0 additions & 1 deletion gtsam/nonlinear/PriorFactor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -94,7 +94,6 @@ namespace gtsam {
Vector evaluateError(const T& x, boost::optional<Matrix&> H = boost::none) const override {
if (H) (*H) = Matrix::Identity(traits<T>::GetDimension(x),traits<T>::GetDimension(x));
// manifold equivalent of z-x -> Local(x,z)
// TODO(ASL) Add Jacobians.
return -traits<T>::Local(x, prior_);
}

Expand Down
38 changes: 35 additions & 3 deletions gtsam/slam/tests/testPriorFactor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,12 +5,16 @@
* @date Nov 4, 2014
*/

#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Vector.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/nonlinear/factorTesting.h>

using namespace std;
using namespace std::placeholders;
using namespace gtsam;
using namespace imuBias;

/* ************************************************************************* */

Expand All @@ -23,16 +27,44 @@ TEST(PriorFactor, ConstructorScalar) {
// Constructor vector3
TEST(PriorFactor, ConstructorVector3) {
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0);
PriorFactor<Vector3> factor(1, Vector3(1,2,3), model);
PriorFactor<Vector3> 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<Vector> factor(1, v, model);
}

Vector callEvaluateError(const PriorFactor<ConstantBias>& 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<ConstantBias> 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;
Expand Down
18 changes: 9 additions & 9 deletions gtsam/symbolic/tests/testSymbolicBayesNet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
"}");
}

Expand Down
10 changes: 5 additions & 5 deletions python/gtsam/tests/test_DiscreteBayesNet.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down Expand Up @@ -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
}"""
Expand Down
40 changes: 20 additions & 20 deletions tests/testNonlinearFactorGraph.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand All @@ -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();
Expand Down

0 comments on commit fb1da00

Please sign in to comment.