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); +} +/* ************************************************************************* */ +