Merge branch 'feature/GPSFactor'
Luca
commit
98c507d70d
40
.cproject
40
.cproject
|
|
@ -1,19 +1,17 @@
|
|||
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
|
||||
<?fileVersion 4.0.0?>
|
||||
|
||||
<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
|
||||
<?fileVersion 4.0.0?><cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
|
||||
<storageModule moduleId="org.eclipse.cdt.core.settings">
|
||||
<cconfiguration id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544">
|
||||
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544" moduleId="org.eclipse.cdt.core.settings" name="MacOSX GCC">
|
||||
<externalSettings/>
|
||||
<extensions>
|
||||
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
</extensions>
|
||||
</storageModule>
|
||||
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
|
||||
|
|
@ -62,13 +60,13 @@
|
|||
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.1441575890" moduleId="org.eclipse.cdt.core.settings" name="Timing">
|
||||
<externalSettings/>
|
||||
<extensions>
|
||||
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
</extensions>
|
||||
</storageModule>
|
||||
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
|
||||
|
|
@ -118,13 +116,13 @@
|
|||
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216" moduleId="org.eclipse.cdt.core.settings" name="fast">
|
||||
<externalSettings/>
|
||||
<extensions>
|
||||
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
|
||||
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
</extensions>
|
||||
</storageModule>
|
||||
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
|
||||
|
|
@ -784,18 +782,18 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testGaussianFactorGraph.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testGaussianFactorGraphUnordered.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testGaussianFactorGraph.run</buildTarget>
|
||||
<buildTarget>testGaussianFactorGraphUnordered.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testGaussianBayesNet.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testGaussianBayesNetUnordered.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testGaussianBayesNet.run</buildTarget>
|
||||
<buildTarget>testGaussianBayesNetUnordered.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
@ -2622,6 +2620,22 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testGPSFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testGPSFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<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,105 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 BiasedGPSFactor.h
|
||||
* @author Luca Carlone
|
||||
**/
|
||||
#pragma once
|
||||
|
||||
#include <ostream>
|
||||
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
|
||||
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 BiasedGPSFactor: public NoiseModelFactor2<Pose3, Point3> {
|
||||
|
||||
private:
|
||||
|
||||
typedef BiasedGPSFactor This;
|
||||
typedef NoiseModelFactor2<Pose3, Point3> Base;
|
||||
|
||||
Point3 measured_; /** The measurement */
|
||||
|
||||
public:
|
||||
|
||||
// shorthand for a smart pointer to a factor
|
||||
typedef typename boost::shared_ptr<BiasedGPSFactor> shared_ptr;
|
||||
|
||||
/** default constructor - only use for serialization */
|
||||
BiasedGPSFactor() {}
|
||||
|
||||
/** Constructor */
|
||||
BiasedGPSFactor(Key posekey, Key biaskey, const Point3 measured,
|
||||
const SharedNoiseModel& model) :
|
||||
Base(model, posekey, biaskey), measured_(measured) {
|
||||
}
|
||||
|
||||
virtual ~BiasedGPSFactor() {}
|
||||
|
||||
/** implement functions needed for Testable */
|
||||
|
||||
/** print */
|
||||
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
|
||||
std::cout << s << "BiasedGPSFactor("
|
||||
<< 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<const This*> (&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<Matrix&> H1 = boost::none, boost::optional<Matrix&> 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_;
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & boost::serialization::make_nvp("NoiseModelFactor2",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
ar & BOOST_SERIALIZATION_NVP(measured_);
|
||||
}
|
||||
}; // \class BiasedGPSFactor
|
||||
|
||||
} /// namespace gtsam
|
||||
|
|
@ -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, Luca Carlone
|
||||
* @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(calcDiscreteNoiseModel(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 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)
|
||||
* */
|
||||
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,85 @@
|
|||
/**
|
||||
* @file testBiasedGPSFactor.cpp
|
||||
* @brief
|
||||
* @author Luca Carlone
|
||||
* @date July 30, 2014
|
||||
*/
|
||||
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam_unstable/slam/BiasedGPSFactor.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
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(BiasedGPSFactor, 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;
|
||||
|
||||
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(BiasedGPSFactor, 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;
|
||||
|
||||
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(BiasedGPSFactor, 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;
|
||||
|
||||
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<Vector(const Pose3&, const Point3&)>(boost::bind(
|
||||
&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<Vector(const Pose3&, const Point3&)>(boost::bind(
|
||||
&BiasedGPSFactor::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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -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_unstable/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