From c298c277938bf09c5a77091bea1c5a0b5022f681 Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 30 Jul 2014 14:43:24 -0400 Subject: [PATCH 1/6] implemented and unit tested initial version of GPS factor --- .cproject | 32 +++++---- gtsam/slam/GPSFactor.h | 110 +++++++++++++++++++++++++++++ gtsam/slam/tests/testGPSFactor.cpp | 85 ++++++++++++++++++++++ 3 files changed, 214 insertions(+), 13 deletions(-) create mode 100644 gtsam/slam/GPSFactor.h create mode 100644 gtsam/slam/tests/testGPSFactor.cpp diff --git a/.cproject b/.cproject index 21ac9d913..a0cec2816 100644 --- a/.cproject +++ b/.cproject @@ -1,19 +1,17 @@ - - - + - - + + @@ -62,13 +60,13 @@ - - + + @@ -118,13 +116,13 @@ - - + + @@ -784,18 +782,18 @@ true true - + make -j5 - testGaussianFactorGraph.run + testGaussianFactorGraphUnordered.run true true true - + make -j5 - testGaussianBayesNet.run + testGaussianBayesNetUnordered.run true true true @@ -2622,6 +2620,14 @@ true true + + make + -j5 + testGPSFactor.run + true + true + true + make -j2 diff --git a/gtsam/slam/GPSFactor.h b/gtsam/slam/GPSFactor.h new file mode 100644 index 000000000..7d720cd15 --- /dev/null +++ b/gtsam/slam/GPSFactor.h @@ -0,0 +1,110 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file GPSFactor.h + * @author Luca Carlone + **/ +#pragma once + +#include + +#include +#include + +namespace gtsam { + + /** + * A class to model GPS measurements, including a bias term which models + * common-mode errors and that can be partially corrected if other sensors are used + * @addtogroup SLAM + */ + class GPSFactor: public NoiseModelFactor2 { + + private: + + typedef GPSFactor This; + typedef NoiseModelFactor2 Base; + + Point3 measured_; /** The measurement */ + + public: + + // shorthand for a smart pointer to a factor + typedef typename boost::shared_ptr shared_ptr; + + /** default constructor - only use for serialization */ + GPSFactor() {} + + /** Constructor */ + GPSFactor(Key posekey, Key biaskey, const Point3 measured, + const SharedNoiseModel& model) : + Base(model, posekey, biaskey), measured_(measured) { + } + + virtual ~GPSFactor() {} + + /** implement functions needed for Testable */ + + /** print */ + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "GPSFactor(" + << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << ")\n"; + measured_.print(" measured: "); + this->noiseModel_->print(" noise model: "); + } + + /** equals */ + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + const This *e = dynamic_cast (&expected); + return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol); + } + + /** implement functions needed to derive from Factor */ + + /** vector of errors */ + Vector evaluateError(const Pose3& pose, const Point3& bias, + boost::optional H1 = boost::none, boost::optional H2 = + boost::none) const { + + if (H1 || H2){ + H1->resize(3,6); // jacobian wrt pose + (*H1) << Matrix3::Zero(), pose.rotation().matrix(); + H2->resize(3,3); // jacobian wrt bias + (*H2) << Matrix3::Identity(); + } + return pose.translation().vector() + bias.vector() - measured_.vector(); + } + + /** return the measured */ + const Point3 measured() const { + return measured_; + } + + /** number of variables attached to this factor */ + size_t size() const { + return 2; + } + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NoiseModelFactor2", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(measured_); + } + }; // \class GPSFactor + +} /// namespace gtsam diff --git a/gtsam/slam/tests/testGPSFactor.cpp b/gtsam/slam/tests/testGPSFactor.cpp new file mode 100644 index 000000000..f501183b3 --- /dev/null +++ b/gtsam/slam/tests/testGPSFactor.cpp @@ -0,0 +1,85 @@ +/** + * @file testGPSFactor.cpp + * @brief + * @author Luca Carlone + * @date July 30, 2014 + */ + +#include +#include +#include +#include +#include + +using namespace gtsam; +using namespace gtsam::symbol_shorthand; +using namespace gtsam::noiseModel; +// Convenience for named keys + +using symbol_shorthand::X; +using symbol_shorthand::B; + +TEST(GPSFactor, errorNoiseless) { + + Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); + Point3 t(1.0, 0.5, 0.2); + Pose3 pose(R,t); + Point3 bias(0.0,0.0,0.0); + Point3 noise(0.0,0.0,0.0); + Point3 measured = t + noise; + + GPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05)); + Vector expectedError = (Vector(3) << 0.0, 0.0, 0.0 ); + Vector actualError = factor.evaluateError(pose,bias); + EXPECT(assert_equal(expectedError,actualError, 1E-5)); +} + +TEST(GPSFactor, errorNoisy) { + + Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); + Point3 t(1.0, 0.5, 0.2); + Pose3 pose(R,t); + Point3 bias(0.0,0.0,0.0); + Point3 noise(1.0,2.0,3.0); + Point3 measured = t - noise; + + GPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05)); + Vector expectedError = (Vector(3) << 1.0, 2.0, 3.0 ); + Vector actualError = factor.evaluateError(pose,bias); + EXPECT(assert_equal(expectedError,actualError, 1E-5)); +} + +TEST(GPSFactor, jacobian) { + + Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); + Point3 t(1.0, 0.5, 0.2); + Pose3 pose(R,t); + Point3 bias(0.0,0.0,0.0); + + Point3 noise(0.0,0.0,0.0); + Point3 measured = t + noise; + + GPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05)); + + Matrix actualH1, actualH2; + factor.evaluateError(pose,bias, actualH1, actualH2); + + Matrix numericalH1 = numericalDerivative21( + boost::function(boost::bind( + &GPSFactor::evaluateError, factor, _1, _2, boost::none, + boost::none)), pose, bias, 1e-5); + EXPECT(assert_equal(numericalH1,actualH1, 1E-5)); + + Matrix numericalH2 = numericalDerivative22( + boost::function(boost::bind( + &GPSFactor::evaluateError, factor, _1, _2, boost::none, + boost::none)), pose, bias, 1e-5); + EXPECT(assert_equal(numericalH2,actualH2, 1E-5)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ From 001f61ce324bd462f90bd7656192f4b6fbc69fd5 Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 30 Jul 2014 15:26:48 -0400 Subject: [PATCH 2/6] removed size --- gtsam/slam/GPSFactor.h | 5 ----- 1 file changed, 5 deletions(-) diff --git a/gtsam/slam/GPSFactor.h b/gtsam/slam/GPSFactor.h index 7d720cd15..9add83be0 100644 --- a/gtsam/slam/GPSFactor.h +++ b/gtsam/slam/GPSFactor.h @@ -90,11 +90,6 @@ namespace gtsam { return measured_; } - /** number of variables attached to this factor */ - size_t size() const { - return 2; - } - private: /** Serialization function */ From cedabdce8131922a3ba93848903867807895cd01 Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 30 Jul 2014 18:51:12 -0400 Subject: [PATCH 3/6] added first order Gauss Markov model --- .cproject | 8 ++ gtsam/slam/GaussMarkov1stOrderFactor.h | 135 ++++++++++++++++++ .../tests/testGaussMarkov1stOrderFactor.cpp | 122 ++++++++++++++++ 3 files changed, 265 insertions(+) create mode 100644 gtsam/slam/GaussMarkov1stOrderFactor.h create mode 100644 gtsam/slam/tests/testGaussMarkov1stOrderFactor.cpp diff --git a/.cproject b/.cproject index a0cec2816..46b623bb9 100644 --- a/.cproject +++ b/.cproject @@ -2628,6 +2628,14 @@ true true + + make + -j5 + testGaussMarkov1stOrderFactor.run + true + true + true + make -j2 diff --git a/gtsam/slam/GaussMarkov1stOrderFactor.h b/gtsam/slam/GaussMarkov1stOrderFactor.h new file mode 100644 index 000000000..bb5f68e02 --- /dev/null +++ b/gtsam/slam/GaussMarkov1stOrderFactor.h @@ -0,0 +1,135 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file GaussMarkov1stOrderFactor.h + * @author Vadim Indelman, Stephen Williams + * @date Jan 17, 2012 + **/ +#pragma once + +#include + +#include +#include +#include +#include +#include + +namespace gtsam { + +/* + * - The 1st order GaussMarkov factor relates two keys of the same type. This relation is given via + * key_2 = exp(-1/tau*delta_t) * key1 + w_d + * where tau is the time constant and delta_t is the time difference between the two keys. + * w_d is the equivalent discrete noise, whose covariance is calculated from the continuous noise model and delta_t. + * - w_d is approximated as a Gaussian noise. + * - In the multi-dimensional case, tau is a vector, and the above equation is applied on each element + * in the state (represented by keys), using the appropriate time constant in the vector tau. + */ + +/* + * A class for a measurement predicted by "GaussMarkov1stOrderFactor(config[key1],config[key2])" + * KEY1::Value is the Lie Group type + * T is the measurement type, by default the same + */ +template +class GaussMarkov1stOrderFactor: public NoiseModelFactor2 { + +private: + + typedef GaussMarkov1stOrderFactor This; + typedef NoiseModelFactor2 Base; + + double dt_; + Vector tau_; + +public: + + // shorthand for a smart pointer to a factor + typedef typename boost::shared_ptr shared_ptr; + + /** default constructor - only use for serialization */ + GaussMarkov1stOrderFactor() {} + + /** Constructor */ + GaussMarkov1stOrderFactor(const Key& key1, const Key& key2, double delta_t, Vector tau, + const SharedGaussian& model) : + Base(calc_descrete_noise_model(model, delta_t), key1, key2), dt_(delta_t), tau_(tau) { + } + + virtual ~GaussMarkov1stOrderFactor() {} + + /** implement functions needed for Testable */ + + /** print */ + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "GaussMarkov1stOrderFactor(" + << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << ")\n"; + this->noiseModel_->print(" noise model"); + } + + /** equals */ + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + const This *e = dynamic_cast (&expected); + return e != NULL && Base::equals(*e, tol); + } + + /** implement functions needed to derive from Factor */ + + /** vector of errors */ + Vector evaluateError(const VALUE& p1, const VALUE& p2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const { + + Vector v1( VALUE::Logmap(p1) ); + Vector v2( VALUE::Logmap(p2) ); + + Vector alpha(tau_.size()); + Vector alpha_v1(tau_.size()); + for(int i=0; i + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(dt_); + ar & BOOST_SERIALIZATION_NVP(tau_); + } + + SharedGaussian calc_descrete_noise_model(const SharedGaussian& model, double delta_t){ + /* Q_d (approx)= Q * delta_t */ + /* In practice, square root of the information matrix is represented, so that: + * R_d (approx)= R / sqrt(delta_t) + * */ + noiseModel::Gaussian::shared_ptr gaussian_model = boost::dynamic_pointer_cast(model); + SharedGaussian model_d(noiseModel::Gaussian::SqrtInformation(gaussian_model->R()/sqrt(delta_t))); + return model_d; + } + +}; // \class GaussMarkov1stOrderFactor + +} /// namespace gtsam diff --git a/gtsam/slam/tests/testGaussMarkov1stOrderFactor.cpp b/gtsam/slam/tests/testGaussMarkov1stOrderFactor.cpp new file mode 100644 index 000000000..d5a3ed8f7 --- /dev/null +++ b/gtsam/slam/tests/testGaussMarkov1stOrderFactor.cpp @@ -0,0 +1,122 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file testGaussMarkov1stOrderFactor.cpp + * @brief Unit tests for the GaussMarkov1stOrder factor + * @author Vadim Indelman + * @date Jan 17, 2012 + */ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +//! Factors +typedef GaussMarkov1stOrderFactor GaussMarkovFactor; + +/* ************************************************************************* */ +gtsam::LieVector predictionError(const gtsam::LieVector& v1, const gtsam::LieVector& v2, const GaussMarkovFactor factor) { + return factor.evaluateError(v1, v2); +} + +/* ************************************************************************* */ +TEST( GaussMarkovFactor, equals ) +{ + // Create two identical factors and make sure they're equal + gtsam::Key x1(1); + gtsam::Key x2(2); + double delta_t = 0.10; + Vector tau = (Vector(3) << 100.0, 150.0, 10.0); + gtsam::SharedGaussian model = gtsam::noiseModel::Isotropic::Sigma(3, 1.0); + + GaussMarkovFactor factor1(x1, x2, delta_t, tau, model); + GaussMarkovFactor factor2(x1, x2, delta_t, tau, model); + + CHECK(gtsam::assert_equal(factor1, factor2)); +} + +/* ************************************************************************* */ +TEST( GaussMarkovFactor, error ) +{ + gtsam::Values linPoint; + gtsam::Key x1(1); + gtsam::Key x2(2); + double delta_t = 0.10; + Vector tau = (Vector(3) << 100.0, 150.0, 10.0); + gtsam::SharedGaussian model = gtsam::noiseModel::Isotropic::Sigma(3, 1.0); + + gtsam::LieVector v1 = gtsam::LieVector((gtsam::Vector(3) << 10.0, 12.0, 13.0)); + gtsam::LieVector v2 = gtsam::LieVector((gtsam::Vector(3) << 10.0, 15.0, 14.0)); + + // Create two nodes + linPoint.insert(x1, v1); + linPoint.insert(x2, v2); + + GaussMarkovFactor factor(x1, x2, delta_t, tau, model); + gtsam::Vector Err1( factor.evaluateError(v1, v2) ); + + // Manually calculate the error + Vector alpha(tau.size()); + Vector alpha_v1(tau.size()); + for(int i=0; i(boost::bind(&predictionError, _1, _2, factor), v1_upd, v2_upd); + numerical_H2 = gtsam::numericalDerivative22(boost::bind(&predictionError, _1, _2, factor), v1_upd, v2_upd); + + // Verify they are equal for this choice of state + CHECK( gtsam::assert_equal(numerical_H1, computed_H1, 1e-9)); + CHECK( gtsam::assert_equal(numerical_H2, computed_H2, 1e-9)); +} + +/* ************************************************************************* */ +int main() +{ + TestResult tr; return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + From 92b113d1733f834e1911f97706c8fe5cf8e195bc Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 30 Jul 2014 19:01:11 -0400 Subject: [PATCH 4/6] minor --- gtsam/slam/GaussMarkov1stOrderFactor.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/slam/GaussMarkov1stOrderFactor.h b/gtsam/slam/GaussMarkov1stOrderFactor.h index bb5f68e02..01b32b39b 100644 --- a/gtsam/slam/GaussMarkov1stOrderFactor.h +++ b/gtsam/slam/GaussMarkov1stOrderFactor.h @@ -11,7 +11,7 @@ /** * @file GaussMarkov1stOrderFactor.h - * @author Vadim Indelman, Stephen Williams + * @author Vadim Indelman, Stephen Williams, Luca Carlone * @date Jan 17, 2012 **/ #pragma once @@ -63,7 +63,7 @@ public: /** Constructor */ GaussMarkov1stOrderFactor(const Key& key1, const Key& key2, double delta_t, Vector tau, const SharedGaussian& model) : - Base(calc_descrete_noise_model(model, delta_t), key1, key2), dt_(delta_t), tau_(tau) { + Base(calcDiscreteNoiseModel(model, delta_t), key1, key2), dt_(delta_t), tau_(tau) { } virtual ~GaussMarkov1stOrderFactor() {} @@ -120,7 +120,7 @@ private: ar & BOOST_SERIALIZATION_NVP(tau_); } - SharedGaussian calc_descrete_noise_model(const SharedGaussian& model, double delta_t){ + SharedGaussian calcDiscreteNoiseModel(const SharedGaussian& model, double delta_t){ /* Q_d (approx)= Q * delta_t */ /* In practice, square root of the information matrix is represented, so that: * R_d (approx)= R / sqrt(delta_t) From db411214ff1b3bdd5700d1ea2713109dbf6db77b Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 10 Sep 2014 15:30:06 -0400 Subject: [PATCH 5/6] renamed GPS factor --- gtsam/slam/{GPSFactor.h => BiasedGPSFactor.h} | 18 ++++++++--------- ...tGPSFactor.cpp => testBiasedGPSFactor.cpp} | 20 +++++++++---------- 2 files changed, 19 insertions(+), 19 deletions(-) rename gtsam/slam/{GPSFactor.h => BiasedGPSFactor.h} (87%) rename gtsam/slam/tests/{testGPSFactor.cpp => testBiasedGPSFactor.cpp} (78%) diff --git a/gtsam/slam/GPSFactor.h b/gtsam/slam/BiasedGPSFactor.h similarity index 87% rename from gtsam/slam/GPSFactor.h rename to gtsam/slam/BiasedGPSFactor.h index 9add83be0..fc525f52a 100644 --- a/gtsam/slam/GPSFactor.h +++ b/gtsam/slam/BiasedGPSFactor.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file GPSFactor.h + * @file BiasedGPSFactor.h * @author Luca Carlone **/ #pragma once @@ -27,11 +27,11 @@ namespace gtsam { * common-mode errors and that can be partially corrected if other sensors are used * @addtogroup SLAM */ - class GPSFactor: public NoiseModelFactor2 { + class BiasedGPSFactor: public NoiseModelFactor2 { private: - typedef GPSFactor This; + typedef BiasedGPSFactor This; typedef NoiseModelFactor2 Base; Point3 measured_; /** The measurement */ @@ -39,24 +39,24 @@ namespace gtsam { public: // shorthand for a smart pointer to a factor - typedef typename boost::shared_ptr shared_ptr; + typedef typename boost::shared_ptr shared_ptr; /** default constructor - only use for serialization */ - GPSFactor() {} + BiasedGPSFactor() {} /** Constructor */ - GPSFactor(Key posekey, Key biaskey, const Point3 measured, + BiasedGPSFactor(Key posekey, Key biaskey, const Point3 measured, const SharedNoiseModel& model) : Base(model, posekey, biaskey), measured_(measured) { } - virtual ~GPSFactor() {} + virtual ~BiasedGPSFactor() {} /** implement functions needed for Testable */ /** print */ virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "GPSFactor(" + std::cout << s << "BiasedGPSFactor(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << ")\n"; measured_.print(" measured: "); @@ -100,6 +100,6 @@ namespace gtsam { boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); } - }; // \class GPSFactor + }; // \class BiasedGPSFactor } /// namespace gtsam diff --git a/gtsam/slam/tests/testGPSFactor.cpp b/gtsam/slam/tests/testBiasedGPSFactor.cpp similarity index 78% rename from gtsam/slam/tests/testGPSFactor.cpp rename to gtsam/slam/tests/testBiasedGPSFactor.cpp index f501183b3..d907852c5 100644 --- a/gtsam/slam/tests/testGPSFactor.cpp +++ b/gtsam/slam/tests/testBiasedGPSFactor.cpp @@ -1,5 +1,5 @@ /** - * @file testGPSFactor.cpp + * @file testBiasedGPSFactor.cpp * @brief * @author Luca Carlone * @date July 30, 2014 @@ -8,7 +8,7 @@ #include #include #include -#include +#include #include using namespace gtsam; @@ -19,7 +19,7 @@ using namespace gtsam::noiseModel; using symbol_shorthand::X; using symbol_shorthand::B; -TEST(GPSFactor, errorNoiseless) { +TEST(BiasedGPSFactor, errorNoiseless) { Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); Point3 t(1.0, 0.5, 0.2); @@ -28,13 +28,13 @@ TEST(GPSFactor, errorNoiseless) { Point3 noise(0.0,0.0,0.0); Point3 measured = t + noise; - GPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05)); + BiasedGPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05)); Vector expectedError = (Vector(3) << 0.0, 0.0, 0.0 ); Vector actualError = factor.evaluateError(pose,bias); EXPECT(assert_equal(expectedError,actualError, 1E-5)); } -TEST(GPSFactor, errorNoisy) { +TEST(BiasedGPSFactor, errorNoisy) { Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); Point3 t(1.0, 0.5, 0.2); @@ -43,13 +43,13 @@ TEST(GPSFactor, errorNoisy) { Point3 noise(1.0,2.0,3.0); Point3 measured = t - noise; - GPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05)); + BiasedGPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05)); Vector expectedError = (Vector(3) << 1.0, 2.0, 3.0 ); Vector actualError = factor.evaluateError(pose,bias); EXPECT(assert_equal(expectedError,actualError, 1E-5)); } -TEST(GPSFactor, jacobian) { +TEST(BiasedGPSFactor, jacobian) { Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); Point3 t(1.0, 0.5, 0.2); @@ -59,20 +59,20 @@ TEST(GPSFactor, jacobian) { Point3 noise(0.0,0.0,0.0); Point3 measured = t + noise; - GPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05)); + BiasedGPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05)); Matrix actualH1, actualH2; factor.evaluateError(pose,bias, actualH1, actualH2); Matrix numericalH1 = numericalDerivative21( boost::function(boost::bind( - &GPSFactor::evaluateError, factor, _1, _2, boost::none, + &BiasedGPSFactor::evaluateError, factor, _1, _2, boost::none, boost::none)), pose, bias, 1e-5); EXPECT(assert_equal(numericalH1,actualH1, 1E-5)); Matrix numericalH2 = numericalDerivative22( boost::function(boost::bind( - &GPSFactor::evaluateError, factor, _1, _2, boost::none, + &BiasedGPSFactor::evaluateError, factor, _1, _2, boost::none, boost::none)), pose, bias, 1e-5); EXPECT(assert_equal(numericalH2,actualH2, 1E-5)); } From 7a4f3073422b898dfb9c49df6e9a66ef33cce92d Mon Sep 17 00:00:00 2001 From: Luca Date: Mon, 6 Oct 2014 13:11:27 -0400 Subject: [PATCH 6/6] moved biasedGPS and GaussMarkov to unstable --- {gtsam => gtsam_unstable}/slam/BiasedGPSFactor.h | 0 {gtsam => gtsam_unstable}/slam/GaussMarkov1stOrderFactor.h | 0 {gtsam => gtsam_unstable}/slam/tests/testBiasedGPSFactor.cpp | 2 +- .../slam/tests/testGaussMarkov1stOrderFactor.cpp | 2 +- 4 files changed, 2 insertions(+), 2 deletions(-) rename {gtsam => gtsam_unstable}/slam/BiasedGPSFactor.h (100%) rename {gtsam => gtsam_unstable}/slam/GaussMarkov1stOrderFactor.h (100%) rename {gtsam => gtsam_unstable}/slam/tests/testBiasedGPSFactor.cpp (98%) rename {gtsam => gtsam_unstable}/slam/tests/testGaussMarkov1stOrderFactor.cpp (98%) diff --git a/gtsam/slam/BiasedGPSFactor.h b/gtsam_unstable/slam/BiasedGPSFactor.h similarity index 100% rename from gtsam/slam/BiasedGPSFactor.h rename to gtsam_unstable/slam/BiasedGPSFactor.h diff --git a/gtsam/slam/GaussMarkov1stOrderFactor.h b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h similarity index 100% rename from gtsam/slam/GaussMarkov1stOrderFactor.h rename to gtsam_unstable/slam/GaussMarkov1stOrderFactor.h diff --git a/gtsam/slam/tests/testBiasedGPSFactor.cpp b/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp similarity index 98% rename from gtsam/slam/tests/testBiasedGPSFactor.cpp rename to gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp index d907852c5..38e8a1466 100644 --- a/gtsam/slam/tests/testBiasedGPSFactor.cpp +++ b/gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp @@ -8,7 +8,7 @@ #include #include #include -#include +#include #include using namespace gtsam; diff --git a/gtsam/slam/tests/testGaussMarkov1stOrderFactor.cpp b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp similarity index 98% rename from gtsam/slam/tests/testGaussMarkov1stOrderFactor.cpp rename to gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp index d5a3ed8f7..c708c1949 100644 --- a/gtsam/slam/tests/testGaussMarkov1stOrderFactor.cpp +++ b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp @@ -17,7 +17,7 @@ */ #include -#include +#include #include #include #include