diff --git a/cpp/BetweenFactor.h b/cpp/BetweenFactor.h index 2358e6073..b9b7157b6 100644 --- a/cpp/BetweenFactor.h +++ b/cpp/BetweenFactor.h @@ -33,7 +33,7 @@ namespace gtsam { /** Constructor */ BetweenFactor(const Key& key1, const Key& key2, const T& measured, - const sharedGaussian& model) : + const SharedGaussian& model) : Base(model, key1, key2), measured_(measured) { } diff --git a/cpp/GaussianFactor.cpp b/cpp/GaussianFactor.cpp index 89bb03d65..dfa0b6068 100644 --- a/cpp/GaussianFactor.cpp +++ b/cpp/GaussianFactor.cpp @@ -323,7 +323,7 @@ GaussianFactor::eliminate(const Symbol& key) const Matrix Ab = matrix_augmented(ordering,false); // Use in-place QR on dense Ab appropriate to NoiseModel - sharedDiagonal noiseModel = model_->QR(Ab); + SharedDiagonal noiseModel = model_->QR(Ab); // get dimensions of the eliminated variable // TODO: this is another map find that should be avoided ! diff --git a/cpp/GaussianFactor.h b/cpp/GaussianFactor.h index a3dc787c2..669034916 100644 --- a/cpp/GaussianFactor.h +++ b/cpp/GaussianFactor.h @@ -18,7 +18,7 @@ #include "Factor.h" #include "Matrix.h" #include "VectorConfig.h" -#include "NoiseModel.h" +#include "SharedDiagonal.h" #include "SymbolMap.h" namespace gtsam { @@ -40,7 +40,7 @@ public: protected: - sharedDiagonal model_; // Gaussian noise model with diagonal covariance matrix + SharedDiagonal model_; // Gaussian noise model with diagonal covariance matrix SymbolMap As_; // linear matrices Vector b_; // right-hand-side @@ -57,7 +57,7 @@ public: /** Construct unary factor */ GaussianFactor(const Symbol& key1, const Matrix& A1, - const Vector& b, const sharedDiagonal& model) : + const Vector& b, const SharedDiagonal& model) : b_(b), model_(model) { As_.insert(make_pair(key1, A1)); } @@ -65,7 +65,7 @@ public: /** Construct binary factor */ GaussianFactor(const Symbol& key1, const Matrix& A1, const Symbol& key2, const Matrix& A2, - const Vector& b, const sharedDiagonal& model) : + const Vector& b, const SharedDiagonal& model) : b_(b), model_(model) { As_.insert(make_pair(key1, A1)); As_.insert(make_pair(key2, A2)); @@ -75,7 +75,7 @@ public: GaussianFactor(const Symbol& key1, const Matrix& A1, const Symbol& key2, const Matrix& A2, const Symbol& key3, const Matrix& A3, - const Vector& b, const sharedDiagonal& model) : + const Vector& b, const SharedDiagonal& model) : b_(b), model_(model) { As_.insert(make_pair(key1, A1)); As_.insert(make_pair(key2, A2)); @@ -84,7 +84,7 @@ public: /** Construct an n-ary factor */ GaussianFactor(const std::vector > &terms, - const Vector &b, const sharedDiagonal& model) : + const Vector &b, const SharedDiagonal& model) : b_(b), model_(model) { for(unsigned int i=0; isigmas(); } /** get a copy of model */ - const sharedDiagonal& get_model() const { return model_; } + const SharedDiagonal& get_model() const { return model_; } /** * get a copy of the A matrix from a specific node diff --git a/cpp/GaussianFactorGraph.cpp b/cpp/GaussianFactorGraph.cpp index 611589405..3c5e7b96f 100644 --- a/cpp/GaussianFactorGraph.cpp +++ b/cpp/GaussianFactorGraph.cpp @@ -181,7 +181,7 @@ GaussianFactorGraph GaussianFactorGraph::add_priors(double sigma) const { FOREACH_PAIR(key,dim,vs) { Matrix A = eye(dim); Vector b = zero(dim); - sharedDiagonal model = noiseModel::Isotropic::Sigma(dim,sigma); + SharedDiagonal model = noiseModel::Isotropic::Sigma(dim,sigma); sharedFactor prior(new GaussianFactor(key,A,b, model)); result.push_back(prior); } diff --git a/cpp/GaussianFactorGraph.h b/cpp/GaussianFactorGraph.h index cb1d2e6cd..42d7c38d8 100644 --- a/cpp/GaussianFactorGraph.h +++ b/cpp/GaussianFactorGraph.h @@ -48,14 +48,14 @@ namespace gtsam { /** Add a unary factor */ inline void add(const Symbol& key1, const Matrix& A1, - const Vector& b, const sharedDiagonal& model) { + const Vector& b, const SharedDiagonal& model) { push_back(sharedFactor(new GaussianFactor(key1,A1,b,model))); } /** Add a binary factor */ inline void add(const Symbol& key1, const Matrix& A1, const Symbol& key2, const Matrix& A2, - const Vector& b, const sharedDiagonal& model) { + const Vector& b, const SharedDiagonal& model) { push_back(sharedFactor(new GaussianFactor(key1,A1,key2,A2,b,model))); } @@ -63,13 +63,13 @@ namespace gtsam { inline void add(const Symbol& key1, const Matrix& A1, const Symbol& key2, const Matrix& A2, const Symbol& key3, const Matrix& A3, - const Vector& b, const sharedDiagonal& model) { + const Vector& b, const SharedDiagonal& model) { push_back(sharedFactor(new GaussianFactor(key1,A1,key2,A2,key3,A3,b,model))); } /** Add an n-ary factor */ inline void add(const std::vector > &terms, - const Vector &b, const sharedDiagonal& model) { + const Vector &b, const SharedDiagonal& model) { push_back(sharedFactor(new GaussianFactor(terms,b,model))); } diff --git a/cpp/Makefile.am b/cpp/Makefile.am index a71657ff7..64e45123e 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -88,7 +88,7 @@ testBinaryBayesNet_SOURCES = testBinaryBayesNet.cpp testBinaryBayesNet_LDADD = libgtsam.la # Gaussian inference -headers += GaussianFactorSet.h +headers += GaussianFactorSet.h SharedGaussian.h SharedDiagonal.h sources += NoiseModel.cpp Errors.cpp VectorConfig.cpp GaussianFactor.cpp GaussianFactorGraph.cpp GaussianConditional.cpp GaussianBayesNet.cpp check_PROGRAMS += testVectorConfig testGaussianFactor testGaussianFactorGraph testGaussianConditional testGaussianBayesNet testNoiseModel testVectorConfig_SOURCES = testVectorConfig.cpp diff --git a/cpp/NoiseModel.cpp b/cpp/NoiseModel.cpp index ab2c2312e..caa9866b0 100644 --- a/cpp/NoiseModel.cpp +++ b/cpp/NoiseModel.cpp @@ -24,6 +24,7 @@ #include #include "NoiseModel.h" +#include "SharedDiagonal.h" namespace ublas = boost::numeric::ublas; typedef ublas::matrix_column column; @@ -121,7 +122,7 @@ namespace gtsam { } // General QR, see also special version in Constrained - sharedDiagonal Gaussian::QR(Matrix& Ab) const { + SharedDiagonal Gaussian::QR(Matrix& Ab) const { // get size(A) and maxRank // TODO: really no rank problems ? size_t m = Ab.size1(), n = Ab.size2()-1; @@ -207,7 +208,7 @@ namespace gtsam { // Special version of QR for Constrained calls slower but smarter code // that deals with possibly zero sigmas // It is Gram-Schmidt orthogonalization rather than Householder - sharedDiagonal Diagonal::QR(Matrix& Ab) const { + SharedDiagonal Diagonal::QR(Matrix& Ab) const { // get size(A) and maxRank size_t m = Ab.size1(), n = Ab.size2()-1; size_t maxRank = min(m,n); diff --git a/cpp/NoiseModel.h b/cpp/NoiseModel.h index 7e6a0302b..044af4146 100644 --- a/cpp/NoiseModel.h +++ b/cpp/NoiseModel.h @@ -15,7 +15,7 @@ namespace gtsam { - class sharedDiagonal; // forward declare, defined at end + class SharedDiagonal; // forward declare, defined at end namespace noiseModel { @@ -146,7 +146,7 @@ namespace gtsam { * @param Ab is the m*(n+1) augmented system matrix [A b] * @return in-place QR factorization [R d]. Below-diagonal is undefined !!!!! */ - virtual sharedDiagonal QR(Matrix& Ab) const; + virtual SharedDiagonal QR(Matrix& Ab) const; /** * Return R itself, but note that Whiten(H) is cheaper than R*H @@ -210,7 +210,7 @@ namespace gtsam { /** * Apply QR factorization to the system [A b], taking into account constraints */ - virtual sharedDiagonal QR(Matrix& Ab) const; + virtual SharedDiagonal QR(Matrix& Ab) const; /** * Return standard deviations (sqrt of diagonal) @@ -373,42 +373,5 @@ namespace gtsam { } // namespace noiseModel - using namespace noiseModel; - - // TODO: very ugly, just keep shared pointers and use Sigma/Sigmas everywhere - - // A useful convenience class to refer to a shared Gaussian model - // Define GTSAM_MAGIC_GAUSSIAN to desired dimension to have access to slightly - // dangerous and non-shared (inefficient, wasteful) noise models. Only in tests! - struct sharedGaussian : public Gaussian::shared_ptr { - sharedGaussian() {} - sharedGaussian(const Gaussian::shared_ptr& p):Gaussian::shared_ptr(p) {} - sharedGaussian(const Diagonal::shared_ptr& p):Gaussian::shared_ptr(p) {} - sharedGaussian(const Constrained::shared_ptr& p):Gaussian::shared_ptr(p) {} - sharedGaussian(const Isotropic::shared_ptr& p):Gaussian::shared_ptr(p) {} - sharedGaussian(const Unit::shared_ptr& p):Gaussian::shared_ptr(p) {} -#ifdef GTSAM_MAGIC_GAUSSIAN - sharedGaussian(const Matrix& covariance):Gaussian::shared_ptr(Gaussian::Covariance(covariance)) {} - sharedGaussian(const Vector& sigmas):Gaussian::shared_ptr(Diagonal::Sigmas(sigmas)) {} - sharedGaussian(const double& s):Gaussian::shared_ptr(Isotropic::Sigma(GTSAM_MAGIC_GAUSSIAN,s)) {} -#endif - }; - - // A useful convenience class to refer to a shared Diagonal model - // There are (somewhat dangerous) constructors from Vector and pair - // that call Sigmas and Sigma, respectively. - struct sharedDiagonal : public Diagonal::shared_ptr { - sharedDiagonal() {} - sharedDiagonal(const Diagonal::shared_ptr& p):Diagonal::shared_ptr(p) {} - sharedDiagonal(const Constrained::shared_ptr& p):Diagonal::shared_ptr(p) {} - sharedDiagonal(const Isotropic::shared_ptr& p):Diagonal::shared_ptr(p) {} - sharedDiagonal(const Unit::shared_ptr& p):Diagonal::shared_ptr(p) {} - sharedDiagonal(const Vector& sigmas):Diagonal::shared_ptr(Diagonal::Sigmas(sigmas)) {} - }; - - // TODO: make these the ones really used in unit tests - inline sharedDiagonal sharedSigmas(const Vector& sigmas) { return Diagonal::Sigmas(sigmas);} - inline sharedDiagonal sharedSigma(int dim, double sigma) { return Isotropic::Sigma(dim,sigma);} - - } // namespace gtsam + diff --git a/cpp/NonlinearConstraint-inl.h b/cpp/NonlinearConstraint-inl.h index 91f4a2127..1af200d94 100644 --- a/cpp/NonlinearConstraint-inl.h +++ b/cpp/NonlinearConstraint-inl.h @@ -122,12 +122,12 @@ NonlinearConstraint1::linearize(const Config& config, const Vect // construct probabilistic factor Matrix A1 = vector_scale(lambda, grad); - sharedDiagonal probModel = sharedSigma(this->p_,1.0); + SharedDiagonal probModel = sharedSigma(this->p_,1.0); GaussianFactor::shared_ptr factor(new GaussianFactor(key_, A1, this->lagrange_key_, eye(this->p_), zero(this->p_), probModel)); // construct the constraint - sharedDiagonal constraintModel = noiseModel::Constrained::All(this->p_); + SharedDiagonal constraintModel = noiseModel::Constrained::All(this->p_); GaussianFactor::shared_ptr constraint(new GaussianFactor(key_, grad, -1*g, constraintModel)); return std::make_pair(factor, constraint); @@ -222,12 +222,12 @@ std::pair NonlinearConst // construct probabilistic factor Matrix A1 = vector_scale(lambda, grad1); Matrix A2 = vector_scale(lambda, grad2); - sharedDiagonal probModel = sharedSigma(this->p_,1.0); + SharedDiagonal probModel = sharedSigma(this->p_,1.0); GaussianFactor::shared_ptr factor(new GaussianFactor(key1_, A1, key2_, A2, this->lagrange_key_, eye(this->p_), zero(this->p_), probModel)); // construct the constraint - sharedDiagonal constraintModel = noiseModel::Constrained::All(this->p_); + SharedDiagonal constraintModel = noiseModel::Constrained::All(this->p_); GaussianFactor::shared_ptr constraint(new GaussianFactor(key1_, grad1, key2_, grad2, -1.0 * g, constraintModel)); diff --git a/cpp/NonlinearEquality.h b/cpp/NonlinearEquality.h index 727a672e1..a3ea999fb 100644 --- a/cpp/NonlinearEquality.h +++ b/cpp/NonlinearEquality.h @@ -83,7 +83,7 @@ namespace gtsam { Matrix A; Vector b = - evaluateError(xj, A); // TODO pass unwhitened + noise model to Gaussian factor - sharedDiagonal model = noiseModel::Constrained::All(b.size()); + SharedDiagonal model = noiseModel::Constrained::All(b.size()); return GaussianFactor::shared_ptr(new GaussianFactor(this->key_, A, b, model)); } diff --git a/cpp/NonlinearFactor.h b/cpp/NonlinearFactor.h index 6c2771177..5713f28d8 100644 --- a/cpp/NonlinearFactor.h +++ b/cpp/NonlinearFactor.h @@ -18,7 +18,7 @@ #include "Factor.h" #include "Vector.h" #include "Matrix.h" -#include "NoiseModel.h" +#include "SharedGaussian.h" #include "GaussianFactor.h" #define INSTANTIATE_NONLINEAR_FACTOR1(C,J,X) \ @@ -43,7 +43,7 @@ namespace gtsam { typedef NonlinearFactor This; - sharedGaussian noiseModel_; /** Noise model */ + SharedGaussian noiseModel_; /** Noise model */ std::list keys_; /** cached keys */ public: @@ -56,7 +56,7 @@ namespace gtsam { * Constructor * @param noiseModel shared pointer to a noise model */ - NonlinearFactor(const sharedGaussian& noiseModel) : + NonlinearFactor(const SharedGaussian& noiseModel) : noiseModel_(noiseModel) { } @@ -148,7 +148,7 @@ namespace gtsam { * @param z measurement * @param key by which to look up X value in Config */ - NonlinearFactor1(const sharedGaussian& noiseModel, + NonlinearFactor1(const SharedGaussian& noiseModel, const Key& key1) : Base(noiseModel), key_(key1) { this->keys_.push_back(key_); @@ -242,7 +242,7 @@ namespace gtsam { * @param j1 key of the first variable * @param j2 key of the second variable */ - NonlinearFactor2(const sharedGaussian& noiseModel, Key1 j1, + NonlinearFactor2(const SharedGaussian& noiseModel, Key1 j1, Key2 j2) : Base(noiseModel), key1_(j1), key2_(j2) { this->keys_.push_back(key1_); diff --git a/cpp/PriorFactor.h b/cpp/PriorFactor.h index e68480de9..99531a5dc 100644 --- a/cpp/PriorFactor.h +++ b/cpp/PriorFactor.h @@ -31,7 +31,7 @@ namespace gtsam { /** Constructor */ PriorFactor(const Key& key, const T& prior, - const sharedGaussian& model) : + const SharedGaussian& model) : Base(model, key), prior_(prior) { } diff --git a/cpp/SharedDiagonal.h b/cpp/SharedDiagonal.h new file mode 100644 index 000000000..7d4276e45 --- /dev/null +++ b/cpp/SharedDiagonal.h @@ -0,0 +1,45 @@ +/* + * SharedDiagonal.h + * @brief Class that wraps a shared noise model with diagonal covariance + * @Author: Frank Dellaert + * Created on: Jan 22, 2010 + */ + +#pragma once + +#include "NoiseModel.h" + +namespace gtsam { // note, deliberately not in noiseModel namespace + + // A useful convenience class to refer to a shared Diagonal model + // There are (somewhat dangerous) constructors from Vector and pair + // that call Sigmas and Sigma, respectively. + struct SharedDiagonal: public noiseModel::Diagonal::shared_ptr { + SharedDiagonal() { + } + SharedDiagonal(const noiseModel::Diagonal::shared_ptr& p) : + noiseModel::Diagonal::shared_ptr(p) { + } + SharedDiagonal(const noiseModel::Constrained::shared_ptr& p) : + noiseModel::Diagonal::shared_ptr(p) { + } + SharedDiagonal(const noiseModel::Isotropic::shared_ptr& p) : + noiseModel::Diagonal::shared_ptr(p) { + } + SharedDiagonal(const noiseModel::Unit::shared_ptr& p) : + noiseModel::Diagonal::shared_ptr(p) { + } + SharedDiagonal(const Vector& sigmas) : + noiseModel::Diagonal::shared_ptr(noiseModel::Diagonal::Sigmas(sigmas)) { + } + }; + + // TODO: make these the ones really used in unit tests + inline SharedDiagonal sharedSigmas(const Vector& sigmas) { + return noiseModel::Diagonal::Sigmas(sigmas); + } + inline SharedDiagonal sharedSigma(int dim, double sigma) { + return noiseModel::Isotropic::Sigma(dim, sigma); + } + +} diff --git a/cpp/SharedGaussian.h b/cpp/SharedGaussian.h new file mode 100644 index 000000000..ad63aa94c --- /dev/null +++ b/cpp/SharedGaussian.h @@ -0,0 +1,59 @@ +/* + * SharedGaussian.h + * @brief Class that wraps a shared noise model with diagonal covariance + * @Author: Frank Dellaert + * Created on: Jan 22, 2010 + */ + +#pragma once + +#include "NoiseModel.h" + +namespace gtsam { // note, deliberately not in noiseModel namespace + + /** + * A useful convenience class to refer to a shared Gaussian model + * Also needed to make noise models in matlab + */ + struct SharedGaussian: public noiseModel::Gaussian::shared_ptr { + SharedGaussian() { + } + // TODO: better way ? + SharedGaussian(const noiseModel::Gaussian::shared_ptr& p) : + noiseModel::Gaussian::shared_ptr(p) { + } + SharedGaussian(const noiseModel::Diagonal::shared_ptr& p) : + noiseModel::Gaussian::shared_ptr(p) { + } + SharedGaussian(const noiseModel::Constrained::shared_ptr& p) : + noiseModel::Gaussian::shared_ptr(p) { + } + SharedGaussian(const noiseModel::Isotropic::shared_ptr& p) : + noiseModel::Gaussian::shared_ptr(p) { + } + SharedGaussian(const noiseModel::Unit::shared_ptr& p) : + noiseModel::Gaussian::shared_ptr(p) { + } + + // Define GTSAM_MAGIC_GAUSSIAN to have access to slightly + // dangerous and non-shared (inefficient, wasteful) noise models. + // Intended to be used only in tests (if you must) and the MATLAB wrapper +#ifdef GTSAM_MAGIC_GAUSSIAN + SharedGaussian(const Matrix& covariance) : + noiseModel::Gaussian::shared_ptr(noiseModel::Gaussian::Covariance(covariance)) { + } + SharedGaussian(const Vector& sigmas) : + noiseModel::Gaussian::shared_ptr(noiseModel::Diagonal::Sigmas(sigmas)) { + } +#endif +// Define GTSAM_DANGEROUS_GAUSSIAN to have access to bug-prone fixed dimension Gaussians +// Not intended for human use, only for backwards compatibility of old unit tests +#ifdef GTSAM_DANGEROUS_GAUSSIAN + SharedGaussian(const double& s) : + noiseModel::Gaussian::shared_ptr(noiseModel::Isotropic::Sigma( + GTSAM_DANGEROUS_GAUSSIAN, s)) { + } +#endif + }; + +} diff --git a/cpp/Simulated3D.h b/cpp/Simulated3D.h index 219274076..6c373013a 100644 --- a/cpp/Simulated3D.h +++ b/cpp/Simulated3D.h @@ -49,7 +49,7 @@ namespace simulated3D { Vector z_; Point2Prior3D(const Vector& z, - const sharedGaussian& model, const PoseKey& j) : + const SharedGaussian& model, const PoseKey& j) : NonlinearFactor1 (model, j), z_(z) { } @@ -66,7 +66,7 @@ namespace simulated3D { Vector z_; Simulated3DMeasurement(const Vector& z, - const sharedGaussian& model, PoseKey& j1, PointKey j2) : + const SharedGaussian& model, PoseKey& j1, PointKey j2) : z_(z), NonlinearFactor2 ( model, j1, j2) { diff --git a/cpp/gtsam-broken.h b/cpp/gtsam-broken.h new file mode 100644 index 000000000..0a8f78622 --- /dev/null +++ b/cpp/gtsam-broken.h @@ -0,0 +1,60 @@ +// These are currently broken +// Solve by parsing a namespace pose2SLAM::Config and making a Pose2SLAMConfig class +// We also have to solve the shared pointer mess to avoid duplicate methods + +class Point2Prior { + Point2Prior(Vector mu, double sigma, string key); + Vector error_vector(const VectorConfig& c) const; + GaussianFactor* linearize(const VectorConfig& c) const; + double sigma(); + Vector measurement(); + double error(const VectorConfig& c) const; + void print(string s) const; +}; + +class Simulated2DOdometry { + Simulated2DOdometry(Vector odo, double sigma, string key, string key2); + Vector error_vector(const VectorConfig& c) const; + GaussianFactor* linearize(const VectorConfig& c) const; + double sigma(); + Vector measurement(); + double error(const VectorConfig& c) const; + void print(string s) const; +}; + +class Simulated2DMeasurement { + Simulated2DMeasurement(Vector odo, double sigma, string key, string key2); + Vector error_vector(const VectorConfig& c) const; + GaussianFactor* linearize(const VectorConfig& c) const; + double sigma(); + Vector measurement(); + double error(const VectorConfig& c) const; + void print(string s) const; +}; + +class Pose2Config{ + Pose2Config(); + Pose2 get(string key) const; + void insert(string name, const Pose2& val); + void print(string s) const; + void clear(); + int size(); +}; + +class Pose2Factor { + Pose2Factor(string key1, string key2, + const Pose2& measured, Matrix measurement_covariance); + void print(string name) const; + double error(const Pose2Config& c) const; + size_t size() const; + GaussianFactor* linearize(const Pose2Config& config) const; +}; + +class Pose2Graph{ + Pose2Graph(); + void print(string s) const; + GaussianFactorGraph* linearize_(const Pose2Config& config) const; + void push_back(Pose2Factor* factor); +}; + + diff --git a/cpp/gtsam.h b/cpp/gtsam.h index 34f5f787c..3b2825271 100644 --- a/cpp/gtsam.h +++ b/cpp/gtsam.h @@ -1,3 +1,12 @@ +class SharedGaussian { + SharedGaussian(Matrix covariance); + SharedGaussian(Vector sigmas); +}; + +class SharedDiagonal { + SharedDiagonal(Vector sigmas); +}; + class Ordering { Ordering(); Ordering(string key); @@ -25,22 +34,17 @@ class VectorConfig { void clear(); }; -class GaussianFactorSet { - GaussianFactorSet(); - void push_back(GaussianFactor* factor); -}; - class GaussianFactor { GaussianFactor(string key1, Matrix A1, Vector b_in, - double sigma); + const SharedDiagonal& model); GaussianFactor(string key1, Matrix A1, string key2, Matrix A2, Vector b_in, - double sigma); + const SharedDiagonal& model); GaussianFactor(string key1, Matrix A1, string key2, @@ -48,7 +52,7 @@ class GaussianFactor { string key3, Matrix A3, Vector b_in, - double sigma); + const SharedDiagonal& model); bool empty() const; Vector get_b() const; Matrix get_A(string key) const; @@ -59,6 +63,11 @@ class GaussianFactor { pair matrix(const Ordering& ordering) const; }; +class GaussianFactorSet { + GaussianFactorSet(); + void push_back(GaussianFactor* factor); +}; + class GaussianConditional { GaussianConditional(); GaussianConditional(string key, @@ -132,36 +141,6 @@ class Point3 { void print(string s) const; }; -class Point2Prior { - Point2Prior(Vector mu, double sigma, string key); - Vector error_vector(const VectorConfig& c) const; - GaussianFactor* linearize(const VectorConfig& c) const; - double sigma(); - Vector measurement(); - double error(const VectorConfig& c) const; - void print(string s) const; -}; - -class Simulated2DOdometry { - Simulated2DOdometry(Vector odo, double sigma, string key, string key2); - Vector error_vector(const VectorConfig& c) const; - GaussianFactor* linearize(const VectorConfig& c) const; - double sigma(); - Vector measurement(); - double error(const VectorConfig& c) const; - void print(string s) const; -}; - -class Simulated2DMeasurement { - Simulated2DMeasurement(Vector odo, double sigma, string key, string key2); - Vector error_vector(const VectorConfig& c) const; - GaussianFactor* linearize(const VectorConfig& c) const; - double sigma(); - Vector measurement(); - double error(const VectorConfig& c) const; - void print(string s) const; -}; - class Pose2 { Pose2(); Pose2(const Pose2& pose); @@ -179,30 +158,3 @@ class Pose2 { Point2 t() const; Rot2 r() const; }; - -class Pose2Config{ - Pose2Config(); - Pose2 get(string key) const; - void insert(string name, const Pose2& val); - void print(string s) const; - void clear(); - int size(); -}; - -class Pose2Factor { - Pose2Factor(string key1, string key2, - const Pose2& measured, Matrix measurement_covariance); - void print(string name) const; - double error(const Pose2Config& c) const; - size_t size() const; - GaussianFactor* linearize(const Pose2Config& config) const; -}; - -class Pose2Graph{ - Pose2Graph(); - void print(string s) const; - GaussianFactorGraph* linearize_(const Pose2Config& config) const; - void push_back(Pose2Factor* factor); -}; - - diff --git a/cpp/planarSLAM.cpp b/cpp/planarSLAM.cpp index c6dcc82bb..ba89cbf3e 100644 --- a/cpp/planarSLAM.cpp +++ b/cpp/planarSLAM.cpp @@ -26,19 +26,19 @@ namespace gtsam { } void Graph::addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z, - const sharedGaussian& model) { + const SharedGaussian& model) { sharedFactor factor(new Odometry(i, j, z, model)); push_back(factor); } void Graph::addBearing(const PoseKey& i, const PointKey& j, const Rot2& z, - const sharedGaussian& model) { + const SharedGaussian& model) { sharedFactor factor(new Bearing(i, j, z, model)); push_back(factor); } void Graph::addRange(const PoseKey& i, const PointKey& j, double z, - const sharedGaussian& model) { + const SharedGaussian& model) { sharedFactor factor(new Range(i, j, z, model)); push_back(factor); } diff --git a/cpp/planarSLAM.h b/cpp/planarSLAM.h index c09d285e1..2411e7364 100644 --- a/cpp/planarSLAM.h +++ b/cpp/planarSLAM.h @@ -35,7 +35,7 @@ namespace gtsam { BearingFactor(); /* Default constructor */ BearingFactor(const PoseKey& i, const PointKey& j, const Rot2& z, - const sharedGaussian& model) : + const SharedGaussian& model) : Base(model, i, j), z_(z) { } @@ -64,7 +64,7 @@ namespace gtsam { RangeFactor(); /* Default constructor */ RangeFactor(const PoseKey& i, const PointKey& j, double z, - const sharedGaussian& model) : + const SharedGaussian& model) : Base(model, i, j), z_(z) { } @@ -94,11 +94,11 @@ namespace gtsam { struct Graph: public NonlinearFactorGraph { void addPoseConstraint(const PoseKey& i, const Pose2& p); void addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z, - const sharedGaussian& model); + const SharedGaussian& model); void addBearing(const PoseKey& i, const PointKey& j, const Rot2& z, - const sharedGaussian& model); + const SharedGaussian& model); void addRange(const PoseKey& i, const PointKey& j, double z, - const sharedGaussian& model); + const SharedGaussian& model); }; // Optimizer diff --git a/cpp/pose2SLAM.cpp b/cpp/pose2SLAM.cpp index 80fbb41c7..852ff2205 100644 --- a/cpp/pose2SLAM.cpp +++ b/cpp/pose2SLAM.cpp @@ -30,13 +30,13 @@ namespace gtsam { /* ************************************************************************* */ void Graph::addPrior(const Key& i, const Pose2& p, - const sharedGaussian& model) { + const SharedGaussian& model) { sharedFactor factor(new Prior(i, p, model)); push_back(factor); } void Graph::addConstraint(const Key& i, const Key& j, const Pose2& z, - const sharedGaussian& model) { + const SharedGaussian& model) { sharedFactor factor(new Constraint(i, j, z, model)); push_back(factor); } diff --git a/cpp/pose2SLAM.h b/cpp/pose2SLAM.h index e1fab8946..8c237ab76 100644 --- a/cpp/pose2SLAM.h +++ b/cpp/pose2SLAM.h @@ -42,8 +42,8 @@ namespace gtsam { struct Graph: public NonlinearFactorGraph { typedef BetweenFactor Constraint; typedef Pose2 Pose; - void addPrior(const Key& i, const Pose2& p, const sharedGaussian& model); - void addConstraint(const Key& i, const Key& j, const Pose2& z, const sharedGaussian& model); + void addPrior(const Key& i, const Pose2& p, const SharedGaussian& model); + void addConstraint(const Key& i, const Key& j, const Pose2& z, const SharedGaussian& model); void addHardConstraint(const Key& i, const Pose2& p); }; diff --git a/cpp/pose3SLAM.cpp b/cpp/pose3SLAM.cpp index 76da9593d..ed909d4c8 100644 --- a/cpp/pose3SLAM.cpp +++ b/cpp/pose3SLAM.cpp @@ -33,13 +33,13 @@ namespace gtsam { /* ************************************************************************* */ void Graph::addPrior(const Key& i, const Pose3& p, - const sharedGaussian& model) { + const SharedGaussian& model) { sharedFactor factor(new Prior(i, p, model)); push_back(factor); } void Graph::addConstraint(const Key& i, const Key& j, const Pose3& z, - const sharedGaussian& model) { + const SharedGaussian& model) { sharedFactor factor(new Constraint(i, j, z, model)); push_back(factor); } diff --git a/cpp/pose3SLAM.h b/cpp/pose3SLAM.h index 9b9917ff3..28b6f0a56 100644 --- a/cpp/pose3SLAM.h +++ b/cpp/pose3SLAM.h @@ -41,9 +41,9 @@ namespace gtsam { // Graph struct Graph: public NonlinearFactorGraph { void addPrior(const Key& i, const Pose3& p, - const sharedGaussian& model); + const SharedGaussian& model); void addConstraint(const Key& i, const Key& j, const Pose3& z, - const sharedGaussian& model); + const SharedGaussian& model); void addHardConstraint(const Key& i, const Pose3& p); }; diff --git a/cpp/simulated2D.h b/cpp/simulated2D.h index 6c3aadbe3..660b0da3d 100644 --- a/cpp/simulated2D.h +++ b/cpp/simulated2D.h @@ -56,7 +56,7 @@ namespace gtsam { Point2 z_; - Prior(const Point2& z, const sharedGaussian& model, const PoseKey& key) : + Prior(const Point2& z, const SharedGaussian& model, const PoseKey& key) : NonlinearFactor1 (model, key), z_(z) { } @@ -74,7 +74,7 @@ namespace gtsam { Point2> { Point2 z_; - Odometry(const Point2& z, const sharedGaussian& model, const PoseKey& j1, + Odometry(const Point2& z, const SharedGaussian& model, const PoseKey& j1, const PoseKey& j2) : z_(z), NonlinearFactor2 ( model, j1, j2) { @@ -95,7 +95,7 @@ namespace gtsam { Point2 z_; - Measurement(const Point2& z, const sharedGaussian& model, + Measurement(const Point2& z, const SharedGaussian& model, const PoseKey& j1, const PointKey& j2) : z_(z), NonlinearFactor2 ( model, j1, j2) { diff --git a/cpp/smallExample.cpp b/cpp/smallExample.cpp index b49df8bc9..09a996dbd 100644 --- a/cpp/smallExample.cpp +++ b/cpp/smallExample.cpp @@ -12,8 +12,6 @@ using namespace std; -// TODO: DANGEROUS, create shared pointers -#define GTSAM_MAGIC_GAUSSIAN 2 #define GTSAM_MAGIC_KEY #include "Ordering.h" @@ -30,9 +28,10 @@ namespace example { typedef boost::shared_ptr > shared; - static sharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1); - static sharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2); - static sharedDiagonal constraintModel = noiseModel::Constrained::All(2); + static SharedDiagonal sigma1_0 = noiseModel::Isotropic::Sigma(2,1.0); + static SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1); + static SharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2); + static SharedDiagonal constraintModel = noiseModel::Constrained::All(2); /* ************************************************************************* */ boost::shared_ptr sharedNonlinearFactorGraph() { @@ -41,27 +40,23 @@ namespace example { new Graph); // prior on x1 - double sigma1 = 0.1; Point2 mu; - shared f1(new simulated2D::Prior(mu, sigma1, 1)); + shared f1(new simulated2D::Prior(mu, sigma0_1, 1)); nlfg->push_back(f1); // odometry between x1 and x2 - double sigma2 = 0.1; Point2 z2(1.5, 0); - shared f2(new simulated2D::Odometry(z2, sigma2, 1, 2)); + shared f2(new simulated2D::Odometry(z2, sigma0_1, 1, 2)); nlfg->push_back(f2); // measurement between x1 and l1 - double sigma3 = 0.2; Point2 z3(0, -1); - shared f3(new simulated2D::Measurement(z3, sigma3, 1, 1)); + shared f3(new simulated2D::Measurement(z3, sigma0_2, 1, 1)); nlfg->push_back(f3); // measurement between x2 and l1 - double sigma4 = 0.2; Point2 z4(-1.5, -1.); - shared f4(new simulated2D::Measurement(z4, sigma4, 2, 1)); + shared f4(new simulated2D::Measurement(z4, sigma0_2, 2, 1)); nlfg->push_back(f4); return nlfg; @@ -193,7 +188,7 @@ namespace example { Point2 z_; - UnaryFactor(const Point2& z, const sharedGaussian& model, + UnaryFactor(const Point2& z, const SharedGaussian& model, const simulated2D::PoseKey& key) : gtsam::NonlinearFactor1(model, key), z_(z) { @@ -228,28 +223,25 @@ namespace example { /* ************************************************************************* */ pair createNonlinearSmoother(int T) { - // noise on measurements and odometry, respectively - double sigma1 = 1, sigma2 = 1; - // Create Graph nlfg; Config poses; // prior on x1 Point2 x1(1.0, 0.0); - shared prior(new simulated2D::Prior(x1, sigma1, 1)); + shared prior(new simulated2D::Prior(x1, sigma1_0, 1)); nlfg.push_back(prior); poses.insert(simulated2D::PoseKey(1), x1); for (int t = 2; t <= T; t++) { // odometry between x_t and x_{t-1} Point2 odo(1.0, 0.0); - shared odometry(new simulated2D::Odometry(odo, sigma2, t - 1, t)); + shared odometry(new simulated2D::Odometry(odo, sigma1_0, t - 1, t)); nlfg.push_back(odometry); // measurement on x_t is like perfect GPS Point2 xt(t, 0); - shared measurement(new simulated2D::Prior(xt, sigma1, t)); + shared measurement(new simulated2D::Prior(xt, sigma1_0, t)); nlfg.push_back(measurement); // initial estimate @@ -524,17 +516,14 @@ namespace example { NonlinearFactorGraph nlfg; // Create almost hard constraint on x11, sigma=0 will work for PCG not for normal - double sigma0 = 1e-3; - shared constraint(new simulated2D::Prior(Point2(1.0, 1.0), sigma0, key(1,1))); + shared constraint(new simulated2D::Prior(Point2(1.0, 1.0), sharedSigma(2,1e-3), key(1,1))); nlfg.push_back(constraint); - double sigma = 0.01; - // Create horizontal constraints, 1...N*(N-1) Point2 z1(1.0, 0.0); // move right for (size_t x = 1; x < N; x++) for (size_t y = 1; y <= N; y++) { - shared f(new simulated2D::Odometry(z1, sigma, key(x, y), key(x + 1, y))); + shared f(new simulated2D::Odometry(z1, sharedSigma(2,0.01), key(x, y), key(x + 1, y))); nlfg.push_back(f); } @@ -542,7 +531,7 @@ namespace example { Point2 z2(0.0, 1.0); // move up for (size_t x = 1; x <= N; x++) for (size_t y = 1; y < N; y++) { - shared f(new simulated2D::Odometry(z2, sigma, key(x, y), key(x, y + 1))); + shared f(new simulated2D::Odometry(z2, sharedSigma(2,0.01), key(x, y), key(x, y + 1))); nlfg.push_back(f); } diff --git a/cpp/testGaussianFactor.cpp b/cpp/testGaussianFactor.cpp index 786597ad2..939cf259a 100644 --- a/cpp/testGaussianFactor.cpp +++ b/cpp/testGaussianFactor.cpp @@ -27,7 +27,7 @@ using namespace gtsam; using namespace example; using namespace boost; -static sharedDiagonal +static SharedDiagonal sigma0_1 = sharedSigma(2,0.1), sigma_02 = sharedSigma(2,0.2), constraintModel = noiseModel::Constrained::All(2); @@ -225,7 +225,7 @@ TEST( NonlinearFactorGraph, combine2){ TEST( GaussianFactor, linearFactorN){ Matrix I = eye(2); vector f; - sharedDiagonal model = sharedSigma(2,1.0); + SharedDiagonal model = sharedSigma(2,1.0); f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x1", I, Vector_(2, 10.0, 5.0), model))); f.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x1", -10 * I, diff --git a/cpp/testGaussianFactorGraph.cpp b/cpp/testGaussianFactorGraph.cpp index a93a49466..3ee1c8184 100644 --- a/cpp/testGaussianFactorGraph.cpp +++ b/cpp/testGaussianFactorGraph.cpp @@ -263,7 +263,7 @@ TEST( GaussianFactorGraph, add_priors ) GaussianFactorGraph expected = createGaussianFactorGraph(); Matrix A = eye(2); Vector b = zero(2); - sharedDiagonal sigma = sharedSigma(2,3.0); + SharedDiagonal sigma = sharedSigma(2,3.0); expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor("l1",A,b,sigma))); expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x1",A,b,sigma))); expected.push_back(GaussianFactor::shared_ptr(new GaussianFactor("x2",A,b,sigma))); @@ -629,7 +629,7 @@ TEST( GaussianFactorGraph, elimination ) GaussianFactorGraph fg; Matrix Ap = eye(1), An = eye(1) * -1; Vector b = Vector_(1, 0.0); - sharedDiagonal sigma = sharedSigma(2,2.0); + SharedDiagonal sigma = sharedSigma(2,2.0); fg.add("x1", An, "x2", Ap, b, sigma); fg.add("x1", Ap, b, sigma); fg.add("x2", Ap, b, sigma); @@ -735,7 +735,7 @@ TEST( GaussianFactorGraph, constrained_multi2 ) /* ************************************************************************* */ -sharedDiagonal model = sharedSigma(2,1); +SharedDiagonal model = sharedSigma(2,1); TEST( GaussianFactorGraph, findMinimumSpanningTree ) { diff --git a/cpp/testIterative.cpp b/cpp/testIterative.cpp index 634ac1c4b..92cb31d2c 100644 --- a/cpp/testIterative.cpp +++ b/cpp/testIterative.cpp @@ -10,7 +10,7 @@ using namespace boost::assign; #include // TODO: DANGEROUS, create shared pointers -#define GTSAM_MAGIC_GAUSSIAN 3 +#define GTSAM_DANGEROUS_GAUSSIAN 3 #define GTSAM_MAGIC_KEY #include "Ordering.h" @@ -114,8 +114,8 @@ TEST( Iterative, conjugateGradientDescent_soft_constraint ) config.insert(2, Pose2(1.5,0.,0.)); Pose2Graph graph; - graph.addPrior(1, Pose2(0.,0.,0.), Isotropic::Sigma(3, 1e-10)); - graph.addConstraint(1,2, Pose2(1.,0.,0.), Isotropic::Sigma(3, 1)); + graph.addPrior(1, Pose2(0.,0.,0.), sharedSigma(3, 1e-10)); + graph.addConstraint(1,2, Pose2(1.,0.,0.), sharedSigma(3, 1)); VectorConfig zeros; zeros.insert("x1",zero(3)); @@ -140,8 +140,8 @@ TEST( Iterative, subgraphPCG ) theta_bar.insert(2, Pose2(1.5,0.,0.)); Pose2Graph graph; - graph.addPrior(1, Pose2(0.,0.,0.), Isotropic::Sigma(3, 1e-10)); - graph.addConstraint(1,2, Pose2(1.,0.,0.), Isotropic::Sigma(3, 1)); + graph.addPrior(1, Pose2(0.,0.,0.), sharedSigma(3, 1e-10)); + graph.addConstraint(1,2, Pose2(1.,0.,0.), sharedSigma(3, 1)); VectorConfig zeros; zeros.insert("x1",zero(3)); diff --git a/cpp/testMatrix.cpp b/cpp/testMatrix.cpp index 67704cb99..785c99584 100644 --- a/cpp/testMatrix.cpp +++ b/cpp/testMatrix.cpp @@ -11,7 +11,6 @@ #include #include #include "Matrix.h" -#include "NoiseModel.h" using namespace std; using namespace gtsam; @@ -419,6 +418,8 @@ TEST( matrix, inverse ) A(2,0)= 1; A(2,1)=0; A(2,2)=6; Matrix Ainv = inverse(A); + CHECK(assert_equal(eye(3), A*Ainv)); + CHECK(assert_equal(eye(3), Ainv*A)); Matrix expected(3,3); expected(0,0)= 1.0909; expected(0,1)=-0.5454; expected(0,2)=-0.0909; @@ -426,6 +427,26 @@ TEST( matrix, inverse ) expected(2,0)= -0.1818; expected(2,1)= 0.0909; expected(2,2)=0.1818; CHECK(assert_equal(expected, Ainv, 1e-4)); + + // These two matrices failed before version 2003 because we called LU incorrectly + Matrix lMg(Matrix_(3,3, + 0.0, 1.0,-2.0, + -1.0, 0.0, 1.0, + 0.0, 0.0, 1.0)); + CHECK(assert_equal(Matrix_(3,3, + 0.0, -1.0, 1.0, + 1.0, 0.0, 2.0, + 0.0, 0.0, 1.0), + inverse(lMg))); + Matrix gMl(Matrix_(3,3, + 0.0, -1.0, 1.0, + 1.0, 0.0, 2.0, + 0.0, 0.0, 1.0)); + CHECK(assert_equal(Matrix_(3,3, + 0.0, 1.0,-2.0, + -1.0, 0.0, 1.0, + 0.0, 0.0, 1.0), + inverse(gMl))); } /* ************************************************************************* */ @@ -623,69 +644,6 @@ static void updateAb(Matrix& A, Vector& b, int j, const Vector& a, } } -/* ************************************************************************* */ -list > -weighted_eliminate2(Matrix& A, Vector& b, const sharedGaussian& model) { - size_t m = A.size1(), n = A.size2(); // get size(A) - size_t maxRank = min(m,n); - - // pre-whiten everything - model->WhitenInPlace(A); - b = model->whiten(b); - - // create list - list > results; - - // We loop over all columns, because the columns that can be eliminated - // are not necessarily contiguous. For each one, estimate the corresponding - // scalar variable x as d-rS, with S the separator (remaining columns). - // Then update A and b by substituting x with d-rS, zero-ing out x's column. - for (int j=0; j(A, j2)); - - // create the rhs - double d = inner_prod(pseudo, b); - - // construct solution (r, d, sigma) - // TODO: avoid sqrt, store precision or at least variance - results.push_back(boost::make_tuple(r, d, 1./sqrt(precision))); - - // exit after rank exhausted - if (results.size()>=maxRank) break; - - // update A, b, expensive, suing outer product - // A' \define A_{S}-a*r and b'\define b-d*a - updateAb(A, b, j, a, r, d); - } - - return results; -} - -/* ************************************************************************* */ -void weighted_eliminate3(Matrix& A, Vector& b, const sharedGaussian& model) { - size_t m = A.size1(), n = A.size2(); // get size(A) - size_t maxRank = min(m,n); - - // pre-whiten everything - model->WhitenInPlace(A); - b = model->whiten(b); - - householder_(A, maxRank); -} - /* ************************************************************************* */ TEST( matrix, weighted_elimination ) { @@ -727,27 +685,6 @@ TEST( matrix, weighted_elimination ) DOUBLES_EQUAL(newSigmas(i), sigma, 1e-5); // verify sigma i += 1; } - - // perform elimination with NoiseModel - Matrix A2 = A; Vector b2 = b; - sharedGaussian model = noiseModel::Diagonal::Sigmas(sigmas); - std::list > solution2 = - weighted_eliminate2(A2, b2, model); - - // unpack and verify - i=0; - BOOST_FOREACH(boost::tie(r, di, sigma), solution2) { - CHECK(assert_equal(r, row(expectedR, i))); // verify r - DOUBLES_EQUAL(d(i), di, 1e-8); // verify d - DOUBLES_EQUAL(newSigmas(i), sigma, 1e-5); // verify sigma - i += 1; - } - - // perform elimination with NoiseModel - weighted_eliminate3(A, b, model); - sharedGaussian newModel = noiseModel::Diagonal::Sigmas(newSigmas); -// print(A); -// print(newModel->Whiten(expectedR)); } /* ************************************************************************* */ diff --git a/cpp/testNoiseModel.cpp b/cpp/testNoiseModel.cpp index 3cff1ebca..dc5020316 100644 --- a/cpp/testNoiseModel.cpp +++ b/cpp/testNoiseModel.cpp @@ -11,6 +11,8 @@ #include #include #include "NoiseModel.h" +#include "SharedGaussian.h" +#include "SharedDiagonal.h" using namespace std; using namespace gtsam; @@ -148,12 +150,12 @@ TEST(NoiseModel, ConstrainedAll ) // // // Expected result // Vector expectedSigmas = Vector_(4, 0.0894427, 0.0894427, 0.223607, 0.223607); -// sharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas); +// SharedDiagonal expectedModel = noiseModel::Diagonal::Sigmas(expectedSigmas); // // // Call Gaussian version -// sharedDiagonal diagonal = noiseModel::Diagonal::Sigmas(sigmas); -// sharedDiagonal actual1 = diagonal->QR(Ab1); -// sharedDiagonal expected = noiseModel::Unit::Create(4); +// SharedDiagonal diagonal = noiseModel::Diagonal::Sigmas(sigmas); +// SharedDiagonal actual1 = diagonal->QR(Ab1); +// SharedDiagonal expected = noiseModel::Unit::Create(4); // CHECK(assert_equal(*expected,*actual1)); // Matrix expectedRd1 = Matrix_(4, 6+1, // 11.1803, 0.0, -2.23607, 0.0, -8.94427, 0.0, 2.23607, @@ -163,8 +165,8 @@ TEST(NoiseModel, ConstrainedAll ) // CHECK(assert_equal(expectedRd1,Ab1,1e-4)); // Ab was modified in place !!! // // // Call Constrained version -// sharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(sigmas); -// sharedDiagonal actual2 = constrained->QR(Ab2); +// SharedDiagonal constrained = noiseModel::Constrained::MixedSigmas(sigmas); +// SharedDiagonal actual2 = constrained->QR(Ab2); // CHECK(assert_equal(*expectedModel,*actual2)); // Matrix expectedRd2 = Matrix_(4, 6+1, // 1., 0., -0.2, 0., -0.8, 0., 0.2, @@ -177,13 +179,13 @@ TEST(NoiseModel, ConstrainedAll ) /* ************************************************************************* */ TEST(NoiseModel, QRNan ) { - sharedDiagonal constrained = noiseModel::Constrained::All(2); + SharedDiagonal constrained = noiseModel::Constrained::All(2); Matrix Ab = Matrix_(2, 5, 1., 2., 1., 2., 3., 2., 1., 2., 4., 4.); - sharedDiagonal expected = noiseModel::Constrained::All(2); + SharedDiagonal expected = noiseModel::Constrained::All(2); Matrix expectedAb = Matrix_(2, 5, 1., 2., 1., 2., 3., 0., 1., 0., 0., 2.0/3); - sharedDiagonal actual = constrained->QR(Ab); + SharedDiagonal actual = constrained->QR(Ab); CHECK(assert_equal(*expected,*actual)); CHECK(assert_equal(expectedAb,Ab)); } @@ -192,8 +194,8 @@ TEST(NoiseModel, QRNan ) TEST(NoiseModel, SmartCovariance ) { bool smart = true; - sharedGaussian expected = Unit::Create(3); - sharedGaussian actual = Gaussian::Covariance(eye(3), smart); + SharedGaussian expected = Unit::Create(3); + SharedGaussian actual = Gaussian::Covariance(eye(3), smart); CHECK(assert_equal(*expected,*actual)); } @@ -201,8 +203,8 @@ TEST(NoiseModel, SmartCovariance ) TEST(NoiseModel, ScalarOrVector ) { bool smart = true; - sharedGaussian expected = Unit::Create(3); - sharedGaussian actual = Gaussian::Covariance(eye(3), smart); + SharedGaussian expected = Unit::Create(3); + SharedGaussian actual = Gaussian::Covariance(eye(3), smart); CHECK(assert_equal(*expected,*actual)); } diff --git a/cpp/testNonlinearConstraint.cpp b/cpp/testNonlinearConstraint.cpp index b54d9adad..5b6948a38 100644 --- a/cpp/testNonlinearConstraint.cpp +++ b/cpp/testNonlinearConstraint.cpp @@ -87,9 +87,9 @@ TEST( NonlinearConstraint1, unary_scalar_linearize ) { boost::tie(actualFactor, actualConstraint) = c1.linearize(realconfig, lagrangeConfig); // verify - sharedDiagonal probModel = sharedSigma(p,1.0); + SharedDiagonal probModel = sharedSigma(p,1.0); GaussianFactor expectedFactor(x1, Matrix_(1,1, 6.0), "L1", eye(1), zero(1), probModel); - sharedDiagonal constraintModel = noiseModel::Constrained::All(p); + SharedDiagonal constraintModel = noiseModel::Constrained::All(p); GaussianFactor expectedConstraint(x1, Matrix_(1,1, 2.0), Vector_(1, 4.0), constraintModel); CHECK(assert_equal(*actualFactor, expectedFactor)); CHECK(assert_equal(*actualConstraint, expectedConstraint)); @@ -188,11 +188,11 @@ TEST( NonlinearConstraint2, binary_scalar_linearize ) { boost::tie(actualFactor, actualConstraint) = c1.linearize(realconfig, lagrangeConfig); // verify - sharedDiagonal probModel = sharedSigma(p,1.0); + SharedDiagonal probModel = sharedSigma(p,1.0); GaussianFactor expectedFactor(x0, Matrix_(1,1, 6.0), x1, Matrix_(1,1, -3.0), "L12", eye(1), zero(1), probModel); - sharedDiagonal constraintModel = noiseModel::Constrained::All(p); + SharedDiagonal constraintModel = noiseModel::Constrained::All(p); GaussianFactor expectedConstraint(x0, Matrix_(1,1, 2.0), x1, Matrix_(1,1, -1.0), Vector_(1, 6.0), constraintModel); @@ -289,9 +289,9 @@ TEST( NonlinearConstraint1, unary_inequality_linearize ) { CHECK(c1.active(config2)); // verify - sharedDiagonal probModel = sharedSigma(p,1.0); + SharedDiagonal probModel = sharedSigma(p,1.0); GaussianFactor expectedFactor(x0, Matrix_(1,1, 6.0), "L1", eye(1), zero(1), probModel); - sharedDiagonal constraintModel = noiseModel::Constrained::All(p); + SharedDiagonal constraintModel = noiseModel::Constrained::All(p); GaussianFactor expectedConstraint(x0, Matrix_(1,1, 2.0), Vector_(1, 4.0), constraintModel); CHECK(assert_equal(*actualFactor2, expectedFactor)); CHECK(assert_equal(*actualConstraint2, expectedConstraint)); diff --git a/cpp/testNonlinearEquality.cpp b/cpp/testNonlinearEquality.cpp index b3b71a65d..0e61a00c3 100644 --- a/cpp/testNonlinearEquality.cpp +++ b/cpp/testNonlinearEquality.cpp @@ -31,7 +31,7 @@ TEST ( NonlinearEquality, linearization ) { shared_nle nle(new NLE(key, value,vector_compare)); // check linearize - sharedDiagonal constraintModel = noiseModel::Constrained::All(2); + SharedDiagonal constraintModel = noiseModel::Constrained::All(2); GaussianFactor expLF(key, eye(2), zero(2), constraintModel); GaussianFactor::shared_ptr actualLF = nle->linearize(linearize); CHECK(assert_equal(*actualLF, expLF)); diff --git a/cpp/testNonlinearFactor.cpp b/cpp/testNonlinearFactor.cpp index 4d20afe65..bc6c2fd9a 100644 --- a/cpp/testNonlinearFactor.cpp +++ b/cpp/testNonlinearFactor.cpp @@ -29,7 +29,7 @@ typedef boost::shared_ptr > shared_nlf; /* ************************************************************************* */ TEST( NonlinearFactor, equals ) { - sharedGaussian sigma(noiseModel::Isotropic::Sigma(2,1.0)); + SharedGaussian sigma(noiseModel::Isotropic::Sigma(2,1.0)); // create two nonlinear2 factors Point2 z3(0.,-1.); diff --git a/cpp/testNonlinearOptimizer.cpp b/cpp/testNonlinearOptimizer.cpp index d763a9bea..c82c5ea8c 100644 --- a/cpp/testNonlinearOptimizer.cpp +++ b/cpp/testNonlinearOptimizer.cpp @@ -170,8 +170,8 @@ TEST( NonlinearOptimizer, Factorization ) config->insert(2, Pose2(1.5,0.,0.)); boost::shared_ptr graph(new Pose2Graph); - graph->addPrior(1, Pose2(0.,0.,0.), Isotropic::Sigma(3, 1e-10)); - graph->addConstraint(1,2, Pose2(1.,0.,0.), Isotropic::Sigma(3, 1)); + graph->addPrior(1, Pose2(0.,0.,0.), sharedSigma(3, 1e-10)); + graph->addConstraint(1,2, Pose2(1.,0.,0.), sharedSigma(3, 1)); boost::shared_ptr ordering(new Ordering); ordering->push_back(Pose2Config::Key(1)); @@ -197,8 +197,8 @@ TEST( NonlinearOptimizer, SubgraphPCG ) config->insert(2, Pose2(1.5,0.,0.)); boost::shared_ptr graph(new Pose2Graph); - graph->addPrior(1, Pose2(0.,0.,0.), Isotropic::Sigma(3, 1e-10)); - graph->addConstraint(1,2, Pose2(1.,0.,0.), Isotropic::Sigma(3, 1)); + graph->addPrior(1, Pose2(0.,0.,0.), sharedSigma(3, 1e-10)); + graph->addConstraint(1,2, Pose2(1.,0.,0.), sharedSigma(3, 1)); double relativeThreshold = 1e-5; double absoluteThreshold = 1e-5; diff --git a/cpp/testPlanarSLAM.cpp b/cpp/testPlanarSLAM.cpp index b9b50a054..ee5f53f91 100644 --- a/cpp/testPlanarSLAM.cpp +++ b/cpp/testPlanarSLAM.cpp @@ -15,7 +15,7 @@ using namespace gtsam; static Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4); static Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3); -sharedGaussian +SharedGaussian sigma(noiseModel::Isotropic::Sigma(1,0.1)), I3(noiseModel::Unit::Create(3)); diff --git a/cpp/testPose2.cpp b/cpp/testPose2.cpp index 1309cf484..fbd399fc7 100644 --- a/cpp/testPose2.cpp +++ b/cpp/testPose2.cpp @@ -181,15 +181,15 @@ Matrix matrix(const Pose2& gTl) { TEST( Pose2, matrix ) { Point2 origin, t(1,2); - Pose2 gT1(M_PI_2, t); // robot at (1,2) looking towards y - Matrix gM1 = matrix(gT1); + Pose2 gTl(M_PI_2, t); // robot at (1,2) looking towards y + Matrix gMl = matrix(gTl); CHECK(assert_equal(Matrix_(3,3, 0.0, -1.0, 1.0, 1.0, 0.0, 2.0, 0.0, 0.0, 1.0), - gM1)); - Rot2 gR1 = gT1.r(); - CHECK(assert_equal(homogeneous(t),gM1*homogeneous(origin))); + gMl)); + Rot2 gR1 = gTl.r(); + CHECK(assert_equal(homogeneous(t),gMl*homogeneous(origin))); Point2 x_axis(1,0), y_axis(0,1); CHECK(assert_equal(Matrix_(2,2, 0.0, -1.0, @@ -197,14 +197,26 @@ TEST( Pose2, matrix ) gR1.matrix())); CHECK(assert_equal(Point2(0,1),gR1*x_axis)); CHECK(assert_equal(Point2(-1,0),gR1*y_axis)); - CHECK(assert_equal(homogeneous(Point2(1+0,2+1)),gM1*homogeneous(x_axis))); - CHECK(assert_equal(homogeneous(Point2(1-1,2+0)),gM1*homogeneous(y_axis))); + CHECK(assert_equal(homogeneous(Point2(1+0,2+1)),gMl*homogeneous(x_axis))); + CHECK(assert_equal(homogeneous(Point2(1-1,2+0)),gMl*homogeneous(y_axis))); // check inverse pose - Matrix _1Mg = matrix(inverse(gT1)); - CHECK(assert_equal(inverse(gM1),_1Mg)); - CHECK(assert_equal(eye(3),inverse(_1Mg)*_1Mg)); - CHECK(assert_equal(eye(3),inverse(gM1)*gM1)); + Matrix lMg = matrix(inverse(gTl)); + CHECK(assert_equal(Matrix_(3,3, + 0.0, 1.0,-2.0, + -1.0, 0.0, 1.0, + 0.0, 0.0, 1.0), + lMg)); +} + +/* ************************************************************************* */ +TEST( Pose2, compose_matrix ) +{ + Pose2 gT1(M_PI_2, Point2(1,2)); // robot at (1,2) looking towards y + Pose2 _1T2(M_PI, Point2(-1,4)); // local robot at (-1,4) loooking at negative x + Matrix gM1(matrix(gT1)),_1M2(matrix(_1T2)); + CHECK(assert_equal(gM1*_1M2,matrix(compose(_1T2,gT1)))); // WRONG CHECKS OUT ! + // CHECK(assert_equal(gM1*_1M2,matrix(compose(gT1,_1T2)))); RIGHT DOES NOT } /* ************************************************************************* */ @@ -221,13 +233,6 @@ TEST( Pose2, between ) Matrix actualH1,actualH2; Pose2 expected(M_PI_2, Point2(2,2)); Pose2 actual1 = between(gT1,gT2); // gT2 * 1Tg does not make sense !!!!! - GTSAM_PRINT(between(gT1,gT2)); - // what we want is 1T2 = 1Tg * gT2 = between(2Tg,1Tg) -// print(matrix(gT1)); -// print(matrix(gT2)); -// print(inverse(matrix(gT1))*matrix(gT2)); // 1T2 -// print(inverse(matrix(gT2))*matrix(gT1)); // 2T1 -// GTSAM_PRINT(between(inverse(gT2),inverse(gT1))); Pose2 actual2 = between(gT1,gT2,actualH1,actualH2); CHECK(assert_equal(expected,actual1)); CHECK(assert_equal(expected,actual2)); diff --git a/cpp/testPose2Factor.cpp b/cpp/testPose2Factor.cpp index 1bcf678f2..6beab698e 100644 --- a/cpp/testPose2Factor.cpp +++ b/cpp/testPose2Factor.cpp @@ -114,7 +114,7 @@ TEST( Pose2Factor, linearize ) Vector expected_b = Vector_(3, 0.0, 0.0, 0.0); // expected linear factor - sharedDiagonal probModel1 = noiseModel::Unit::Create(3); + SharedDiagonal probModel1 = noiseModel::Unit::Create(3); GaussianFactor expected("x1", expectedH1, "x2", expectedH2, expected_b, probModel1); // Actual linearization diff --git a/cpp/testPose2Prior.cpp b/cpp/testPose2Prior.cpp index 1e44a3116..cd3801851 100644 --- a/cpp/testPose2Prior.cpp +++ b/cpp/testPose2Prior.cpp @@ -16,7 +16,7 @@ using namespace gtsam; // Common measurement covariance static double sx=0.5, sy=0.5,st=0.1; -static sharedGaussian sigmas = Diagonal::Sigmas(Vector_(3,sx,sy,st)); +static SharedGaussian sigmas = sharedSigmas(Vector_(3,sx,sy,st)); /* ************************************************************************* */ // Very simple test establishing Ax-b \approx z-h(x) diff --git a/cpp/testPose2SLAM.cpp b/cpp/testPose2SLAM.cpp index 92ed4d907..34aefd4b5 100644 --- a/cpp/testPose2SLAM.cpp +++ b/cpp/testPose2SLAM.cpp @@ -79,7 +79,7 @@ TEST( Pose2Graph, linearization ) double sigma = 1; Vector b = Vector_(3,-0.1/sx,0.1/sy,0.0); - sharedDiagonal probModel1 = noiseModel::Unit::Create(3); + SharedDiagonal probModel1 = noiseModel::Unit::Create(3); lfg_expected.add("x1", A1, "x2", A2, b, probModel1); CHECK(assert_equal(lfg_expected, lfg_linearized)); diff --git a/cpp/testPose3Factor.cpp b/cpp/testPose3Factor.cpp index 7c74057f3..8af57517d 100644 --- a/cpp/testPose3Factor.cpp +++ b/cpp/testPose3Factor.cpp @@ -23,7 +23,7 @@ TEST( Pose3Factor, error ) Pose3 z(rodriguez(0.2,0.2,0.3),Point3(0,1.1,0));; // Create factor - sharedGaussian I6(noiseModel::Unit::Create(6)); + SharedGaussian I6(noiseModel::Unit::Create(6)); Pose3Factor factor(1,2, z, I6); // Create config diff --git a/cpp/testSQP.cpp b/cpp/testSQP.cpp index 55ba3db60..713b74eb9 100644 --- a/cpp/testSQP.cpp +++ b/cpp/testSQP.cpp @@ -13,7 +13,7 @@ #include // TODO: DANGEROUS, create shared pointers -#define GTSAM_MAGIC_GAUSSIAN 2 +#define GTSAM_DANGEROUS_GAUSSIAN 2 #define GTSAM_MAGIC_KEY #include @@ -36,9 +36,9 @@ using namespace boost; using namespace boost::assign; // Models to use -sharedDiagonal probModel1 = sharedSigma(1,1.0); -sharedDiagonal probModel2 = sharedSigma(2,1.0); -sharedDiagonal constraintModel1 = noiseModel::Constrained::All(1); +SharedDiagonal probModel1 = sharedSigma(1,1.0); +SharedDiagonal probModel2 = sharedSigma(2,1.0); +SharedDiagonal constraintModel1 = noiseModel::Constrained::All(1); // trick from some reading group #define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) @@ -279,7 +279,7 @@ TEST (SQP, two_pose_truth ) { // measurement from x1 to l1 Point2 z1(0.0, 5.0); - sharedGaussian sigma(noiseModel::Isotropic::Sigma(2, 0.1)); + SharedGaussian sigma(noiseModel::Isotropic::Sigma(2, 0.1)); shared f1(new simulated2D::Measurement(z1, sigma, x1,l1)); graph->push_back(f1); @@ -387,7 +387,7 @@ TEST (SQP, two_pose ) { // measurement from x1 to l1 Point2 z1(0.0, 5.0); - sharedGaussian sigma(noiseModel::Isotropic::Sigma(2, 0.1)); + SharedGaussian sigma(noiseModel::Isotropic::Sigma(2, 0.1)); shared f1(new simulated2D::Measurement(z1, sigma, x1,l1)); graph->push_back(f1); diff --git a/cpp/testSQPOptimizer.cpp b/cpp/testSQPOptimizer.cpp index 634360376..80fd919c2 100644 --- a/cpp/testSQPOptimizer.cpp +++ b/cpp/testSQPOptimizer.cpp @@ -29,7 +29,7 @@ using namespace boost; using namespace boost::assign; using namespace simulated2D; -static sharedGaussian sigma(noiseModel::Isotropic::Sigma(1,0.1)); +static SharedGaussian sigma(noiseModel::Isotropic::Sigma(1,0.1)); // typedefs typedef simulated2D::Config Config2D; diff --git a/cpp/testVSLAMFactor.cpp b/cpp/testVSLAMFactor.cpp index a8ce8218a..a6bab9f98 100644 --- a/cpp/testVSLAMFactor.cpp +++ b/cpp/testVSLAMFactor.cpp @@ -24,7 +24,7 @@ static double fov = 60; // degrees static size_t w=640,h=480; static Cal3_S2 K(fov,w,h); -static sharedGaussian sigma(noiseModel::Unit::Create(1)); +static SharedGaussian sigma(noiseModel::Unit::Create(1)); static shared_ptrK sK(new Cal3_S2(K)); // make cameras @@ -52,7 +52,7 @@ TEST( ProjectionFactor, error ) Matrix Al1 = Matrix_(2, 3, 61.584, 0., 0., 0., 61.584, 0.); Matrix Ax1 = Matrix_(2, 6, 0., -369.504, 0., -61.584, 0., 0., 369.504, 0., 0., 0., -61.584, 0.); Vector b = Vector_(2,3.,0.); - sharedDiagonal probModel1 = noiseModel::Unit::Create(2); + SharedDiagonal probModel1 = noiseModel::Unit::Create(2); GaussianFactor expected("l1", Al1, "x1", Ax1, b, probModel1); GaussianFactor::shared_ptr actual = factor->linearize(config); CHECK(assert_equal(expected,*actual,1e-3)); diff --git a/cpp/testVSLAMGraph.cpp b/cpp/testVSLAMGraph.cpp index 9a3b36839..a15693eeb 100644 --- a/cpp/testVSLAMGraph.cpp +++ b/cpp/testVSLAMGraph.cpp @@ -23,7 +23,7 @@ using namespace gtsam; using namespace gtsam::visualSLAM; using namespace boost; typedef NonlinearOptimizer Optimizer; -static sharedGaussian sigma(noiseModel::Unit::Create(1)); +static SharedGaussian sigma(noiseModel::Unit::Create(1)); /* ************************************************************************* */ Point3 landmark1(-1.0,-1.0, 0.0); diff --git a/cpp/visualSLAM.h b/cpp/visualSLAM.h index 7a1d48edf..5c7f616f1 100644 --- a/cpp/visualSLAM.h +++ b/cpp/visualSLAM.h @@ -63,7 +63,7 @@ namespace gtsam { namespace visualSLAM { * @param K the constant calibration */ ProjectionFactor(const Point2& z, - const sharedGaussian& model, PoseKey j_pose, + const SharedGaussian& model, PoseKey j_pose, PointKey j_landmark, const shared_ptrK& K) : z_(z), K_(K), Base(model, j_pose, j_landmark) { } diff --git a/matlab/createGaussianFactorGraph.m b/matlab/createGaussianFactorGraph.m index 8b50b0075..be3f96747 100644 --- a/matlab/createGaussianFactorGraph.m +++ b/matlab/createGaussianFactorGraph.m @@ -6,46 +6,36 @@ c = createNoisyConfig(); % Create fg = GaussianFactorGraph; -sigma1=.1; + +% Create shared Noise models +sigma0_1 = SharedDiagonal([0.1;0.1]); +sigma0_2 = SharedDiagonal([0.2;0.2]); % prior on x1 A11=eye(2); b = - c.get('x1'); - -f1 = GaussianFactor('x1', A11, b, sigma1); % generate a Gaussian factor of odometry +f1 = GaussianFactor('x1', A11, b, sigma0_1); % generate a Gaussian factor of odometry fg.push_back(f1); % odometry between x1 and x2 -sigma2=.1; - A21=-eye(2); A22=eye(2); b = [.2;-.1]; - -f2 = GaussianFactor('x1', A21, 'x2', A22, b,sigma2); - +f2 = GaussianFactor('x1', A21, 'x2', A22, b,sigma0_1); fg.push_back(f2); % measurement between x1 and l1 -sigma3=.2; A31=-eye(2); A33=eye(2); b = [0;.2]; - - -f3 = GaussianFactor('x1', A31, 'l1', A33, b,sigma3); - +f3 = GaussianFactor('x1', A31, 'l1', A33, b,sigma0_2); fg.push_back(f3); % measurement between x2 and l1 -sigma4=.2; A42=-eye(2); A43=eye(2); b = [-.2;.3]; - - -f4 = GaussianFactor('x2', A42, 'l1', A43, b,sigma4); - +f4 = GaussianFactor('x2', A42, 'l1', A43, b,sigma0_2); fg.push_back(f4); end \ No newline at end of file diff --git a/matlab/testGaussianFactor.m b/matlab/testGaussianFactor.m index 0b678c2b3..f718ec395 100644 --- a/matlab/testGaussianFactor.m +++ b/matlab/testGaussianFactor.m @@ -25,14 +25,13 @@ Ax1 = [ % the RHS b2=[-1;1.5;2;-1]; - -combined = GaussianFactor('x2', Ax2, 'l1', Al1, 'x1', Ax1, b2, 1); +model = SharedDiagonal([1;1]); +combined = GaussianFactor('x2', Ax2, 'l1', Al1, 'x1', Ax1, b2, model); % eliminate the combined factor % NOT WORKING -% this is not working because there is no elimination function for a linear -% factor. Just for the MutableGaussianFactor -%[actualCG,actualLF] = combined.eliminate('x2'); +% this is not working because the eliminate has to be added back to gtsam.h +[actualCG,actualLF] = combined.eliminate('x2'); % create expected Conditional Gaussian R11 = [ @@ -48,7 +47,7 @@ S13 = [ +0.00,-8.94427 ]; d=[2.23607;-1.56525]; -expectedCG = GaussianConditional('x2',d,R11,'l1',S12,'x1',S13,[1 1]'); +expectedCG = GaussianConditional('x2',d,R11,'l1',S12,'x1',S13,model); % the expected linear factor Bl1 = [ @@ -70,5 +69,5 @@ expectedLF = GaussianFactor('l1', Bl1, 'x1', Bx1, b1, 1); % check if the result matches % NOT WORKING % because can not be computed with GaussianFactor.eliminate -%if(~actualCG.equals(expectedCG)), warning('GTSAM:unit','~actualCG.equals(expectedCG)'); end -%if(~actualLF.equals(expectedLF,1e-5)), warning('GTSAM:unit','~actualLF.equals(expectedLF,1e-5)');end +if(~actualCG.equals(expectedCG)), warning('GTSAM:unit','~actualCG.equals(expectedCG)'); end +if(~actualLF.equals(expectedLF,1e-5)), warning('GTSAM:unit','~actualLF.equals(expectedLF,1e-5)');end diff --git a/matlab/testLinearFactorGraph.m b/matlab/testLinearFactorGraph.m index 8c264dba1..0639f9d9c 100644 --- a/matlab/testLinearFactorGraph.m +++ b/matlab/testLinearFactorGraph.m @@ -2,7 +2,7 @@ % equals fg = createGaussianFactorGraph(); fg2 = createGaussianFactorGraph(); -CHECK('equals',fg.equals(fg2)); +CHECK('equals',fg.equals(fg2,1e-9)); %----------------------------------------------------------------------- % error diff --git a/wrap/wrap-matlab.h b/wrap/wrap-matlab.h index ff218d3a5..5ed48cbbf 100644 --- a/wrap/wrap-matlab.h +++ b/wrap/wrap-matlab.h @@ -15,10 +15,16 @@ extern "C" { #include using namespace std; -using namespace boost; // not usual, but for consiseness of generated code +using namespace boost; // not usual, but for conciseness of generated code +// start GTSAM Specifics ///////////////////////////////////////////////// typedef numeric::ublas::vector Vector; typedef numeric::ublas::matrix Matrix; +// to make keys be constructed from strings: +#define GTSAM_MAGIC_KEY +// to enable Matrix and Vector constructor for SharedGaussian: +#define GTSAM_MAGIC_GAUSSIAN +// end GTSAM Specifics ///////////////////////////////////////////////// #ifdef __LP64__ // 64-bit Mac