added first order Gauss Markov model
Luca
parent
001f61ce32
commit
cedabdce81
|
|
@ -2628,6 +2628,14 @@
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</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">
|
<target name="SimpleRotation.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j2</buildArguments>
|
<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