Moved inertial nav factors and unit tests to gtsam/navigation
Richard Roberts
parent
b68fee7b00
commit
35cebaa873
|
|
@ -9,6 +9,7 @@ set (gtsam_subdirs
|
|||
linear
|
||||
nonlinear
|
||||
slam
|
||||
navigation
|
||||
)
|
||||
|
||||
set(gtsam_srcs)
|
||||
|
|
@ -73,6 +74,7 @@ set(gtsam_srcs
|
|||
${linear_srcs}
|
||||
${nonlinear_srcs}
|
||||
${slam_srcs}
|
||||
${navigation_srcs}
|
||||
)
|
||||
|
||||
# Versions
|
||||
|
|
|
|||
|
|
@ -0,0 +1,21 @@
|
|||
# Install headers
|
||||
file(GLOB navigation_headers "*.h")
|
||||
install(FILES ${navigation_headers} DESTINATION include/gtsam/navigation)
|
||||
|
||||
# Components to link tests in this subfolder against
|
||||
set(navigation_local_libs
|
||||
nonlinear
|
||||
linear
|
||||
inference
|
||||
geometry
|
||||
base
|
||||
ccolamd
|
||||
)
|
||||
|
||||
# Exclude tests that don't work
|
||||
set (navigation_excluded_tests "")
|
||||
|
||||
# Add all tests
|
||||
if (GTSAM_BUILD_TESTS)
|
||||
gtsam_add_subdir_tests(navigation "${navigation_local_libs}" "${gtsam-default}" "${navigation_excluded_tests}")
|
||||
endif()
|
||||
|
|
@ -15,8 +15,8 @@
|
|||
* @author Vadim Indelman, Stephen Williams
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/navigation/EquivInertialNavFactor_GlobalVel.h>
|
||||
#include <gtsam_unstable/navigation/ImuBias.h>
|
||||
#include <gtsam/navigation/EquivInertialNavFactor_GlobalVel.h>
|
||||
#include <gtsam/navigation/ImuBias.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/nonlinear/Key.h>
|
||||
|
|
@ -15,7 +15,7 @@
|
|||
* @author Vadim Indelman, Stephen Williams
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/navigation/ImuBias.h>
|
||||
#include <gtsam/navigation/ImuBias.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
using namespace std;
|
||||
|
|
@ -17,8 +17,8 @@
|
|||
|
||||
#include <iostream>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam_unstable/dynamics/ImuBias.h>
|
||||
#include <gtsam_unstable/dynamics/InertialNavFactor_GlobalVelocity.h>
|
||||
#include <gtsam/navigation/ImuBias.h>
|
||||
#include <gtsam/navigation/InertialNavFactor_GlobalVelocity.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/nonlinear/Key.h>
|
||||
|
|
@ -8,7 +8,6 @@ set (gtsam_unstable_subdirs
|
|||
dynamics
|
||||
nonlinear
|
||||
slam
|
||||
navigation
|
||||
)
|
||||
|
||||
set(GTSAM_UNSTABLE_BOOST_LIBRARIES ${GTSAM_BOOST_LIBRARIES} ${Boost_THREAD_LIBRARY})
|
||||
|
|
@ -41,7 +40,6 @@ set(gtsam_unstable_srcs
|
|||
${linear_srcs}
|
||||
${nonlinear_srcs}
|
||||
${slam_srcs}
|
||||
${navigation_srcs}
|
||||
)
|
||||
|
||||
# Versions - same as core gtsam library
|
||||
|
|
@ -81,7 +79,6 @@ if (GTSAM_BUILD_SHARED_LIBRARY)
|
|||
PREFIX ""
|
||||
DEFINE_SYMBOL GTSAM_UNSTABLE_EXPORTS
|
||||
RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/bin")
|
||||
add_definitions(/wd4251 /wd4275) # Disable non-DLL-exported base class warnings
|
||||
endif()
|
||||
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)
|
||||
|
|
|
|||
|
|
@ -1,184 +0,0 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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
|
||||
|
||||
|
||||
|
|
@ -1,26 +0,0 @@
|
|||
# Install headers
|
||||
file(GLOB navigation_headers "*.h")
|
||||
install(FILES ${navigation_headers} DESTINATION include/gtsam_unstable/navigation)
|
||||
|
||||
# Components to link tests in this subfolder against
|
||||
set(navigation_local_libs
|
||||
# navigation_unstable
|
||||
slam
|
||||
nonlinear
|
||||
linear
|
||||
inference
|
||||
geometry
|
||||
base
|
||||
ccolamd
|
||||
)
|
||||
|
||||
set (navigation_full_libs
|
||||
${gtsam-default}
|
||||
${gtsam_unstable-default})
|
||||
|
||||
# Exclude tests that don't work
|
||||
set (navigation_excluded_tests "")
|
||||
|
||||
# Add all tests
|
||||
gtsam_add_subdir_tests(navigation_unstable "${navigation_local_libs}" "${navigation_full_libs}" "${navigation_excluded_tests}")
|
||||
add_dependencies(check.unstable check.navigation_unstable)
|
||||
Loading…
Reference in New Issue