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

Merge pull request #1077 from borglab/feature/more_wrapping

release/4.3a0
Frank Dellaert 2022-01-31 23:36:34 -05:00 committed by GitHub
parent 3ba6566911 113fbf10fe
commit ee4c75ffdf
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
12 changed files with 177 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/discrete/discrete.i
View File

@ -270,11 +270,14 @@ class DiscreteFactorGraph {
gtsam::DiscreteLookupDAG maxProduct(const gtsam::Ordering& ordering);
gtsam::DiscreteBayesNet eliminateSequential();
gtsam::DiscreteBayesNet eliminateSequential(gtsam::Ordering::OrderingType type);
gtsam::DiscreteBayesNet eliminateSequential(const gtsam::Ordering& ordering);
std::pair<gtsam::DiscreteBayesNet, gtsam::DiscreteFactorGraph>
eliminatePartialSequential(const gtsam::Ordering& ordering);
gtsam::DiscreteBayesTree eliminateMultifrontal();
gtsam::DiscreteBayesTree eliminateMultifrontal(const gtsam::Ordering& ordering);
gtsam::DiscreteBayesTree eliminateMultifrontal(gtsam::Ordering::OrderingType type);
gtsam::DiscreteBayesTree eliminateMultifrontal(const gtsam::Ordering& ordering);
std::pair<gtsam::DiscreteBayesTree, gtsam::DiscreteFactorGraph>
eliminatePartialMultifrontal(const gtsam::Ordering& ordering);

22
gtsam/discrete/tests/testDiscreteBayesTree.cpp
View File

@ -243,27 +243,27 @@ TEST(DiscreteBayesTree, Dot) {
string actual = self.bayesTree->dot();
EXPECT(actual ==
"digraph G{\n"
"0[label=\"13,11,6,7\"];\n"
"0[label=\"13, 11, 6, 7\"];\n"
"0->1\n"
"1[label=\"14 : 11,13\"];\n"
"1[label=\"14 : 11, 13\"];\n"
"1->2\n"
"2[label=\"9,12 : 14\"];\n"
"2[label=\"9, 12 : 14\"];\n"
"2->3\n"
"3[label=\"3 : 9,12\"];\n"
"3[label=\"3 : 9, 12\"];\n"
"2->4\n"
"4[label=\"2 : 9,12\"];\n"
"4[label=\"2 : 9, 12\"];\n"
"2->5\n"
"5[label=\"8 : 12,14\"];\n"
"5[label=\"8 : 12, 14\"];\n"
"5->6\n"
"6[label=\"1 : 8,12\"];\n"
"6[label=\"1 : 8, 12\"];\n"
"5->7\n"
"7[label=\"0 : 8,12\"];\n"
"7[label=\"0 : 8, 12\"];\n"
"1->8\n"
"8[label=\"10 : 13,14\"];\n"
"8[label=\"10 : 13, 14\"];\n"
"8->9\n"
"9[label=\"5 : 10,13\"];\n"
"9[label=\"5 : 10, 13\"];\n"
"8->10\n"
"10[label=\"4 : 10,13\"];\n"
"10[label=\"4 : 10, 13\"];\n"
"}");
}

16
gtsam/inference/BayesTree-inst.h
View File

@ -95,7 +95,7 @@ namespace gtsam {
/* ************************************************************************* */
template <class CLIQUE>
void BayesTree<CLIQUE>::dot(std::ostream& s, sharedClique clique,
const KeyFormatter& indexFormatter,
const KeyFormatter& keyFormatter,
int parentnum) const {
static int num = 0;
bool first = true;
@ -104,10 +104,10 @@ namespace gtsam {
std::string parent = out.str();
parent += "[label=\"";
for (Key index : clique->conditional_->frontals()) {
if (!first) parent += ",";
for (Key key : clique->conditional_->frontals()) {
if (!first) parent += ", ";
first = false;
parent += indexFormatter(index);
parent += keyFormatter(key);
}
if (clique->parent()) {
@ -116,10 +116,10 @@ namespace gtsam {
}
first = true;
for (Key sep : clique->conditional_->parents()) {
if (!first) parent += ",";
for (Key parentKey : clique->conditional_->parents()) {
if (!first) parent += ", ";
first = false;
parent += indexFormatter(sep);
parent += keyFormatter(parentKey);
}
parent += "\"];\n";
s << parent;
@ -127,7 +127,7 @@ namespace gtsam {
for (sharedClique c : clique->children) {
num++;
dot(s, c, indexFormatter, parentnum);
dot(s, c, keyFormatter, parentnum);
}
}

47
gtsam/inference/inference.i
View File

@ -4,6 +4,12 @@
namespace gtsam {
// Headers for overloaded methods below, break hierarchy :-/
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/symbolic/SymbolicFactorGraph.h>
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/inference/Key.h>
// Default keyformatter
@ -98,10 +104,41 @@ class Ordering {
Ordering();
Ordering(const gtsam::Ordering& other);
template <FACTOR_GRAPH = {gtsam::NonlinearFactorGraph,
gtsam::GaussianFactorGraph}>
template <
FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}>
static gtsam::Ordering Colamd(const FACTOR_GRAPH& graph);
template <
FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}>
static gtsam::Ordering ColamdConstrainedLast(
const FACTOR_GRAPH& graph, const gtsam::KeyVector& constrainLast,
bool forceOrder = false);
template <
FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}>
static gtsam::Ordering ColamdConstrainedFirst(
const FACTOR_GRAPH& graph, const gtsam::KeyVector& constrainFirst,
bool forceOrder = false);
template <
FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}>
static gtsam::Ordering Natural(const FACTOR_GRAPH& graph);
template <
FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}>
static gtsam::Ordering Metis(const FACTOR_GRAPH& graph);
template <
FACTOR_GRAPH = {gtsam::NonlinearFactorGraph, gtsam::DiscreteFactorGraph,
gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph}>
static gtsam::Ordering Create(gtsam::Ordering::OrderingType orderingType,
const FACTOR_GRAPH& graph);
// Testable
void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
@ -135,12 +172,6 @@ class DotWriter {
};
#include <gtsam/inference/VariableIndex.h>
// Headers for overloaded methods below, break hierarchy :-/
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/symbolic/SymbolicFactorGraph.h>
class VariableIndex {
// Standard Constructors and Named Constructors
VariableIndex();

10
gtsam/linear/linear.i
View File

@ -407,8 +407,10 @@ class GaussianFactorGraph {
// Elimination and marginals
gtsam::GaussianBayesNet* eliminateSequential();
gtsam::GaussianBayesNet* eliminateSequential(gtsam::Ordering::OrderingType type);
gtsam::GaussianBayesNet* eliminateSequential(const gtsam::Ordering& ordering);
gtsam::GaussianBayesTree* eliminateMultifrontal();
gtsam::GaussianBayesTree* eliminateMultifrontal(gtsam::Ordering::OrderingType type);
gtsam::GaussianBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering);
pair<gtsam::GaussianBayesNet*, gtsam::GaussianFactorGraph*> eliminatePartialSequential(
const gtsam::Ordering& ordering);
@ -527,6 +529,7 @@ virtual class GaussianBayesNet {
gtsam::VectorValues optimize() const;
gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const;
std::pair<Matrix, Vector> matrix() const;
gtsam::VectorValues optimizeGradientSearch() const;
gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const;
gtsam::VectorValues gradientAtZero() const;
@ -556,7 +559,12 @@ virtual class GaussianBayesTree {
size_t size() const;
bool empty() const;
size_t numCachedSeparatorMarginals() const;
void saveGraph(string s) const;
string dot(const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
void saveGraph(string s,
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
gtsam::VectorValues optimize() const;
gtsam::VectorValues optimizeGradientSearch() const;

4
gtsam/nonlinear/nonlinear.i
View File

@ -98,11 +98,11 @@ class NonlinearFactorGraph {
string dot(
const gtsam::Values& values,
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,
const GraphvizFormatting& formatting = GraphvizFormatting());
const GraphvizFormatting& writer = GraphvizFormatting());
void saveGraph(
const string& s, const gtsam::Values& values,
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter,
const GraphvizFormatting& formatting = GraphvizFormatting()) const;
const GraphvizFormatting& writer = GraphvizFormatting()) const;
// enabling serialization functionality
void serialize() const;

11
gtsam/sam/sam.i
View File

@ -22,10 +22,19 @@ virtual class RangeFactor : gtsam::NoiseModelFactor {
void serialize() const;
};
// between points:
typedef gtsam::RangeFactor<gtsam::Point2, gtsam::Point2> RangeFactor2;
typedef gtsam::RangeFactor<gtsam::Point3, gtsam::Point3> RangeFactor3;
// between pose and point:
typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactor2D;
typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactor3D;
typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Pose2> RangeFactorPose2;
// between poses:
typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactor3D;
typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Pose3> RangeFactorPose3;
// more specialized types:
typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3>
RangeFactorCalibratedCameraPoint;
typedef gtsam::RangeFactor<gtsam::PinholeCamera<gtsam::Cal3_S2>, gtsam::Point3>

10
gtsam/slam/dataset.cpp
View File

@ -208,15 +208,15 @@ std::map<size_t, Point2> parseVariables<Point2>(const std::string &filename,
/* ************************************************************************* */
// Interpret noise parameters according to flags
static SharedNoiseModel
createNoiseModel(const Vector6 v, bool smart, NoiseFormat noiseFormat,
KernelFunctionType kernelFunctionType) {
static SharedNoiseModel createNoiseModel(
const Vector6 &v, bool smart, NoiseFormat noiseFormat,
KernelFunctionType kernelFunctionType) {
if (noiseFormat == NoiseFormatAUTO) {
// Try to guess covariance matrix layout
if (v(0) != 0.0 && v(1) == 0.0 && v(2) != 0.0 && //
if (v(0) != 0.0 && v(1) == 0.0 && v(2) != 0.0 && //
v(3) != 0.0 && v(4) == 0.0 && v(5) == 0.0) {
noiseFormat = NoiseFormatGRAPH;
} else if (v(0) != 0.0 && v(1) == 0.0 && v(2) == 0.0 && //
} else if (v(0) != 0.0 && v(1) == 0.0 && v(2) == 0.0 && //
v(3) != 0.0 && v(4) == 0.0 && v(5) != 0.0) {
noiseFormat = NoiseFormatCOV;
} else {

14
gtsam/slam/dataset.h
View File

@ -167,10 +167,11 @@ GTSAM_EXPORT GraphAndValues load2D(
* @param kernelFunctionType whether to wrap the noise model in a robust kernel
* @return graph and initial values
*/
GTSAM_EXPORT GraphAndValues load2D(const std::string& filename,
SharedNoiseModel model = SharedNoiseModel(), size_t maxIndex = 0, bool addNoise =
false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatAUTO, //
KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
GTSAM_EXPORT GraphAndValues
load2D(const std::string& filename, SharedNoiseModel model = SharedNoiseModel(),
size_t maxIndex = 0, bool addNoise = false, bool smart = true,
NoiseFormat noiseFormat = NoiseFormatAUTO, //
KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
/** save 2d graph */
GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph,
@ -185,8 +186,9 @@ GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph,
* @param kernelFunctionType whether to wrap the noise model in a robust kernel
* @return graph and initial values
*/
GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile, const bool is3D = false,
KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
GTSAM_EXPORT GraphAndValues
readG2o(const std::string& g2oFile, const bool is3D = false,
KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
/**
* @brief This function writes a g2o file from

39
gtsam/slam/slam.i
View File

@ -253,17 +253,27 @@ bool writeBAL(string filename, gtsam::SfmData& data);
gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db);
gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(
string filename, gtsam::noiseModel::Diagonal* model, int maxIndex,
bool addNoise, bool smart);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(
string filename, gtsam::noiseModel::Diagonal* model, int maxIndex,
bool addNoise);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(
string filename, gtsam::noiseModel::Diagonal* model, int maxIndex);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(
string filename, gtsam::noiseModel::Diagonal* model);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename);
enum NoiseFormat {
NoiseFormatG2O,
NoiseFormatTORO,
NoiseFormatGRAPH,
NoiseFormatCOV,
NoiseFormatAUTO
};
enum KernelFunctionType {
KernelFunctionTypeNONE,
KernelFunctionTypeHUBER,
KernelFunctionTypeTUKEY
};
std::pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(
string filename, gtsam::noiseModel::Diagonal* model = nullptr,
size_t maxIndex = 0, bool addNoise = false, bool smart = true,
gtsam::NoiseFormat noiseFormat = gtsam::NoiseFormatAUTO,
gtsam::KernelFunctionType kernelFunctionType =
gtsam::KernelFunctionTypeNONE);
void save2D(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& config, gtsam::noiseModel::Diagonal* model,
string filename);
@ -290,9 +300,10 @@ gtsam::BetweenFactorPose3s parse3DFactors(string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load3D(string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename,
bool is3D);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(
string filename, const bool is3D = false,
gtsam::KernelFunctionType kernelFunctionType =
gtsam::KernelFunctionTypeNONE);
void writeG2o(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& estimate, string filename);

53
python/gtsam/tests/test_GaussianBayesNet.py Normal file
View File

@ -0,0 +1,53 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
Unit tests for Gaussian Bayes Nets.
Author: Frank Dellaert
"""
# pylint: disable=invalid-name, no-name-in-module, no-member
from __future__ import print_function
import unittest
import gtsam
import numpy as np
from gtsam import GaussianBayesNet, GaussianConditional
from gtsam.utils.test_case import GtsamTestCase
# some keys
_x_ = 11
_y_ = 22
_z_ = 33
def smallBayesNet():
"""Create a small Bayes Net for testing"""
bayesNet = GaussianBayesNet()
I_1x1 = np.eye(1, dtype=float)
bayesNet.push_back(GaussianConditional(
_x_, [9.0], I_1x1, _y_, I_1x1))
bayesNet.push_back(GaussianConditional(_y_, [5.0], I_1x1))
return bayesNet
class TestGaussianBayesNet(GtsamTestCase):
"""Tests for Gaussian Bayes nets."""
def test_matrix(self):
"""Test matrix method"""
R, d = smallBayesNet().matrix() # get matrix and RHS
R1 = np.array([
[1.0, 1.0],
[0.0, 1.0]])
d1 = np.array([9.0, 5.0])
np.testing.assert_equal(R, R1)
np.testing.assert_equal(d, d1)
if __name__ == '__main__':
unittest.main()

6
python/gtsam/tests/test_GraphvizFormatting.py
View File

@ -78,7 +78,7 @@ class TestGraphvizFormatting(GtsamTestCase):
graphviz_formatting.paperHorizontalAxis = gtsam.GraphvizFormatting.Axis.X
graphviz_formatting.paperVerticalAxis = gtsam.GraphvizFormatting.Axis.Y
self.assertEqual(self.graph.dot(self.values,
formatting=graphviz_formatting),
writer=graphviz_formatting),
textwrap.dedent(expected_result))
def test_factor_points(self):
@ -100,7 +100,7 @@ class TestGraphvizFormatting(GtsamTestCase):
graphviz_formatting.plotFactorPoints = False
self.assertEqual(self.graph.dot(self.values,
formatting=graphviz_formatting),
writer=graphviz_formatting),
textwrap.dedent(expected_result))
def test_width_height(self):
@ -127,7 +127,7 @@ class TestGraphvizFormatting(GtsamTestCase):
graphviz_formatting.figureHeightInches = 10
self.assertEqual(self.graph.dot(self.values,
formatting=graphviz_formatting),
writer=graphviz_formatting),
textwrap.dedent(expected_result))