diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index d5046d81a..929b80b33 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -229,19 +229,19 @@ bool assert_equal(const ConstSubVector& expected, const ConstSubVector& actual, /* ************************************************************************* */ bool linear_dependent(const Vector& vec1, const Vector& vec2, double tol) { if (vec1.size()!=vec2.size()) return false; - boost::optional scale; + bool flag = false; double scale = 1.0; size_t m = vec1.size(); for(size_t i=0; itol&&fabs(vec2[i])tol)) return false; - if(vec1[i] == 0 && vec2[i] == 0) - continue; - if (!scale) + if(vec1[i] == 0 && vec2[i] == 0) continue; + if (!flag) { scale = vec1[i] / vec2[i]; - else if (fabs(vec1[i] - vec2[i] * (*scale)) > tol) - return false; + flag = true ; + } + else if (fabs(vec1[i] - vec2[i]*scale) > tol) return false; } - return scale.is_initialized(); + return flag; } /* ************************************************************************* */ diff --git a/gtsam/linear/SharedGaussian.h b/gtsam/linear/SharedGaussian.h index 7d86ab6d2..e7853c989 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 @@ -26,44 +27,32 @@ 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) { - } + struct SharedGaussian: public SharedNoiseModel { + + typedef SharedNoiseModel Base; + + SharedGaussian() {} + SharedGaussian(const noiseModel::Gaussian::shared_ptr& p) : Base(p) {} + SharedGaussian(const noiseModel::Diagonal::shared_ptr& p) : Base(p) {} + SharedGaussian(const noiseModel::Constrained::shared_ptr& p) : Base(p) {} + SharedGaussian(const noiseModel::Isotropic::shared_ptr& p) : Base(p) {} + SharedGaussian(const noiseModel::Unit::shared_ptr& p) : Base(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)) { - } + SharedGaussian(const Matrix& covariance) + : Base(noiseModel::Gaussian::Covariance(covariance)) {} + SharedGaussian(const Vector& sigmas) + : Base(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)) { - } + Base(noiseModel::Isotropic::Sigma(GTSAM_DANGEROUS_GAUSSIAN, s)) {} #endif /** Serialization function */ @@ -71,7 +60,7 @@ namespace gtsam { // note, deliberately not in noiseModel namespace template void serialize(ARCHIVE & ar, const unsigned int version) { ar & boost::serialization::make_nvp("SharedGaussian", - boost::serialization::base_object(*this)); + boost::serialization::base_object(*this)); } }; } diff --git a/gtsam/linear/SharedNoiseModel.h b/gtsam/linear/SharedNoiseModel.h index c7cc84ab5..5216057ce 100644 --- a/gtsam/linear/SharedNoiseModel.h +++ b/gtsam/linear/SharedNoiseModel.h @@ -12,7 +12,6 @@ #pragma once #include -#include namespace gtsam { // note, deliberately not in noiseModel namespace @@ -21,7 +20,6 @@ namespace gtsam { // note, deliberately not in noiseModel namespace typedef noiseModel::Base::shared_ptr Base; SharedNoiseModel() {} - SharedNoiseModel(const SharedGaussian &p): Base(p) {} 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) {} diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 6cf02b255..a0ac06847 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; - SharedNoiseModel expected = Unit::Create(3); - SharedNoiseModel actual = Gaussian::Covariance(eye(3), smart); + SharedGaussian expected = Unit::Create(3); + SharedGaussian actual = Gaussian::Covariance(eye(3), smart); EXPECT(assert_equal(*expected,*actual)); } @@ -247,8 +247,8 @@ TEST(NoiseModel, SmartCovariance ) TEST(NoiseModel, ScalarOrVector ) { bool smart = true; - SharedNoiseModel expected = Unit::Create(3); - SharedNoiseModel actual = Gaussian::Covariance(eye(3), smart); + SharedGaussian expected = Unit::Create(3); + SharedGaussian actual = Gaussian::Covariance(eye(3), smart); EXPECT(assert_equal(*expected,*actual)); }