12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #1187 from borglab/dot-printer

release/4.3a0
Varun Agrawal 2022-05-08 18:13:14 -04:00 committed by GitHub
parent 63ffeafd0d 1d77ba55e4
commit fb1da007f8
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
9 changed files with 85 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
gtsam/discrete/tests/testDiscreteFactorGraph.cpp
View File

@ -415,16 +415,16 @@ TEST(DiscreteFactorGraph, DotWithNames) {
"graph {\n" "graph {\n"
" size=\"5,5\";\n" " size=\"5,5\";\n"
"\n" "\n"
" varC[label=\"C\"];\n" " var0[label=\"C\"];\n"
" varA[label=\"A\"];\n" " var1[label=\"A\"];\n"
" varB[label=\"B\"];\n" " var2[label=\"B\"];\n"
"\n" "\n"
" factor0[label=\"\", shape=point];\n" " factor0[label=\"\", shape=point];\n"
" varC--factor0;\n" " var0--factor0;\n"
" varA--factor0;\n" " var1--factor0;\n"
" factor1[label=\"\", shape=point];\n" " factor1[label=\"\", shape=point];\n"
" varC--factor1;\n" " var0--factor1;\n"
" varB--factor1;\n" " var2--factor1;\n"
"}\n"; "}\n";
EXPECT(actual == expected); EXPECT(actual == expected);
} }

5
gtsam/inference/BayesNet-inst.h
View File

@ -53,8 +53,9 @@ void BayesNet<CONDITIONAL>::dot(std::ostream& os,
auto frontals = conditional->frontals(); auto frontals = conditional->frontals();
const Key me = frontals.front(); const Key me = frontals.front();
auto parents = conditional->parents(); auto parents = conditional->parents();
for (const Key& p : parents) for (const Key& p : parents) {
os << " var" << keyFormatter(p) << "->var" << keyFormatter(me) << "\n"; os << " var" << p << "->var" << me << "\n";
}
} }
os << "}"; os << "}";

8
gtsam/inference/DotWriter.cpp
View File

@ -43,7 +43,7 @@ void DotWriter::drawVariable(Key key, const KeyFormatter& keyFormatter,
const boost::optional<Vector2>& position, const boost::optional<Vector2>& position,
ostream* os) const { ostream* os) const {
// Label the node with the label from the KeyFormatter // Label the node with the label from the KeyFormatter
*os << " var" << keyFormatter(key) << "[label=\"" << keyFormatter(key) *os << " var" << key << "[label=\"" << keyFormatter(key)
<< "\""; << "\"";
if (position) { if (position) {
*os << ", pos=\"" << position->x() << "," << position->y() << "!\""; *os << ", pos=\"" << position->x() << "," << position->y() << "!\"";
@ -65,13 +65,13 @@ void DotWriter::DrawFactor(size_t i, const boost::optional<Vector2>& position,
static void ConnectVariables(Key key1, Key key2, static void ConnectVariables(Key key1, Key key2,
const KeyFormatter& keyFormatter, ostream* os) { const KeyFormatter& keyFormatter, ostream* os) {
*os << " var" << keyFormatter(key1) << "--" *os << " var" << key1 << "--"
<< "var" << keyFormatter(key2) << ";\n"; << "var" << key2 << ";\n";
} }
static void ConnectVariableFactor(Key key, const KeyFormatter& keyFormatter, static void ConnectVariableFactor(Key key, const KeyFormatter& keyFormatter,
size_t i, ostream* os) { size_t i, ostream* os) {
*os << " var" << keyFormatter(key) << "--" *os << " var" << key << "--"
<< "factor" << i << ";\n"; << "factor" << i << ";\n";
} }

1
gtsam/nonlinear/PriorFactor.h
View File

@ -94,7 +94,6 @@ namespace gtsam {
Vector evaluateError(const T& x, boost::optional<Matrix&> H = boost::none) const override { 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)); if (H) (*H) = Matrix::Identity(traits<T>::GetDimension(x),traits<T>::GetDimension(x));
// manifold equivalent of z-x -> Local(x,z) // manifold equivalent of z-x -> Local(x,z)
// TODO(ASL) Add Jacobians.
return -traits<T>::Local(x, prior_); return -traits<T>::Local(x, prior_);
} }

38
gtsam/slam/tests/testPriorFactor.cpp
View File

@ -5,12 +5,16 @@
* @date Nov 4, 2014 * @date Nov 4, 2014
*/ */
#include <gtsam/base/Vector.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Vector.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/nonlinear/factorTesting.h>
using namespace std; using namespace std;
using namespace std::placeholders;
using namespace gtsam; using namespace gtsam;
using namespace imuBias;
/* ************************************************************************* */ /* ************************************************************************* */
@ -28,11 +32,39 @@ TEST(PriorFactor, ConstructorVector3) {
// Constructor dynamic sized vector // Constructor dynamic sized vector
TEST(PriorFactor, ConstructorDynamicSizeVector) { 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); SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 1.0);
PriorFactor<Vector> factor(1, v, model); 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() { int main() {
TestResult tr; TestResult tr;

18
gtsam/symbolic/tests/testSymbolicBayesNet.cpp
View File

@ -104,16 +104,16 @@ TEST(SymbolicBayesNet, Dot) {
"digraph {\n" "digraph {\n"
" size=\"5,5\";\n" " size=\"5,5\";\n"
"\n" "\n"
" vara1[label=\"a1\", pos=\"1,2!\", shape=box];\n" " var6989586621679009793[label=\"a1\", pos=\"1,2!\", shape=box];\n"
" vara2[label=\"a2\", pos=\"2,2!\", shape=box];\n" " var6989586621679009794[label=\"a2\", pos=\"2,2!\", shape=box];\n"
" varx1[label=\"x1\", pos=\"1,1!\"];\n" " var8646911284551352321[label=\"x1\", pos=\"1,1!\"];\n"
" varx2[label=\"x2\", pos=\"2,1!\"];\n" " var8646911284551352322[label=\"x2\", pos=\"2,1!\"];\n"
" varx3[label=\"x3\", pos=\"3,1!\"];\n" " var8646911284551352323[label=\"x3\", pos=\"3,1!\"];\n"
"\n" "\n"
" varx1->varx2\n" " var8646911284551352321->var8646911284551352322\n"
" vara1->varx2\n" " var6989586621679009793->var8646911284551352322\n"
" varx2->varx3\n" " var8646911284551352322->var8646911284551352323\n"
" vara2->varx3\n" " var6989586621679009794->var8646911284551352323\n"
"}"); "}");
} }

10
python/gtsam/tests/test_DiscreteBayesNet.py
View File

@ -11,12 +11,12 @@ Author: Frank Dellaert
# pylint: disable=no-name-in-module, invalid-name # pylint: disable=no-name-in-module, invalid-name
import unittest
import textwrap import textwrap
import unittest
import gtsam import gtsam
from gtsam import (DiscreteBayesNet, DiscreteConditional, DiscreteFactorGraph, from gtsam import (DiscreteBayesNet, DiscreteConditional, DiscreteDistribution,
DiscreteKeys, DiscreteDistribution, DiscreteValues, Ordering) DiscreteFactorGraph, DiscreteKeys, DiscreteValues, Ordering)
from gtsam.utils.test_case import GtsamTestCase from gtsam.utils.test_case import GtsamTestCase
# Some keys: # Some keys:
@ -152,10 +152,10 @@ class TestDiscreteBayesNet(GtsamTestCase):
var4[label="4"]; var4[label="4"];
var5[label="5"]; var5[label="5"];
var6[label="6"]; var6[label="6"];
vara0[label="a0", pos="0,2!"]; var6989586621679009792[label="a0", pos="0,2!"];
var4->var6 var4->var6
vara0->var3 var6989586621679009792->var3
var3->var5 var3->var5
var6->var5 var6->var5
}""" }"""

40
tests/testNonlinearFactorGraph.cpp
View File

@ -335,21 +335,21 @@ TEST(NonlinearFactorGraph, dot) {
"graph {\n" "graph {\n"
" size=\"5,5\";\n" " size=\"5,5\";\n"
"\n" "\n"
" varl1[label=\"l1\"];\n" " var7782220156096217089[label=\"l1\"];\n"
" varx1[label=\"x1\"];\n" " var8646911284551352321[label=\"x1\"];\n"
" varx2[label=\"x2\"];\n" " var8646911284551352322[label=\"x2\"];\n"
"\n" "\n"
" factor0[label=\"\", shape=point];\n" " factor0[label=\"\", shape=point];\n"
" varx1--factor0;\n" " var8646911284551352321--factor0;\n"
" factor1[label=\"\", shape=point];\n" " factor1[label=\"\", shape=point];\n"
" varx1--factor1;\n" " var8646911284551352321--factor1;\n"
" varx2--factor1;\n" " var8646911284551352322--factor1;\n"
" factor2[label=\"\", shape=point];\n" " factor2[label=\"\", shape=point];\n"
" varx1--factor2;\n" " var8646911284551352321--factor2;\n"
" varl1--factor2;\n" " var7782220156096217089--factor2;\n"
" factor3[label=\"\", shape=point];\n" " factor3[label=\"\", shape=point];\n"
" varx2--factor3;\n" " var8646911284551352322--factor3;\n"
" varl1--factor3;\n" " var7782220156096217089--factor3;\n"
"}\n"; "}\n";
const NonlinearFactorGraph fg = createNonlinearFactorGraph(); const NonlinearFactorGraph fg = createNonlinearFactorGraph();
@ -363,21 +363,21 @@ TEST(NonlinearFactorGraph, dot_extra) {
"graph {\n" "graph {\n"
" size=\"5,5\";\n" " size=\"5,5\";\n"
"\n" "\n"
" varl1[label=\"l1\", pos=\"0,0!\"];\n" " var7782220156096217089[label=\"l1\", pos=\"0,0!\"];\n"
" varx1[label=\"x1\", pos=\"1,0!\"];\n" " var8646911284551352321[label=\"x1\", pos=\"1,0!\"];\n"
" varx2[label=\"x2\", pos=\"1,1.5!\"];\n" " var8646911284551352322[label=\"x2\", pos=\"1,1.5!\"];\n"
"\n" "\n"
" factor0[label=\"\", shape=point];\n" " factor0[label=\"\", shape=point];\n"
" varx1--factor0;\n" " var8646911284551352321--factor0;\n"
" factor1[label=\"\", shape=point];\n" " factor1[label=\"\", shape=point];\n"
" varx1--factor1;\n" " var8646911284551352321--factor1;\n"
" varx2--factor1;\n" " var8646911284551352322--factor1;\n"
" factor2[label=\"\", shape=point];\n" " factor2[label=\"\", shape=point];\n"
" varx1--factor2;\n" " var8646911284551352321--factor2;\n"
" varl1--factor2;\n" " var7782220156096217089--factor2;\n"
" factor3[label=\"\", shape=point];\n" " factor3[label=\"\", shape=point];\n"
" varx2--factor3;\n" " var8646911284551352322--factor3;\n"
" varl1--factor3;\n" " var7782220156096217089--factor3;\n"
"}\n"; "}\n";
const NonlinearFactorGraph fg = createNonlinearFactorGraph(); const NonlinearFactorGraph fg = createNonlinearFactorGraph();