From fd07d61a2e7da9c23771f1172d2f216d75dc9f1c Mon Sep 17 00:00:00 2001 From: Yong-Dian Jian Date: Fri, 26 Aug 2011 21:41:01 +0000 Subject: [PATCH] Add a robust noise model. Change SharedGaussian to SharedNoiseModel --- examples/CameraResectioning.cpp | 2 +- examples/Pose2SLAMwSPCG_advanced.cpp | 2 +- examples/Pose2SLAMwSPCG_easy.cpp | 2 +- examples/vSLAMexample/vISAMexample.cpp | 6 +- examples/vSLAMexample/vSFMexample.cpp | 4 +- gtsam/linear/NoiseModel.cpp | 134 ++++++++++++++++++ gtsam/linear/NoiseModel.h | 102 ++++++++++++- gtsam/linear/SharedGaussian.h | 1 + gtsam/linear/SharedNoiseModel.h | 56 ++++++++ gtsam/linear/tests/testNoiseModel.cpp | 10 +- gtsam/nonlinear/NonlinearFactor.h | 32 ++--- gtsam/slam/BearingFactor.h | 2 +- gtsam/slam/BearingRangeFactor.h | 2 +- gtsam/slam/BetweenFactor.h | 2 +- gtsam/slam/GeneralSFMFactor.h | 2 +- gtsam/slam/PartialPriorFactor.h | 8 +- gtsam/slam/PriorFactor.h | 2 +- gtsam/slam/ProjectionFactor.h | 2 +- gtsam/slam/RangeFactor.h | 2 +- gtsam/slam/Simulated3D.h | 4 +- gtsam/slam/StereoFactor.h | 2 +- gtsam/slam/dataset.cpp | 2 +- gtsam/slam/planarSLAM.cpp | 10 +- gtsam/slam/planarSLAM.h | 10 +- gtsam/slam/pose2SLAM.cpp | 4 +- gtsam/slam/pose2SLAM.h | 4 +- gtsam/slam/pose3SLAM.cpp | 4 +- gtsam/slam/pose3SLAM.h | 4 +- gtsam/slam/simulated2D.h | 6 +- gtsam/slam/simulated2DOriented.h | 4 +- gtsam/slam/smallExample.cpp | 2 +- gtsam/slam/tests/testGeneralSFMFactor.cpp | 8 +- .../testGeneralSFMFactor_Cal3Bundler.cpp | 8 +- gtsam/slam/tests/testPlanarSLAM.cpp | 2 +- gtsam/slam/tests/testPose2SLAM.cpp | 2 +- gtsam/slam/tests/testPose3SLAM.cpp | 2 +- gtsam/slam/tests/testProjectionFactor.cpp | 2 +- gtsam/slam/tests/testStereoFactor.cpp | 2 +- gtsam/slam/tests/testVSLAM.cpp | 2 +- gtsam/slam/visualSLAM.h | 6 +- tests/testNonlinearEqualityConstraint.cpp | 2 +- tests/testNonlinearFactor.cpp | 2 +- tests/testPose2SLAMwSPCG.cpp | 2 +- tests/testSerialization.cpp | 42 +++--- 44 files changed, 395 insertions(+), 116 deletions(-) create mode 100644 gtsam/linear/SharedNoiseModel.h diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index d6ebb760b..740ae61e7 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -29,7 +29,7 @@ class ResectioningFactor: public NonlinearFactor1 { Point2 p_; // 2D measurement of the 3D point public: - ResectioningFactor(const SharedGaussian& model, const PoseKey& key, + ResectioningFactor(const SharedNoiseModel& model, const PoseKey& key, const shared_ptrK& calib, const Point2& p, const Point3& P) : Base(model, key), K_(calib), P_(P), p_(p) { } diff --git a/examples/Pose2SLAMwSPCG_advanced.cpp b/examples/Pose2SLAMwSPCG_advanced.cpp index 475dd0b82..c63b249dc 100644 --- a/examples/Pose2SLAMwSPCG_advanced.cpp +++ b/examples/Pose2SLAMwSPCG_advanced.cpp @@ -43,7 +43,7 @@ Values result; int main(void) { /* generate synthetic data */ - const SharedGaussian sigma(noiseModel::Unit::Create(0.1)); + const SharedNoiseModel sigma(noiseModel::Unit::Create(0.1)); Key x1(1), x2(2), x3(3), x4(4), x5(5), x6(6), x7(7), x8(8), x9(9); graph = boost::make_shared() ; diff --git a/examples/Pose2SLAMwSPCG_easy.cpp b/examples/Pose2SLAMwSPCG_easy.cpp index 54345c576..a1c1cb57e 100644 --- a/examples/Pose2SLAMwSPCG_easy.cpp +++ b/examples/Pose2SLAMwSPCG_easy.cpp @@ -34,7 +34,7 @@ Values result; int main(void) { /* generate synthetic data */ - const SharedGaussian sigma(noiseModel::Unit::Create(0.1)); + const SharedNoiseModel sigma(noiseModel::Unit::Create(0.1)); Key x1(1), x2(2), x3(3), x4(4), x5(5), x6(6), x7(7), x8(8), x9(9); // create a 3 by 3 grid diff --git a/examples/vSLAMexample/vISAMexample.cpp b/examples/vSLAMexample/vISAMexample.cpp index caf6c2a5c..dd67d3566 100644 --- a/examples/vSLAMexample/vISAMexample.cpp +++ b/examples/vSLAMexample/vISAMexample.cpp @@ -55,8 +55,8 @@ std::vector g_poses; // prior camera poses at each frame std::vector > g_measurements; // feature sets detected at each frame // Noise models -SharedGaussian measurementSigma(noiseModel::Isotropic::Sigma(2, 5.0f)); -SharedGaussian poseSigma(noiseModel::Unit::Create(1)); +SharedNoiseModel measurementSigma(noiseModel::Isotropic::Sigma(2, 5.0f)); +SharedNoiseModel poseSigma(noiseModel::Unit::Create(1)); /* ************************************************************************* */ @@ -82,7 +82,7 @@ void readAllDataISAM() { * Setup newFactors and initialValues for each new pose and set of measurements at each frame. */ void createNewFactors(shared_ptr& newFactors, boost::shared_ptr& initialValues, - int pose_id, Pose3& pose, std::vector& measurements, SharedGaussian measurementSigma, shared_ptrK calib) { + int pose_id, Pose3& pose, std::vector& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) { // Create a graph of newFactors with new measurements newFactors = shared_ptr (new Graph()); diff --git a/examples/vSLAMexample/vSFMexample.cpp b/examples/vSLAMexample/vSFMexample.cpp index 31873e940..b7ac7310c 100644 --- a/examples/vSLAMexample/vSFMexample.cpp +++ b/examples/vSLAMexample/vSFMexample.cpp @@ -52,7 +52,7 @@ map g_poses; // map: std::vector g_measurements; // Feature2D: {camera_id, landmark_id, 2d feature_position} // Noise models -SharedGaussian measurementSigma(noiseModel::Isotropic::Sigma(2, 5.0f)); +SharedNoiseModel measurementSigma(noiseModel::Isotropic::Sigma(2, 5.0f)); /* ************************************************************************* */ /** @@ -79,7 +79,7 @@ void readAllData() { * by adding and linking 2D features (measurements) detected in each captured image * with their corresponding landmarks. */ -Graph setupGraph(std::vector& measurements, SharedGaussian measurementSigma, shared_ptrK calib) { +Graph setupGraph(std::vector& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) { Graph g; diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 0a51ad966..03c09502e 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -164,6 +164,24 @@ SharedDiagonal Gaussian::Cholesky(Matrix& Ab, size_t nFrontals) const { return Unit::Create(maxrank); } +void Gaussian::WhitenSystem(Matrix& A, Vector& b) const { + WhitenInPlace(A); + whitenInPlace(b); +} + +void Gaussian::WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const { + WhitenInPlace(A1); + WhitenInPlace(A2); + whitenInPlace(b); +} + +void Gaussian::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const{ + WhitenInPlace(A1); + WhitenInPlace(A2); + WhitenInPlace(A3); + whitenInPlace(b); +} + /* ************************************************************************* */ // Diagonal /* ************************************************************************* */ @@ -414,6 +432,122 @@ void Unit::print(const std::string& name) const { cout << name << ": unit (" << dim_ << ") " << endl; } +/* ************************************************************************* */ +// M-Estimator +/* ************************************************************************* */ + +namespace MEstimator { +Vector Base::weight(const Vector &error) const { + const size_t n = error.rows(); + Vector w(n); + for ( size_t i = 0 ; i < n ; ++i ) + w(i) = weight(error(i)); + return w; +} + +void Base::reweight(Matrix &A, Vector &error) const { + const Vector W = weight(error); + vector_scale_inplace(W,A); + error = emul(W, error); +} + +void Base::reweight(Matrix &A1, Matrix &A2, Vector &error) const { + const Vector W = weight(error); + vector_scale_inplace(W,A1); + vector_scale_inplace(W,A2); + error = emul(W, error); +} + +void Base::reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const { + const Vector W = weight(error); + vector_scale_inplace(W,A1); + vector_scale_inplace(W,A2); + vector_scale_inplace(W,A3); + error = emul(W, error); +} + +Fair::Fair(const double c): c_(c) { + if ( c_ <= 0 ) { + cout << "MEstimator Fair takes only positive double in constructor. forced to 1.0" << endl; + c_ = 1.0; + } +} + +double Fair::weight(const double &error) const +{ return 1.0 / (1.0 + fabs(error)/c_); } + +void Fair::print(const std::string &s) const +{ cout << s << ": fair (" << c_ << ")" << endl; } + +bool Fair::equals(const Base &expected, const double tol) const { + const Fair* p = dynamic_cast (&expected); + if (p == NULL) return false; + return fabs(c_ - p->c_ ) < tol; +} + +Fair::shared_ptr Fair::Create(const double c) +{ return shared_ptr(new Fair(c)); } + +Huber::Huber(const double k): k_(k) { + if ( k_ <= 0 ) { + cout << "MEstimator Huber takes only positive double in constructor. forced to 1.0" << endl; + k_ = 1.0; + } +} + +double Huber::weight(const double &error) const +{ return (error < k_) ? (1.0) : (k_ / fabs(error)); } + +void Huber::print(const std::string &s) const +{ cout << s << ": huber (" << k_ << ")" << endl; } + +bool Huber::equals(const Base &expected, const double tol) const { + const Huber* p = dynamic_cast (&expected); + if (p == NULL) return false; + return fabs(k_ - p->k_ ) < tol; +} + +Huber::shared_ptr Huber::Create(const double c) +{ return shared_ptr(new Huber(c)); } + +} + +/* ************************************************************************* */ +// Robust +/* ************************************************************************* */ + +void Robust::print(const std::string& name) const { + robust_->print(name); + noise_->print(name); +} + +bool Robust::equals(const Base& expected, double tol) const { + const Robust* p = dynamic_cast (&expected); + if (p == NULL) return false; + return noise_->equals(*p->noise_,tol) && robust_->equals(*p->robust_,tol); +} + +void Robust::WhitenSystem(Matrix& A, Vector& b) const { + noise_->WhitenSystem(A,b); + robust_->reweight(A,b); +} + +void Robust::WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const { + noise_->WhitenSystem(A1,A2,b); + robust_->reweight(A1,A2,b); +} + +void Robust::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const{ + noise_->WhitenSystem(A1,A2,A3,b); + robust_->reweight(A1,A2,A3,b); +} + +Robust::shared_ptr Robust::Create( + const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise){ + return shared_ptr(new Robust(robust,noise)); +} + + /* ************************************************************************* */ } diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index d9f9c80e2..2808472b6 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -43,6 +43,9 @@ namespace gtsam { */ class Base : public Testable { + public: + typedef boost::shared_ptr shared_ptr; + protected: size_t dim_; @@ -58,6 +61,10 @@ namespace gtsam { */ inline size_t dim() const { return dim_;} + virtual void print(const std::string& name) const = 0; + + virtual bool equals(const Base& expected, double tol=1e-9) const = 0; + /** * Whiten an error vector. */ @@ -68,6 +75,15 @@ namespace gtsam { */ virtual Vector unwhiten(const Vector& v) const = 0; + /** + * Mahalanobis distance v'*R'*R*v = + */ + virtual double Mahalanobis(const Vector& v) const = 0; + + virtual void WhitenSystem(Matrix& A, Vector& b) const = 0; + virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const = 0; + virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const = 0; + /** in-place whiten, override if can be done more efficiently */ virtual void whitenInPlace(Vector& v) const { v = whiten(v); @@ -166,10 +182,9 @@ namespace gtsam { /** * Whiten a system, in place as well */ - inline virtual void WhitenSystem(Matrix& A, Vector& b) const { - WhitenInPlace(A); - whitenInPlace(b); - } + virtual void WhitenSystem(Matrix& A, Vector& b) const ; + virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const ; + virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const; /** * Apply appropriately weighted QR factorization to the system [A b] @@ -488,6 +503,85 @@ namespace gtsam { } }; + namespace MEstimator { + + class Base { + public: + typedef boost::shared_ptr shared_ptr; + Base() {} + virtual ~Base() {} + virtual double weight(const double &error) const = 0; + virtual void print(const std::string &s) const = 0; + virtual bool equals(const Base& expected, const double tol=1e-8) const = 0; + Vector weight(const Vector &error) const; + void reweight(Matrix &A, Vector &error) const; + void reweight(Matrix &A1, Matrix &A2, Vector &error) const; + void reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const; + }; + + class Fair : public Base { + public: + typedef boost::shared_ptr shared_ptr; + protected: + double c_; + public: + Fair(const double c); + virtual ~Fair() {} + virtual double weight(const double &error) const ; + virtual void print(const std::string &s) const ; + virtual bool equals(const Base& expected, const double tol=1e-8) const ; + static shared_ptr Create(const double c) ; + }; + + class Huber : public Base { + public: + typedef boost::shared_ptr shared_ptr; + protected: + double k_; + public: + Huber(const double k); + virtual ~Huber() {} + virtual double weight(const double &error) const ; + virtual void print(const std::string &s) const ; + virtual bool equals(const Base& expected, const double tol=1e-8) const ; + static shared_ptr Create(const double k) ; + }; + } + + class Robust : public Base { + public: + typedef boost::shared_ptr shared_ptr; + + protected: + typedef MEstimator::Base RobustModel; + typedef noiseModel::Base NoiseModel; + + const RobustModel::shared_ptr robust_; + const NoiseModel::shared_ptr noise_; + + public: + Robust(const RobustModel::shared_ptr robust, const NoiseModel::shared_ptr noise) + : Base(noise->dim()), robust_(robust), noise_(noise) {} + virtual ~Robust() {} + virtual void print(const std::string& name) const; + virtual bool equals(const Base& expected, double tol=1e-9) const; + inline virtual Vector whiten(const Vector& v) const + { return noise_->whiten(v); } + inline virtual Vector unwhiten(const Vector& v) const + { return noise_->unwhiten(v); } + inline virtual double Mahalanobis(const Vector& v) const + { return noise_->Mahalanobis(v); } + virtual void WhitenSystem(Matrix& A, Vector& b) const ; + virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const ; + virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const; + + static shared_ptr Create( + const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise); + + private: + Robust(); + }; + } // namespace noiseModel } // namespace gtsam diff --git a/gtsam/linear/SharedGaussian.h b/gtsam/linear/SharedGaussian.h index 7d86ab6d2..3e6c5ce0f 100644 --- a/gtsam/linear/SharedGaussian.h +++ b/gtsam/linear/SharedGaussian.h @@ -19,6 +19,7 @@ #pragma once #include +#include namespace gtsam { // note, deliberately not in noiseModel namespace diff --git a/gtsam/linear/SharedNoiseModel.h b/gtsam/linear/SharedNoiseModel.h new file mode 100644 index 000000000..5216057ce --- /dev/null +++ b/gtsam/linear/SharedNoiseModel.h @@ -0,0 +1,56 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +#pragma once + +#include + +namespace gtsam { // note, deliberately not in noiseModel namespace + + struct SharedNoiseModel: public noiseModel::Base::shared_ptr { + + typedef noiseModel::Base::shared_ptr Base; + + SharedNoiseModel() {} + SharedNoiseModel(const noiseModel::Robust::shared_ptr& p): Base(p) {} + SharedNoiseModel(const noiseModel::Gaussian::shared_ptr& p): Base(p) {} + SharedNoiseModel(const noiseModel::Diagonal::shared_ptr& p): Base(p) {} + SharedNoiseModel(const noiseModel::Constrained::shared_ptr& p): Base(p) {} + SharedNoiseModel(const noiseModel::Isotropic::shared_ptr& p): Base(p) {} + SharedNoiseModel(const noiseModel::Unit::shared_ptr& p): Base(p) {} + + #ifdef GTSAM_MAGIC_GAUSSIAN + SharedNoiseModel(const Matrix& covariance) : + Base(boost::static_pointer_cast( + noiseModel::Gaussian::Covariance(covariance))) {} + + SharedNoiseModel(const Vector& sigmas) : + Base(boost::static_pointer_cast( + 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 + SharedNoiseModel(const double& s) : + Base(boost::static_pointer_cast( + noiseModel::Isotropic::Sigma(GTSAM_DANGEROUS_GAUSSIAN, s))) {} + #endif + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("SharedNoiseModel", + boost::serialization::base_object(*this)); + } + }; +} diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index a0ac06847..6cf02b255 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -26,7 +26,7 @@ using namespace boost::assign; #include #include #include -#include +#include #include using namespace std; @@ -238,8 +238,8 @@ TEST(NoiseModel, QRNan ) TEST(NoiseModel, SmartCovariance ) { bool smart = true; - SharedGaussian expected = Unit::Create(3); - SharedGaussian actual = Gaussian::Covariance(eye(3), smart); + SharedNoiseModel expected = Unit::Create(3); + SharedNoiseModel actual = Gaussian::Covariance(eye(3), smart); EXPECT(assert_equal(*expected,*actual)); } @@ -247,8 +247,8 @@ TEST(NoiseModel, SmartCovariance ) TEST(NoiseModel, ScalarOrVector ) { bool smart = true; - SharedGaussian expected = Unit::Create(3); - SharedGaussian actual = Gaussian::Covariance(eye(3), smart); + SharedNoiseModel expected = Unit::Create(3); + SharedNoiseModel actual = Gaussian::Covariance(eye(3), smart); EXPECT(assert_equal(*expected,*actual)); } diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index d92c14079..bb7508093 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -27,7 +27,7 @@ #include #include -#include +#include #include #include @@ -49,7 +49,7 @@ namespace gtsam { typedef NonlinearFactor This; - SharedGaussian noiseModel_; /** Noise model */ + SharedNoiseModel noiseModel_; /** Noise model */ public: @@ -65,7 +65,7 @@ namespace gtsam { * Constructor * @param noiseModel shared pointer to a noise model */ - NonlinearFactor(const SharedGaussian& noiseModel) : + NonlinearFactor(const SharedNoiseModel& noiseModel) : noiseModel_(noiseModel) { } @@ -74,7 +74,7 @@ namespace gtsam { * @param z measurement * @param key by which to look up X value in Values */ - NonlinearFactor(const SharedGaussian& noiseModel, const Symbol& key1) : + NonlinearFactor(const SharedNoiseModel& noiseModel, const Symbol& key1) : Factor(key1), noiseModel_(noiseModel) { } @@ -83,7 +83,7 @@ namespace gtsam { * @param j1 key of the first variable * @param j2 key of the second variable */ - NonlinearFactor(const SharedGaussian& noiseModel, const Symbol& j1, const Symbol& j2) : + NonlinearFactor(const SharedNoiseModel& noiseModel, const Symbol& j1, const Symbol& j2) : Factor(j1,j2), noiseModel_(noiseModel) { } @@ -91,7 +91,7 @@ namespace gtsam { * Constructor - arbitrary number of keys * @param keys is the set of Symbols in the factor */ - NonlinearFactor(const SharedGaussian& noiseModel, const std::set& keys) : + NonlinearFactor(const SharedNoiseModel& noiseModel, const std::set& keys) : Factor(keys), noiseModel_(noiseModel) { } @@ -120,7 +120,7 @@ namespace gtsam { } /** access to the noise model */ - SharedGaussian get_noiseModel() const { + SharedNoiseModel get_noiseModel() const { return noiseModel_; } @@ -197,7 +197,7 @@ namespace gtsam { * @param z measurement * @param key by which to look up X value in Values */ - NonlinearFactor1(const SharedGaussian& noiseModel, const KEY& key1) : + NonlinearFactor1(const SharedNoiseModel& noiseModel, const KEY& key1) : Base(noiseModel,key1), key_(key1) { } @@ -235,8 +235,7 @@ namespace gtsam { boost::shared_dynamic_cast(this->noiseModel_); if (constrained.get() != NULL) return GaussianFactor::shared_ptr(new JacobianFactor(var, A, b, constrained)); - this->noiseModel_->WhitenInPlace(A); - this->noiseModel_->whitenInPlace(b); + this->noiseModel_->WhitenSystem(A,b); return GaussianFactor::shared_ptr(new JacobianFactor(var, A, b, noiseModel::Unit::Create(b.size()))); } @@ -304,7 +303,7 @@ namespace gtsam { * @param j1 key of the first variable * @param j2 key of the second variable */ - NonlinearFactor2(const SharedGaussian& noiseModel, KEY1 j1, KEY2 j2) : + NonlinearFactor2(const SharedNoiseModel& noiseModel, KEY1 j1, KEY2 j2) : Base(noiseModel,j1,j2), key1_(j1), key2_(j2) { } @@ -349,9 +348,7 @@ namespace gtsam { return JacobianFactor::shared_ptr(new JacobianFactor(var1, A1, var2, A2, b, constrained)); } - this->noiseModel_->WhitenInPlace(A1); - this->noiseModel_->WhitenInPlace(A2); - this->noiseModel_->whitenInPlace(b); + this->noiseModel_->WhitenSystem(A1,A2,b); return GaussianFactor::shared_ptr(new JacobianFactor(var1, A1, var2, A2, b, noiseModel::Unit::Create(b.size()))); } @@ -435,7 +432,7 @@ namespace gtsam { * @param j2 key of the second variable * @param j3 key of the third variable */ - NonlinearFactor3(const SharedGaussian& noiseModel, KEY1 j1, KEY2 j2, KEY3 j3) : + NonlinearFactor3(const SharedNoiseModel& noiseModel, KEY1 j1, KEY2 j2, KEY3 j3) : Base(noiseModel), key1_(j1), key2_(j2), key3_(j3) { this->keys_.reserve(3); this->keys_.push_back(key1_); @@ -487,10 +484,7 @@ namespace gtsam { return GaussianFactor::shared_ptr( new JacobianFactor(var1, A1, var2, A2, var3, A3, b, constrained)); } - this->noiseModel_->WhitenInPlace(A1); - this->noiseModel_->WhitenInPlace(A2); - this->noiseModel_->WhitenInPlace(A3); - this->noiseModel_->whitenInPlace(b); + this->noiseModel_->WhitenSystem(A1,A2,A3,b); return GaussianFactor::shared_ptr( new JacobianFactor(var1, A1, var2, A2, var3, A3, b, noiseModel::Unit::Create(b.size()))); } diff --git a/gtsam/slam/BearingFactor.h b/gtsam/slam/BearingFactor.h index dbdc4111d..1860343c8 100644 --- a/gtsam/slam/BearingFactor.h +++ b/gtsam/slam/BearingFactor.h @@ -40,7 +40,7 @@ namespace gtsam { /** primary constructor */ BearingFactor(const POSEKEY& i, const POINTKEY& j, const Rot2& z, - const SharedGaussian& model) : + const SharedNoiseModel& model) : Base(model, i, j), z_(z) { } diff --git a/gtsam/slam/BearingRangeFactor.h b/gtsam/slam/BearingRangeFactor.h index 3003983b9..070f6cf73 100644 --- a/gtsam/slam/BearingRangeFactor.h +++ b/gtsam/slam/BearingRangeFactor.h @@ -42,7 +42,7 @@ namespace gtsam { BearingRangeFactor() {} /* Default constructor */ BearingRangeFactor(const POSEKEY& i, const POINTKEY& j, const Rot2& bearing, const double range, - const SharedGaussian& model) : + const SharedNoiseModel& model) : Base(model, i, j), bearing_(bearing), range_(range) { } diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index b76ab4b80..b74efeeb0 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -48,7 +48,7 @@ namespace gtsam { /** Constructor */ BetweenFactor(const KEY1& key1, const KEY1& key2, const T& measured, - const SharedGaussian& model) : + const SharedNoiseModel& model) : Base(model, key1, key2), measured_(measured) { } diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 4a8a6f640..48c10e444 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -55,7 +55,7 @@ namespace gtsam { GeneralSFMFactor():z_(0.0,0.0) {} GeneralSFMFactor(const Point2 & p):z_(p) {} GeneralSFMFactor(double x, double y):z_(x,y) {} - GeneralSFMFactor(const Point2& z, const SharedGaussian& model, const CamK& i, const LmK& j) : Base(model, i, j), z_(z) {} + GeneralSFMFactor(const Point2& z, const SharedNoiseModel& model, const CamK& i, const LmK& j) : Base(model, i, j), z_(z) {} virtual ~GeneralSFMFactor() {} diff --git a/gtsam/slam/PartialPriorFactor.h b/gtsam/slam/PartialPriorFactor.h index 5aaeefddd..6d5129574 100644 --- a/gtsam/slam/PartialPriorFactor.h +++ b/gtsam/slam/PartialPriorFactor.h @@ -60,7 +60,7 @@ namespace gtsam { * computation in the constructor. This should only be used by subclasses * Sets the size of the mask with all values off */ - PartialPriorFactor(const KEY& key, const SharedGaussian& model) + PartialPriorFactor(const KEY& key, const SharedNoiseModel& model) : Base(model, key), mask_(model->dim(), false) {} public: @@ -72,7 +72,7 @@ namespace gtsam { /** Full Constructor: requires mask and vector - not for typical use */ PartialPriorFactor(const KEY& key, const std::vector& mask, - const Vector& prior, const SharedGaussian& model) : + const Vector& prior, const SharedNoiseModel& model) : Base(model, key), prior_(prior), mask_(mask) { assert(mask_.size() == T::Dim()); // NOTE: assumes constant size variable assert(nrTrue() == model->dim()); @@ -80,7 +80,7 @@ namespace gtsam { } /** Single Element Constructor: acts on a single parameter specified by idx */ - PartialPriorFactor(const KEY& key, size_t idx, double prior, const SharedGaussian& model) : + PartialPriorFactor(const KEY& key, size_t idx, double prior, const SharedNoiseModel& model) : Base(model, key), prior_(Vector_(1, prior)), mask_(T::Dim(), false) { assert(model->dim() == 1); mask_[idx] = true; @@ -89,7 +89,7 @@ namespace gtsam { /** Indices Constructor: specify the mask with a set of indices */ PartialPriorFactor(const KEY& key, const std::vector& mask, const Vector& prior, - const SharedGaussian& model) : + const SharedNoiseModel& model) : Base(model, key), prior_(prior), mask_(T::Dim(), false) { assert((size_t)prior_.size() == mask.size()); assert(model->dim() == (size_t) prior.size()); diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index b23245ca9..032bbae39 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -51,7 +51,7 @@ namespace gtsam { /** Constructor */ PriorFactor(const KEY& key, const T& prior, - const SharedGaussian& model) : + const SharedNoiseModel& model) : Base(model, key), prior_(prior) { } diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index f097e8f2c..6135bfc4b 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -59,7 +59,7 @@ namespace gtsam { * @param K the constant calibration */ GenericProjectionFactor(const Point2& z, - const SharedGaussian& model, POSK j_pose, + const SharedNoiseModel& model, POSK j_pose, LMK j_landmark, const shared_ptrK& K) : Base(model, j_pose, j_landmark), z_(z), K_(K) { } diff --git a/gtsam/slam/RangeFactor.h b/gtsam/slam/RangeFactor.h index 1bda0b7bb..dc91b529b 100644 --- a/gtsam/slam/RangeFactor.h +++ b/gtsam/slam/RangeFactor.h @@ -38,7 +38,7 @@ namespace gtsam { RangeFactor() {} /* Default constructor */ RangeFactor(const POSEKEY& i, const POINTKEY& j, double z, - const SharedGaussian& model) : + const SharedNoiseModel& model) : Base(model, i, j), z_(z) { } diff --git a/gtsam/slam/Simulated3D.h b/gtsam/slam/Simulated3D.h index 80694eb51..5689cefda 100644 --- a/gtsam/slam/Simulated3D.h +++ b/gtsam/slam/Simulated3D.h @@ -62,7 +62,7 @@ namespace simulated3D { Point3 z_; PointPrior3D(const Point3& z, - const SharedGaussian& model, const PoseKey& j) : + const SharedNoiseModel& model, const PoseKey& j) : NonlinearFactor1 (model, j), z_(z) { } @@ -78,7 +78,7 @@ namespace simulated3D { Point3 z_; Simulated3DMeasurement(const Point3& z, - const SharedGaussian& model, PoseKey& j1, PointKey j2) : + const SharedNoiseModel& model, PoseKey& j1, PointKey j2) : NonlinearFactor2 ( model, j1, j2), z_(z) { } diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index 5b0c1cce6..13dc8b3e4 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -51,7 +51,7 @@ public: * @param landmarkNumber is the index of the landmark * @param K the constant calibration */ - GenericStereoFactor(const StereoPoint2& z, const SharedGaussian& model, KEY1 j_pose, + GenericStereoFactor(const StereoPoint2& z, const SharedNoiseModel& model, KEY1 j_pose, KEY2 j_landmark, const shared_ptrKStereo& K) : Base(model, j_pose, j_landmark), z_(z), K_(K) { } diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 087d7d5e5..5d1e03358 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -124,7 +124,7 @@ pair load2D(const string& filename, Pose2 l1Xl2(x, y, yaw); - // SharedGaussian noise = noiseModel::Gaussian::Covariance(m, smart); + // SharedNoiseModel noise = noiseModel::Gaussian::Covariance(m, smart); if (!model) { Vector variances = Vector_(3,m(0,0),m(1,1),m(2,2)); model = noiseModel::Diagonal::Variances(variances, smart); diff --git a/gtsam/slam/planarSLAM.cpp b/gtsam/slam/planarSLAM.cpp index 756d5228f..596e43a94 100644 --- a/gtsam/slam/planarSLAM.cpp +++ b/gtsam/slam/planarSLAM.cpp @@ -35,7 +35,7 @@ namespace gtsam { NonlinearFactorGraph(graph) {} void Graph::addPrior(const PoseKey& i, const Pose2& p, - const SharedGaussian& model) { + const SharedNoiseModel& model) { sharedFactor factor(new Prior(i, p, model)); push_back(factor); } @@ -46,25 +46,25 @@ namespace gtsam { } void Graph::addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z, - const SharedGaussian& model) { + const SharedNoiseModel& 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 SharedNoiseModel& 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 SharedNoiseModel& model) { sharedFactor factor(new Range(i, j, z, model)); push_back(factor); } void Graph::addBearingRange(const PoseKey& i, const PointKey& j, const Rot2& z1, - double z2, const SharedGaussian& model) { + double z2, const SharedNoiseModel& model) { sharedFactor factor(new BearingRange(i, j, z1, z2, model)); push_back(factor); } diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h index 8d1c44a1b..b4ca6bd8b 100644 --- a/gtsam/slam/planarSLAM.h +++ b/gtsam/slam/planarSLAM.h @@ -50,16 +50,16 @@ namespace gtsam { struct Graph: public NonlinearFactorGraph { Graph(){} Graph(const NonlinearFactorGraph& graph); - void addPrior(const PoseKey& i, const Pose2& p, const SharedGaussian& model); + void addPrior(const PoseKey& i, const Pose2& p, const SharedNoiseModel& model); void addPoseConstraint(const PoseKey& i, const Pose2& p); void addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z, - const SharedGaussian& model); + const SharedNoiseModel& model); void addBearing(const PoseKey& i, const PointKey& j, const Rot2& z, - const SharedGaussian& model); + const SharedNoiseModel& model); void addRange(const PoseKey& i, const PointKey& j, double z, - const SharedGaussian& model); + const SharedNoiseModel& model); void addBearingRange(const PoseKey& i, const PointKey& j, - const Rot2& z1, double z2, const SharedGaussian& model); + const Rot2& z1, double z2, const SharedNoiseModel& model); }; // Optimizer diff --git a/gtsam/slam/pose2SLAM.cpp b/gtsam/slam/pose2SLAM.cpp index e606f8d85..b54e18f28 100644 --- a/gtsam/slam/pose2SLAM.cpp +++ b/gtsam/slam/pose2SLAM.cpp @@ -42,13 +42,13 @@ namespace gtsam { /* ************************************************************************* */ void Graph::addPrior(const Key& i, const Pose2& p, - const SharedGaussian& model) { + const SharedNoiseModel& 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 SharedNoiseModel& model) { sharedFactor factor(new Constraint(i, j, z, model)); push_back(factor); } diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h index e7cac8552..a00b91ff2 100644 --- a/gtsam/slam/pose2SLAM.h +++ b/gtsam/slam/pose2SLAM.h @@ -56,8 +56,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 SharedNoiseModel& model); + void addConstraint(const Key& i, const Key& j, const Pose2& z, const SharedNoiseModel& model); void addHardConstraint(const Key& i, const Pose2& p); }; diff --git a/gtsam/slam/pose3SLAM.cpp b/gtsam/slam/pose3SLAM.cpp index c0897b87c..96a58859b 100644 --- a/gtsam/slam/pose3SLAM.cpp +++ b/gtsam/slam/pose3SLAM.cpp @@ -52,13 +52,13 @@ namespace gtsam { /* ************************************************************************* */ void Graph::addPrior(const Key& i, const Pose3& p, - const SharedGaussian& model) { + const SharedNoiseModel& 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 SharedNoiseModel& model) { sharedFactor factor(new Constraint(i, j, z, model)); push_back(factor); } diff --git a/gtsam/slam/pose3SLAM.h b/gtsam/slam/pose3SLAM.h index 945fa57c9..0125452fb 100644 --- a/gtsam/slam/pose3SLAM.h +++ b/gtsam/slam/pose3SLAM.h @@ -52,9 +52,9 @@ namespace gtsam { // Graph struct Graph: public NonlinearFactorGraph { void addPrior(const Key& i, const Pose3& p, - const SharedGaussian& model); + const SharedNoiseModel& model); void addConstraint(const Key& i, const Key& j, const Pose3& z, - const SharedGaussian& model); + const SharedNoiseModel& model); void addHardConstraint(const Key& i, const Pose3& p); }; diff --git a/gtsam/slam/simulated2D.h b/gtsam/slam/simulated2D.h index 07cc69f5b..e95f699f1 100644 --- a/gtsam/slam/simulated2D.h +++ b/gtsam/slam/simulated2D.h @@ -105,7 +105,7 @@ namespace gtsam { typedef typename KEY::Value Pose; Pose z_; - GenericPrior(const Pose& z, const SharedGaussian& model, const KEY& key) : + GenericPrior(const Pose& z, const SharedNoiseModel& model, const KEY& key) : NonlinearFactor1 (model, key), z_(z) { } @@ -136,7 +136,7 @@ namespace gtsam { typedef typename KEY::Value Pose; Pose z_; - GenericOdometry(const Pose& z, const SharedGaussian& model, + GenericOdometry(const Pose& z, const SharedNoiseModel& model, const KEY& i1, const KEY& i2) : NonlinearFactor2 (model, i1, i2), z_(z) { } @@ -170,7 +170,7 @@ namespace gtsam { Point z_; - GenericMeasurement(const Point& z, const SharedGaussian& model, + GenericMeasurement(const Point& z, const SharedNoiseModel& model, const XKEY& i, const LKEY& j) : NonlinearFactor2 (model, i, j), z_(z) { } diff --git a/gtsam/slam/simulated2DOriented.h b/gtsam/slam/simulated2DOriented.h index 115a1f71d..420be1819 100644 --- a/gtsam/slam/simulated2DOriented.h +++ b/gtsam/slam/simulated2DOriented.h @@ -63,7 +63,7 @@ namespace gtsam { Pose2 z_; - GenericPosePrior(const Pose2& z, const SharedGaussian& model, const Key& key) : + GenericPosePrior(const Pose2& z, const SharedNoiseModel& model, const Key& key) : NonlinearFactor1 (model, key), z_(z) { } @@ -81,7 +81,7 @@ namespace gtsam { struct GenericOdometry: public NonlinearFactor2 { Pose2 z_; - GenericOdometry(const Pose2& z, const SharedGaussian& model, + GenericOdometry(const Pose2& z, const SharedNoiseModel& model, const KEY& i1, const KEY& i2) : NonlinearFactor2 (model, i1, i2), z_(z) { } diff --git a/gtsam/slam/smallExample.cpp b/gtsam/slam/smallExample.cpp index 92767eb97..78ffeb6f8 100644 --- a/gtsam/slam/smallExample.cpp +++ b/gtsam/slam/smallExample.cpp @@ -201,7 +201,7 @@ namespace example { Point2 z_; - UnaryFactor(const Point2& z, const SharedGaussian& model, + UnaryFactor(const Point2& z, const SharedNoiseModel& model, const simulated2D::PoseKey& key) : gtsam::NonlinearFactor1(model, key), z_(z) { } diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 45aba8af1..1bc991da6 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -42,7 +42,7 @@ typedef NonlinearEquality Point3Constraint; class Graph: public NonlinearFactorGraph { public: - void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedGaussian& model) { + void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedNoiseModel& model) { push_back(boost::make_shared(z, model, i, j)); } @@ -75,7 +75,7 @@ double getGaussian() typedef NonlinearOptimizer Optimizer; -const SharedGaussian sigma1(noiseModel::Unit::Create(1)); +const SharedNoiseModel sigma1(noiseModel::Unit::Create(1)); /* ************************************************************************* */ TEST( GeneralSFMFactor, equals ) @@ -83,7 +83,7 @@ TEST( GeneralSFMFactor, equals ) // Create two identical factors and make sure they're equal Vector z = Vector_(2,323.,240.); const int cameraFrameNumber=1, landmarkNumber=1; - const SharedGaussian sigma(noiseModel::Unit::Create(1)); + const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); boost::shared_ptr factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); @@ -97,7 +97,7 @@ TEST( GeneralSFMFactor, equals ) TEST( GeneralSFMFactor, error ) { Point2 z(3.,0.); const int cameraFrameNumber=1, landmarkNumber=1; - const SharedGaussian sigma(noiseModel::Unit::Create(1)); + const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); boost::shared_ptr factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); // For the following configuration, the factor predicts 320,240 diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index 5954882e1..03e1bb2ed 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -41,7 +41,7 @@ typedef NonlinearEquality Point3Constraint; class Graph: public NonlinearFactorGraph { public: - void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedGaussian& model) { + void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedNoiseModel& model) { push_back(boost::make_shared(z, model, i, j)); } @@ -74,7 +74,7 @@ double getGaussian() typedef NonlinearOptimizer Optimizer; -const SharedGaussian sigma1(noiseModel::Unit::Create(1)); +const SharedNoiseModel sigma1(noiseModel::Unit::Create(1)); /* ************************************************************************* */ TEST( GeneralSFMFactor, equals ) @@ -82,7 +82,7 @@ TEST( GeneralSFMFactor, equals ) // Create two identical factors and make sure they're equal Vector z = Vector_(2,323.,240.); const int cameraFrameNumber=1, landmarkNumber=1; - const SharedGaussian sigma(noiseModel::Unit::Create(1)); + const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); boost::shared_ptr factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); @@ -96,7 +96,7 @@ TEST( GeneralSFMFactor, equals ) TEST( GeneralSFMFactor, error ) { Point2 z(3.,0.); const int cameraFrameNumber=1, landmarkNumber=1; - const SharedGaussian sigma(noiseModel::Unit::Create(1)); + const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); boost::shared_ptr factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); // For the following configuration, the factor predicts 320,240 diff --git a/gtsam/slam/tests/testPlanarSLAM.cpp b/gtsam/slam/tests/testPlanarSLAM.cpp index a7f45f6d9..61bccef46 100644 --- a/gtsam/slam/tests/testPlanarSLAM.cpp +++ b/gtsam/slam/tests/testPlanarSLAM.cpp @@ -28,7 +28,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 +SharedNoiseModel sigma(noiseModel::Isotropic::Sigma(1,0.1)), sigma2(noiseModel::Isotropic::Sigma(2,0.1)), I3(noiseModel::Unit::Create(3)); diff --git a/gtsam/slam/tests/testPose2SLAM.cpp b/gtsam/slam/tests/testPose2SLAM.cpp index 777a37946..8cd0428a8 100644 --- a/gtsam/slam/tests/testPose2SLAM.cpp +++ b/gtsam/slam/tests/testPose2SLAM.cpp @@ -331,7 +331,7 @@ TEST( Pose2Values, expmap ) } // Common measurement covariance -static SharedGaussian sigmas = sharedSigmas(Vector_(3,sx,sy,st)); +static SharedNoiseModel sigmas = sharedSigmas(Vector_(3,sx,sy,st)); /* ************************************************************************* */ // Very simple test establishing Ax-b \approx z-h(x) diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp index 999ac93c7..a1226a4e6 100644 --- a/gtsam/slam/tests/testPose3SLAM.cpp +++ b/gtsam/slam/tests/testPose3SLAM.cpp @@ -132,7 +132,7 @@ TEST( Pose3Factor, error ) Pose3 z(Rot3::rodriguez(0.2,0.2,0.3),Point3(0,1.1,0));; // Create factor - SharedGaussian I6(noiseModel::Unit::Create(6)); + SharedNoiseModel I6(noiseModel::Unit::Create(6)); Pose3Factor factor(1,2, z, I6); // Create config diff --git a/gtsam/slam/tests/testProjectionFactor.cpp b/gtsam/slam/tests/testProjectionFactor.cpp index a3342f858..952c2975f 100644 --- a/gtsam/slam/tests/testProjectionFactor.cpp +++ b/gtsam/slam/tests/testProjectionFactor.cpp @@ -37,7 +37,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 SharedNoiseModel sigma(noiseModel::Unit::Create(1)); static shared_ptrK sK(new Cal3_S2(K)); // make cameras diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp index 4e7143ebd..5182f567b 100644 --- a/gtsam/slam/tests/testStereoFactor.cpp +++ b/gtsam/slam/tests/testStereoFactor.cpp @@ -45,7 +45,7 @@ StereoCamera stereoCam(Pose3(), K); // point X Y Z in meters Point3 p(0, 0, 5); -static SharedGaussian sigma(noiseModel::Unit::Create(1)); +static SharedNoiseModel sigma(noiseModel::Unit::Create(1)); /* ************************************************************************* */ TEST( StereoFactor, singlePoint) diff --git a/gtsam/slam/tests/testVSLAM.cpp b/gtsam/slam/tests/testVSLAM.cpp index 3bb7dbbb0..9651ee4e1 100644 --- a/gtsam/slam/tests/testVSLAM.cpp +++ b/gtsam/slam/tests/testVSLAM.cpp @@ -35,7 +35,7 @@ using namespace gtsam; using namespace gtsam::visualSLAM; using namespace boost; -static SharedGaussian sigma(noiseModel::Unit::Create(1)); +static SharedNoiseModel sigma(noiseModel::Unit::Create(1)); /* ************************************************************************* */ Point3 landmark1(-1.0,-1.0, 0.0); diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h index 3a58395a8..044e96e43 100644 --- a/gtsam/slam/visualSLAM.h +++ b/gtsam/slam/visualSLAM.h @@ -79,7 +79,7 @@ namespace gtsam { * @param j index of camera * @param p to which pose to constrain it to */ - void addMeasurement(const Point2& z, const SharedGaussian& model, + void addMeasurement(const Point2& z, const SharedNoiseModel& model, PoseKey i, PointKey j, const shared_ptrK& K) { boost::shared_ptr factor(new ProjectionFactor(z, model, i, j, K)); push_back(factor); @@ -111,7 +111,7 @@ namespace gtsam { * @param p to which pose to constrain it to * @param model uncertainty model of this prior */ - void addPosePrior(int j, const Pose3& p = Pose3(), const SharedGaussian& model = noiseModel::Unit::Create(1)) { + void addPosePrior(int j, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(1)) { boost::shared_ptr factor(new PosePrior(j, p, model)); push_back(factor); } @@ -121,7 +121,7 @@ namespace gtsam { * @param j index of landmark * @param model uncertainty model of this prior */ - void addPointPrior(int j, const Point3& p = Point3(), const SharedGaussian& model = noiseModel::Unit::Create(1)) { + void addPointPrior(int j, const Point3& p = Point3(), const SharedNoiseModel& model = noiseModel::Unit::Create(1)) { boost::shared_ptr factor(new PointPrior(j, p, model)); push_back(factor); } diff --git a/tests/testNonlinearEqualityConstraint.cpp b/tests/testNonlinearEqualityConstraint.cpp index ee14349f1..f2cabc27b 100644 --- a/tests/testNonlinearEqualityConstraint.cpp +++ b/tests/testNonlinearEqualityConstraint.cpp @@ -221,7 +221,7 @@ TEST (testNonlinearEqualityConstraint, two_pose ) { graph->add(eq2D::UnaryEqualityConstraint(pt_x2, x2)); Point2 z1(0.0, 5.0); - SharedGaussian sigma(noiseModel::Isotropic::Sigma(2, 0.1)); + SharedNoiseModel sigma(noiseModel::Isotropic::Sigma(2, 0.1)); graph->add(simulated2D::Measurement(z1, sigma, x1,l1)); Point2 z2(-4.0, 0.0); diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 2804eba45..183f8a4e7 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -43,7 +43,7 @@ typedef boost::shared_ptr > shared_nlf; /* ************************************************************************* */ TEST( NonlinearFactor, equals ) { - SharedGaussian sigma(noiseModel::Isotropic::Sigma(2,1.0)); + SharedNoiseModel sigma(noiseModel::Isotropic::Sigma(2,1.0)); // create two nonlinear2 factors Point2 z3(0.,-1.); diff --git a/tests/testPose2SLAMwSPCG.cpp b/tests/testPose2SLAMwSPCG.cpp index 339f3f578..c69b12ea0 100644 --- a/tests/testPose2SLAMwSPCG.cpp +++ b/tests/testPose2SLAMwSPCG.cpp @@ -18,7 +18,7 @@ const double tol = 1e-5; TEST(testPose2SLAMwSPCG, example1) { /* generate synthetic data */ - const SharedGaussian sigma(noiseModel::Unit::Create(0.1)); + const SharedNoiseModel sigma(noiseModel::Unit::Create(0.1)); Key x1(1), x2(2), x3(3), x4(4), x5(5), x6(6), x7(7), x8(8), x9(9); // create a 3 by 3 grid diff --git a/tests/testSerialization.cpp b/tests/testSerialization.cpp index 37700badb..75d215535 100644 --- a/tests/testSerialization.cpp +++ b/tests/testSerialization.cpp @@ -255,7 +255,7 @@ BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian" BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedGaussian, "gtsam_SharedGaussian"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); /* ************************************************************************* */ @@ -286,25 +286,25 @@ TEST (Serialization, noiseModels) { } /* ************************************************************************* */ -TEST (Serialization, SharedGaussian_noiseModels) { - SharedGaussian diag3_sg = diag3; - EXPECT(equalsDereferenced(diag3_sg)); - EXPECT(equalsDereferencedXML(diag3_sg)); +TEST (Serialization, SharedNoiseModel_noiseModels) { + SharedNoiseModel diag3_sg = diag3; + EXPECT(equalsDereferenced(diag3_sg)); + EXPECT(equalsDereferencedXML(diag3_sg)); - EXPECT(equalsDereferenced(diag3)); - EXPECT(equalsDereferencedXML(diag3)); + EXPECT(equalsDereferenced(diag3)); + EXPECT(equalsDereferencedXML(diag3)); - EXPECT(equalsDereferenced(iso3)); - EXPECT(equalsDereferencedXML(iso3)); + EXPECT(equalsDereferenced(iso3)); + EXPECT(equalsDereferencedXML(iso3)); - EXPECT(equalsDereferenced(gaussian3)); - EXPECT(equalsDereferencedXML(gaussian3)); + EXPECT(equalsDereferenced(gaussian3)); + EXPECT(equalsDereferencedXML(gaussian3)); - EXPECT(equalsDereferenced(unit3)); - EXPECT(equalsDereferencedXML(unit3)); + EXPECT(equalsDereferenced(unit3)); + EXPECT(equalsDereferencedXML(unit3)); - EXPECT(equalsDereferenced(constrained3)); - EXPECT(equalsDereferencedXML(constrained3)); + EXPECT(equalsDereferenced(constrained3)); + EXPECT(equalsDereferencedXML(constrained3)); } /* ************************************************************************* */ @@ -484,9 +484,9 @@ TEST (Serialization, planar_system) { values.insert(PointKey(3), Point2(1.0, 2.0)); values.insert(PoseKey(4), Pose2(1.0, 2.0, 0.3)); - SharedGaussian model1 = noiseModel::Isotropic::Sigma(1, 0.3); - SharedGaussian model2 = noiseModel::Isotropic::Sigma(2, 0.3); - SharedGaussian model3 = noiseModel::Isotropic::Sigma(3, 0.3); + SharedNoiseModel model1 = noiseModel::Isotropic::Sigma(1, 0.3); + SharedNoiseModel model2 = noiseModel::Isotropic::Sigma(2, 0.3); + SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.3); Prior prior(PoseKey(3), Pose2(0.1,-0.3, 0.2), model1); Bearing bearing(PoseKey(3), PointKey(5), Rot2::fromDegrees(0.5), model1); @@ -547,9 +547,9 @@ TEST (Serialization, visual_system) { Point3 pt1(1.0, 2.0, 3.0), pt2(4.0, 5.0, 6.0); values.insert(x1, pose1); values.insert(l1, pt1); - SharedGaussian model2 = noiseModel::Isotropic::Sigma(2, 0.3); - SharedGaussian model3 = noiseModel::Isotropic::Sigma(3, 0.3); - SharedGaussian model6 = noiseModel::Isotropic::Sigma(6, 0.3); + SharedNoiseModel model2 = noiseModel::Isotropic::Sigma(2, 0.3); + SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.3); + SharedNoiseModel model6 = noiseModel::Isotropic::Sigma(6, 0.3); boost::shared_ptr K(new Cal3_S2(cal1)); Graph graph;