added first order Gauss Markov model
Luca
parent
001f61ce32
commit
cedabdce81
|
|
@ -2628,6 +2628,14 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testGaussMarkov1stOrderFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testGaussMarkov1stOrderFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="SimpleRotation.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
|
|||
|
|
@ -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 <ostream>
|
||||
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/Lie.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/linear/GaussianFactor.h>
|
||||
#include <gtsam/linear/NoiseModel.h>
|
||||
|
||||
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 VALUE>
|
||||
class GaussMarkov1stOrderFactor: public NoiseModelFactor2<VALUE, VALUE> {
|
||||
|
||||
private:
|
||||
|
||||
typedef GaussMarkov1stOrderFactor<VALUE> This;
|
||||
typedef NoiseModelFactor2<VALUE, VALUE> Base;
|
||||
|
||||
double dt_;
|
||||
Vector tau_;
|
||||
|
||||
public:
|
||||
|
||||
// shorthand for a smart pointer to a factor
|
||||
typedef typename boost::shared_ptr<GaussMarkov1stOrderFactor> 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<const This*> (&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<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> 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<tau_.size(); i++){
|
||||
alpha(i) = exp(- 1/tau_(i)*dt_ );
|
||||
alpha_v1(i) = alpha(i) * v1(i);
|
||||
}
|
||||
|
||||
Vector hx(v2 - alpha_v1);
|
||||
|
||||
if(H1) *H1 = - diag(alpha);
|
||||
if(H2) *H2 = eye(v2.size());
|
||||
|
||||
return hx;
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
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<noiseModel::Gaussian>(model);
|
||||
SharedGaussian model_d(noiseModel::Gaussian::SqrtInformation(gaussian_model->R()/sqrt(delta_t)));
|
||||
return model_d;
|
||||
}
|
||||
|
||||
}; // \class GaussMarkov1stOrderFactor
|
||||
|
||||
} /// namespace gtsam
|
||||
|
|
@ -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 <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/slam/GaussMarkov1stOrderFactor.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
//! Factors
|
||||
typedef GaussMarkov1stOrderFactor<gtsam::LieVector> 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<tau.size(); i++){
|
||||
alpha(i) = exp(- 1/tau(i)*delta_t );
|
||||
alpha_v1(i) = alpha(i) * v1(i);
|
||||
}
|
||||
gtsam::Vector Err2( v2 - alpha_v1 );
|
||||
|
||||
CHECK(gtsam::assert_equal(Err1, Err2, 1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST (GaussMarkovFactor, jacobian ) {
|
||||
|
||||
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);
|
||||
|
||||
GaussMarkovFactor factor(x1, x2, delta_t, tau, model);
|
||||
|
||||
// Update the linearization point
|
||||
gtsam::LieVector v1_upd = gtsam::LieVector((gtsam::Vector(3) << 0.5, -0.7, 0.3));
|
||||
gtsam::LieVector v2_upd = gtsam::LieVector((gtsam::Vector(3) << -0.7, 0.4, 0.9));
|
||||
|
||||
// Calculate the Jacobian matrix using the factor
|
||||
Matrix computed_H1, computed_H2;
|
||||
factor.evaluateError(v1_upd, v2_upd, computed_H1, computed_H2);
|
||||
|
||||
// Calculate the Jacobian matrices H1 and H2 using the numerical derivative function
|
||||
gtsam::Matrix numerical_H1, numerical_H2;
|
||||
numerical_H1 = gtsam::numericalDerivative21<gtsam::LieVector, gtsam::LieVector,
|
||||
gtsam::LieVector>(boost::bind(&predictionError, _1, _2, factor), v1_upd, v2_upd);
|
||||
numerical_H2 = gtsam::numericalDerivative22<gtsam::LieVector, gtsam::LieVector,
|
||||
gtsam::LieVector>(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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
Loading…
Reference in New Issue