add KeyGroupMap to wrap ISAM2 update with group ordering to matlab. Wrap at() in SymbolicBayesNet, and GaussianConditional constructors with no noise model.

release/4.3a0
Duy-Nguyen Ta 2013-12-04 14:36:13 +00:00
parent 737e3806bb
commit 72a1feca2c
3 changed files with 31 additions and 2 deletions

29
gtsam.h
View File

@ -896,6 +896,7 @@ class SymbolicBayesNet {
// Standard interface
size_t size() const;
void saveGraph(string s) const;
gtsam::SymbolicConditional* at(size_t idx) const;
gtsam::SymbolicConditional* front() const;
gtsam::SymbolicConditional* back() const;
void push_back(gtsam::SymbolicConditional* conditional);
@ -947,7 +948,7 @@ class SymbolicBayesTree {
// // BayesNet<ConditionalType> shortcut(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;
//
//
// void deleteCachedShortcuts();
// };
@ -1317,13 +1318,19 @@ class GaussianFactorGraph {
#include <gtsam/linear/GaussianConditional.h>
virtual class GaussianConditional : gtsam::GaussianFactor {
//Constructors
//Constructors
GaussianConditional(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas);
GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S,
const gtsam::noiseModel::Diagonal* sigmas);
GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S,
size_t name2, Matrix T, const gtsam::noiseModel::Diagonal* sigmas);
//Constructors with no noise model
GaussianConditional(size_t key, Vector d, Matrix R);
GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S);
GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S,
size_t name2, Matrix T);
//Standard Interface
void print(string s) const;
bool equals(const gtsam::GaussianConditional &cg, double tol) const;
@ -1704,6 +1711,23 @@ class KeyVector {
void serialize() const;
};
// Actually a FastMap<Key,int>
class KeyGroupMap {
KeyGroupMap();
// Note: no print function
// common STL methods
size_t size() const;
bool empty() const;
void clear();
// structure specific methods
size_t at(size_t key) const;
int erase(size_t key);
bool insert2(size_t key, int val);
};
#include <gtsam/nonlinear/Marginals.h>
class Marginals {
Marginals(const gtsam::NonlinearFactorGraph& graph,
@ -1959,6 +1983,7 @@ class ISAM2 {
gtsam::ISAM2Result update();
gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta);
gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices);
gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, const gtsam::KeyGroupMap& constrainedKeys);
// TODO: wrap the full version of update
//void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap<Key,int>& constrainedKeys);
//void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap<Key,int>& constrainedKeys, bool force_relinearize);

View File

@ -60,6 +60,8 @@ public:
return std::map<KEY,VALUE>(this->begin(), this->end());
}
/** Handy 'insert' function for Matlab wrapper */
bool insert2(const KEY& key, const VALUE& val) { return Base::insert(std::make_pair<KEY,VALUE>(key, val)).second; }
/** Handy 'exists' function */
bool exists(const KEY& e) const { return this->find(e) != this->end(); }

View File

@ -24,6 +24,7 @@
#include <gtsam/base/FastVector.h>
#include <gtsam/base/FastList.h>
#include <gtsam/base/FastSet.h>
#include <gtsam/base/FastMap.h>
namespace gtsam {
@ -43,6 +44,7 @@ namespace gtsam {
typedef FastList<Key> KeyList;
typedef FastVector<Key> KeyVector;
typedef FastSet<Key> KeySet;
typedef FastMap<Key,int> KeyGroupMap;
/// Utility function to print sets of keys with optional prefix
GTSAM_EXPORT void printKeySet(const KeySet& keys, const std::string& s = "",