diff --git a/.cproject b/.cproject
index b09810484..f5675f5ee 100644
--- a/.cproject
+++ b/.cproject
@@ -2192,22 +2192,6 @@
true
true
-
- make
- -j5
- wrap_gtsam_clean
- true
- true
- true
-
-
- make
- -j5
- wrap_gtsam_unstable_clean
- true
- true
- true
-
make
-j5
diff --git a/gtsam.h b/gtsam.h
index 8f13ba39c..d15ef08d8 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -1063,15 +1063,25 @@ class InvertedOrdering {
class NonlinearFactorGraph {
NonlinearFactorGraph();
- void print(string s) const;
- size_t size() const;
- double error(const gtsam::Values& c) const;
- double probPrime(const gtsam::Values& c) const;
- gtsam::NonlinearFactor* at(size_t i) const;
+ NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph);
+
+ // FactorGraph
+ void print(string s) const;
+ bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const;
+ size_t size() const;
+ bool empty() const;
+ void remove(size_t i);
+ size_t nrFactors() const;
void add(const gtsam::NonlinearFactor* factor);
- gtsam::Ordering* orderingCOLAMD(const gtsam::Values& c) const;
+ gtsam::NonlinearFactor* at(size_t i) const;
+
+ // NonlinearFactorGraph
+ double error(const gtsam::Values& values) const;
+ double probPrime(const gtsam::Values& values) const;
+ gtsam::Ordering* orderingCOLAMD(const gtsam::Values& values) const;
// Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map& constraints) const;
- gtsam::GaussianFactorGraph* linearize(const gtsam::Values& c, const gtsam::Ordering& ordering) const;
+ gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values,
+ const gtsam::Ordering& ordering) const;
gtsam::NonlinearFactorGraph clone() const;
};
@@ -1382,6 +1392,29 @@ class ISAM2 {
gtsam::ISAM2Params params() const;
};
+#include
+class NonlinearISAM {
+ NonlinearISAM();
+ NonlinearISAM(int reorderInterval);
+ void print(string s) const;
+ void printStats() const;
+ void saveGraph(string s) const;
+ gtsam::Values estimate() const;
+ Matrix marginalCovariance(size_t key) const;
+ int reorderInterval() const;
+ int reorderCounter() const;
+ void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& initialValues);
+ void reorder_relinearize();
+ void addKey(size_t key);
+ void setOrdering(const gtsam::Ordering& new_ordering);
+
+ // These might be expensive as instead of a reference the wrapper will make a copy
+ gtsam::GaussianISAM bayesTree() const;
+ gtsam::Values getLinearizationPoint() const;
+ gtsam::Ordering getOrdering() const;
+ gtsam::NonlinearFactorGraph getFactorsUnsafe() const;
+};
+
//*************************************************************************
// Nonlinear factor types
//*************************************************************************
diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h
index 60bd833ff..1bb6d20e0 100644
--- a/gtsam_unstable/gtsam_unstable.h
+++ b/gtsam_unstable/gtsam_unstable.h
@@ -172,15 +172,21 @@ virtual class DGroundConstraint : gtsam::NonlinearFactor {
#include
#include
-template
+template
virtual class PoseTranslationPrior : gtsam::NonlinearFactor {
PoseTranslationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel);
};
+typedef gtsam::PoseTranslationPrior PoseTranslationPrior2D;
+typedef gtsam::PoseTranslationPrior PoseTranslationPrior3D;
+
#include
-template
+template
virtual class PoseRotationPrior : gtsam::NonlinearFactor {
PoseRotationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel);
};
+typedef gtsam::PoseRotationPrior PoseRotationPrior2D;
+typedef gtsam::PoseRotationPrior PoseRotationPrior3D;
+
} //\namespace gtsam