diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index f27e216af..34ded8259 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -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 diff --git a/gtsam/navigation/CMakeLists.txt b/gtsam/navigation/CMakeLists.txt new file mode 100644 index 000000000..c49631708 --- /dev/null +++ b/gtsam/navigation/CMakeLists.txt @@ -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() diff --git a/gtsam_unstable/navigation/EquivInertialNavFactor_GlobalVel.h b/gtsam/navigation/EquivInertialNavFactor_GlobalVel.h similarity index 100% rename from gtsam_unstable/navigation/EquivInertialNavFactor_GlobalVel.h rename to gtsam/navigation/EquivInertialNavFactor_GlobalVel.h diff --git a/gtsam_unstable/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h similarity index 100% rename from gtsam_unstable/navigation/ImuBias.h rename to gtsam/navigation/ImuBias.h diff --git a/gtsam_unstable/dynamics/InertialNavFactor_GlobalVelocity.h b/gtsam/navigation/InertialNavFactor_GlobalVelocity.h similarity index 100% rename from gtsam_unstable/dynamics/InertialNavFactor_GlobalVelocity.h rename to gtsam/navigation/InertialNavFactor_GlobalVelocity.h diff --git a/gtsam_unstable/navigation/tests/testEquivInertialNavFactor_GlobalVel.h b/gtsam/navigation/tests/testEquivInertialNavFactor_GlobalVel.h similarity index 94% rename from gtsam_unstable/navigation/tests/testEquivInertialNavFactor_GlobalVel.h rename to gtsam/navigation/tests/testEquivInertialNavFactor_GlobalVel.h index 280b001af..db450216c 100644 --- a/gtsam_unstable/navigation/tests/testEquivInertialNavFactor_GlobalVel.h +++ b/gtsam/navigation/tests/testEquivInertialNavFactor_GlobalVel.h @@ -15,8 +15,8 @@ * @author Vadim Indelman, Stephen Williams */ -#include -#include +#include +#include #include #include #include diff --git a/gtsam_unstable/navigation/tests/testImuBias.cpp b/gtsam/navigation/tests/testImuBias.cpp similarity index 96% rename from gtsam_unstable/navigation/tests/testImuBias.cpp rename to gtsam/navigation/tests/testImuBias.cpp index 4f36f4999..2fdd2fb4e 100644 --- a/gtsam_unstable/navigation/tests/testImuBias.cpp +++ b/gtsam/navigation/tests/testImuBias.cpp @@ -15,7 +15,7 @@ * @author Vadim Indelman, Stephen Williams */ -#include +#include #include using namespace std; diff --git a/gtsam_unstable/dynamics/tests/testInertialNavFactor_GlobalVelocity.cpp b/gtsam/navigation/tests/testInertialNavFactor_GlobalVelocity.cpp similarity index 99% rename from gtsam_unstable/dynamics/tests/testInertialNavFactor_GlobalVelocity.cpp rename to gtsam/navigation/tests/testInertialNavFactor_GlobalVelocity.cpp index ca1036442..9ae92f5c4 100644 --- a/gtsam_unstable/dynamics/tests/testInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam/navigation/tests/testInertialNavFactor_GlobalVelocity.cpp @@ -17,8 +17,8 @@ #include #include -#include -#include +#include +#include #include #include #include diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt index b872b0ff1..fb6768c1b 100644 --- a/gtsam_unstable/CMakeLists.txt +++ b/gtsam_unstable/CMakeLists.txt @@ -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) diff --git a/gtsam_unstable/dynamics/ImuBias.h b/gtsam_unstable/dynamics/ImuBias.h deleted file mode 100644 index 2269cd8a4..000000000 --- a/gtsam_unstable/dynamics/ImuBias.h +++ /dev/null @@ -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 -#include -#include -#include -#include - -/* - * 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 { - 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 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 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 H1=boost::none, boost::optional 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 H1=boost::none, - boost::optional 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 H1=boost::none, - boost::optional 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 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 - - diff --git a/gtsam_unstable/navigation/CMakeLists.txt b/gtsam_unstable/navigation/CMakeLists.txt deleted file mode 100644 index b33658a13..000000000 --- a/gtsam_unstable/navigation/CMakeLists.txt +++ /dev/null @@ -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)