Merge remote-tracking branch 'refs/remotes/svn/trunk' into isam2-chain-optimization
Conflicts: gtsam/linear/VectorValues.hrelease/4.3a0
Richard Roberts
commit
9a23c2936a
|
|
@ -68,7 +68,6 @@ endif()
|
|||
option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF)
|
||||
option(GTSAM_BUILD_WRAP "Enable/Disable building of matlab wrap utility (necessary for matlab interface)" ON)
|
||||
option(GTSAM_INSTALL_WRAP "Enable/Disable installation of wrap utility for wrapping other libraries" ON)
|
||||
set(GTSAM_TOOLBOX_INSTALL_PATH "" CACHE PATH "Matlab toolbox destination, blank defaults to CMAKE_INSTALL_PREFIX/borg/toolbox")
|
||||
|
||||
# Check / set dependent variables for MATLAB wrapper
|
||||
set(GTSAM_WRAP_HEADER_PATH "${PROJECT_SOURCE_DIR}/wrap")
|
||||
|
|
@ -78,9 +77,6 @@ endif()
|
|||
if(GTSAM_INSTALL_WRAP AND NOT GTSAM_BUILD_WRAP)
|
||||
message(FATAL_ERROR "GTSAM_INSTALL_WRAP is enabled, please also enable GTSAM_BUILD_WRAP")
|
||||
endif()
|
||||
if(NOT GTSAM_TOOLBOX_INSTALL_PATH)
|
||||
set(GTSAM_TOOLBOX_INSTALL_PATH "${CMAKE_INSTALL_PREFIX}/borg/toolbox")
|
||||
endif()
|
||||
|
||||
# Flags for choosing default packaging tools
|
||||
set(CPACK_SOURCE_GENERATOR "TGZ" CACHE STRING "CPack Default Source Generator")
|
||||
|
|
@ -111,8 +107,10 @@ endif()
|
|||
|
||||
###############################################################################
|
||||
# Find boost
|
||||
if(CYGWIN OR MSVC OR WIN32)
|
||||
set(Boost_USE_STATIC_LIBS 1) # Use static libs on Windows
|
||||
|
||||
# If using Boost shared libs, set up auto linking for shared libs
|
||||
if(NOT Boost_USE_STATIC_LIBS)
|
||||
add_definitions(-DBOOST_ALL_DYN_LINK)
|
||||
endif()
|
||||
|
||||
find_package(Boost 1.43 COMPONENTS serialization system filesystem thread date_time regex timer chrono)
|
||||
|
|
@ -126,7 +124,7 @@ endif()
|
|||
# Allow for not using the timer libraries on boost < 1.48 (GTSAM timing code falls back to old timer library)
|
||||
set(GTSAM_BOOST_LIBRARIES ${Boost_SERIALIZATION_LIBRARY} ${Boost_SYSTEM_LIBRARY} ${Boost_FILESYSTEM_LIBRARY})
|
||||
if(Boost_TIMER_LIBRARY)
|
||||
set(GTSAM_BOOST_LIBRARIES ${GTSAM_BOOST_LIBRARIES} ${Boost_TIMER_LIBRARY} ${Boost_CHRONO_LIBRARY})
|
||||
list(APPEND GTSAM_BOOST_LIBRARIES ${GTSAM_BOOST_LIBRARIES} ${Boost_TIMER_LIBRARY} ${Boost_CHRONO_LIBRARY})
|
||||
else()
|
||||
message("WARNING: Boost older than 1.48 was found, GTSAM timing instrumentation will use the older, less accurate, Boost timer library.")
|
||||
endif()
|
||||
|
|
|
|||
39
gtsam.h
39
gtsam.h
|
|
@ -10,7 +10,7 @@
|
|||
* Only one Method/Constructor per line, though methods/constructors can extend across multiple lines
|
||||
* Methods can return
|
||||
* - Eigen types: Matrix, Vector
|
||||
* - C/C++ basic types: string, bool, size_t, size_t, double, char, unsigned char
|
||||
* - C/C++ basic types: string, bool, size_t, int, double, char, unsigned char
|
||||
* - void
|
||||
* - Any class with which be copied with boost::make_shared()
|
||||
* - boost::shared_ptr of any object type
|
||||
|
|
@ -815,6 +815,7 @@ virtual class BayesTree {
|
|||
void deleteCachedShortcuts();
|
||||
void insert(const CLIQUE* subtree);
|
||||
size_t numCachedSeparatorMarginals() const;
|
||||
CLIQUE* clique(size_t j) const;
|
||||
};
|
||||
|
||||
template<CONDITIONAL>
|
||||
|
|
@ -865,7 +866,6 @@ virtual class SymbolicBayesNet : gtsam::SymbolicBayesNetBase {
|
|||
void permuteWithInverse(const gtsam::Permutation& inversePermutation);
|
||||
};
|
||||
|
||||
#include <gtsam/inference/SymbolicFactorGraph.h>
|
||||
typedef gtsam::BayesTreeClique<gtsam::IndexConditional> SymbolicBayesTreeClique;
|
||||
typedef gtsam::BayesTree<gtsam::IndexConditional, gtsam::SymbolicBayesTreeClique> SymbolicBayesTreeBase;
|
||||
virtual class SymbolicBayesTree : gtsam::SymbolicBayesTreeBase {
|
||||
|
|
@ -1107,21 +1107,21 @@ virtual class GaussianBayesNet : gtsam::GaussianBayesNetBase {
|
|||
|
||||
//Non-Class methods found in GaussianBayesNet.h
|
||||
//FIXME: No MATLAB documentation for these functions!
|
||||
/*gtsam::GaussianBayesNet scalarGaussian(size_t key, double mu, double sigma);
|
||||
gtsam::GaussianBayesNet simpleGaussian(size_t key, const Vector& mu, double sigma);
|
||||
void push_front(gtsam::GaussianBayesNet& bn, size_t key, Vector d, Matrix R, size_t name1, Matrix S, Vector sigmas);
|
||||
void push_front(gtsam::GaussianBayesNet& bn, size_t key, Vector d, Matrix R, size_t name1, Matrix S, size_t name2, Matrix T, Vector sigmas);
|
||||
gtsam::VectorValues* allocateVectorValues(const gtsam::GaussianBayesNet& bn);
|
||||
gtsam::VectorValues optimize(const gtsam::GaussianBayesNet& bn);
|
||||
void optimizeInPlace(const gtsam::GaussianBayesNet& bn, gtsam::VectorValues& x);
|
||||
gtsam::VectorValues optimizeGradientSearch(const gtsam::GaussianBayesNet& bn);
|
||||
void optimizeGradientSearchInPlace(const gtsam::GaussianBayesNet& bn, gtsam::VectorValues& grad);
|
||||
gtsam::VectorValues backSubstitute(const gtsam::GaussianBayesNet& bn, const gtsam::VectorValues& gx);
|
||||
gtsam::VectorValues backSubstituteTranspose(const gtsam::GaussianBayesNet& bn, const gtsam::VectorValues& gx);
|
||||
pair<Matrix, Vector> matrix(const gtsam::GaussianBayesNet& bn);
|
||||
//gtsam::GaussianBayesNet scalarGaussian(size_t key, double mu, double sigma);
|
||||
//gtsam::GaussianBayesNet simpleGaussian(size_t key, const Vector& mu, double sigma);
|
||||
//void push_front(gtsam::GaussianBayesNet& bn, size_t key, Vector d, Matrix R, size_t name1, Matrix S, Vector sigmas);
|
||||
//void push_front(gtsam::GaussianBayesNet& bn, size_t key, Vector d, Matrix R, size_t name1, Matrix S, size_t name2, Matrix T, Vector sigmas);
|
||||
//gtsam::VectorValues* allocateVectorValues(const gtsam::GaussianBayesNet& bn);
|
||||
//gtsam::VectorValues optimize(const gtsam::GaussianBayesNet& bn);
|
||||
//void optimizeInPlace(const gtsam::GaussianBayesNet& bn, gtsam::VectorValues& x);
|
||||
//gtsam::VectorValues optimizeGradientSearch(const gtsam::GaussianBayesNet& bn);
|
||||
//void optimizeGradientSearchInPlace(const gtsam::GaussianBayesNet& bn, gtsam::VectorValues& grad);
|
||||
//gtsam::VectorValues backSubstitute(const gtsam::GaussianBayesNet& bn, const gtsam::VectorValues& gx);
|
||||
//gtsam::VectorValues backSubstituteTranspose(const gtsam::GaussianBayesNet& bn, const gtsam::VectorValues& gx);
|
||||
//pair<Matrix, Vector> matrix(const gtsam::GaussianBayesNet& bn);
|
||||
double determinant(const gtsam::GaussianBayesNet& bayesNet);
|
||||
gtsam::VectorValues gradient(const gtsam::GaussianBayesNet& bayesNet, const gtsam::VectorValues& x0);
|
||||
void gradientAtZero(const gtsam::GaussianBayesNet& bayesNet, const gtsam::VectorValues& g);*/
|
||||
//gtsam::VectorValues gradient(const gtsam::GaussianBayesNet& bayesNet, const gtsam::VectorValues& x0);
|
||||
//void gradientAtZero(const gtsam::GaussianBayesNet& bayesNet, const gtsam::VectorValues& g);
|
||||
|
||||
#include <gtsam/linear/GaussianBayesTree.h>
|
||||
typedef gtsam::BayesTreeClique<gtsam::GaussianConditional> GaussianBayesTreeClique;
|
||||
|
|
@ -1133,6 +1133,13 @@ virtual class GaussianBayesTree : gtsam::GaussianBayesTreeBase {
|
|||
GaussianBayesTree(const gtsam::GaussianBayesNet& other);
|
||||
};
|
||||
|
||||
// namespace functions for GaussianBayesTree
|
||||
gtsam::VectorValues optimize(const gtsam::GaussianBayesTree& bayesTree);
|
||||
gtsam::VectorValues optimizeGradientSearch(const gtsam::GaussianBayesTree& bayesTree);
|
||||
gtsam::VectorValues gradient(const gtsam::GaussianBayesTree& bayesTree, const gtsam::VectorValues& x0);
|
||||
gtsam::VectorValues* allocateVectorValues(const gtsam::GaussianBayesTree& bt);
|
||||
double determinant(const gtsam::GaussianBayesTree& bayesTree);
|
||||
|
||||
virtual class GaussianFactor {
|
||||
void print(string s) const;
|
||||
bool equals(const gtsam::GaussianFactor& lf, double tol) const;
|
||||
|
|
|
|||
|
|
@ -162,6 +162,11 @@ namespace gtsam {
|
|||
return 0;
|
||||
}
|
||||
|
||||
/** Check if there are any cliques in the tree */
|
||||
inline bool empty() const {
|
||||
return nodes_.empty();
|
||||
}
|
||||
|
||||
/** return nodes */
|
||||
const Nodes& nodes() const { return nodes_; }
|
||||
|
||||
|
|
@ -169,7 +174,12 @@ namespace gtsam {
|
|||
const sharedClique& root() const { return root_; }
|
||||
|
||||
/** find the clique that contains the variable with Index j */
|
||||
sharedClique operator[](Index j) const {
|
||||
inline sharedClique operator[](Index j) const {
|
||||
return nodes_.at(j);
|
||||
}
|
||||
|
||||
/** alternate syntax for matlab: find the clique that contains the variable with Index j */
|
||||
inline sharedClique clique(Index j) const {
|
||||
return nodes_.at(j);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -93,6 +93,7 @@ TEST( BayesTree, constructor )
|
|||
|
||||
// Check Size
|
||||
LONGS_EQUAL(4,bayesTree.size());
|
||||
EXPECT(!bayesTree.empty());
|
||||
|
||||
// Check root
|
||||
boost::shared_ptr<IndexConditional> actual_root = bayesTree.root()->conditional();
|
||||
|
|
@ -155,6 +156,7 @@ TEST( BayesTree, removePath )
|
|||
E(new IndexConditional(_E_, _B_)),
|
||||
F(new IndexConditional(_F_, _E_));
|
||||
SymbolicBayesTree bayesTree;
|
||||
EXPECT(bayesTree.empty());
|
||||
// Ordering ord; ord += _A_,_B_,_C_,_D_,_E_,_F_;
|
||||
SymbolicBayesTree::insert(bayesTree, A);
|
||||
SymbolicBayesTree::insert(bayesTree, B);
|
||||
|
|
|
|||
|
|
@ -23,6 +23,10 @@
|
|||
|
||||
#include <gtsam/linear/GaussianBayesTree.h> // Only to help Eclipse
|
||||
|
||||
#include <stdarg.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -36,6 +40,22 @@ void optimizeInPlace(const typename BAYESTREE::sharedClique& clique, VectorValue
|
|||
BOOST_FOREACH(const typename BAYESTREE::sharedClique& child, clique->children_)
|
||||
optimizeInPlace<BAYESTREE>(child, result);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class BAYESTREE>
|
||||
double logDeterminant(const typename BAYESTREE::sharedClique& clique) {
|
||||
double result = 0.0;
|
||||
|
||||
// this clique
|
||||
result += clique->conditional()->get_R().diagonal().unaryExpr(ptr_fun<double,double>(log)).sum();
|
||||
|
||||
// sum of children
|
||||
BOOST_FOREACH(const typename BAYESTREE::sharedClique& child, clique->children_)
|
||||
result += logDeterminant<BAYESTREE>(child);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
} // \namespace internal
|
||||
} // \namespace gtsam
|
||||
|
|
|
|||
|
|
@ -80,7 +80,16 @@ void gradientAtZero(const GaussianBayesTree& bayesTree, VectorValues& g) {
|
|||
gradientAtZero(FactorGraph<JacobianFactor>(bayesTree), g);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double determinant(const GaussianBayesTree& bayesTree) {
|
||||
if (!bayesTree.root())
|
||||
return 0.0;
|
||||
|
||||
return exp(internal::logDeterminant<GaussianBayesTree>(bayesTree.root()));
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
} // \namespace gtsam
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -91,6 +91,23 @@ VectorValues gradient(const GaussianBayesTree& bayesTree, const VectorValues& x0
|
|||
*/
|
||||
void gradientAtZero(const GaussianBayesTree& bayesTree, VectorValues& g);
|
||||
|
||||
/**
|
||||
* Computes the determinant of a GassianBayesTree
|
||||
* A GassianBayesTree is an upper triangular matrix and for an upper triangular matrix
|
||||
* determinant is the product of the diagonal elements. Instead of actually multiplying
|
||||
* we add the logarithms of the diagonal elements and take the exponent at the end
|
||||
* because this is more numerically stable.
|
||||
* @param bayesTree The input GassianBayesTree
|
||||
* @return The determinant
|
||||
*/
|
||||
double determinant(const GaussianBayesTree& bayesTree);
|
||||
|
||||
|
||||
namespace internal {
|
||||
template<class BAYESTREE>
|
||||
double logDeterminant(const typename BAYESTREE::sharedClique& clique);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#include <gtsam/linear/GaussianBayesTree-inl.h>
|
||||
|
|
|
|||
|
|
@ -20,6 +20,7 @@
|
|||
#include <gtsam/base/Vector.h>
|
||||
#include <gtsam/base/types.h>
|
||||
|
||||
#include <boost/format.hpp>
|
||||
#include <boost/lexical_cast.hpp>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
|
|
@ -305,13 +306,13 @@ namespace gtsam {
|
|||
private:
|
||||
// Throw an exception if j does not exist
|
||||
void checkExists(Index j) const {
|
||||
if(!exists(j))
|
||||
throw std::out_of_range("VectorValues: requested variable index is not in this VectorValues.");
|
||||
if(!exists(j)) {
|
||||
const std::string msg =
|
||||
(boost::format("VectorValues: requested variable index j=%1% is not in this VectorValues.") % j).str();
|
||||
throw std::out_of_range(msg);
|
||||
}
|
||||
}
|
||||
|
||||
// Resize
|
||||
void copyStructureFrom(const VectorValues& other);
|
||||
|
||||
public:
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -10,6 +10,8 @@ set (gtsam_unstable_subdirs
|
|||
slam
|
||||
)
|
||||
|
||||
set(GTSAM_UNSTABLE_BOOST_LIBRARIES ${GTSAM_BOOST_LIBRARIES} ${Boost_THREAD_LIBRARY})
|
||||
|
||||
add_custom_target(check.unstable COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure)
|
||||
|
||||
# assemble core libaries
|
||||
|
|
@ -58,7 +60,7 @@ set_target_properties(gtsam_unstable-static PROPERTIES
|
|||
CLEAN_DIRECT_OUTPUT 1
|
||||
VERSION ${gtsam_unstable_version}
|
||||
SOVERSION ${gtsam_unstable_soversion})
|
||||
target_link_libraries(gtsam_unstable-static gtsam-static)
|
||||
target_link_libraries(gtsam_unstable-static gtsam-static ${GTSAM_UNSTABLE_BOOST_LIBRARIES})
|
||||
set(gtsam_unstable-lib "gtsam_unstable-static")
|
||||
install(TARGETS gtsam_unstable-static EXPORT GTSAM-exports ARCHIVE DESTINATION lib)
|
||||
list(APPEND GTSAM_EXPORTED_TARGETS gtsam_unstable-static)
|
||||
|
|
@ -72,7 +74,7 @@ if (GTSAM_UNSTABLE_BUILD_SHARED_LIBRARY)
|
|||
CLEAN_DIRECT_OUTPUT 1
|
||||
VERSION ${gtsam_unstable_version}
|
||||
SOVERSION ${gtsam_unstable_soversion})
|
||||
target_link_libraries(gtsam_unstable-shared gtsam-shared)
|
||||
target_link_libraries(gtsam_unstable-shared gtsam-shared ${GTSAM_UNSTABLE_BOOST_LIBRARIES})
|
||||
install(TARGETS gtsam_unstable-shared EXPORT GTSAM-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin)
|
||||
list(APPEND GTSAM_EXPORTED_TARGETS gtsam_unstable-shared)
|
||||
set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)
|
||||
|
|
|
|||
|
|
@ -0,0 +1,184 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 ImuBias.h
|
||||
* @date Feb 2, 2012
|
||||
* @author Vadim Indelman, Stephen Williams
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <boost/serialization/nvp.hpp>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include <gtsam/base/DerivedValue.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
|
||||
/*
|
||||
* NOTES:
|
||||
* - Earth-rate correction:
|
||||
* + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to Local-Level system (NED or ENU as defened by the user).
|
||||
* + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system.
|
||||
* + A relatively small distance is traveled w.r.t. to initial pose is assumed, since R_ECEF_to_G is constant.
|
||||
* Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon.
|
||||
*
|
||||
* - Currently, an empty constructed is not enabled so that the user is forced to specify R_ECEF_to_G.
|
||||
*/
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
||||
|
||||
/// All noise models live in the noiseModel namespace
|
||||
namespace imuBias {
|
||||
|
||||
class ConstantBias : public DerivedValue<ConstantBias> {
|
||||
private:
|
||||
Vector bias_acc_;
|
||||
Vector bias_gyro_;
|
||||
|
||||
public:
|
||||
|
||||
ConstantBias():
|
||||
bias_acc_(Vector_(3, 0.0, 0.0, 0.0)), bias_gyro_(Vector_(3, 0.0, 0.0, 0.0)) {
|
||||
}
|
||||
|
||||
ConstantBias(const Vector& bias_acc, const Vector& bias_gyro):
|
||||
bias_acc_(bias_acc), bias_gyro_(bias_gyro) {
|
||||
}
|
||||
|
||||
Vector CorrectAcc(Vector measurment, boost::optional<Matrix&> H=boost::none) const {
|
||||
if (H){
|
||||
Matrix zeros3_3(zeros(3,3));
|
||||
Matrix m_eye3(-eye(3));
|
||||
|
||||
*H = collect(2, &m_eye3, &zeros3_3);
|
||||
}
|
||||
|
||||
return measurment - bias_acc_;
|
||||
}
|
||||
|
||||
|
||||
Vector CorrectGyro(Vector measurment, boost::optional<Matrix&> H=boost::none) const {
|
||||
if (H){
|
||||
Matrix zeros3_3(zeros(3,3));
|
||||
Matrix m_eye3(-eye(3));
|
||||
|
||||
*H = collect(2, &zeros3_3, &m_eye3);
|
||||
}
|
||||
|
||||
return measurment - bias_gyro_;
|
||||
}
|
||||
|
||||
// H1: Jacobian w.r.t. IMUBias
|
||||
// H2: Jacobian w.r.t. pose
|
||||
Vector CorrectGyroWithEarthRotRate(Vector measurement, const Pose3& pose, const Vector& w_earth_rate_G,
|
||||
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const {
|
||||
|
||||
Matrix R_G_to_I( pose.rotation().matrix().transpose() );
|
||||
Vector w_earth_rate_I = R_G_to_I * w_earth_rate_G;
|
||||
|
||||
if (H1){
|
||||
Matrix zeros3_3(zeros(3,3));
|
||||
Matrix m_eye3(-eye(3));
|
||||
|
||||
*H1 = collect(2, &zeros3_3, &m_eye3);
|
||||
}
|
||||
|
||||
if (H2){
|
||||
Matrix zeros3_3(zeros(3,3));
|
||||
Matrix H = -skewSymmetric(w_earth_rate_I);
|
||||
|
||||
*H2 = collect(2, &H, &zeros3_3);
|
||||
}
|
||||
|
||||
//TODO: Make sure H2 is correct.
|
||||
|
||||
return measurement - bias_gyro_ - w_earth_rate_I;
|
||||
|
||||
// Vector bias_gyro_temp(Vector_(3, -bias_gyro_(0), bias_gyro_(1), bias_gyro_(2)));
|
||||
// return measurement - bias_gyro_temp - R_G_to_I * w_earth_rate_G;
|
||||
}
|
||||
|
||||
/** Expmap around identity */
|
||||
static inline ConstantBias Expmap(const Vector& v) { return ConstantBias(v.head(3), v.tail(3)); }
|
||||
|
||||
/** Logmap around identity - just returns with default cast back */
|
||||
static inline Vector Logmap(const ConstantBias& p) { return concatVectors(2, &p.bias_acc_, &p.bias_gyro_); }
|
||||
|
||||
/** Update the LieVector with a tangent space update */
|
||||
inline ConstantBias retract(const Vector& v) const { return ConstantBias(bias_acc_ + v.head(3), bias_gyro_ + v.tail(3)); }
|
||||
|
||||
/** @return the local coordinates of another object */
|
||||
inline Vector localCoordinates(const ConstantBias& t2) const {
|
||||
Vector delta_acc(t2.bias_acc_ - bias_acc_);
|
||||
Vector delta_gyro(t2.bias_gyro_ - bias_gyro_);
|
||||
return concatVectors(2, &delta_acc, &delta_gyro);
|
||||
}
|
||||
|
||||
/** Returns dimensionality of the tangent space */
|
||||
inline size_t dim() const { return this->bias_acc_.size() + this->bias_gyro_.size(); }
|
||||
|
||||
/// print with optional string
|
||||
void print(const std::string& s = "") const {
|
||||
// explicit printing for now.
|
||||
std::cout << s + ".bias_acc [" << bias_acc_.transpose() << "]" << std::endl;
|
||||
std::cout << s + ".bias_gyro [" << bias_gyro_.transpose() << "]" << std::endl;
|
||||
}
|
||||
|
||||
/** equality up to tolerance */
|
||||
inline bool equals(const ConstantBias& expected, double tol=1e-5) const {
|
||||
return gtsam::equal(bias_acc_, expected.bias_acc_, tol) && gtsam::equal(bias_gyro_, expected.bias_gyro_, tol);
|
||||
}
|
||||
|
||||
/** get bias_acc */
|
||||
const Vector& bias_acc() const { return bias_acc_; }
|
||||
|
||||
/** get bias_gyro */
|
||||
const Vector& bias_gyro() const { return bias_gyro_; }
|
||||
|
||||
|
||||
ConstantBias compose(const ConstantBias& b2,
|
||||
boost::optional<gtsam::Matrix&> H1=boost::none,
|
||||
boost::optional<gtsam::Matrix&> H2=boost::none) const {
|
||||
if(H1) *H1 = eye(dim());
|
||||
if(H2) *H2 = eye(b2.dim());
|
||||
|
||||
return ConstantBias(bias_acc_ + b2.bias_acc_, bias_gyro_ + b2.bias_gyro_);
|
||||
}
|
||||
|
||||
/** between operation */
|
||||
ConstantBias between(const ConstantBias& b2,
|
||||
boost::optional<gtsam::Matrix&> H1=boost::none,
|
||||
boost::optional<gtsam::Matrix&> H2=boost::none) const {
|
||||
if(H1) *H1 = -eye(dim());
|
||||
if(H2) *H2 = eye(b2.dim());
|
||||
return ConstantBias(b2.bias_acc_ - bias_acc_, b2.bias_gyro_ - bias_gyro_);
|
||||
}
|
||||
|
||||
/** invert the object and yield a new one */
|
||||
inline ConstantBias inverse(boost::optional<gtsam::Matrix&> H=boost::none) const {
|
||||
if(H) *H = -eye(dim());
|
||||
|
||||
return ConstantBias(-1.0 * bias_acc_, -1.0 * bias_gyro_);
|
||||
}
|
||||
|
||||
|
||||
|
||||
}; // ConstantBias class
|
||||
|
||||
|
||||
} // namespace ImuBias
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,396 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 InertialNavFactor_GlobalVelocity.h
|
||||
* @author Vadim Indelman, Stephen Williams
|
||||
* @brief Inertial navigation factor (velocity in the global frame)
|
||||
* @date Sept 13, 2012
|
||||
**/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/linear/NoiseModel.h>
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <gtsam/base/LieVector.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
|
||||
// Using numerical derivative to calculate d(Pose3::Expmap)/dw
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
|
||||
#include <boost/optional.hpp>
|
||||
|
||||
#include <ostream>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/*
|
||||
* NOTES:
|
||||
* =====
|
||||
* - The global frame (NED or ENU) is defined by the user by specifying the gravity vector in this frame.
|
||||
* - The IMU frame is implicitly defined by the user via the rotation matrix between global and imu frames.
|
||||
* - Camera and IMU frames are identical
|
||||
* - The user should specify a continuous equivalent noise covariance, which can be calculated using
|
||||
* the static function CalcEquivalentNoiseCov based on the IMU gyro and acc measurement noise covariance
|
||||
* matrices and the process\modeling covariance matrix. The IneritalNavFactor converts this into a
|
||||
* discrete form using the supplied delta_t between sub-sequential measurements.
|
||||
* - Earth-rate correction:
|
||||
* + Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to the global
|
||||
* frame (Local-Level system: ENU or NED, see above).
|
||||
* + R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system.
|
||||
* + Currently it is assumed that a relatively small distance is traveled w.r.t. to initial pose, since R_ECEF_to_G is constant.
|
||||
* Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon.
|
||||
*
|
||||
* - Frame Notation:
|
||||
* Quantities are written as {Frame of Representation/Destination Frame}_{Quantity Type}_{Quatity Description/Origination Frame}
|
||||
* So, the rotational velocity of the sensor written in the body frame is: body_omega_sensor
|
||||
* And the transformation from the body frame to the world frame would be: world_P_body
|
||||
* This allows visual chaining. For example, converting the sensed angular velocity of the IMU
|
||||
* (angular velocity of the sensor in the sensor frame) into the world frame can be performed as:
|
||||
* world_R_body * body_R_sensor * sensor_omega_sensor = world_omega_sensor
|
||||
*
|
||||
*
|
||||
* - Common Quantity Types
|
||||
* P : pose/3d transformation
|
||||
* R : rotation
|
||||
* omega : angular velocity
|
||||
* t : translation
|
||||
* v : velocity
|
||||
* a : acceleration
|
||||
*
|
||||
* - Common Frames
|
||||
* sensor : the coordinate system attached to the sensor origin
|
||||
* body : the coordinate system attached to body/inertial frame.
|
||||
* Unless an optional frame transformation is provided, the
|
||||
* sensor frame and the body frame will be identical
|
||||
* world : the global/world coordinate frame. This is assumed to be
|
||||
* a tangent plane to the earth's surface somewhere near the
|
||||
* vehicle
|
||||
*/
|
||||
template<class POSE, class VELOCITY, class IMUBIAS>
|
||||
class InertialNavFactor_GlobalVelocity : public NoiseModelFactor5<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> {
|
||||
|
||||
private:
|
||||
|
||||
typedef InertialNavFactor_GlobalVelocity<POSE, VELOCITY, IMUBIAS> This;
|
||||
typedef NoiseModelFactor5<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> Base;
|
||||
|
||||
Vector measurement_acc_;
|
||||
Vector measurement_gyro_;
|
||||
double dt_;
|
||||
|
||||
Vector world_g_;
|
||||
Vector world_rho_;
|
||||
Vector world_omega_earth_;
|
||||
|
||||
boost::optional<POSE> body_P_sensor_; // The pose of the sensor in the body frame
|
||||
|
||||
public:
|
||||
|
||||
// shorthand for a smart pointer to a factor
|
||||
typedef typename boost::shared_ptr<InertialNavFactor_GlobalVelocity> shared_ptr;
|
||||
|
||||
/** default constructor - only use for serialization */
|
||||
InertialNavFactor_GlobalVelocity() {}
|
||||
|
||||
/** Constructor */
|
||||
InertialNavFactor_GlobalVelocity(const Key& Pose1, const Key& Vel1, const Key& IMUBias1, const Key& Pose2, const Key& Vel2,
|
||||
const Vector& measurement_acc, const Vector& measurement_gyro, const double measurement_dt, const Vector world_g, const Vector world_rho,
|
||||
const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_continuous, boost::optional<POSE> body_P_sensor = boost::none) :
|
||||
Base(calc_descrete_noise_model(model_continuous, measurement_dt ),
|
||||
Pose1, Vel1, IMUBias1, Pose2, Vel2), measurement_acc_(measurement_acc), measurement_gyro_(measurement_gyro),
|
||||
dt_(measurement_dt), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), body_P_sensor_(body_P_sensor) { }
|
||||
|
||||
virtual ~InertialNavFactor_GlobalVelocity() {}
|
||||
|
||||
/** implement functions needed for Testable */
|
||||
|
||||
/** print */
|
||||
virtual void print(const std::string& s = "InertialNavFactor_GlobalVelocity", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
|
||||
std::cout << s << "("
|
||||
<< keyFormatter(this->key1()) << ","
|
||||
<< keyFormatter(this->key2()) << ","
|
||||
<< keyFormatter(this->key3()) << ","
|
||||
<< keyFormatter(this->key4()) << ","
|
||||
<< keyFormatter(this->key5()) << "\n";
|
||||
std::cout << "acc measurement: " << this->measurement_acc_.transpose() << std::endl;
|
||||
std::cout << "gyro measurement: " << this->measurement_gyro_.transpose() << std::endl;
|
||||
std::cout << "dt: " << this->dt_ << std::endl;
|
||||
std::cout << "gravity (in world frame): " << this->world_g_.transpose() << std::endl;
|
||||
std::cout << "craft rate (in world frame): " << this->world_rho_.transpose() << std::endl;
|
||||
std::cout << "earth's rotation (in world frame): " << this->world_omega_earth_.transpose() << std::endl;
|
||||
if(this->body_P_sensor_)
|
||||
this->body_P_sensor_->print(" sensor pose in body frame: ");
|
||||
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)
|
||||
&& (measurement_acc_ - e->measurement_acc_).norm() < tol
|
||||
&& (measurement_gyro_ - e->measurement_gyro_).norm() < tol
|
||||
&& (dt_ - e->dt_) < tol
|
||||
&& (world_g_ - e->world_g_).norm() < tol
|
||||
&& (world_rho_ - e->world_rho_).norm() < tol
|
||||
&& (world_omega_earth_ - e->world_omega_earth_).norm() < tol
|
||||
&& ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
|
||||
}
|
||||
|
||||
POSE predictPose(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const {
|
||||
// Calculate the corrected measurements using the Bias object
|
||||
Vector GyroCorrected(Bias1.CorrectGyro(measurement_gyro_));
|
||||
|
||||
const POSE& world_P1_body = Pose1;
|
||||
const VELOCITY& world_V1_body = Vel1;
|
||||
|
||||
// Calculate the acceleration and angular velocity of the body in the body frame (including earth-related rotations)
|
||||
Vector body_omega_body;
|
||||
if(body_P_sensor_) {
|
||||
body_omega_body = body_P_sensor_->rotation().matrix() * GyroCorrected;
|
||||
} else {
|
||||
body_omega_body = GyroCorrected;
|
||||
}
|
||||
|
||||
// Convert earth-related terms into the body frame
|
||||
Matrix body_R_world(world_P1_body.rotation().inverse().matrix());
|
||||
Vector body_rho = body_R_world * world_rho_;
|
||||
Vector body_omega_earth = body_R_world * world_omega_earth_;
|
||||
|
||||
// Correct for earth-related terms
|
||||
body_omega_body -= body_rho + body_omega_earth;
|
||||
|
||||
// The velocity is in the global frame, so composing Pose1 with v*dt is incorrect
|
||||
return POSE(Pose1.rotation() * POSE::Rotation::Expmap(body_omega_body*dt_), Pose1.translation() + typename POSE::Translation(world_V1_body*dt_));
|
||||
}
|
||||
|
||||
VELOCITY predictVelocity(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const {
|
||||
// Calculate the corrected measurements using the Bias object
|
||||
Vector AccCorrected(Bias1.CorrectAcc(measurement_acc_));
|
||||
|
||||
const POSE& world_P1_body = Pose1;
|
||||
const VELOCITY& world_V1_body = Vel1;
|
||||
|
||||
// Calculate the acceleration and angular velocity of the body in the body frame (including earth-related rotations)
|
||||
Vector body_a_body, body_omega_body;
|
||||
if(body_P_sensor_) {
|
||||
Matrix body_R_sensor = body_P_sensor_->rotation().matrix();
|
||||
|
||||
Vector GyroCorrected(Bias1.CorrectGyro(measurement_gyro_));
|
||||
body_omega_body = body_R_sensor * GyroCorrected;
|
||||
Matrix body_omega_body__cross = skewSymmetric(body_omega_body);
|
||||
body_a_body = body_R_sensor * AccCorrected - body_omega_body__cross * body_omega_body__cross * body_P_sensor_->translation().vector();
|
||||
} else {
|
||||
body_a_body = AccCorrected;
|
||||
}
|
||||
|
||||
// Correct for earth-related terms
|
||||
Vector world_a_body = world_P1_body.rotation().matrix() * body_a_body + world_g_ - 2*skewSymmetric(world_rho_ + world_omega_earth_)*world_V1_body;
|
||||
|
||||
// Calculate delta in the body frame
|
||||
VELOCITY VelDelta(world_a_body*dt_);
|
||||
|
||||
// Predict
|
||||
return Vel1.compose(VelDelta);
|
||||
}
|
||||
|
||||
void predict(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, POSE& Pose2, VELOCITY& Vel2) const {
|
||||
Pose2 = predictPose(Pose1, Vel1, Bias1);
|
||||
Vel2 = predictVelocity(Pose1, Vel1, Bias1);
|
||||
}
|
||||
|
||||
POSE evaluatePoseError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2) const {
|
||||
// Predict
|
||||
POSE Pose2Pred = predictPose(Pose1, Vel1, Bias1);
|
||||
|
||||
// Calculate error
|
||||
return Pose2.between(Pose2Pred);
|
||||
}
|
||||
|
||||
VELOCITY evaluateVelocityError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2) const {
|
||||
// Predict
|
||||
VELOCITY Vel2Pred = predictVelocity(Pose1, Vel1, Bias1);
|
||||
|
||||
// Calculate error
|
||||
return Vel2.between(Vel2Pred);
|
||||
}
|
||||
|
||||
/** implement functions needed to derive from Factor */
|
||||
Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2,
|
||||
boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none,
|
||||
boost::optional<Matrix&> H3 = boost::none,
|
||||
boost::optional<Matrix&> H4 = boost::none,
|
||||
boost::optional<Matrix&> H5 = boost::none) const {
|
||||
|
||||
// TODO: Write analytical derivative calculations
|
||||
// Jacobian w.r.t. Pose1
|
||||
if (H1){
|
||||
Matrix H1_Pose = gtsam::numericalDerivative11<POSE, POSE>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, _1, Vel1, Bias1, Pose2, Vel2), Pose1);
|
||||
Matrix H1_Vel = gtsam::numericalDerivative11<VELOCITY, POSE>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, _1, Vel1, Bias1, Pose2, Vel2), Pose1);
|
||||
*H1 = stack(2, &H1_Pose, &H1_Vel);
|
||||
}
|
||||
|
||||
// Jacobian w.r.t. Vel1
|
||||
if (H2){
|
||||
Matrix H2_Pose = gtsam::numericalDerivative11<POSE, VELOCITY>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1);
|
||||
Matrix H2_Vel = gtsam::numericalDerivative11<VELOCITY, VELOCITY>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1);
|
||||
*H2 = stack(2, &H2_Pose, &H2_Vel);
|
||||
}
|
||||
|
||||
// Jacobian w.r.t. IMUBias1
|
||||
if (H3){
|
||||
Matrix H3_Pose = gtsam::numericalDerivative11<POSE, IMUBIAS>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, _1, Pose2, Vel2), Bias1);
|
||||
Matrix H3_Vel = gtsam::numericalDerivative11<VELOCITY, IMUBIAS>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, _1, Pose2, Vel2), Bias1);
|
||||
*H3 = stack(2, &H3_Pose, &H3_Vel);
|
||||
}
|
||||
|
||||
// Jacobian w.r.t. Pose2
|
||||
if (H4){
|
||||
Matrix H4_Pose = gtsam::numericalDerivative11<POSE, POSE>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, Bias1, _1, Vel2), Pose2);
|
||||
Matrix H4_Vel = gtsam::numericalDerivative11<VELOCITY, POSE>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, Bias1, _1, Vel2), Pose2);
|
||||
*H4 = stack(2, &H4_Pose, &H4_Vel);
|
||||
}
|
||||
|
||||
// Jacobian w.r.t. Vel2
|
||||
if (H5){
|
||||
Matrix H5_Pose = gtsam::numericalDerivative11<POSE, VELOCITY>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2);
|
||||
Matrix H5_Vel = gtsam::numericalDerivative11<VELOCITY, VELOCITY>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2);
|
||||
*H5 = stack(2, &H5_Pose, &H5_Vel);
|
||||
}
|
||||
|
||||
Vector ErrPoseVector(POSE::Logmap(evaluatePoseError(Pose1, Vel1, Bias1, Pose2, Vel2)));
|
||||
Vector ErrVelVector(VELOCITY::Logmap(evaluateVelocityError(Pose1, Vel1, Bias1, Pose2, Vel2)));
|
||||
|
||||
return concatVectors(2, &ErrPoseVector, &ErrVelVector);
|
||||
}
|
||||
|
||||
static inline noiseModel::Gaussian::shared_ptr CalcEquivalentNoiseCov(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro,
|
||||
const noiseModel::Gaussian::shared_ptr& gaussian_process){
|
||||
|
||||
Matrix cov_acc = inverse( gaussian_acc->R().transpose() * gaussian_acc->R() );
|
||||
Matrix cov_gyro = inverse( gaussian_gyro->R().transpose() * gaussian_gyro->R() );
|
||||
Matrix cov_process = inverse( gaussian_process->R().transpose() * gaussian_process->R() );
|
||||
|
||||
cov_process.block(0,0, 3,3) += cov_gyro;
|
||||
cov_process.block(6,6, 3,3) += cov_acc;
|
||||
|
||||
return noiseModel::Gaussian::Covariance(cov_process);
|
||||
}
|
||||
|
||||
static inline void Calc_g_rho_omega_earth_NED(const Vector& Pos_NED, const Vector& Vel_NED, const Vector& LatLonHeight_IC, const Vector& Pos_NED_Initial,
|
||||
Vector& g_NED, Vector& rho_NED, Vector& omega_earth_NED) {
|
||||
|
||||
Matrix ENU_to_NED = Matrix_(3, 3,
|
||||
0.0, 1.0, 0.0,
|
||||
1.0, 0.0, 0.0,
|
||||
0.0, 0.0, -1.0);
|
||||
|
||||
Matrix NED_to_ENU = Matrix_(3, 3,
|
||||
0.0, 1.0, 0.0,
|
||||
1.0, 0.0, 0.0,
|
||||
0.0, 0.0, -1.0);
|
||||
|
||||
// Convert incoming parameters to ENU
|
||||
Vector Pos_ENU = NED_to_ENU * Pos_NED;
|
||||
Vector Vel_ENU = NED_to_ENU * Vel_NED;
|
||||
Vector Pos_ENU_Initial = NED_to_ENU * Pos_NED_Initial;
|
||||
|
||||
// Call ENU version
|
||||
Vector g_ENU;
|
||||
Vector rho_ENU;
|
||||
Vector omega_earth_ENU;
|
||||
Calc_g_rho_omega_earth_ENU(Pos_ENU, Vel_ENU, LatLonHeight_IC, Pos_ENU_Initial, g_ENU, rho_ENU, omega_earth_ENU);
|
||||
|
||||
// Convert output to NED
|
||||
g_NED = ENU_to_NED * g_ENU;
|
||||
rho_NED = ENU_to_NED * rho_ENU;
|
||||
omega_earth_NED = ENU_to_NED * omega_earth_ENU;
|
||||
}
|
||||
|
||||
static inline void Calc_g_rho_omega_earth_ENU(const Vector& Pos_ENU, const Vector& Vel_ENU, const Vector& LatLonHeight_IC, const Vector& Pos_ENU_Initial,
|
||||
Vector& g_ENU, Vector& rho_ENU, Vector& omega_earth_ENU){
|
||||
double R0 = 6.378388e6;
|
||||
double e = 1/297;
|
||||
double Re( R0*( 1-e*(sin( LatLonHeight_IC(0) ))*(sin( LatLonHeight_IC(0) )) ) );
|
||||
|
||||
// Calculate current lat, lon
|
||||
Vector delta_Pos_ENU(Pos_ENU - Pos_ENU_Initial);
|
||||
double delta_lat(delta_Pos_ENU(1)/Re);
|
||||
double delta_lon(delta_Pos_ENU(0)/(Re*cos(LatLonHeight_IC(0))));
|
||||
double lat_new(LatLonHeight_IC(0) + delta_lat);
|
||||
double lon_new(LatLonHeight_IC(1) + delta_lon);
|
||||
|
||||
// Rotation of lon about z axis
|
||||
Rot3 C1(cos(lon_new), sin(lon_new), 0.0,
|
||||
-sin(lon_new), cos(lon_new), 0.0,
|
||||
0.0, 0.0, 1.0);
|
||||
|
||||
// Rotation of lat about y axis
|
||||
Rot3 C2(cos(lat_new), 0.0, sin(lat_new),
|
||||
0.0, 1.0, 0.0,
|
||||
-sin(lat_new), 0.0, cos(lat_new));
|
||||
|
||||
Rot3 UEN_to_ENU(0, 1, 0,
|
||||
0, 0, 1,
|
||||
1, 0, 0);
|
||||
|
||||
Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 );
|
||||
|
||||
Vector omega_earth_ECEF(Vector_(3, 0.0, 0.0, 7.292115e-5));
|
||||
omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF;
|
||||
|
||||
// Calculating g
|
||||
double height(LatLonHeight_IC(2));
|
||||
double EQUA_RADIUS = 6378137.0; // equatorial radius of the earth; WGS-84
|
||||
double ECCENTRICITY = 0.0818191908426; // eccentricity of the earth ellipsoid
|
||||
double e2( pow(ECCENTRICITY,2) );
|
||||
double den( 1-e2*pow(sin(lat_new),2) );
|
||||
double Rm( (EQUA_RADIUS*(1-e2))/( pow(den,(3/2)) ) );
|
||||
double Rp( EQUA_RADIUS/( sqrt(den) ) );
|
||||
double Ro( sqrt(Rp*Rm) ); // mean earth radius of curvature
|
||||
double g0( 9.780318*( 1 + 5.3024e-3 * pow(sin(lat_new),2) - 5.9e-6 * pow(sin(2*lat_new),2) ) );
|
||||
double g_calc( g0/( pow(1 + height/Ro, 2) ) );
|
||||
g_ENU = Vector_(3, 0.0, 0.0, -g_calc);
|
||||
|
||||
|
||||
// Calculate rho
|
||||
double Ve( Vel_ENU(0) );
|
||||
double Vn( Vel_ENU(1) );
|
||||
double rho_E = -Vn/(Rm + height);
|
||||
double rho_N = Ve/(Rp + height);
|
||||
double rho_U = Ve*tan(lat_new)/(Rp + height);
|
||||
rho_ENU = Vector_(3, rho_E, rho_N, rho_U);
|
||||
}
|
||||
|
||||
static inline noiseModel::Gaussian::shared_ptr calc_descrete_noise_model(const noiseModel::Gaussian::shared_ptr& 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)
|
||||
* */
|
||||
return noiseModel::Gaussian::SqrtInformation(model->R()/sqrt(delta_t));
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & boost::serialization::make_nvp("NonlinearFactor2",
|
||||
boost::serialization::base_object<Base>(*this));
|
||||
}
|
||||
|
||||
}; // \class GaussMarkov1stOrderFactor
|
||||
|
||||
|
||||
} /// namespace aspn
|
||||
|
|
@ -0,0 +1,682 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 testInertialNavFactor_GlobalVelocity.cpp
|
||||
* @brief Unit test for the InertialNavFactor_GlobalVelocity
|
||||
* @author Vadim Indelman, Stephen Williams
|
||||
*/
|
||||
|
||||
#include <iostream>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam_unstable/dynamics/ImuBias.h>
|
||||
#include <gtsam_unstable/dynamics/InertialNavFactor_GlobalVelocity.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/nonlinear/Key.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/base/LieVector.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
gtsam::Rot3 world_R_ECEF(
|
||||
0.31686, 0.51505, 0.79645,
|
||||
0.85173, -0.52399, 0,
|
||||
0.41733, 0.67835, -0.60471);
|
||||
|
||||
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5));
|
||||
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
|
||||
|
||||
/* ************************************************************************* */
|
||||
gtsam::Pose3 predictionErrorPose(const Pose3& p1, const LieVector& v1, const imuBias::ConstantBias& b1, const Pose3& p2, const LieVector& v2, const InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias>& factor) {
|
||||
return Pose3::Expmap(factor.evaluateError(p1, v1, b1, p2, v2).head(6));
|
||||
}
|
||||
|
||||
gtsam::LieVector predictionErrorVel(const Pose3& p1, const LieVector& v1, const imuBias::ConstantBias& b1, const Pose3& p2, const LieVector& v2, const InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias>& factor) {
|
||||
return LieVector::Expmap(factor.evaluateError(p1, v1, b1, p2, v2).tail(3));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( InertialNavFactor_GlobalVelocity, Constructor)
|
||||
{
|
||||
gtsam::Key Pose1(11);
|
||||
gtsam::Key Pose2(12);
|
||||
gtsam::Key Vel1(21);
|
||||
gtsam::Key Vel2(22);
|
||||
gtsam::Key Bias1(31);
|
||||
|
||||
Vector measurement_acc(Vector_(3,0.1,0.2,0.4));
|
||||
Vector measurement_gyro(Vector_(3, -0.2, 0.5, 0.03));
|
||||
|
||||
double measurement_dt(0.1);
|
||||
Vector world_g(Vector_(3, 0.0, 0.0, 9.81));
|
||||
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system
|
||||
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5));
|
||||
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
|
||||
|
||||
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
|
||||
|
||||
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( InertialNavFactor_GlobalVelocity, Equals)
|
||||
{
|
||||
gtsam::Key Pose1(11);
|
||||
gtsam::Key Pose2(12);
|
||||
gtsam::Key Vel1(21);
|
||||
gtsam::Key Vel2(22);
|
||||
gtsam::Key Bias1(31);
|
||||
|
||||
Vector measurement_acc(Vector_(3,0.1,0.2,0.4));
|
||||
Vector measurement_gyro(Vector_(3, -0.2, 0.5, 0.03));
|
||||
|
||||
double measurement_dt(0.1);
|
||||
Vector world_g(Vector_(3, 0.0, 0.0, 9.81));
|
||||
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system
|
||||
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5));
|
||||
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
|
||||
|
||||
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
|
||||
|
||||
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
|
||||
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> g(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
|
||||
CHECK(assert_equal(f, g, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( InertialNavFactor_GlobalVelocity, Predict)
|
||||
{
|
||||
gtsam::Key PoseKey1(11);
|
||||
gtsam::Key PoseKey2(12);
|
||||
gtsam::Key VelKey1(21);
|
||||
gtsam::Key VelKey2(22);
|
||||
gtsam::Key BiasKey1(31);
|
||||
|
||||
double measurement_dt(0.1);
|
||||
Vector world_g(Vector_(3, 0.0, 0.0, 9.81));
|
||||
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system
|
||||
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5));
|
||||
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
|
||||
|
||||
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
|
||||
|
||||
|
||||
// First test: zero angular motion, some acceleration
|
||||
Vector measurement_acc(Vector_(3,0.1,0.2,0.3-9.81));
|
||||
Vector measurement_gyro(Vector_(3, 0.0, 0.0, 0.0));
|
||||
|
||||
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
|
||||
|
||||
Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00));
|
||||
LieVector Vel1(3, 0.50, -0.50, 0.40);
|
||||
imuBias::ConstantBias Bias1;
|
||||
Pose3 expectedPose2(Rot3(), Point3(2.05, 0.95, 3.04));
|
||||
LieVector expectedVel2(3, 0.51, -0.48, 0.43);
|
||||
Pose3 actualPose2;
|
||||
LieVector actualVel2;
|
||||
f.predict(Pose1, Vel1, Bias1, actualPose2, actualVel2);
|
||||
|
||||
CHECK(assert_equal(expectedPose2, actualPose2, 1e-5));
|
||||
CHECK(assert_equal(expectedVel2, actualVel2, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( InertialNavFactor_GlobalVelocity, ErrorPosVel)
|
||||
{
|
||||
gtsam::Key PoseKey1(11);
|
||||
gtsam::Key PoseKey2(12);
|
||||
gtsam::Key VelKey1(21);
|
||||
gtsam::Key VelKey2(22);
|
||||
gtsam::Key BiasKey1(31);
|
||||
|
||||
double measurement_dt(0.1);
|
||||
Vector world_g(Vector_(3, 0.0, 0.0, 9.81));
|
||||
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system
|
||||
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5));
|
||||
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
|
||||
|
||||
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
|
||||
|
||||
|
||||
// First test: zero angular motion, some acceleration
|
||||
Vector measurement_acc(Vector_(3,0.1,0.2,0.3-9.81));
|
||||
Vector measurement_gyro(Vector_(3, 0.0, 0.0, 0.0));
|
||||
|
||||
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
|
||||
|
||||
Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00));
|
||||
Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04));
|
||||
LieVector Vel1(3, 0.50, -0.50, 0.40);
|
||||
LieVector Vel2(3, 0.51, -0.48, 0.43);
|
||||
imuBias::ConstantBias Bias1;
|
||||
|
||||
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
|
||||
Vector ExpectedErr(zero(9));
|
||||
|
||||
CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( InertialNavFactor_GlobalVelocity, ErrorRot)
|
||||
{
|
||||
gtsam::Key PoseKey1(11);
|
||||
gtsam::Key PoseKey2(12);
|
||||
gtsam::Key VelKey1(21);
|
||||
gtsam::Key VelKey2(22);
|
||||
gtsam::Key BiasKey1(31);
|
||||
|
||||
double measurement_dt(0.1);
|
||||
Vector world_g(Vector_(3, 0.0, 0.0, 9.81));
|
||||
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system
|
||||
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5));
|
||||
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
|
||||
|
||||
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
|
||||
|
||||
// Second test: zero angular motion, some acceleration
|
||||
Vector measurement_acc(Vector_(3,0.0,0.0,0.0-9.81));
|
||||
Vector measurement_gyro(Vector_(3, 0.1, 0.2, 0.3));
|
||||
|
||||
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
|
||||
|
||||
Pose3 Pose1(Rot3(), Point3(2.0,1.0,3.0));
|
||||
Pose3 Pose2(Rot3::Expmap(measurement_gyro*measurement_dt), Point3(2.0,1.0,3.0));
|
||||
LieVector Vel1(3,0.0,0.0,0.0);
|
||||
LieVector Vel2(3,0.0,0.0,0.0);
|
||||
imuBias::ConstantBias Bias1;
|
||||
|
||||
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
|
||||
Vector ExpectedErr(zero(9));
|
||||
|
||||
CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel)
|
||||
{
|
||||
gtsam::Key PoseKey1(11);
|
||||
gtsam::Key PoseKey2(12);
|
||||
gtsam::Key VelKey1(21);
|
||||
gtsam::Key VelKey2(22);
|
||||
gtsam::Key BiasKey1(31);
|
||||
|
||||
double measurement_dt(0.1);
|
||||
Vector world_g(Vector_(3, 0.0, 0.0, 9.81));
|
||||
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system
|
||||
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5));
|
||||
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
|
||||
|
||||
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
|
||||
|
||||
// Second test: zero angular motion, some acceleration - generated in matlab
|
||||
Vector measurement_acc(Vector_(3, 6.501390843381716, -6.763926150509185, -2.300389940090343));
|
||||
Vector measurement_gyro(Vector_(3, 0.1, 0.2, 0.3));
|
||||
|
||||
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
|
||||
|
||||
Rot3 R1(0.487316618, 0.125253866, 0.86419557,
|
||||
0.580273724, 0.693095498, -0.427669306,
|
||||
-0.652537293, 0.709880342, 0.265075427);
|
||||
Point3 t1(2.0,1.0,3.0);
|
||||
Pose3 Pose1(R1, t1);
|
||||
LieVector Vel1(3,0.5,-0.5,0.4);
|
||||
Rot3 R2(0.473618898, 0.119523052, 0.872582019,
|
||||
0.609241153, 0.67099888, -0.422594037,
|
||||
-0.636011287, 0.731761397, 0.244979388);
|
||||
Point3 t2 = t1.compose( Point3(Vel1*measurement_dt) );
|
||||
Pose3 Pose2(R2, t2);
|
||||
Vector dv = measurement_dt * (R1.matrix() * measurement_acc + world_g);
|
||||
LieVector Vel2 = Vel1.compose( dv );
|
||||
imuBias::ConstantBias Bias1;
|
||||
|
||||
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
|
||||
Vector ExpectedErr(zero(9));
|
||||
|
||||
// TODO: Expected values need to be updated for global velocity version
|
||||
CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
|
||||
}
|
||||
|
||||
|
||||
///* VADIM - START ************************************************************************* */
|
||||
//LieVector predictionRq(const LieVector angles, const LieVector q) {
|
||||
// return (Rot3().RzRyRx(angles) * q).vector();
|
||||
//}
|
||||
//
|
||||
//TEST (InertialNavFactor_GlobalVelocity, Rotation_Deriv ) {
|
||||
// LieVector angles(Vector_(3, 3.001, -1.0004, 2.0005));
|
||||
// Rot3 R1(Rot3().RzRyRx(angles));
|
||||
// LieVector q(Vector_(3, 5.8, -2.2, 4.105));
|
||||
// Rot3 qx(0.0, -q[2], q[1],
|
||||
// q[2], 0.0, -q[0],
|
||||
// -q[1], q[0],0.0);
|
||||
// Matrix J_hyp( -(R1*qx).matrix() );
|
||||
//
|
||||
// gtsam::Matrix J_expected;
|
||||
//
|
||||
// LieVector v(predictionRq(angles, q));
|
||||
//
|
||||
// J_expected = gtsam::numericalDerivative11<LieVector, LieVector>(boost::bind(&predictionRq, _1, q), angles);
|
||||
//
|
||||
// cout<<"J_hyp"<<J_hyp<<endl;
|
||||
// cout<<"J_expected"<<J_expected<<endl;
|
||||
//
|
||||
// CHECK( gtsam::assert_equal(J_expected, J_hyp, 1e-6));
|
||||
//}
|
||||
///* VADIM - END ************************************************************************* */
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST (InertialNavFactor_GlobalVelocity, Jacobian ) {
|
||||
|
||||
gtsam::Key PoseKey1(11);
|
||||
gtsam::Key PoseKey2(12);
|
||||
gtsam::Key VelKey1(21);
|
||||
gtsam::Key VelKey2(22);
|
||||
gtsam::Key BiasKey1(31);
|
||||
|
||||
double measurement_dt(0.01);
|
||||
Vector world_g(Vector_(3, 0.0, 0.0, 9.81));
|
||||
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system
|
||||
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5));
|
||||
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
|
||||
|
||||
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
|
||||
|
||||
Vector measurement_acc(Vector_(3, 6.501390843381716, -6.763926150509185, -2.300389940090343));
|
||||
Vector measurement_gyro(Vector_(3, 3.14, 3.14/2, -3.14));
|
||||
|
||||
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> factor(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
|
||||
|
||||
Rot3 R1(0.487316618, 0.125253866, 0.86419557,
|
||||
0.580273724, 0.693095498, -0.427669306,
|
||||
-0.652537293, 0.709880342, 0.265075427);
|
||||
Point3 t1(2.0,1.0,3.0);
|
||||
Pose3 Pose1(R1, t1);
|
||||
LieVector Vel1(3,0.5,-0.5,0.4);
|
||||
Rot3 R2(0.473618898, 0.119523052, 0.872582019,
|
||||
0.609241153, 0.67099888, -0.422594037,
|
||||
-0.636011287, 0.731761397, 0.244979388);
|
||||
Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800);
|
||||
Pose3 Pose2(R2, t2);
|
||||
LieVector Vel2(3,0.510000000000000, -0.480000000000000, 0.430000000000000);
|
||||
imuBias::ConstantBias Bias1;
|
||||
|
||||
Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual;
|
||||
|
||||
Vector ActualErr(factor.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2, H1_actual, H2_actual, H3_actual, H4_actual, H5_actual));
|
||||
|
||||
// Checking for Pose part in the jacobians
|
||||
// ******
|
||||
Matrix H1_actualPose(H1_actual.block(0,0,6,H1_actual.cols()));
|
||||
Matrix H2_actualPose(H2_actual.block(0,0,6,H2_actual.cols()));
|
||||
Matrix H3_actualPose(H3_actual.block(0,0,6,H3_actual.cols()));
|
||||
Matrix H4_actualPose(H4_actual.block(0,0,6,H4_actual.cols()));
|
||||
Matrix H5_actualPose(H5_actual.block(0,0,6,H5_actual.cols()));
|
||||
|
||||
// Calculate the Jacobian matrices H1 until H5 using the numerical derivative function
|
||||
gtsam::Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, H5_expectedPose;
|
||||
H1_expectedPose = gtsam::numericalDerivative11<Pose3, Pose3>(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1);
|
||||
H2_expectedPose = gtsam::numericalDerivative11<Pose3, LieVector>(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1);
|
||||
H3_expectedPose = gtsam::numericalDerivative11<Pose3, imuBias::ConstantBias>(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1);
|
||||
H4_expectedPose = gtsam::numericalDerivative11<Pose3, Pose3>(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2);
|
||||
H5_expectedPose = gtsam::numericalDerivative11<Pose3, LieVector>(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2);
|
||||
|
||||
// Verify they are equal for this choice of state
|
||||
CHECK( gtsam::assert_equal(H1_expectedPose, H1_actualPose, 1e-6));
|
||||
CHECK( gtsam::assert_equal(H2_expectedPose, H2_actualPose, 1e-6));
|
||||
CHECK( gtsam::assert_equal(H3_expectedPose, H3_actualPose, 1e-6));
|
||||
CHECK( gtsam::assert_equal(H4_expectedPose, H4_actualPose, 1e-6));
|
||||
CHECK( gtsam::assert_equal(H5_expectedPose, H5_actualPose, 1e-6));
|
||||
|
||||
// Checking for Vel part in the jacobians
|
||||
// ******
|
||||
Matrix H1_actualVel(H1_actual.block(6,0,3,H1_actual.cols()));
|
||||
Matrix H2_actualVel(H2_actual.block(6,0,3,H2_actual.cols()));
|
||||
Matrix H3_actualVel(H3_actual.block(6,0,3,H3_actual.cols()));
|
||||
Matrix H4_actualVel(H4_actual.block(6,0,3,H4_actual.cols()));
|
||||
Matrix H5_actualVel(H5_actual.block(6,0,3,H5_actual.cols()));
|
||||
|
||||
// Calculate the Jacobian matrices H1 until H5 using the numerical derivative function
|
||||
gtsam::Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel;
|
||||
H1_expectedVel = gtsam::numericalDerivative11<LieVector, Pose3>(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1);
|
||||
H2_expectedVel = gtsam::numericalDerivative11<LieVector, LieVector>(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1);
|
||||
H3_expectedVel = gtsam::numericalDerivative11<LieVector, imuBias::ConstantBias>(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1);
|
||||
H4_expectedVel = gtsam::numericalDerivative11<LieVector, Pose3>(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2);
|
||||
H5_expectedVel = gtsam::numericalDerivative11<LieVector, LieVector>(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2);
|
||||
|
||||
// Verify they are equal for this choice of state
|
||||
CHECK( gtsam::assert_equal(H1_expectedVel, H1_actualVel, 1e-6));
|
||||
CHECK( gtsam::assert_equal(H2_expectedVel, H2_actualVel, 1e-6));
|
||||
CHECK( gtsam::assert_equal(H3_expectedVel, H3_actualVel, 1e-6));
|
||||
CHECK( gtsam::assert_equal(H4_expectedVel, H4_actualVel, 1e-6));
|
||||
CHECK( gtsam::assert_equal(H5_expectedVel, H5_actualVel, 1e-6));
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( InertialNavFactor_GlobalVelocity, ConstructorWithTransform)
|
||||
{
|
||||
gtsam::Key Pose1(11);
|
||||
gtsam::Key Pose2(12);
|
||||
gtsam::Key Vel1(21);
|
||||
gtsam::Key Vel2(22);
|
||||
gtsam::Key Bias1(31);
|
||||
|
||||
Vector measurement_acc(Vector_(3, 0.1, 0.2, 0.4));
|
||||
Vector measurement_gyro(Vector_(3, -0.2, 0.5, 0.03));
|
||||
|
||||
double measurement_dt(0.1);
|
||||
Vector world_g(Vector_(3, 0.0, 0.0, 9.81));
|
||||
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system
|
||||
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5));
|
||||
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
|
||||
|
||||
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
|
||||
|
||||
Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(0.0, 0.0, 0.0)); // IMU is in ENU orientation
|
||||
|
||||
|
||||
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( InertialNavFactor_GlobalVelocity, EqualsWithTransform)
|
||||
{
|
||||
gtsam::Key Pose1(11);
|
||||
gtsam::Key Pose2(12);
|
||||
gtsam::Key Vel1(21);
|
||||
gtsam::Key Vel2(22);
|
||||
gtsam::Key Bias1(31);
|
||||
|
||||
Vector measurement_acc(Vector_(3, 0.1, 0.2, 0.4));
|
||||
Vector measurement_gyro(Vector_(3, -0.2, 0.5, 0.03));
|
||||
|
||||
double measurement_dt(0.1);
|
||||
Vector world_g(Vector_(3, 0.0, 0.0, 9.81));
|
||||
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system
|
||||
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5));
|
||||
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
|
||||
|
||||
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
|
||||
|
||||
Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(0.0, 0.0, 0.0)); // IMU is in ENU orientation
|
||||
|
||||
|
||||
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor);
|
||||
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> g(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor);
|
||||
CHECK(assert_equal(f, g, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( InertialNavFactor_GlobalVelocity, PredictWithTransform)
|
||||
{
|
||||
gtsam::Key PoseKey1(11);
|
||||
gtsam::Key PoseKey2(12);
|
||||
gtsam::Key VelKey1(21);
|
||||
gtsam::Key VelKey2(22);
|
||||
gtsam::Key BiasKey1(31);
|
||||
|
||||
double measurement_dt(0.1);
|
||||
Vector world_g(Vector_(3, 0.0, 0.0, 9.81));
|
||||
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system
|
||||
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5));
|
||||
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
|
||||
|
||||
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
|
||||
|
||||
Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation
|
||||
|
||||
|
||||
// First test: zero angular motion, some acceleration
|
||||
Vector measurement_gyro(Vector_(3, 0.0, 0.0, 0.0)); // Measured in ENU orientation
|
||||
Matrix omega__cross = skewSymmetric(measurement_gyro);
|
||||
Vector measurement_acc = Vector_(3, 0.2, 0.1, -0.3+9.81) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector(); // Measured in ENU orientation
|
||||
|
||||
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor);
|
||||
|
||||
Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00));
|
||||
LieVector Vel1(3, 0.50, -0.50, 0.40);
|
||||
imuBias::ConstantBias Bias1;
|
||||
Pose3 expectedPose2(Rot3(), Point3(2.05, 0.95, 3.04));
|
||||
LieVector expectedVel2(3, 0.51, -0.48, 0.43);
|
||||
Pose3 actualPose2;
|
||||
LieVector actualVel2;
|
||||
f.predict(Pose1, Vel1, Bias1, actualPose2, actualVel2);
|
||||
|
||||
CHECK(assert_equal(expectedPose2, actualPose2, 1e-5));
|
||||
CHECK(assert_equal(expectedVel2, actualVel2, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( InertialNavFactor_GlobalVelocity, ErrorPosVelWithTransform)
|
||||
{
|
||||
gtsam::Key PoseKey1(11);
|
||||
gtsam::Key PoseKey2(12);
|
||||
gtsam::Key VelKey1(21);
|
||||
gtsam::Key VelKey2(22);
|
||||
gtsam::Key BiasKey1(31);
|
||||
|
||||
double measurement_dt(0.1);
|
||||
Vector world_g(Vector_(3, 0.0, 0.0, 9.81));
|
||||
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system
|
||||
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5));
|
||||
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
|
||||
|
||||
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
|
||||
|
||||
Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation
|
||||
|
||||
|
||||
// First test: zero angular motion, some acceleration
|
||||
Vector measurement_gyro(Vector_(3, 0.0, 0.0, 0.0)); // Measured in ENU orientation
|
||||
Matrix omega__cross = skewSymmetric(measurement_gyro);
|
||||
Vector measurement_acc = Vector_(3, 0.2, 0.1, -0.3+9.81) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector(); // Measured in ENU orientation
|
||||
|
||||
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor);
|
||||
|
||||
Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00));
|
||||
Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04));
|
||||
LieVector Vel1(3, 0.50, -0.50, 0.40);
|
||||
LieVector Vel2(3, 0.51, -0.48, 0.43);
|
||||
imuBias::ConstantBias Bias1;
|
||||
|
||||
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
|
||||
Vector ExpectedErr(zero(9));
|
||||
|
||||
CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( InertialNavFactor_GlobalVelocity, ErrorRotWithTransform)
|
||||
{
|
||||
gtsam::Key PoseKey1(11);
|
||||
gtsam::Key PoseKey2(12);
|
||||
gtsam::Key VelKey1(21);
|
||||
gtsam::Key VelKey2(22);
|
||||
gtsam::Key BiasKey1(31);
|
||||
|
||||
double measurement_dt(0.1);
|
||||
Vector world_g(Vector_(3, 0.0, 0.0, 9.81));
|
||||
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system
|
||||
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5));
|
||||
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
|
||||
|
||||
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
|
||||
|
||||
Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation
|
||||
|
||||
|
||||
// Second test: zero angular motion, some acceleration
|
||||
Vector measurement_gyro(Vector_(3, 0.2, 0.1, -0.3)); // Measured in ENU orientation
|
||||
Matrix omega__cross = skewSymmetric(measurement_gyro);
|
||||
Vector measurement_acc = Vector_(3, 0.0, 0.0, 0.0+9.81) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector(); // Measured in ENU orientation
|
||||
|
||||
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor);
|
||||
|
||||
Pose3 Pose1(Rot3(), Point3(2.0,1.0,3.0));
|
||||
Pose3 Pose2(Rot3::Expmap(body_P_sensor.rotation().matrix()*measurement_gyro*measurement_dt), Point3(2.0, 1.0, 3.0));
|
||||
LieVector Vel1(3,0.0,0.0,0.0);
|
||||
LieVector Vel2(3,0.0,0.0,0.0);
|
||||
imuBias::ConstantBias Bias1;
|
||||
|
||||
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
|
||||
Vector ExpectedErr(zero(9));
|
||||
|
||||
CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform)
|
||||
{
|
||||
gtsam::Key PoseKey1(11);
|
||||
gtsam::Key PoseKey2(12);
|
||||
gtsam::Key VelKey1(21);
|
||||
gtsam::Key VelKey2(22);
|
||||
gtsam::Key BiasKey1(31);
|
||||
|
||||
double measurement_dt(0.1);
|
||||
Vector world_g(Vector_(3, 0.0, 0.0, 9.81));
|
||||
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system
|
||||
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5));
|
||||
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
|
||||
|
||||
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
|
||||
|
||||
Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation
|
||||
|
||||
|
||||
// Second test: zero angular motion, some acceleration - generated in matlab
|
||||
Vector measurement_gyro(Vector_(3, 0.2, 0.1, -0.3)); // Measured in ENU orientation
|
||||
Matrix omega__cross = skewSymmetric(measurement_gyro);
|
||||
Vector measurement_acc = Vector_(3, -6.763926150509185, 6.501390843381716, +2.300389940090343) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector(); // Measured in ENU orientation
|
||||
|
||||
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor);
|
||||
|
||||
Rot3 R1(0.487316618, 0.125253866, 0.86419557,
|
||||
0.580273724, 0.693095498, -0.427669306,
|
||||
-0.652537293, 0.709880342, 0.265075427);
|
||||
Point3 t1(2.0,1.0,3.0);
|
||||
Pose3 Pose1(R1, t1);
|
||||
LieVector Vel1(3,0.5,-0.5,0.4);
|
||||
Rot3 R2(0.473618898, 0.119523052, 0.872582019,
|
||||
0.609241153, 0.67099888, -0.422594037,
|
||||
-0.636011287, 0.731761397, 0.244979388);
|
||||
Point3 t2 = t1.compose( Point3(Vel1*measurement_dt) );
|
||||
Pose3 Pose2(R2, t2);
|
||||
Vector dv = measurement_dt * (R1.matrix() * body_P_sensor.rotation().matrix() * Vector_(3, -6.763926150509185, 6.501390843381716, +2.300389940090343) + world_g);
|
||||
LieVector Vel2 = Vel1.compose( dv );
|
||||
imuBias::ConstantBias Bias1;
|
||||
|
||||
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
|
||||
Vector ExpectedErr(zero(9));
|
||||
|
||||
// TODO: Expected values need to be updated for global velocity version
|
||||
CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) {
|
||||
|
||||
gtsam::Key PoseKey1(11);
|
||||
gtsam::Key PoseKey2(12);
|
||||
gtsam::Key VelKey1(21);
|
||||
gtsam::Key VelKey2(22);
|
||||
gtsam::Key BiasKey1(31);
|
||||
|
||||
double measurement_dt(0.01);
|
||||
Vector world_g(Vector_(3, 0.0, 0.0, 9.81));
|
||||
Vector world_rho(Vector_(3, 0.0, -1.5724e-05, 0.0)); // NED system
|
||||
gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5));
|
||||
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
|
||||
|
||||
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
|
||||
|
||||
Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation
|
||||
|
||||
|
||||
Vector measurement_gyro(Vector_(3, 3.14/2, 3.14, +3.14)); // Measured in ENU orientation
|
||||
Matrix omega__cross = skewSymmetric(measurement_gyro);
|
||||
Vector measurement_acc = Vector_(3, -6.763926150509185, 6.501390843381716, +2.300389940090343) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector(); // Measured in ENU orientation
|
||||
|
||||
|
||||
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> factor(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor);
|
||||
|
||||
Rot3 R1(0.487316618, 0.125253866, 0.86419557,
|
||||
0.580273724, 0.693095498, -0.427669306,
|
||||
-0.652537293, 0.709880342, 0.265075427);
|
||||
Point3 t1(2.0,1.0,3.0);
|
||||
Pose3 Pose1(R1, t1);
|
||||
LieVector Vel1(3,0.5,-0.5,0.4);
|
||||
Rot3 R2(0.473618898, 0.119523052, 0.872582019,
|
||||
0.609241153, 0.67099888, -0.422594037,
|
||||
-0.636011287, 0.731761397, 0.244979388);
|
||||
Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800);
|
||||
Pose3 Pose2(R2, t2);
|
||||
LieVector Vel2(3,0.510000000000000, -0.480000000000000, 0.430000000000000);
|
||||
imuBias::ConstantBias Bias1;
|
||||
|
||||
Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual;
|
||||
|
||||
Vector ActualErr(factor.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2, H1_actual, H2_actual, H3_actual, H4_actual, H5_actual));
|
||||
|
||||
// Checking for Pose part in the jacobians
|
||||
// ******
|
||||
Matrix H1_actualPose(H1_actual.block(0,0,6,H1_actual.cols()));
|
||||
Matrix H2_actualPose(H2_actual.block(0,0,6,H2_actual.cols()));
|
||||
Matrix H3_actualPose(H3_actual.block(0,0,6,H3_actual.cols()));
|
||||
Matrix H4_actualPose(H4_actual.block(0,0,6,H4_actual.cols()));
|
||||
Matrix H5_actualPose(H5_actual.block(0,0,6,H5_actual.cols()));
|
||||
|
||||
// Calculate the Jacobian matrices H1 until H5 using the numerical derivative function
|
||||
gtsam::Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, H5_expectedPose;
|
||||
H1_expectedPose = gtsam::numericalDerivative11<Pose3, Pose3>(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1);
|
||||
H2_expectedPose = gtsam::numericalDerivative11<Pose3, LieVector>(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1);
|
||||
H3_expectedPose = gtsam::numericalDerivative11<Pose3, imuBias::ConstantBias>(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1);
|
||||
H4_expectedPose = gtsam::numericalDerivative11<Pose3, Pose3>(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2);
|
||||
H5_expectedPose = gtsam::numericalDerivative11<Pose3, LieVector>(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2);
|
||||
|
||||
// Verify they are equal for this choice of state
|
||||
CHECK( gtsam::assert_equal(H1_expectedPose, H1_actualPose, 1e-6));
|
||||
CHECK( gtsam::assert_equal(H2_expectedPose, H2_actualPose, 1e-6));
|
||||
CHECK( gtsam::assert_equal(H3_expectedPose, H3_actualPose, 1e-6));
|
||||
CHECK( gtsam::assert_equal(H4_expectedPose, H4_actualPose, 1e-6));
|
||||
CHECK( gtsam::assert_equal(H5_expectedPose, H5_actualPose, 1e-6));
|
||||
|
||||
// Checking for Vel part in the jacobians
|
||||
// ******
|
||||
Matrix H1_actualVel(H1_actual.block(6,0,3,H1_actual.cols()));
|
||||
Matrix H2_actualVel(H2_actual.block(6,0,3,H2_actual.cols()));
|
||||
Matrix H3_actualVel(H3_actual.block(6,0,3,H3_actual.cols()));
|
||||
Matrix H4_actualVel(H4_actual.block(6,0,3,H4_actual.cols()));
|
||||
Matrix H5_actualVel(H5_actual.block(6,0,3,H5_actual.cols()));
|
||||
|
||||
// Calculate the Jacobian matrices H1 until H5 using the numerical derivative function
|
||||
gtsam::Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel;
|
||||
H1_expectedVel = gtsam::numericalDerivative11<LieVector, Pose3>(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1);
|
||||
H2_expectedVel = gtsam::numericalDerivative11<LieVector, LieVector>(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1);
|
||||
H3_expectedVel = gtsam::numericalDerivative11<LieVector, imuBias::ConstantBias>(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1);
|
||||
H4_expectedVel = gtsam::numericalDerivative11<LieVector, Pose3>(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2);
|
||||
H5_expectedVel = gtsam::numericalDerivative11<LieVector, LieVector>(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2);
|
||||
|
||||
// Verify they are equal for this choice of state
|
||||
CHECK( gtsam::assert_equal(H1_expectedVel, H1_actualVel, 1e-6));
|
||||
CHECK( gtsam::assert_equal(H2_expectedVel, H2_actualVel, 1e-6));
|
||||
CHECK( gtsam::assert_equal(H3_expectedVel, H3_actualVel, 1e-6));
|
||||
CHECK( gtsam::assert_equal(H4_expectedVel, H4_actualVel, 1e-6));
|
||||
CHECK( gtsam::assert_equal(H5_expectedVel, H5_actualVel, 1e-6));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -1,5 +1,7 @@
|
|||
# Install matlab components
|
||||
|
||||
include(GtsamMatlabWrap)
|
||||
|
||||
# Tests
|
||||
message(STATUS "Installing Matlab Toolbox Tests")
|
||||
install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_tests" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m" PATTERN ".svn" EXCLUDE)
|
||||
|
|
|
|||
|
|
@ -7,6 +7,7 @@ file(GLOB wrap_srcs "*.cpp")
|
|||
file(GLOB wrap_headers "*.h")
|
||||
list(REMOVE_ITEM wrap_srcs ${CMAKE_CURRENT_SOURCE_DIR}/wrap.cpp)
|
||||
add_library(wrap_lib STATIC ${wrap_srcs} ${wrap_headers})
|
||||
target_link_libraries(wrap_lib ${WRAP_BOOST_LIBRARIES})
|
||||
gtsam_assign_source_folders(${wrap_srcs} ${wrap_headers})
|
||||
add_executable(wrap wrap.cpp)
|
||||
target_link_libraries(wrap wrap_lib ${WRAP_BOOST_LIBRARIES})
|
||||
|
|
|
|||
|
|
@ -245,7 +245,7 @@ void Module::parseMarkup(const std::string& data) {
|
|||
|
||||
Rule void_p = str_p("void")[assign_a(retVal.type1)][assign_a(retVal.category1, RETURN_VOID)];
|
||||
|
||||
Rule returnType_p = void_p | returnType1_p | pair_p;
|
||||
Rule returnType_p = void_p | pair_p | returnType1_p;
|
||||
|
||||
Rule methodName_p = lexeme_d[lower_p >> *(alnum_p | '_')];
|
||||
|
||||
|
|
@ -407,9 +407,9 @@ void verifyReturnTypes(const vector<string>& validtypes, const map<string,T>& vt
|
|||
BOOST_FOREACH(const Name_Method& name_method, vt) {
|
||||
const T& t = name_method.second;
|
||||
BOOST_FOREACH(const ReturnValue& retval, t.returnVals) {
|
||||
if (find(validtypes.begin(), validtypes.end(), retval.qualifiedType1("::")) == validtypes.end())
|
||||
if (find(validtypes.begin(), validtypes.end(), retval.qualifiedType1("::")) == validtypes.end())
|
||||
throw DependencyMissing(retval.qualifiedType1("::"), t.name);
|
||||
if (retval.isPair && find(validtypes.begin(), validtypes.end(), retval.qualifiedType2("::")) == validtypes.end())
|
||||
if (retval.isPair && find(validtypes.begin(), validtypes.end(), retval.qualifiedType2("::")) == validtypes.end())
|
||||
throw DependencyMissing(retval.qualifiedType2("::"), t.name);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -65,7 +65,13 @@ int main(int argc, const char* argv[]) {
|
|||
cerr << endl;
|
||||
usage();
|
||||
}
|
||||
else
|
||||
generate_matlab_toolbox(argv[1],argv[2],argv[3],argv[4]);
|
||||
else {
|
||||
try {
|
||||
generate_matlab_toolbox(argv[1],argv[2],argv[3],argv[4]);
|
||||
} catch(std::exception& e) {
|
||||
cerr << e.what() << endl;
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue