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

Added position hints

release/4.3a0
Frank Dellaert 2022-01-27 12:31:25 -05:00
parent 7ccee875fe
commit 87eeb0d27e
6 changed files with 101 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
gtsam/inference/DotWriter.cpp
View File

@ -16,9 +16,11 @@
* @date December, 2021
*/
#include <gtsam/base/Vector.h>
#include <gtsam/inference/DotWriter.h>
#include <gtsam/base/Vector.h>
#include <gtsam/inference/Symbol.h>
#include <ostream>
using namespace std;
@ -59,18 +61,35 @@ void DotWriter::DrawFactor(size_t i, const boost::optional<Vector2>& position,
}
static void ConnectVariables(Key key1, Key key2,
const KeyFormatter& keyFormatter,
ostream* os) {
const KeyFormatter& keyFormatter, ostream* os) {
*os << " var" << keyFormatter(key1) << "--"
<< "var" << keyFormatter(key2) << ";\n";
}
static void ConnectVariableFactor(Key key, const KeyFormatter& keyFormatter,
size_t i, ostream* os) {
size_t i, ostream* os) {
*os << " var" << keyFormatter(key) << "--"
<< "factor" << i << ";\n";
}
/// Return variable position or none
boost::optional<Vector2> DotWriter::variablePos(Key key) const {
boost::optional<Vector2> result = boost::none;
// Check position hint
Symbol symbol(key);
auto hint = positionHints.find(symbol.chr());
if (hint != positionHints.end())
result.reset(Vector2(symbol.index(), hint->second));
// Override with explicit position, if given.
auto pos = variablePositions.find(key);
if (pos != variablePositions.end())
result.reset(pos->second);
return result;
}
void DotWriter::processFactor(size_t i, const KeyVector& keys,
const KeyFormatter& keyFormatter,
const boost::optional<Vector2>& position,

20
gtsam/inference/DotWriter.h
View File

@ -39,9 +39,19 @@ struct GTSAM_EXPORT DotWriter {
///< the dot of the factor
bool binaryEdges; ///< just use non-dotted edges for binary factors
/// (optional for each variable) Manually specify variable node positions
/**
* Variable positions can be optionally specified and will be included in the
* dor file with a "!' sign, so "neato" can use it to render them.
*/
std::map<gtsam::Key, Vector2> variablePositions;
/**
* The position hints allow one to use symbol character and index to specify
* position. Unless variable positions are specified, if a hint is present for
* a given symbol, it will be used to calculate the positions as (index,hint).
*/
std::map<char, double> positionHints;
explicit DotWriter(double figureWidthInches = 5,
double figureHeightInches = 5,
bool plotFactorPoints = true,
@ -68,13 +78,7 @@ struct GTSAM_EXPORT DotWriter {
std::ostream* os);
/// Return variable position or none
boost::optional<Vector2> variablePos(Key key) const {
auto it = variablePositions.find(key);
if (it == variablePositions.end())
return boost::none;
else
return it->second;
}
boost::optional<Vector2> variablePos(Key key) const;
/// Draw a single factor, specified by its index i and its variable keys.
void processFactor(size_t i, const KeyVector& keys,

1
gtsam/inference/inference.i
View File

@ -129,6 +129,7 @@ class DotWriter {
bool binaryEdges;
std::map<gtsam::Key, gtsam::Vector2> variablePositions;
std::map<char, double> positionHints;
};
#include <gtsam/inference/VariableIndex.h>

1
gtsam/linear/tests/testGaussianBayesNet.cpp
View File

@ -312,7 +312,6 @@ TEST(GaussianBayesNet, Dot) {
EXPECT(assert_equal(Vector2(10, 20), *position, 1e-5));
string actual = noisyBayesNet.dot(DefaultKeyFormatter, writer);
noisyBayesNet.saveGraph("noisyBayesNet.dot", DefaultKeyFormatter, writer);
EXPECT(actual ==
"digraph {\n"
" size=\"5,5\";\n"

39
gtsam/nonlinear/NonlinearFactorGraph.h
View File

@ -43,12 +43,14 @@ namespace gtsam {
class ExpressionFactor;
/**
* A non-linear factor graph is a graph of non-Gaussian, i.e. non-linear factors,
* which derive from NonlinearFactor. The values structures are typically (in SAM) more general
* than just vectors, e.g., Rot3 or Pose3, which are objects in non-linear manifolds.
* Linearizing the non-linear factor graph creates a linear factor graph on the
* tangent vector space at the linearization point. Because the tangent space is a true
* vector space, the config type will be an VectorValues in that linearized factor graph.
* A NonlinearFactorGraph is a graph of non-Gaussian, i.e. non-linear factors,
* which derive from NonlinearFactor. The values structures are typically (in
* SAM) more general than just vectors, e.g., Rot3 or Pose3, which are objects
* in non-linear manifolds. Linearizing the non-linear factor graph creates a
* linear factor graph on the tangent vector space at the linearization point.
* Because the tangent space is a true vector space, the config type will be
* an VectorValues in that linearized factor graph.
* @addtogroup nonlinear
*/
class GTSAM_EXPORT NonlinearFactorGraph: public FactorGraph<NonlinearFactor> {
@ -58,6 +60,9 @@ namespace gtsam {
typedef NonlinearFactorGraph This;
typedef boost::shared_ptr<This> shared_ptr;
/// @name Standard Constructors
/// @{
/** Default constructor */
NonlinearFactorGraph() {}
@ -76,6 +81,10 @@ namespace gtsam {
/// Destructor
virtual ~NonlinearFactorGraph() {}
/// @}
/// @name Testable
/// @{
/** print */
void print(
const std::string& str = "NonlinearFactorGraph: ",
@ -90,6 +99,10 @@ namespace gtsam {
/** Test equality */
bool equals(const NonlinearFactorGraph& other, double tol = 1e-9) const;
/// @}
/// @name Standard Interface
/// @{
/** unnormalized error, \f$ \sum_i 0.5 (h_i(X_i)-z)^2 / \sigma^2 \f$ in the most common case */
double error(const Values& values) const;
@ -206,6 +219,7 @@ namespace gtsam {
emplace_shared<PriorFactor<T>>(key, prior, covariance);
}
/// @}
/// @name Graph Display
/// @{
@ -250,6 +264,8 @@ namespace gtsam {
public:
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
/// @name Deprecated
/// @{
/** @deprecated */
boost::shared_ptr<HessianFactor> GTSAM_DEPRECATED linearizeToHessianFactor(
const Values& values, boost::none_t, const Dampen& dampen = nullptr) const
@ -261,19 +277,20 @@ namespace gtsam {
{return updateCholesky(values, dampen);}
/** @deprecated */
void GTSAM_DEPRECATED
saveGraph(std::ostream& os, const Values& values = Values(),
const GraphvizFormatting& writer = GraphvizFormatting(),
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
void GTSAM_DEPRECATED saveGraph(
std::ostream& os, const Values& values = Values(),
const GraphvizFormatting& graphvizFormatting = GraphvizFormatting(),
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
dot(os, values, keyFormatter, graphvizFormatting);
}
/** @deprecated */
void GTSAM_DEPRECATED
saveGraph(const std::string& filename, const Values& values,
const GraphvizFormatting& writer,
const GraphvizFormatting& graphvizFormatting,
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
saveGraph(filename, values, keyFormatter, graphvizFormatting);
}
/// @}
#endif
};

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

@ -15,13 +15,16 @@
* @author Frank Dellaert
*/
#include <boost/make_shared.hpp>
#include <gtsam/symbolic/SymbolicBayesNet.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/VectorSpace.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/symbolic/SymbolicConditional.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/symbolic/SymbolicBayesNet.h>
#include <gtsam/symbolic/SymbolicConditional.h>
#include <boost/make_shared.hpp>
using namespace std;
using namespace gtsam;
@ -30,7 +33,6 @@ static const Key _L_ = 0;
static const Key _A_ = 1;
static const Key _B_ = 2;
static const Key _C_ = 3;
static const Key _D_ = 4;
static SymbolicConditional::shared_ptr
B(new SymbolicConditional(_B_)),
@ -78,14 +80,38 @@ TEST( SymbolicBayesNet, combine )
}
/* ************************************************************************* */
TEST(SymbolicBayesNet, saveGraph) {
TEST(SymbolicBayesNet, Dot) {
using symbol_shorthand::A;
using symbol_shorthand::X;
SymbolicBayesNet bn;
bn += SymbolicConditional(_A_, _B_);
KeyVector keys {_B_, _C_, _D_};
bn += SymbolicConditional::FromKeys(keys,2);
bn += SymbolicConditional(_D_);
bn += SymbolicConditional(X(3), X(2), A(2));
bn += SymbolicConditional(X(2), X(1), A(1));
bn += SymbolicConditional(X(1));
bn.saveGraph("SymbolicBayesNet.dot");
DotWriter writer;
writer.positionHints.emplace('a', 2);
writer.positionHints.emplace('x', 1);
auto position = writer.variablePos(A(1));
CHECK(position);
EXPECT(assert_equal(Vector2(1, 2), *position, 1e-5));
string actual = bn.dot(DefaultKeyFormatter, writer);
EXPECT(actual ==
"digraph {\n"
" size=\"5,5\";\n"
"\n"
" vara1[label=\"a1\", pos=\"1,2!\"];\n"
" vara2[label=\"a2\", pos=\"2,2!\"];\n"
" varx1[label=\"x1\", pos=\"1,1!\"];\n"
" varx2[label=\"x2\", pos=\"2,1!\"];\n"
" varx3[label=\"x3\", pos=\"3,1!\"];\n"
"\n"
" varx1->varx2\n"
" vara1->varx2\n"
" varx2->varx3\n"
" vara2->varx3\n"
"}");
}
/* ************************************************************************* */