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

Merge branch 'develop' into feature/essential-mat-with-approx-k

release/4.3a0
Ayush Baid 2021-05-25 15:36:52 -07:00
parent 4572282deb ab92779b25
commit 71b9004a2d
38 changed files with 381 additions and 249 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.github/scripts/boost.sh vendored
View File

@ -2,7 +2,7 @@
BOOST_FOLDER=boost_${BOOST_VERSION//./_} BOOST_FOLDER=boost_${BOOST_VERSION//./_}
# Download Boost # Download Boost
wget https://dl.bintray.com/boostorg/release/${BOOST_VERSION}/source/${BOOST_FOLDER}.tar.gz wget https://boostorg.jfrog.io/artifactory/main/release/${BOOST_VERSION}/source/${BOOST_FOLDER}.tar.gz
# Unzip # Unzip
tar -zxf ${BOOST_FOLDER}.tar.gz tar -zxf ${BOOST_FOLDER}.tar.gz

3
gtsam/discrete/DiscreteBayesNet.h
View File

@ -55,6 +55,9 @@ namespace gtsam {
template<class DERIVEDCONDITIONAL> template<class DERIVEDCONDITIONAL>
DiscreteBayesNet(const FactorGraph<DERIVEDCONDITIONAL>& graph) : Base(graph) {} DiscreteBayesNet(const FactorGraph<DERIVEDCONDITIONAL>& graph) : Base(graph) {}
/// Destructor
virtual ~DiscreteBayesNet() {}
/// @} /// @}
/// @name Testable /// @name Testable

11
gtsam/discrete/DiscreteFactor.h
View File

@ -74,13 +74,14 @@ public:
/// @name Testable /// @name Testable
/// @{ /// @{
// equals /// equals
virtual bool equals(const DiscreteFactor& lf, double tol = 1e-9) const = 0; virtual bool equals(const DiscreteFactor& lf, double tol = 1e-9) const = 0;
// print /// print
virtual void print(const std::string& s = "DiscreteFactor\n", void print(
const KeyFormatter& formatter = DefaultKeyFormatter) const { const std::string& s = "DiscreteFactor\n",
Factor::print(s, formatter); const KeyFormatter& formatter = DefaultKeyFormatter) const override {
Base::print(s, formatter);
} }
/** Test whether the factor is empty */ /** Test whether the factor is empty */

8
gtsam/discrete/DiscreteFactorGraph.h
View File

@ -91,6 +91,9 @@ public:
template<class DERIVEDFACTOR> template<class DERIVEDFACTOR>
DiscreteFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {} DiscreteFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
/// Destructor
virtual ~DiscreteFactorGraph() {}
/// @name Testable /// @name Testable
/// @{ /// @{
@ -129,8 +132,9 @@ public:
double operator()(const DiscreteFactor::Values & values) const; double operator()(const DiscreteFactor::Values & values) const;
/// print /// print
void print(const std::string& s = "DiscreteFactorGraph", void print(
const KeyFormatter& formatter =DefaultKeyFormatter) const; const std::string& s = "DiscreteFactorGraph",
const KeyFormatter& formatter = DefaultKeyFormatter) const override;
/** Solve the factor graph by performing variable elimination in COLAMD order using /** Solve the factor graph by performing variable elimination in COLAMD order using
* the dense elimination function specified in \c function, * the dense elimination function specified in \c function,

25
gtsam/geometry/CalibratedCamera.h
View File

@ -119,22 +119,20 @@ public:
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
/** default constructor */ /// Default constructor
PinholeBase() { PinholeBase() {}
}
/** constructor with pose */ /// Constructor with pose
explicit PinholeBase(const Pose3& pose) : explicit PinholeBase(const Pose3& pose) : pose_(pose) {}
pose_(pose) {
}
/// @} /// @}
/// @name Advanced Constructors /// @name Advanced Constructors
/// @{ /// @{
explicit PinholeBase(const Vector &v) : explicit PinholeBase(const Vector& v) : pose_(Pose3::Expmap(v)) {}
pose_(Pose3::Expmap(v)) {
} /// Default destructor
virtual ~PinholeBase() = default;
/// @} /// @}
/// @name Testable /// @name Testable
@ -144,7 +142,7 @@ public:
bool equals(const PinholeBase &camera, double tol = 1e-9) const; bool equals(const PinholeBase &camera, double tol = 1e-9) const;
/// print /// print
void print(const std::string& s = "PinholeBase") const; virtual void print(const std::string& s = "PinholeBase") const;
/// @} /// @}
/// @name Standard Interface /// @name Standard Interface
@ -324,6 +322,11 @@ public:
/// Return canonical coordinate /// Return canonical coordinate
Vector localCoordinates(const CalibratedCamera& T2) const; Vector localCoordinates(const CalibratedCamera& T2) const;
/// print
void print(const std::string& s = "CalibratedCamera") const override {
PinholeBase::print(s);
}
/// @deprecated /// @deprecated
inline size_t dim() const { inline size_t dim() const {
return dimension; return dimension;

2
gtsam/geometry/PinholeCamera.h
View File

@ -148,7 +148,7 @@ public:
} }
/// print /// print
void print(const std::string& s = "PinholeCamera") const { void print(const std::string& s = "PinholeCamera") const override {
Base::print(s); Base::print(s);
K_.print(s + ".calibration"); K_.print(s + ".calibration");
} }

2
gtsam/geometry/PinholePose.h
View File

@ -340,7 +340,7 @@ public:
} }
/// print /// print
void print(const std::string& s = "PinholePose") const { void print(const std::string& s = "PinholePose") const override {
Base::print(s); Base::print(s);
if (!K_) if (!K_)
std::cout << "s No calibration given" << std::endl; std::cout << "s No calibration given" << std::endl;

2
gtsam/geometry/tests/testRot3.cpp
View File

@ -825,7 +825,7 @@ TEST(Rot3, RQ_derivative) {
const auto R = Rot3::RzRyRx(xyz).matrix(); const auto R = Rot3::RzRyRx(xyz).matrix();
const auto num = numericalDerivative11(RQ_proxy, R); const auto num = numericalDerivative11(RQ_proxy, R);
Matrix39 calc; Matrix39 calc;
RQ(R, calc).second; RQ(R, calc);
const auto err = vec_err.second; const auto err = vec_err.second;
CHECK(assert_equal(num, calc, err)); CHECK(assert_equal(num, calc, err));

260
gtsam/gtsam.i
View File

@ -10,8 +10,11 @@
namespace gtsam { namespace gtsam {
// Actually a FastList<Key>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
const KeyFormatter DefaultKeyFormatter;
// Actually a FastList<Key>
class KeyList { class KeyList {
KeyList(); KeyList();
KeyList(const gtsam::KeyList& other); KeyList(const gtsam::KeyList& other);
@ -47,7 +50,7 @@ class KeySet {
KeySet(const gtsam::KeyList& list); KeySet(const gtsam::KeyList& list);
// Testable // Testable
void print(string s="") const; void print(string s = "") const;
bool equals(const gtsam::KeySet& other) const; bool equals(const gtsam::KeySet& other) const;
// common STL methods // common STL methods
@ -221,7 +224,7 @@ virtual class Value {
// No constructors because this is an abstract class // No constructors because this is an abstract class
// Testable // Testable
void print(string s="") const; void print(string s = "") const;
// Manifold // Manifold
size_t dim() const; size_t dim() const;
@ -245,7 +248,7 @@ class Point2 {
Point2(Vector v); Point2(Vector v);
// Testable // Testable
void print(string s="") const; void print(string s = "") const;
bool equals(const gtsam::Point2& point, double tol) const; bool equals(const gtsam::Point2& point, double tol) const;
// Group // Group
@ -298,7 +301,7 @@ class StereoPoint2 {
StereoPoint2(double uL, double uR, double v); StereoPoint2(double uL, double uR, double v);
// Testable // Testable
void print(string s="") const; void print(string s = "") const;
bool equals(const gtsam::StereoPoint2& point, double tol) const; bool equals(const gtsam::StereoPoint2& point, double tol) const;
// Group // Group
@ -342,7 +345,7 @@ class Point3 {
Point3(Vector v); Point3(Vector v);
// Testable // Testable
void print(string s="") const; void print(string s = "") const;
bool equals(const gtsam::Point3& p, double tol) const; bool equals(const gtsam::Point3& p, double tol) const;
// Group // Group
@ -379,7 +382,7 @@ class Rot2 {
static gtsam::Rot2 fromCosSin(double c, double s); static gtsam::Rot2 fromCosSin(double c, double s);
// Testable // Testable
void print(string s="") const; void print(string s = "theta") const;
bool equals(const gtsam::Rot2& rot, double tol) const; bool equals(const gtsam::Rot2& rot, double tol) const;
// Group // Group
@ -430,7 +433,7 @@ class SO3 {
static gtsam::SO3 ClosestTo(const Matrix M); static gtsam::SO3 ClosestTo(const Matrix M);
// Testable // Testable
void print(string s="") const; void print(string s = "") const;
bool equals(const gtsam::SO3& other, double tol) const; bool equals(const gtsam::SO3& other, double tol) const;
// Group // Group
@ -460,7 +463,7 @@ class SO4 {
static gtsam::SO4 FromMatrix(Matrix R); static gtsam::SO4 FromMatrix(Matrix R);
// Testable // Testable
void print(string s="") const; void print(string s = "") const;
bool equals(const gtsam::SO4& other, double tol) const; bool equals(const gtsam::SO4& other, double tol) const;
// Group // Group
@ -490,7 +493,7 @@ class SOn {
static gtsam::SOn Lift(size_t n, Matrix R); static gtsam::SOn Lift(size_t n, Matrix R);
// Testable // Testable
void print(string s="") const; void print(string s = "") const;
bool equals(const gtsam::SOn& other, double tol) const; bool equals(const gtsam::SOn& other, double tol) const;
// Group // Group
@ -551,7 +554,7 @@ class Rot3 {
static gtsam::Rot3 ClosestTo(const Matrix M); static gtsam::Rot3 ClosestTo(const Matrix M);
// Testable // Testable
void print(string s="") const; void print(string s = "") const;
bool equals(const gtsam::Rot3& rot, double tol) const; bool equals(const gtsam::Rot3& rot, double tol) const;
// Group // Group
@ -608,7 +611,7 @@ class Pose2 {
Pose2(Vector v); Pose2(Vector v);
// Testable // Testable
void print(string s="") const; void print(string s = "") const;
bool equals(const gtsam::Pose2& pose, double tol) const; bool equals(const gtsam::Pose2& pose, double tol) const;
// Group // Group
@ -668,7 +671,7 @@ class Pose3 {
Pose3(Matrix mat); Pose3(Matrix mat);
// Testable // Testable
void print(string s="") const; void print(string s = "") const;
bool equals(const gtsam::Pose3& pose, double tol) const; bool equals(const gtsam::Pose3& pose, double tol) const;
// Group // Group
@ -744,7 +747,7 @@ class Unit3 {
Unit3(const gtsam::Point3& pose); Unit3(const gtsam::Point3& pose);
// Testable // Testable
void print(string s="") const; void print(string s = "") const;
bool equals(const gtsam::Unit3& pose, double tol) const; bool equals(const gtsam::Unit3& pose, double tol) const;
// Other functionality // Other functionality
@ -774,7 +777,7 @@ class EssentialMatrix {
EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb); EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb);
// Testable // Testable
void print(string s="") const; void print(string s = "") const;
bool equals(const gtsam::EssentialMatrix& pose, double tol) const; bool equals(const gtsam::EssentialMatrix& pose, double tol) const;
// Manifold // Manifold
@ -799,7 +802,7 @@ class Cal3_S2 {
Cal3_S2(double fov, int w, int h); Cal3_S2(double fov, int w, int h);
// Testable // Testable
void print(string s="") const; void print(string s = "Cal3_S2") const;
bool equals(const gtsam::Cal3_S2& rhs, double tol) const; bool equals(const gtsam::Cal3_S2& rhs, double tol) const;
// Manifold // Manifold
@ -836,7 +839,7 @@ virtual class Cal3DS2_Base {
Cal3DS2_Base(); Cal3DS2_Base();
// Testable // Testable
void print(string s="") const; void print(string s = "") const;
// Standard Interface // Standard Interface
double fx() const; double fx() const;
@ -922,7 +925,7 @@ class Cal3_S2Stereo {
Cal3_S2Stereo(Vector v); Cal3_S2Stereo(Vector v);
// Testable // Testable
void print(string s="") const; void print(string s = "") const;
bool equals(const gtsam::Cal3_S2Stereo& K, double tol) const; bool equals(const gtsam::Cal3_S2Stereo& K, double tol) const;
// Standard Interface // Standard Interface
@ -943,7 +946,7 @@ class Cal3Bundler {
Cal3Bundler(double fx, double k1, double k2, double u0, double v0, double tol); Cal3Bundler(double fx, double k1, double k2, double u0, double v0, double tol);
// Testable // Testable
void print(string s="") const; void print(string s = "") const;
bool equals(const gtsam::Cal3Bundler& rhs, double tol) const; bool equals(const gtsam::Cal3Bundler& rhs, double tol) const;
// Manifold // Manifold
@ -983,7 +986,7 @@ class CalibratedCamera {
static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, double height); static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, double height);
// Testable // Testable
void print(string s="") const; void print(string s = "CalibratedCamera") const;
bool equals(const gtsam::CalibratedCamera& camera, double tol) const; bool equals(const gtsam::CalibratedCamera& camera, double tol) const;
// Manifold // Manifold
@ -1022,7 +1025,7 @@ class PinholeCamera {
const gtsam::Point3& upVector, const CALIBRATION& K); const gtsam::Point3& upVector, const CALIBRATION& K);
// Testable // Testable
void print(string s="") const; void print(string s = "PinholeCamera") const;
bool equals(const This& camera, double tol) const; bool equals(const This& camera, double tol) const;
// Standard Interface // Standard Interface
@ -1097,7 +1100,7 @@ class StereoCamera {
StereoCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2Stereo* K); StereoCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2Stereo* K);
// Testable // Testable
void print(string s="") const; void print(string s = "") const;
bool equals(const gtsam::StereoCamera& camera, double tol) const; bool equals(const gtsam::StereoCamera& camera, double tol) const;
// Standard Interface // Standard Interface
@ -1160,7 +1163,9 @@ virtual class SymbolicFactor {
// From Factor // From Factor
size_t size() const; size_t size() const;
void print(string s="") const; void print(string s = "SymbolicFactor",
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::SymbolicFactor& other, double tol) const; bool equals(const gtsam::SymbolicFactor& other, double tol) const;
gtsam::KeyVector keys(); gtsam::KeyVector keys();
}; };
@ -1173,7 +1178,9 @@ virtual class SymbolicFactorGraph {
// From FactorGraph // From FactorGraph
void push_back(gtsam::SymbolicFactor* factor); void push_back(gtsam::SymbolicFactor* factor);
void print(string s="") const; void print(string s = "SymbolicFactorGraph",
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::SymbolicFactorGraph& rhs, double tol) const; bool equals(const gtsam::SymbolicFactorGraph& rhs, double tol) const;
size_t size() const; size_t size() const;
bool exists(size_t idx) const; bool exists(size_t idx) const;
@ -1223,7 +1230,8 @@ virtual class SymbolicConditional : gtsam::SymbolicFactor {
static gtsam::SymbolicConditional FromKeys(const gtsam::KeyVector& keys, size_t nrFrontals); static gtsam::SymbolicConditional FromKeys(const gtsam::KeyVector& keys, size_t nrFrontals);
// Testable // Testable
void print(string s="") const; void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::SymbolicConditional& other, double tol) const; bool equals(const gtsam::SymbolicConditional& other, double tol) const;
// Standard interface // Standard interface
@ -1236,7 +1244,9 @@ class SymbolicBayesNet {
SymbolicBayesNet(); SymbolicBayesNet();
SymbolicBayesNet(const gtsam::SymbolicBayesNet& other); SymbolicBayesNet(const gtsam::SymbolicBayesNet& other);
// Testable // Testable
void print(string s="") const; void print(string s = "SymbolicBayesNet",
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::SymbolicBayesNet& other, double tol) const; bool equals(const gtsam::SymbolicBayesNet& other, double tol) const;
// Standard interface // Standard interface
@ -1257,7 +1267,8 @@ class SymbolicBayesTree {
SymbolicBayesTree(const gtsam::SymbolicBayesTree& other); SymbolicBayesTree(const gtsam::SymbolicBayesTree& other);
// Testable // Testable
void print(string s=""); void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter);
bool equals(const gtsam::SymbolicBayesTree& other, double tol) const; bool equals(const gtsam::SymbolicBayesTree& other, double tol) const;
//Standard Interface //Standard Interface
@ -1273,30 +1284,26 @@ class SymbolicBayesTree {
gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const; gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const;
}; };
// class SymbolicBayesTreeClique { class SymbolicBayesTreeClique {
// SymbolicBayesTreeClique(); SymbolicBayesTreeClique();
// SymbolicBayesTreeClique(CONDITIONAL* conditional); // SymbolicBayesTreeClique(gtsam::sharedConditional* conditional);
// SymbolicBayesTreeClique(const pair<typename ConditionalType::shared_ptr, typename ConditionalType::FactorType::shared_ptr>& result) : Base(result) {}
// bool equals(const gtsam::SymbolicBayesTreeClique& other, double tol) const;
// bool equals(const This& other, double tol) const; void print(string s = "",
// void print(string s="") const; const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const;
// void printTree() const; // Default indent of "" size_t numCachedSeparatorMarginals() const;
// void printTree(string indent) const; // gtsam::sharedConditional* conditional() const;
// size_t numCachedSeparatorMarginals() const; bool isRoot() const;
// size_t treeSize() const;
// CONDITIONAL* conditional() const; gtsam::SymbolicBayesTreeClique* parent() const;
// bool isRoot() const;
// size_t treeSize() const;
// const std::list<derived_ptr>& children() const { return children_; }
// derived_ptr parent() const { return parent_.lock(); }
//
// // TODO: need wrapped versions graphs, BayesNet // // TODO: need wrapped versions graphs, BayesNet
// BayesNet<ConditionalType> shortcut(derived_ptr root, Eliminate function) const; // BayesNet<ConditionalType> shortcut(derived_ptr root, Eliminate function) const;
// FactorGraph<FactorType> marginal(derived_ptr root, Eliminate function) const; // FactorGraph<FactorType> marginal(derived_ptr root, Eliminate function) const;
// FactorGraph<FactorType> joint(derived_ptr C2, derived_ptr root, Eliminate function) const; // FactorGraph<FactorType> joint(derived_ptr C2, derived_ptr root, Eliminate function) const;
// //
// void deleteCachedShortcuts(); void deleteCachedShortcuts();
// }; };
#include <gtsam/inference/VariableIndex.h> #include <gtsam/inference/VariableIndex.h>
class VariableIndex { class VariableIndex {
@ -1313,7 +1320,9 @@ class VariableIndex {
// Testable // Testable
bool equals(const gtsam::VariableIndex& other, double tol) const; bool equals(const gtsam::VariableIndex& other, double tol) const;
void print(string s="") const; void print(string s = "VariableIndex: ",
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
// Standard interface // Standard interface
size_t size() const; size_t size() const;
@ -1328,7 +1337,7 @@ class VariableIndex {
namespace noiseModel { namespace noiseModel {
#include <gtsam/linear/NoiseModel.h> #include <gtsam/linear/NoiseModel.h>
virtual class Base { virtual class Base {
void print(string s="") const; void print(string s = "") const;
// Methods below are available for all noise models. However, can't add them // Methods below are available for all noise models. However, can't add them
// because wrap (incorrectly) thinks robust classes derive from this Base as well. // because wrap (incorrectly) thinks robust classes derive from this Base as well.
// bool isConstrained() const; // bool isConstrained() const;
@ -1411,7 +1420,7 @@ virtual class Unit : gtsam::noiseModel::Isotropic {
namespace mEstimator { namespace mEstimator {
virtual class Base { virtual class Base {
void print(string s="") const; void print(string s = "") const;
}; };
virtual class Null: gtsam::noiseModel::mEstimator::Base { virtual class Null: gtsam::noiseModel::mEstimator::Base {
@ -1540,7 +1549,7 @@ class Sampler {
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
class VectorValues { class VectorValues {
//Constructors //Constructors
VectorValues(); VectorValues();
VectorValues(const gtsam::VectorValues& other); VectorValues(const gtsam::VectorValues& other);
@ -1551,7 +1560,9 @@ class VectorValues {
size_t size() const; size_t size() const;
size_t dim(size_t j) const; size_t dim(size_t j) const;
bool exists(size_t j) const; bool exists(size_t j) const;
void print(string s="") const; void print(string s = "VectorValues",
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::VectorValues& expected, double tol) const; bool equals(const gtsam::VectorValues& expected, double tol) const;
void insert(size_t j, Vector value); void insert(size_t j, Vector value);
Vector vector() const; Vector vector() const;
@ -1582,7 +1593,8 @@ class VectorValues {
#include <gtsam/linear/GaussianFactor.h> #include <gtsam/linear/GaussianFactor.h>
virtual class GaussianFactor { virtual class GaussianFactor {
gtsam::KeyVector keys() const; gtsam::KeyVector keys() const;
void print(string s="") const; void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::GaussianFactor& lf, double tol) const; bool equals(const gtsam::GaussianFactor& lf, double tol) const;
double error(const gtsam::VectorValues& c) const; double error(const gtsam::VectorValues& c) const;
gtsam::GaussianFactor* clone() const; gtsam::GaussianFactor* clone() const;
@ -1610,7 +1622,8 @@ virtual class JacobianFactor : gtsam::GaussianFactor {
JacobianFactor(const gtsam::GaussianFactorGraph& graph); JacobianFactor(const gtsam::GaussianFactorGraph& graph);
//Testable //Testable
void print(string s="") const; void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
void printKeys(string s) const; void printKeys(string s) const;
bool equals(const gtsam::GaussianFactor& lf, double tol) const; bool equals(const gtsam::GaussianFactor& lf, double tol) const;
size_t size() const; size_t size() const;
@ -1659,7 +1672,8 @@ virtual class HessianFactor : gtsam::GaussianFactor {
//Testable //Testable
size_t size() const; size_t size() const;
void print(string s="") const; void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
void printKeys(string s) const; void printKeys(string s) const;
bool equals(const gtsam::GaussianFactor& lf, double tol) const; bool equals(const gtsam::GaussianFactor& lf, double tol) const;
double error(const gtsam::VectorValues& c) const; double error(const gtsam::VectorValues& c) const;
@ -1684,7 +1698,8 @@ class GaussianFactorGraph {
GaussianFactorGraph(const gtsam::GaussianBayesTree& bayesTree); GaussianFactorGraph(const gtsam::GaussianBayesTree& bayesTree);
// From FactorGraph // From FactorGraph
void print(string s="") const; void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const; bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const;
size_t size() const; size_t size() const;
gtsam::GaussianFactor* at(size_t idx) const; gtsam::GaussianFactor* at(size_t idx) const;
@ -1775,20 +1790,23 @@ virtual class GaussianConditional : gtsam::JacobianFactor {
size_t name2, Matrix T); size_t name2, Matrix T);
//Standard Interface //Standard Interface
void print(string s="") const; void print(string s = "GaussianConditional",
bool equals(const gtsam::GaussianConditional &cg, double tol) const; const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::GaussianConditional& cg, double tol) const;
//Advanced Interface // Advanced Interface
gtsam::VectorValues solve(const gtsam::VectorValues& parents) const; gtsam::VectorValues solve(const gtsam::VectorValues& parents) const;
gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues& parents, const gtsam::VectorValues& rhs) const; gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues& parents,
void solveTransposeInPlace(gtsam::VectorValues& gy) const; const gtsam::VectorValues& rhs) const;
void scaleFrontalsBySigma(gtsam::VectorValues& gy) const; void solveTransposeInPlace(gtsam::VectorValues& gy) const;
Matrix R() const; void scaleFrontalsBySigma(gtsam::VectorValues& gy) const;
Matrix S() const; Matrix R() const;
Vector d() const; Matrix S() const;
Vector d() const;
// enabling serialization functionality // enabling serialization functionality
void serialize() const; void serialize() const;
}; };
#include <gtsam/linear/GaussianDensity.h> #include <gtsam/linear/GaussianDensity.h>
@ -1797,7 +1815,9 @@ virtual class GaussianDensity : gtsam::GaussianConditional {
GaussianDensity(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); GaussianDensity(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas);
//Standard Interface //Standard Interface
void print(string s="") const; void print(string s = "GaussianDensity",
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::GaussianDensity &cg, double tol) const; bool equals(const gtsam::GaussianDensity &cg, double tol) const;
Vector mean() const; Vector mean() const;
Matrix covariance() const; Matrix covariance() const;
@ -1810,7 +1830,8 @@ virtual class GaussianBayesNet {
GaussianBayesNet(const gtsam::GaussianConditional* conditional); GaussianBayesNet(const gtsam::GaussianConditional* conditional);
// Testable // Testable
void print(string s="") const; void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::GaussianBayesNet& other, double tol) const; bool equals(const gtsam::GaussianBayesNet& other, double tol) const;
size_t size() const; size_t size() const;
@ -1845,7 +1866,8 @@ virtual class GaussianBayesTree {
GaussianBayesTree(); GaussianBayesTree();
GaussianBayesTree(const gtsam::GaussianBayesTree& other); GaussianBayesTree(const gtsam::GaussianBayesTree& other);
bool equals(const gtsam::GaussianBayesTree& other, double tol) const; bool equals(const gtsam::GaussianBayesTree& other, double tol) const;
void print(string s=""); void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter);
size_t size() const; size_t size() const;
bool empty() const; bool empty() const;
size_t numCachedSeparatorMarginals() const; size_t numCachedSeparatorMarginals() const;
@ -1871,7 +1893,7 @@ class Errors {
Errors(const gtsam::VectorValues& V); Errors(const gtsam::VectorValues& V);
//Testable //Testable
void print(string s=""); void print(string s = "Errors");
bool equals(const gtsam::Errors& expected, double tol) const; bool equals(const gtsam::Errors& expected, double tol) const;
}; };
@ -1890,7 +1912,6 @@ class GaussianISAM {
virtual class IterativeOptimizationParameters { virtual class IterativeOptimizationParameters {
string getVerbosity() const; string getVerbosity() const;
void setVerbosity(string s) ; void setVerbosity(string s) ;
void print() const;
}; };
//virtual class IterativeSolver { //virtual class IterativeSolver {
@ -1912,7 +1933,6 @@ virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParamete
void setReset(int value); void setReset(int value);
void setEpsilon_rel(double value); void setEpsilon_rel(double value);
void setEpsilon_abs(double value); void setEpsilon_abs(double value);
void print() const;
}; };
#include <gtsam/linear/Preconditioner.h> #include <gtsam/linear/Preconditioner.h>
@ -1927,14 +1947,13 @@ virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters {
#include <gtsam/linear/PCGSolver.h> #include <gtsam/linear/PCGSolver.h>
virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters { virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters {
PCGSolverParameters(); PCGSolverParameters();
void print(string s=""); void print(string s = "");
void setPreconditionerParams(gtsam::PreconditionerParameters* preconditioner); void setPreconditionerParams(gtsam::PreconditionerParameters* preconditioner);
}; };
#include <gtsam/linear/SubgraphSolver.h> #include <gtsam/linear/SubgraphSolver.h>
virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters { virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters {
SubgraphSolverParameters(); SubgraphSolverParameters();
void print() const;
}; };
virtual class SubgraphSolver { virtual class SubgraphSolver {
@ -1948,7 +1967,7 @@ class KalmanFilter {
KalmanFilter(size_t n); KalmanFilter(size_t n);
// gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0); // gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0);
gtsam::GaussianDensity* init(Vector x0, Matrix P0); gtsam::GaussianDensity* init(Vector x0, Matrix P0);
void print(string s="") const; void print(string s = "") const;
static size_t step(gtsam::GaussianDensity* p); static size_t step(gtsam::GaussianDensity* p);
gtsam::GaussianDensity* predict(gtsam::GaussianDensity* p, Matrix F, gtsam::GaussianDensity* predict(gtsam::GaussianDensity* p, Matrix F,
Matrix B, Vector u, const gtsam::noiseModel::Diagonal* modelQ); Matrix B, Vector u, const gtsam::noiseModel::Diagonal* modelQ);
@ -1974,7 +1993,7 @@ class Symbol {
Symbol(size_t key); Symbol(size_t key);
size_t key() const; size_t key() const;
void print(const string& s) const; void print(const string& s = "") const;
bool equals(const gtsam::Symbol& expected, double tol) const; bool equals(const gtsam::Symbol& expected, double tol) const;
char chr() const; char chr() const;
@ -2039,7 +2058,7 @@ class LabeledSymbol {
gtsam::LabeledSymbol newChr(unsigned char c) const; gtsam::LabeledSymbol newChr(unsigned char c) const;
gtsam::LabeledSymbol newLabel(unsigned char label) const; gtsam::LabeledSymbol newLabel(unsigned char label) const;
void print(string s="") const; void print(string s = "") const;
}; };
size_t mrsymbol(unsigned char c, unsigned char label, size_t j); size_t mrsymbol(unsigned char c, unsigned char label, size_t j);
@ -2054,7 +2073,8 @@ class Ordering {
Ordering(const gtsam::Ordering& other); Ordering(const gtsam::Ordering& other);
// Testable // Testable
void print(string s="") const; void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::Ordering& ord, double tol) const; bool equals(const gtsam::Ordering& ord, double tol) const;
// Standard interface // Standard interface
@ -2075,7 +2095,9 @@ class NonlinearFactorGraph {
NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph); NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph);
// FactorGraph // FactorGraph
void print(string s="") const; void print(string s = "NonlinearFactorGraph: ",
const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const; bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const;
size_t size() const; size_t size() const;
bool empty() const; bool empty() const;
@ -2123,7 +2145,8 @@ virtual class NonlinearFactor {
// Factor base class // Factor base class
size_t size() const; size_t size() const;
gtsam::KeyVector keys() const; gtsam::KeyVector keys() const;
void print(string s="") const; void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
void printKeys(string s) const; void printKeys(string s) const;
// NonlinearFactor // NonlinearFactor
bool equals(const gtsam::NonlinearFactor& other, double tol) const; bool equals(const gtsam::NonlinearFactor& other, double tol) const;
@ -2132,7 +2155,7 @@ virtual class NonlinearFactor {
bool active(const gtsam::Values& c) const; bool active(const gtsam::Values& c) const;
gtsam::GaussianFactor* linearize(const gtsam::Values& c) const; gtsam::GaussianFactor* linearize(const gtsam::Values& c) const;
gtsam::NonlinearFactor* clone() const; gtsam::NonlinearFactor* clone() const;
// gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const; //TODO: Conversion from KeyVector to std::vector does not happen gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const;
}; };
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
@ -2153,7 +2176,8 @@ class Values {
void clear(); void clear();
size_t dim() const; size_t dim() const;
void print(string s="") const; void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::Values& other, double tol) const; bool equals(const gtsam::Values& other, double tol) const;
void insert(const gtsam::Values& values); void insert(const gtsam::Values& values);
@ -2242,7 +2266,8 @@ class Marginals {
Marginals(const gtsam::GaussianFactorGraph& gfgraph, Marginals(const gtsam::GaussianFactorGraph& gfgraph,
const gtsam::VectorValues& solutionvec); const gtsam::VectorValues& solutionvec);
void print(string s="") const; void print(string s = "Marginals: ", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
Matrix marginalCovariance(size_t variable) const; Matrix marginalCovariance(size_t variable) const;
Matrix marginalInformation(size_t variable) const; Matrix marginalInformation(size_t variable) const;
gtsam::JointMarginal jointMarginalCovariance(const gtsam::KeyVector& variables) const; gtsam::JointMarginal jointMarginalCovariance(const gtsam::KeyVector& variables) const;
@ -2252,8 +2277,7 @@ class Marginals {
class JointMarginal { class JointMarginal {
Matrix at(size_t iVariable, size_t jVariable) const; Matrix at(size_t iVariable, size_t jVariable) const;
Matrix fullMatrix() const; Matrix fullMatrix() const;
void print(string s="") const; void print(string s = "", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const;
void print() const;
}; };
#include <gtsam/nonlinear/LinearContainerFactor.h> #include <gtsam/nonlinear/LinearContainerFactor.h>
@ -2296,7 +2320,7 @@ virtual class LinearContainerFactor : gtsam::NonlinearFactor {
#include <gtsam/nonlinear/NonlinearOptimizerParams.h> #include <gtsam/nonlinear/NonlinearOptimizerParams.h>
virtual class NonlinearOptimizerParams { virtual class NonlinearOptimizerParams {
NonlinearOptimizerParams(); NonlinearOptimizerParams();
void print(string s="") const; void print(string s = "") const;
int getMaxIterations() const; int getMaxIterations() const;
double getRelativeErrorTol() const; double getRelativeErrorTol() const;
@ -2407,14 +2431,14 @@ virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer {
LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues);
LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::LevenbergMarquardtParams& params); LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::LevenbergMarquardtParams& params);
double lambda() const; double lambda() const;
void print(string str) const; void print(string s = "") const;
}; };
#include <gtsam/nonlinear/ISAM2.h> #include <gtsam/nonlinear/ISAM2.h>
class ISAM2GaussNewtonParams { class ISAM2GaussNewtonParams {
ISAM2GaussNewtonParams(); ISAM2GaussNewtonParams();
void print(string str) const; void print(string s = "") const;
/** Getters and Setters for all properties */ /** Getters and Setters for all properties */
double getWildfireThreshold() const; double getWildfireThreshold() const;
@ -2424,7 +2448,7 @@ class ISAM2GaussNewtonParams {
class ISAM2DoglegParams { class ISAM2DoglegParams {
ISAM2DoglegParams(); ISAM2DoglegParams();
void print(string str) const; void print(string s = "") const;
/** Getters and Setters for all properties */ /** Getters and Setters for all properties */
double getWildfireThreshold() const; double getWildfireThreshold() const;
@ -2460,7 +2484,7 @@ class ISAM2ThresholdMap {
class ISAM2Params { class ISAM2Params {
ISAM2Params(); ISAM2Params();
void print(string str) const; void print(string s = "") const;
/** Getters and Setters for all properties */ /** Getters and Setters for all properties */
void setOptimizationParams(const gtsam::ISAM2GaussNewtonParams& gauss_newton__params); void setOptimizationParams(const gtsam::ISAM2GaussNewtonParams& gauss_newton__params);
@ -2490,13 +2514,13 @@ class ISAM2Clique {
//Standard Interface //Standard Interface
Vector gradientContribution() const; Vector gradientContribution() const;
void print(string s=""); void print(string s = "", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter);
}; };
class ISAM2Result { class ISAM2Result {
ISAM2Result(); ISAM2Result();
void print(string str) const; void print(string s = "") const;
/** Getters and Setters for all properties */ /** Getters and Setters for all properties */
size_t getVariablesRelinearized() const; size_t getVariablesRelinearized() const;
@ -2512,7 +2536,8 @@ class ISAM2 {
ISAM2(const gtsam::ISAM2& other); ISAM2(const gtsam::ISAM2& other);
bool equals(const gtsam::ISAM2& other, double tol) const; bool equals(const gtsam::ISAM2& other, double tol) const;
void print(string s="") const; void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
void printStats() const; void printStats() const;
void saveGraph(string s) const; void saveGraph(string s) const;
@ -2544,7 +2569,8 @@ class ISAM2 {
class NonlinearISAM { class NonlinearISAM {
NonlinearISAM(); NonlinearISAM();
NonlinearISAM(int reorderInterval); NonlinearISAM(int reorderInterval);
void print(string s="") const; void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
void printStats() const; void printStats() const;
void saveGraph(string s) const; void saveGraph(string s) const;
gtsam::Values estimate() const; gtsam::Values estimate() const;
@ -2679,7 +2705,7 @@ class BearingRange {
static This Measure(const POSE& pose, const POINT& point); static This Measure(const POSE& pose, const POINT& point);
static BEARING MeasureBearing(const POSE& pose, const POINT& point); static BEARING MeasureBearing(const POSE& pose, const POINT& point);
static RANGE MeasureRange(const POSE& pose, const POINT& point); static RANGE MeasureRange(const POSE& pose, const POINT& point);
void print(string s="") const; void print(string s = "") const;
}; };
typedef gtsam::BearingRange<gtsam::Pose2, gtsam::Point2, gtsam::Rot2, double> BearingRange2D; typedef gtsam::BearingRange<gtsam::Pose2, gtsam::Point2, gtsam::Rot2, double> BearingRange2D;
@ -2747,11 +2773,17 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
}; };
#include <gtsam/slam/SmartProjectionFactor.h> #include <gtsam/slam/SmartProjectionFactor.h>
/// Linearization mode: what factor to linearize to
enum LinearizationMode { HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD };
/// How to manage degeneracy
enum DegeneracyMode { IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY };
class SmartProjectionParams { class SmartProjectionParams {
SmartProjectionParams(); SmartProjectionParams();
// TODO(frank): make these work: void setLinearizationMode(gtsam::LinearizationMode linMode);
// void setLinearizationMode(LinearizationMode linMode); void setDegeneracyMode(gtsam::DegeneracyMode degMode);
// void setDegeneracyMode(DegeneracyMode degMode);
void setRankTolerance(double rankTol); void setRankTolerance(double rankTol);
void setEnableEPI(bool enableEPI); void setEnableEPI(bool enableEPI);
void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold); void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold);
@ -3016,7 +3048,6 @@ class ShonanAveragingParameters2 {
bool getUseHuber() const; bool getUseHuber() const;
void setCertifyOptimality(bool value); void setCertifyOptimality(bool value);
bool getCertifyOptimality() const; bool getCertifyOptimality() const;
void print() const;
}; };
class ShonanAveragingParameters3 { class ShonanAveragingParameters3 {
@ -3037,7 +3068,6 @@ class ShonanAveragingParameters3 {
bool getUseHuber() const; bool getUseHuber() const;
void setCertifyOptimality(bool value); void setCertifyOptimality(bool value);
bool getCertifyOptimality() const; bool getCertifyOptimality() const;
void print() const;
}; };
class ShonanAveraging2 { class ShonanAveraging2 {
@ -3167,7 +3197,7 @@ class ConstantBias {
ConstantBias(Vector biasAcc, Vector biasGyro); ConstantBias(Vector biasAcc, Vector biasGyro);
// Testable // Testable
void print(string s="") const; void print(string s = "") const;
bool equals(const gtsam::imuBias::ConstantBias& expected, double tol) const; bool equals(const gtsam::imuBias::ConstantBias& expected, double tol) const;
// Group // Group
@ -3207,7 +3237,7 @@ class NavState {
NavState(const gtsam::Pose3& pose, Vector v); NavState(const gtsam::Pose3& pose, Vector v);
// Testable // Testable
void print(string s="") const; void print(string s = "") const;
bool equals(const gtsam::NavState& expected, double tol) const; bool equals(const gtsam::NavState& expected, double tol) const;
// Access // Access
@ -3225,7 +3255,7 @@ virtual class PreintegratedRotationParams {
PreintegratedRotationParams(); PreintegratedRotationParams();
// Testable // Testable
void print(string s="") const; void print(string s = "") const;
bool equals(const gtsam::PreintegratedRotationParams& expected, double tol); bool equals(const gtsam::PreintegratedRotationParams& expected, double tol);
void setGyroscopeCovariance(Matrix cov); void setGyroscopeCovariance(Matrix cov);
@ -3248,7 +3278,7 @@ virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
static gtsam::PreintegrationParams* MakeSharedU(); // default g = 9.81 static gtsam::PreintegrationParams* MakeSharedU(); // default g = 9.81
// Testable // Testable
void print(string s="") const; void print(string s = "") const;
bool equals(const gtsam::PreintegrationParams& expected, double tol); bool equals(const gtsam::PreintegrationParams& expected, double tol);
void setAccelerometerCovariance(Matrix cov); void setAccelerometerCovariance(Matrix cov);
@ -3268,7 +3298,7 @@ class PreintegratedImuMeasurements {
const gtsam::imuBias::ConstantBias& bias); const gtsam::imuBias::ConstantBias& bias);
// Testable // Testable
void print(string s="") const; void print(string s = "") const;
bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol); bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol);
// Standard Interface // Standard Interface
@ -3311,7 +3341,7 @@ virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams {
static gtsam::PreintegrationCombinedParams* MakeSharedU(); // default g = 9.81 static gtsam::PreintegrationCombinedParams* MakeSharedU(); // default g = 9.81
// Testable // Testable
void print(string s="") const; void print(string s = "") const;
bool equals(const gtsam::PreintegrationCombinedParams& expected, double tol); bool equals(const gtsam::PreintegrationCombinedParams& expected, double tol);
void setBiasAccCovariance(Matrix cov); void setBiasAccCovariance(Matrix cov);
@ -3330,7 +3360,7 @@ class PreintegratedCombinedMeasurements {
PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params, PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params,
const gtsam::imuBias::ConstantBias& bias); const gtsam::imuBias::ConstantBias& bias);
// Testable // Testable
void print(string s="") const; void print(string s = "Preintegrated Measurements:") const;
bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, bool equals(const gtsam::PreintegratedCombinedMeasurements& expected,
double tol); double tol);
@ -3371,7 +3401,7 @@ class PreintegratedAhrsMeasurements {
PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements& rhs); PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements& rhs);
// Testable // Testable
void print(string s="") const; void print(string s = "Preintegrated Measurements: ") const;
bool equals(const gtsam::PreintegratedAhrsMeasurements& expected, double tol); bool equals(const gtsam::PreintegratedAhrsMeasurements& expected, double tol);
// get Data // get Data
@ -3410,7 +3440,8 @@ virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{
const gtsam::Unit3& bRef); const gtsam::Unit3& bRef);
Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model); Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model);
Rot3AttitudeFactor(); Rot3AttitudeFactor();
void print(string s="") const; void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::NonlinearFactor& expected, double tol) const; bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
gtsam::Unit3 nZ() const; gtsam::Unit3 nZ() const;
gtsam::Unit3 bRef() const; gtsam::Unit3 bRef() const;
@ -3423,7 +3454,8 @@ virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor {
Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ,
const gtsam::noiseModel::Diagonal* model); const gtsam::noiseModel::Diagonal* model);
Pose3AttitudeFactor(); Pose3AttitudeFactor();
void print(string s="") const; void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::NonlinearFactor& expected, double tol) const; bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
gtsam::Unit3 nZ() const; gtsam::Unit3 nZ() const;
gtsam::Unit3 bRef() const; gtsam::Unit3 bRef() const;
@ -3435,7 +3467,8 @@ virtual class GPSFactor : gtsam::NonlinearFactor{
const gtsam::noiseModel::Base* model); const gtsam::noiseModel::Base* model);
// Testable // Testable
void print(string s="") const; void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::GPSFactor& expected, double tol); bool equals(const gtsam::GPSFactor& expected, double tol);
// Standard Interface // Standard Interface
@ -3447,7 +3480,8 @@ virtual class GPSFactor2 : gtsam::NonlinearFactor {
const gtsam::noiseModel::Base* model); const gtsam::noiseModel::Base* model);
// Testable // Testable
void print(string s="") const; void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::GPSFactor2& expected, double tol); bool equals(const gtsam::GPSFactor2& expected, double tol);
// Standard Interface // Standard Interface

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

@ -26,30 +26,30 @@
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class CONDITIONAL> template <class CONDITIONAL>
void BayesNet<CONDITIONAL>::print(const std::string& s, const KeyFormatter& formatter) const void BayesNet<CONDITIONAL>::print(
{ const std::string& s, const KeyFormatter& formatter) const {
Base::print(s, formatter); Base::print(s, formatter);
}
/* ************************************************************************* */
template<class CONDITIONAL>
void BayesNet<CONDITIONAL>::saveGraph(const std::string &s, const KeyFormatter& keyFormatter) const
{
std::ofstream of(s.c_str());
of << "digraph G{\n";
for (auto conditional: boost::adaptors::reverse(*this)) {
typename CONDITIONAL::Frontals frontals = conditional->frontals();
Key me = frontals.front();
typename CONDITIONAL::Parents parents = conditional->parents();
for(Key p: parents)
of << keyFormatter(p) << "->" << keyFormatter(me) << std::endl;
}
of << "}";
of.close();
}
} }
/* ************************************************************************* */
template <class CONDITIONAL>
void BayesNet<CONDITIONAL>::saveGraph(const std::string& s,
const KeyFormatter& keyFormatter) const {
std::ofstream of(s.c_str());
of << "digraph G{\n";
for (auto conditional : boost::adaptors::reverse(*this)) {
typename CONDITIONAL::Frontals frontals = conditional->frontals();
Key me = frontals.front();
typename CONDITIONAL::Parents parents = conditional->parents();
for (Key p : parents)
of << keyFormatter(p) << "->" << keyFormatter(me) << std::endl;
}
of << "}";
of.close();
}
} // namespace gtsam

16
gtsam/inference/BayesNet.h
View File

@ -57,16 +57,18 @@ namespace gtsam {
/// @name Testable /// @name Testable
/// @{ /// @{
/** print out graph */ /** print out graph */
void print(const std::string& s = "BayesNet", void print(
const KeyFormatter& formatter = DefaultKeyFormatter) const; const std::string& s = "BayesNet",
const KeyFormatter& formatter = DefaultKeyFormatter) const override;
/// @} /// @}
/// @name Standard Interface /// @name Standard Interface
/// @{ /// @{
void saveGraph(const std::string &s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; void saveGraph(const std::string& s,
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
}; };
} }

4
gtsam/inference/Factor.cpp
View File

@ -33,8 +33,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
void Factor::printKeys(const std::string& s, const KeyFormatter& formatter) const { void Factor::printKeys(const std::string& s, const KeyFormatter& formatter) const {
std::cout << s << " "; std::cout << (s.empty() ? "" : s + " ");
for(Key key: keys_) std::cout << " " << formatter(key); for (Key key : keys_) std::cout << " " << formatter(key);
std::cout << std::endl; std::cout << std::endl;
} }

57
gtsam/inference/Factor.h
View File

@ -102,43 +102,52 @@ typedef FastSet<FactorIndex> FactorIndexSet;
/// @} /// @}
public: public:
/// @name Standard Interface /// Default destructor
/// @{ // public since it is required for boost serialization and static methods.
// virtual since it is public.
// http://isocpp.github.io/CppCoreGuidelines/CppCoreGuidelines#Rc-dtor-virtual
virtual ~Factor() = default;
/// First key /// @name Standard Interface
Key front() const { return keys_.front(); } /// @{
/// Last key /// First key
Key back() const { return keys_.back(); } Key front() const { return keys_.front(); }
/// find /// Last key
const_iterator find(Key key) const { return std::find(begin(), end(), key); } Key back() const { return keys_.back(); }
/// Access the factor's involved variable keys /// find
const KeyVector& keys() const { return keys_; } const_iterator find(Key key) const { return std::find(begin(), end(), key); }
/** Iterator at beginning of involved variable keys */ /// Access the factor's involved variable keys
const_iterator begin() const { return keys_.begin(); } const KeyVector& keys() const { return keys_; }
/** Iterator at end of involved variable keys */ /** Iterator at beginning of involved variable keys */
const_iterator end() const { return keys_.end(); } const_iterator begin() const { return keys_.begin(); }
/** /** Iterator at end of involved variable keys */
const_iterator end() const { return keys_.end(); }
/**
* @return the number of variables involved in this factor * @return the number of variables involved in this factor
*/ */
size_t size() const { return keys_.size(); } size_t size() const { return keys_.size(); }
/// @} /// @}
/// @name Testable
/// @{
/// @name Testable /// print
/// @{ virtual void print(
const std::string& s = "Factor",
const KeyFormatter& formatter = DefaultKeyFormatter) const;
/// print /// print only keys
void print(const std::string& s = "Factor", const KeyFormatter& formatter = DefaultKeyFormatter) const; virtual void printKeys(
const std::string& s = "Factor",
/// print only keys const KeyFormatter& formatter = DefaultKeyFormatter) const;
void printKeys(const std::string& s = "Factor", const KeyFormatter& formatter = DefaultKeyFormatter) const;
protected: protected:
/// check equality /// check equality

2
gtsam/inference/FactorGraph-inst.h
View File

@ -37,7 +37,7 @@ namespace gtsam {
template <class FACTOR> template <class FACTOR>
void FactorGraph<FACTOR>::print(const std::string& s, void FactorGraph<FACTOR>::print(const std::string& s,
const KeyFormatter& formatter) const { const KeyFormatter& formatter) const {
std::cout << s << std::endl; std::cout << (s.empty() ? "" : s + " ") << std::endl;
std::cout << "size: " << size() << std::endl; std::cout << "size: " << size() << std::endl;
for (size_t i = 0; i < factors_.size(); i++) { for (size_t i = 0; i < factors_.size(); i++) {
std::stringstream ss; std::stringstream ss;

12
gtsam/inference/FactorGraph.h
View File

@ -148,6 +148,10 @@ class FactorGraph {
/// @} /// @}
public: public:
/// Default destructor
// Public and virtual so boost serialization can call it.
virtual ~FactorGraph() = default;
/// @name Adding Single Factors /// @name Adding Single Factors
/// @{ /// @{
@ -285,9 +289,9 @@ class FactorGraph {
/// @name Testable /// @name Testable
/// @{ /// @{
/** print out graph */ /// print out graph
void print(const std::string& s = "FactorGraph", virtual void print(const std::string& s = "FactorGraph",
const KeyFormatter& formatter = DefaultKeyFormatter) const; const KeyFormatter& formatter = DefaultKeyFormatter) const;
/** Check equality */ /** Check equality */
bool equals(const This& fg, double tol = 1e-9) const; bool equals(const This& fg, double tol = 1e-9) const;
@ -355,7 +359,7 @@ class FactorGraph {
/** delete factor without re-arranging indexes by inserting a nullptr pointer /** delete factor without re-arranging indexes by inserting a nullptr pointer
*/ */
void remove(size_t i) { factors_[i].reset(); } void remove(size_t i) { factors_.at(i).reset(); }
/** replace a factor by index */ /** replace a factor by index */
void replace(size_t index, sharedFactor factor) { at(index) = factor; } void replace(size_t index, sharedFactor factor) { at(index) = factor; }

10
gtsam/linear/GaussianBayesNet.h
View File

@ -55,6 +55,9 @@ namespace gtsam {
template<class DERIVEDCONDITIONAL> template<class DERIVEDCONDITIONAL>
GaussianBayesNet(const FactorGraph<DERIVEDCONDITIONAL>& graph) : Base(graph) {} GaussianBayesNet(const FactorGraph<DERIVEDCONDITIONAL>& graph) : Base(graph) {}
/// Destructor
virtual ~GaussianBayesNet() {}
/// @} /// @}
/// @name Testable /// @name Testable
@ -177,6 +180,13 @@ namespace gtsam {
*/ */
VectorValues backSubstituteTranspose(const VectorValues& gx) const; VectorValues backSubstituteTranspose(const VectorValues& gx) const;
/// print graph
void print(
const std::string& s = "",
const KeyFormatter& formatter = DefaultKeyFormatter) const override {
Base::print(s, formatter);
}
/** /**
* @brief Save the GaussianBayesNet as an image. Requires `dot` to be * @brief Save the GaussianBayesNet as an image. Requires `dot` to be
* installed. * installed.

7
gtsam/linear/GaussianFactor.h
View File

@ -54,8 +54,11 @@ namespace gtsam {
virtual ~GaussianFactor() {} virtual ~GaussianFactor() {}
// Implementing Testable interface // Implementing Testable interface
virtual void print(const std::string& s = "",
const KeyFormatter& formatter = DefaultKeyFormatter) const = 0; /// print
void print(
const std::string& s = "",
const KeyFormatter& formatter = DefaultKeyFormatter) const override = 0;
/** Equals for testable */ /** Equals for testable */
virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const = 0; virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const = 0;

4
gtsam/linear/Preconditioner.h
View File

@ -44,9 +44,9 @@ struct GTSAM_EXPORT PreconditionerParameters {
inline Kernel kernel() const { return kernel_; } inline Kernel kernel() const { return kernel_; }
inline Verbosity verbosity() const { return verbosity_; } inline Verbosity verbosity() const { return verbosity_; }
void print() const ; void print() const;
virtual void print(std::ostream &os) const ; virtual void print(std::ostream &os) const;
static Kernel kernelTranslator(const std::string &s); static Kernel kernelTranslator(const std::string &s);
static Verbosity verbosityTranslator(const std::string &s); static Verbosity verbosityTranslator(const std::string &s);

3
gtsam/navigation/AttitudeFactor.cpp
View File

@ -42,7 +42,8 @@ Vector AttitudeFactor::attitudeError(const Rot3& nRb,
//*************************************************************************** //***************************************************************************
void Rot3AttitudeFactor::print(const string& s, void Rot3AttitudeFactor::print(const string& s,
const KeyFormatter& keyFormatter) const { const KeyFormatter& keyFormatter) const {
cout << s << "Rot3AttitudeFactor on " << keyFormatter(this->key()) << "\n"; cout << (s.empty() ? "" : s + " ") << "Rot3AttitudeFactor on "
<< keyFormatter(this->key()) << "\n";
nZ_.print(" measured direction in nav frame: "); nZ_.print(" measured direction in nav frame: ");
bRef_.print(" reference direction in body frame: "); bRef_.print(" reference direction in body frame: ");
this->noiseModel_->print(" noise model: "); this->noiseModel_->print(" noise model: ");

8
gtsam/navigation/AttitudeFactor.h
View File

@ -114,8 +114,8 @@ public:
} }
/** print */ /** print */
void print(const std::string& s, const KeyFormatter& keyFormatter = void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override; DefaultKeyFormatter) const override;
/** equals */ /** equals */
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
@ -188,8 +188,8 @@ public:
} }
/** print */ /** print */
void print(const std::string& s, const KeyFormatter& keyFormatter = void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override; DefaultKeyFormatter) const override;
/** equals */ /** equals */
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;

3
gtsam/navigation/GPSFactor.cpp
View File

@ -24,7 +24,8 @@ namespace gtsam {
//*************************************************************************** //***************************************************************************
void GPSFactor::print(const string& s, const KeyFormatter& keyFormatter) const { void GPSFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
cout << s << "GPSFactor on " << keyFormatter(key()) << "\n"; cout << (s.empty() ? "" : s + " ") << "GPSFactor on " << keyFormatter(key())
<< "\n";
cout << " GPS measurement: " << nT_ << "\n"; cout << " GPS measurement: " << nT_ << "\n";
noiseModel_->print(" noise model: "); noiseModel_->print(" noise model: ");
} }

8
gtsam/navigation/GPSFactor.h
View File

@ -71,8 +71,8 @@ public:
} }
/// print /// print
void print(const std::string& s, const KeyFormatter& keyFormatter = void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override; DefaultKeyFormatter) const override;
/// equals /// equals
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
@ -143,8 +143,8 @@ public:
} }
/// print /// print
void print(const std::string& s, const KeyFormatter& keyFormatter = void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override; DefaultKeyFormatter) const override;
/// equals /// equals
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;

4
gtsam/nonlinear/NonlinearFactor.h
View File

@ -70,8 +70,8 @@ public:
/// @{ /// @{
/** print */ /** print */
virtual void print(const std::string& s = "", void print(const std::string& s = "", const KeyFormatter& keyFormatter =
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; DefaultKeyFormatter) const override;
/** Check if two factors are equal */ /** Check if two factors are equal */
virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const; virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const;

8
gtsam/nonlinear/NonlinearFactorGraph.h
View File

@ -98,9 +98,13 @@ namespace gtsam {
template<class DERIVEDFACTOR> template<class DERIVEDFACTOR>
NonlinearFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {} NonlinearFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
/// Destructor
virtual ~NonlinearFactorGraph() {}
/** print */ /** print */
void print(const std::string& str = "NonlinearFactorGraph: ", void print(
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; const std::string& str = "NonlinearFactorGraph: ",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
/** print errors along with factors*/ /** print errors along with factors*/
void printErrors(const Values& values, const std::string& str = "NonlinearFactorGraph: ", void printErrors(const Values& values, const std::string& str = "NonlinearFactorGraph: ",

7
gtsam/sfm/BinaryMeasurement.h
View File

@ -52,6 +52,9 @@ private:
measured_(measured), measured_(measured),
noiseModel_(model) {} noiseModel_(model) {}
/// Destructor
virtual ~BinaryMeasurement() {}
/// @name Standard Interface /// @name Standard Interface
/// @{ /// @{
@ -64,8 +67,8 @@ private:
/// @name Testable /// @name Testable
/// @{ /// @{
void print(const std::string &s, void print(const std::string &s, const KeyFormatter &keyFormatter =
const KeyFormatter &keyFormatter = DefaultKeyFormatter) const { DefaultKeyFormatter) const override {
std::cout << s << "BinaryMeasurement(" << keyFormatter(this->key1()) << "," std::cout << s << "BinaryMeasurement(" << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ")\n"; << keyFormatter(this->key2()) << ")\n";
traits<T>::Print(measured_, " measured: "); traits<T>::Print(measured_, " measured: ");

10
gtsam/symbolic/SymbolicBayesNet.h
View File

@ -55,6 +55,9 @@ namespace gtsam {
template<class DERIVEDCONDITIONAL> template<class DERIVEDCONDITIONAL>
SymbolicBayesNet(const FactorGraph<DERIVEDCONDITIONAL>& graph) : Base(graph) {} SymbolicBayesNet(const FactorGraph<DERIVEDCONDITIONAL>& graph) : Base(graph) {}
/// Destructor
virtual ~SymbolicBayesNet() {}
/// @} /// @}
/// @name Testable /// @name Testable
@ -63,6 +66,13 @@ namespace gtsam {
/** Check equality */ /** Check equality */
GTSAM_EXPORT bool equals(const This& bn, double tol = 1e-9) const; GTSAM_EXPORT bool equals(const This& bn, double tol = 1e-9) const;
/// print
GTSAM_EXPORT void print(
const std::string& s = "SymbolicBayesNet",
const KeyFormatter& formatter = DefaultKeyFormatter) const override {
Base::print(s, formatter);
}
/// @} /// @}
/// @name Standard Interface /// @name Standard Interface

25
gtsam/symbolic/SymbolicConditional.cpp
View File

@ -20,18 +20,17 @@
namespace gtsam { namespace gtsam {
using namespace std; using namespace std;
/* ************************************************************************* */
void SymbolicConditional::print(const std::string& str, const KeyFormatter& keyFormatter) const
{
BaseConditional::print(str, keyFormatter);
}
/* ************************************************************************* */
bool SymbolicConditional::equals(const This& c, double tol) const
{
return BaseFactor::equals(c) && BaseConditional::equals(c);
}
/* ************************************************************************* */
void SymbolicConditional::print(const std::string& str,
const KeyFormatter& keyFormatter) const {
BaseConditional::print(str, keyFormatter);
} }
/* ************************************************************************* */
bool SymbolicConditional::equals(const This& c, double tol) const {
return BaseFactor::equals(c) && BaseConditional::equals(c);
}
} // namespace gtsam

4
gtsam/symbolic/SymbolicConditional.h
View File

@ -105,7 +105,9 @@ namespace gtsam {
/// @name Testable /// @name Testable
/** Print with optional formatter */ /** Print with optional formatter */
virtual void print(const std::string& str = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; void print(
const std::string& str = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
/** Check equality */ /** Check equality */
bool equals(const This& c, double tol = 1e-9) const; bool equals(const This& c, double tol = 1e-9) const;

14
gtsam/symbolic/SymbolicFactor.h
View File

@ -92,6 +92,20 @@ namespace gtsam {
bool equals(const This& other, double tol = 1e-9) const; bool equals(const This& other, double tol = 1e-9) const;
/// print
void print(
const std::string& s = "SymbolicFactor",
const KeyFormatter& formatter = DefaultKeyFormatter) const override {
Base::print(s, formatter);
}
/// print only keys
void printKeys(
const std::string& s = "SymbolicFactor",
const KeyFormatter& formatter = DefaultKeyFormatter) const override {
Base::printKeys(s, formatter);
}
/// @} /// @}
/// @name Advanced Constructors /// @name Advanced Constructors

10
gtsam/symbolic/SymbolicFactorGraph.h
View File

@ -81,6 +81,9 @@ namespace gtsam {
template<class DERIVEDFACTOR> template<class DERIVEDFACTOR>
SymbolicFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {} SymbolicFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
/// Destructor
virtual ~SymbolicFactorGraph() {}
/// @} /// @}
/// @name Testable /// @name Testable
@ -88,6 +91,13 @@ namespace gtsam {
bool equals(const This& fg, double tol = 1e-9) const; bool equals(const This& fg, double tol = 1e-9) const;
/// print
void print(
const std::string& s = "SymbolicFactorGraph",
const KeyFormatter& formatter = DefaultKeyFormatter) const override {
Base::print(s, formatter);
}
/// @} /// @}
/// @name Standard Interface /// @name Standard Interface

5
gtsam_unstable/discrete/Scheduler.cpp
View File

@ -178,8 +178,7 @@ namespace gtsam {
} // buildGraph } // buildGraph
/** print */ /** print */
void Scheduler::print(const string& s) const { void Scheduler::print(const string& s, const KeyFormatter& formatter) const {
cout << s << " Faculty:" << endl; cout << s << " Faculty:" << endl;
for(const string& name: facultyName_) for(const string& name: facultyName_)
cout << name << '\n'; cout << name << '\n';
@ -210,7 +209,7 @@ namespace gtsam {
CSP::print(s + " Factor graph"); CSP::print(s + " Factor graph");
cout << endl; cout << endl;
} // print } // print
/** Print readable form of assignment */ /** Print readable form of assignment */
void Scheduler::printAssignment(sharedValues assignment) const { void Scheduler::printAssignment(sharedValues assignment) const {

18
gtsam_unstable/discrete/Scheduler.h
View File

@ -66,15 +66,17 @@ namespace gtsam {
/** /**
* Constructor * Constructor
* WE need to know the number of students in advance for ordering keys. * We need to know the number of students in advance for ordering keys.
* then add faculty, slots, areas, availability, students, in that order * then add faculty, slots, areas, availability, students, in that order
*/ */
Scheduler(size_t maxNrStudents):maxNrStudents_(maxNrStudents) { Scheduler(size_t maxNrStudents) : maxNrStudents_(maxNrStudents) {}
}
void addFaculty(const std::string& facultyName) { /// Destructor
facultyIndex_[facultyName] = nrFaculty(); virtual ~Scheduler() {}
facultyName_.push_back(facultyName);
void addFaculty(const std::string& facultyName) {
facultyIndex_[facultyName] = nrFaculty();
facultyName_.push_back(facultyName);
} }
size_t nrFaculty() const { size_t nrFaculty() const {
@ -140,7 +142,9 @@ namespace gtsam {
void buildGraph(size_t mutexBound = 7); void buildGraph(size_t mutexBound = 7);
/** print */ /** print */
void print(const std::string& s = "Scheduler") const; void print(
const std::string& s = "Scheduler",
const KeyFormatter& formatter = DefaultKeyFormatter) const override;
/** Print readable form of assignment */ /** Print readable form of assignment */
void printAssignment(sharedValues assignment) const; void printAssignment(sharedValues assignment) const;

5
gtsam_unstable/linear/InequalityFactorGraph.h
View File

@ -37,8 +37,9 @@ public:
typedef boost::shared_ptr<InequalityFactorGraph> shared_ptr; typedef boost::shared_ptr<InequalityFactorGraph> shared_ptr;
/** print */ /** print */
void print(const std::string& str, const KeyFormatter& keyFormatter = void print(
DefaultKeyFormatter) const { const std::string& str = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
Base::print(str, keyFormatter); Base::print(str, keyFormatter);
} }

8
gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h
View File

@ -47,7 +47,9 @@ public:
virtual ~ConcurrentFilter() {}; virtual ~ConcurrentFilter() {};
/** Implement a standard 'print' function */ /** Implement a standard 'print' function */
virtual void print(const std::string& s = "Concurrent Filter:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const = 0; virtual void print(
const std::string& s = "Concurrent Filter:\n",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const = 0;
/** Check if two Concurrent Smoothers are equal */ /** Check if two Concurrent Smoothers are equal */
virtual bool equals(const ConcurrentFilter& rhs, double tol = 1e-9) const = 0; virtual bool equals(const ConcurrentFilter& rhs, double tol = 1e-9) const = 0;
@ -107,7 +109,9 @@ public:
virtual ~ConcurrentSmoother() {}; virtual ~ConcurrentSmoother() {};
/** Implement a standard 'print' function */ /** Implement a standard 'print' function */
virtual void print(const std::string& s = "Concurrent Smoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const = 0; virtual void print(
const std::string& s = "Concurrent Smoother:\n",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const = 0;
/** Check if two Concurrent Smoothers are equal */ /** Check if two Concurrent Smoothers are equal */
virtual bool equals(const ConcurrentSmoother& rhs, double tol = 1e-9) const = 0; virtual bool equals(const ConcurrentSmoother& rhs, double tol = 1e-9) const = 0;

4
gtsam_unstable/nonlinear/FixedLagSmoother.h
View File

@ -69,7 +69,9 @@ public:
virtual ~FixedLagSmoother() { } virtual ~FixedLagSmoother() { }
/** Print the factor for debugging and testing (implementing Testable) */ /** Print the factor for debugging and testing (implementing Testable) */
virtual void print(const std::string& s = "FixedLagSmoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; virtual void print(
const std::string& s = "FixedLagSmoother:\n",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/** Check if two IncrementalFixedLagSmoother Objects are equal */ /** Check if two IncrementalFixedLagSmoother Objects are equal */
virtual bool equals(const FixedLagSmoother& rhs, double tol = 1e-9) const; virtual bool equals(const FixedLagSmoother& rhs, double tol = 1e-9) const;

4
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h
View File

@ -132,7 +132,9 @@ public:
/** implement functions needed for Testable */ /** implement functions needed for Testable */
/** print */ /** print */
virtual void print(const std::string& s = "EquivInertialNavFactor_GlobalVel_NoBias", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { virtual void print(
const std::string& s = "EquivInertialNavFactor_GlobalVel_NoBias",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "(" std::cout << s << "("
<< keyFormatter(this->key1()) << "," << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << "," << keyFormatter(this->key2()) << ","

2
python/gtsam/gtsam.tpl
View File

@ -13,6 +13,8 @@
#include <pybind11/eigen.h> #include <pybind11/eigen.h>
#include <pybind11/stl_bind.h> #include <pybind11/stl_bind.h>
#include <pybind11/pybind11.h> #include <pybind11/pybind11.h>
#include <pybind11/operators.h>
#include <pybind11/functional.h>
#include <pybind11/iostream.h> #include <pybind11/iostream.h>
#include "gtsam/config.h" #include "gtsam/config.h"
#include "gtsam/base/serialization.h" #include "gtsam/base/serialization.h"

1
python/gtsam_unstable/gtsam_unstable.tpl
View File

@ -13,6 +13,7 @@
#include <pybind11/eigen.h> #include <pybind11/eigen.h>
#include <pybind11/stl_bind.h> #include <pybind11/stl_bind.h>
#include <pybind11/pybind11.h> #include <pybind11/pybind11.h>
#include <pybind11/functional.h>
#include <pybind11/iostream.h> #include <pybind11/iostream.h>
#include "gtsam/base/serialization.h" #include "gtsam/base/serialization.h"
#include "gtsam/nonlinear/utilities.h" // for RedirectCout. #include "gtsam/nonlinear/utilities.h" // for RedirectCout.