diff --git a/.github/scripts/python.sh b/.github/scripts/python.sh index 3f5701281..6cc62d2b0 100644 --- a/.github/scripts/python.sh +++ b/.github/scripts/python.sh @@ -75,7 +75,7 @@ cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} \ -DGTSAM_UNSTABLE_BUILD_PYTHON=${GTSAM_BUILD_UNSTABLE:-ON} \ -DGTSAM_PYTHON_VERSION=$PYTHON_VERSION \ -DPYTHON_EXECUTABLE:FILEPATH=$(which $PYTHON) \ - -DGTSAM_ALLOW_DEPRECATED_SINCE_V41=OFF \ + -DGTSAM_ALLOW_DEPRECATED_SINCE_V42=OFF \ -DCMAKE_INSTALL_PREFIX=$GITHUB_WORKSPACE/gtsam_install diff --git a/.github/scripts/unix.sh b/.github/scripts/unix.sh index 9689d346c..d890577b6 100644 --- a/.github/scripts/unix.sh +++ b/.github/scripts/unix.sh @@ -64,7 +64,7 @@ function configure() -DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \ -DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \ -DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \ - -DGTSAM_ALLOW_DEPRECATED_SINCE_V41=${GTSAM_ALLOW_DEPRECATED_SINCE_V41:-OFF} \ + -DGTSAM_ALLOW_DEPRECATED_SINCE_V42=${GTSAM_ALLOW_DEPRECATED_SINCE_V42:-OFF} \ -DGTSAM_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \ -DGTSAM_ROT3_EXPMAP=${GTSAM_ROT3_EXPMAP:-ON} \ -DGTSAM_POSE3_EXPMAP=${GTSAM_POSE3_EXPMAP:-ON} \ diff --git a/.github/workflows/build-special.yml b/.github/workflows/build-special.yml index 647b9c0f1..d357b9a34 100644 --- a/.github/workflows/build-special.yml +++ b/.github/workflows/build-special.yml @@ -110,7 +110,7 @@ jobs: - name: Set Allow Deprecated Flag if: matrix.flag == 'deprecated' run: | - echo "GTSAM_ALLOW_DEPRECATED_SINCE_V41=ON" >> $GITHUB_ENV + echo "GTSAM_ALLOW_DEPRECATED_SINCE_V42=ON" >> $GITHUB_ENV echo "Allow deprecated since version 4.1" - name: Set Use Quaternions Flag diff --git a/README.md b/README.md index ee5746e1c..52ac0a5d8 100644 --- a/README.md +++ b/README.md @@ -4,7 +4,7 @@ As of Dec 2021, the `develop` branch is officially in "Pre 4.2" mode. A great new feature we will be adding in 4.2 is *hybrid inference* a la DCSLAM (Kevin Doherty et al) and we envision several API-breaking changes will happen in the discrete folder. -In addition, features deprecated in 4.1 will be removed. Please use the last [4.1.1 release](https://github.com/borglab/gtsam/releases/tag/4.1.1) if you need those features. However, most (not all, unfortunately) are easily converted and can be tracked down (in 4.1.1) by disabling the cmake flag `GTSAM_ALLOW_DEPRECATED_SINCE_V41`. +In addition, features deprecated in 4.1 will be removed. Please use the last [4.1.1 release](https://github.com/borglab/gtsam/releases/tag/4.1.1) if you need those features. However, most (not all, unfortunately) are easily converted and can be tracked down (in 4.1.1) by disabling the cmake flag `GTSAM_ALLOW_DEPRECATED_SINCE_V42`. ## What is GTSAM? @@ -57,7 +57,7 @@ GTSAM 4 introduces several new features, most notably Expressions and a Python t GTSAM 4 also deprecated some legacy functionality and wrongly named methods. If you are on a 4.0.X release, you can define the flag `GTSAM_ALLOW_DEPRECATED_SINCE_V4` to use the deprecated methods. -GTSAM 4.1 added a new pybind wrapper, and **removed** the deprecated functionality. There is a flag `GTSAM_ALLOW_DEPRECATED_SINCE_V41` for newly deprecated methods since the 4.1 release, which is on by default, allowing anyone to just pull version 4.1 and compile. +GTSAM 4.1 added a new pybind wrapper, and **removed** the deprecated functionality. There is a flag `GTSAM_ALLOW_DEPRECATED_SINCE_V42` for newly deprecated methods since the 4.1 release, which is on by default, allowing anyone to just pull version 4.1 and compile. ## Wrappers diff --git a/Using-GTSAM-EXPORT.md b/Using-GTSAM-EXPORT.md index cae1d499c..faeebc97f 100644 --- a/Using-GTSAM-EXPORT.md +++ b/Using-GTSAM-EXPORT.md @@ -29,7 +29,7 @@ Rule #1 doesn't seem very bad, until you combine it with rule #2 ***Compiler Rule #2*** Anything declared in a header file is not included in a DLL. -When these two rules are combined, you get some very confusing results. For example, a class which is completely defined in a header (e.g. LieMatrix) cannot use `GTSAM_EXPORT` in its definition. If LieMatrix is defined with `GTSAM_EXPORT`, then the compiler _must_ find LieMatrix in a DLL. Because LieMatrix is a header-only class, however, it can't find it, leading to a very confusing "I can't find this symbol" type of error. Note that the linker says it can't find the symbol even though the compiler found the header file that completely defines the class. +When these two rules are combined, you get some very confusing results. For example, a class which is completely defined in a header (e.g. Foo) cannot use `GTSAM_EXPORT` in its definition. If Foo is defined with `GTSAM_EXPORT`, then the compiler _must_ find Foo in a DLL. Because Foo is a header-only class, however, it can't find it, leading to a very confusing "I can't find this symbol" type of error. Note that the linker says it can't find the symbol even though the compiler found the header file that completely defines the class. Also note that when a class that you want to export inherits from another class that is not exportable, this can cause significant issues. According to this [MSVC Warning page](https://docs.microsoft.com/en-us/cpp/error-messages/compiler-warnings/compiler-warning-level-2-c4275?view=vs-2019), it may not strictly be a rule, but we have seen several linker errors when a class that is defined with `GTSAM_EXPORT` extended an Eigen class. In general, it appears that any inheritance of non-exportable class by an exportable class is a bad idea. diff --git a/cmake/HandleGeneralOptions.cmake b/cmake/HandleGeneralOptions.cmake index 64c239f39..7c8f8533f 100644 --- a/cmake/HandleGeneralOptions.cmake +++ b/cmake/HandleGeneralOptions.cmake @@ -25,7 +25,7 @@ option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will a option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON) option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module with pybind11" OFF) option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF) -option(GTSAM_ALLOW_DEPRECATED_SINCE_V41 "Allow use of methods/functions deprecated in GTSAM 4.1" ON) +option(GTSAM_ALLOW_DEPRECATED_SINCE_V42 "Allow use of methods/functions deprecated in GTSAM 4.1" ON) option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON) option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON) option(GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR "Use the slower but correct version of BetweenFactor" OFF) diff --git a/cmake/HandlePrintConfiguration.cmake b/cmake/HandlePrintConfiguration.cmake index ad6ac5c5c..43ee5b57b 100644 --- a/cmake/HandlePrintConfiguration.cmake +++ b/cmake/HandlePrintConfiguration.cmake @@ -86,7 +86,7 @@ print_enabled_config(${GTSAM_USE_QUATERNIONS} "Quaternions as defaul print_enabled_config(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ") print_enabled_config(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ") print_enabled_config(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ") -print_enabled_config(${GTSAM_ALLOW_DEPRECATED_SINCE_V41} "Allow features deprecated in GTSAM 4.1") +print_enabled_config(${GTSAM_ALLOW_DEPRECATED_SINCE_V42} "Allow features deprecated in GTSAM 4.1") print_enabled_config(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ") print_enabled_config(${GTSAM_TANGENT_PREINTEGRATION} "Use tangent-space preintegration") diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h index 667ef09dc..9db32744e 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h @@ -440,7 +440,7 @@ template class TriangularViewImpl<_Mat EIGEN_DEVICE_FUNC void lazyAssign(const TriangularBase& other); - /** \deprecated */ + /** @deprecated */ template EIGEN_DEVICE_FUNC void lazyAssign(const MatrixBase& other); @@ -523,7 +523,7 @@ template class TriangularViewImpl<_Mat call_assignment(derived(), other.const_cast_derived(), internal::swap_assign_op()); } - /** \deprecated + /** @deprecated * Shortcut for \code (*this).swap(other.triangularView<(*this)::Mode>()) \endcode */ template EIGEN_DEVICE_FUNC diff --git a/gtsam/base/CMakeLists.txt b/gtsam/base/CMakeLists.txt index 99984e7b3..66d3ec721 100644 --- a/gtsam/base/CMakeLists.txt +++ b/gtsam/base/CMakeLists.txt @@ -5,8 +5,5 @@ install(FILES ${base_headers} DESTINATION include/gtsam/base) file(GLOB base_headers_tree "treeTraversal/*.h") install(FILES ${base_headers_tree} DESTINATION include/gtsam/base/treeTraversal) -file(GLOB deprecated_headers "deprecated/*.h") -install(FILES ${deprecated_headers} DESTINATION include/gtsam/base/deprecated) - # Build tests add_subdirectory(tests) diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h deleted file mode 100644 index 210bdcc73..000000000 --- a/gtsam/base/LieMatrix.h +++ /dev/null @@ -1,26 +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 LieMatrix.h - * @brief External deprecation warning, see deprecated/LieMatrix.h for details - * @author Paul Drews - */ - -#pragma once - -#ifdef _MSC_VER -#pragma message("LieMatrix.h is deprecated. Please use Eigen::Matrix instead.") -#else -#warning "LieMatrix.h is deprecated. Please use Eigen::Matrix instead." -#endif - -#include "gtsam/base/deprecated/LieMatrix.h" diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h deleted file mode 100644 index e159ffa87..000000000 --- a/gtsam/base/LieScalar.h +++ /dev/null @@ -1,26 +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 LieScalar.h - * @brief External deprecation warning, see deprecated/LieScalar.h for details - * @author Kai Ni - */ - -#pragma once - -#ifdef _MSC_VER -#pragma message("LieScalar.h is deprecated. Please use double/float instead.") -#else - #warning "LieScalar.h is deprecated. Please use double/float instead." -#endif - -#include diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h deleted file mode 100644 index a7491d804..000000000 --- a/gtsam/base/LieVector.h +++ /dev/null @@ -1,26 +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 LieVector.h - * @brief Deprecation warning for LieVector, see deprecated/LieVector.h for details. - * @author Paul Drews - */ - -#pragma once - -#ifdef _MSC_VER -#pragma message("LieVector.h is deprecated. Please use Eigen::Vector instead.") -#else -#warning "LieVector.h is deprecated. Please use Eigen::Vector instead." -#endif - -#include diff --git a/gtsam/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h index c86fbb6d2..e5bd34d19 100644 --- a/gtsam/base/TestableAssertions.h +++ b/gtsam/base/TestableAssertions.h @@ -80,9 +80,10 @@ bool assert_equal(const V& expected, const boost::optional& actual, do return assert_equal(expected, *actual, tol); } +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 /** * Version of assert_equals to work with vectors - * \deprecated: use container equals instead + * @deprecated: use container equals instead */ template bool GTSAM_DEPRECATED assert_equal(const std::vector& expected, const std::vector& actual, double tol = 1e-9) { @@ -108,6 +109,7 @@ bool GTSAM_DEPRECATED assert_equal(const std::vector& expected, const std::ve } return true; } +#endif /** * Function for comparing maps of testable->testable diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index a057da46b..36dc2288d 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -203,15 +203,16 @@ inline double inner_prod(const V1 &a, const V2& b) { return a.dot(b); } +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 /** * BLAS Level 1 scal: x <- alpha*x - * \deprecated: use operators instead + * @deprecated: use operators instead */ inline void GTSAM_DEPRECATED scal(double alpha, Vector& x) { x *= alpha; } /** * BLAS Level 1 axpy: y <- alpha*x + y - * \deprecated: use operators instead + * @deprecated: use operators instead */ template inline void GTSAM_DEPRECATED axpy(double alpha, const V1& x, V2& y) { @@ -222,6 +223,7 @@ inline void axpy(double alpha, const Vector& x, SubVector y) { assert (y.size()==x.size()); y += alpha * x; } +#endif /** * house(x,j) computes HouseHolder vector v and scaling factor beta diff --git a/gtsam/base/deprecated/LieMatrix.h b/gtsam/base/deprecated/LieMatrix.h deleted file mode 100644 index a3d0a4328..000000000 --- a/gtsam/base/deprecated/LieMatrix.h +++ /dev/null @@ -1,152 +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 LieMatrix.h - * @brief A wrapper around Matrix providing Lie compatibility - * @author Richard Roberts and Alex Cunningham - */ - -#pragma once - -#include - -#include -#include - -namespace gtsam { - -/** - * @deprecated: LieMatrix, LieVector and LieMatrix are obsolete in GTSAM 4.0 as - * we can directly add double, Vector, and Matrix into values now, because of - * gtsam::traits. - */ -struct LieMatrix : public Matrix { - - /// @name Constructors - /// @{ - enum { dimension = Eigen::Dynamic }; - - /** default constructor - only for serialize */ - LieMatrix() {} - - /** initialize from a normal matrix */ - LieMatrix(const Matrix& v) : Matrix(v) {} - - template - LieMatrix(const M& v) : Matrix(v) {} - -// Currently TMP constructor causes ICE on MSVS 2013 -#if (_MSC_VER < 1800) - /** initialize from a fixed size normal vector */ - template - LieMatrix(const Eigen::Matrix& v) : Matrix(v) {} -#endif - - /** constructor with size and initial data, row order ! */ - LieMatrix(size_t m, size_t n, const double* const data) : - Matrix(Eigen::Map(data, m, n)) {} - - /// @} - /// @name Testable interface - /// @{ - - /** print @param s optional string naming the object */ - void print(const std::string& name = "") const { - gtsam::print(matrix(), name); - } - /** equality up to tolerance */ - inline bool equals(const LieMatrix& expected, double tol=1e-5) const { - return gtsam::equal_with_abs_tol(matrix(), expected.matrix(), tol); - } - - /// @} - /// @name Standard Interface - /// @{ - - /** get the underlying matrix */ - inline Matrix matrix() const { - return static_cast(*this); - } - - /// @} - - /// @name Group - /// @{ - LieMatrix compose(const LieMatrix& q) { return (*this)+q;} - LieMatrix between(const LieMatrix& q) { return q-(*this);} - LieMatrix inverse() { return -(*this);} - /// @} - - /// @name Manifold - /// @{ - Vector localCoordinates(const LieMatrix& q) { return between(q).vector();} - LieMatrix retract(const Vector& v) {return compose(LieMatrix(v));} - /// @} - - /// @name Lie Group - /// @{ - static Vector Logmap(const LieMatrix& p) {return p.vector();} - static LieMatrix Expmap(const Vector& v) { return LieMatrix(v);} - /// @} - - /// @name VectorSpace requirements - /// @{ - - /** Returns dimensionality of the tangent space */ - inline size_t dim() const { return size(); } - - /** Convert to vector, is done row-wise - TODO why? */ - inline Vector vector() const { - Vector result(size()); - typedef Eigen::Matrix RowMajor; - Eigen::Map(&result(0), rows(), cols()) = *this; - return result; - } - - /** identity - NOTE: no known size at compile time - so zero length */ - inline static LieMatrix identity() { - throw std::runtime_error("LieMatrix::identity(): Don't use this function"); - return LieMatrix(); - } - /// @} - -private: - - // Serialization function - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("Matrix", - boost::serialization::base_object(*this)); - - } - -}; - - -template<> -struct traits : public internal::VectorSpace { - - // Override Retract, as the default version does not know how to initialize - static LieMatrix Retract(const LieMatrix& origin, const TangentVector& v, - ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { - if (H1) *H1 = Eye(origin); - if (H2) *H2 = Eye(origin); - typedef const Eigen::Matrix RowMajor; - return origin + Eigen::Map(&v(0), origin.rows(), origin.cols()); - } - -}; - -} // \namespace gtsam diff --git a/gtsam/base/deprecated/LieScalar.h b/gtsam/base/deprecated/LieScalar.h deleted file mode 100644 index 6c9a5f766..000000000 --- a/gtsam/base/deprecated/LieScalar.h +++ /dev/null @@ -1,88 +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 LieScalar.h - * @brief A wrapper around scalar providing Lie compatibility - * @author Kai Ni - */ - -#pragma once - -#include -#include -#include - -namespace gtsam { - - /** - * @deprecated: LieScalar, LieVector and LieMatrix are obsolete in GTSAM 4.0 as - * we can directly add double, Vector, and Matrix into values now, because of - * gtsam::traits. - */ - struct LieScalar { - - enum { dimension = 1 }; - - /** default constructor */ - LieScalar() : d_(0.0) {} - - /** wrap a double */ - /*explicit*/ LieScalar(double d) : d_(d) {} - - /** access the underlying value */ - double value() const { return d_; } - - /** Automatic conversion to underlying value */ - operator double() const { return d_; } - - /** convert vector */ - Vector1 vector() const { Vector1 v; v< - struct traits : public internal::ScalarTraits {}; - -} // \namespace gtsam diff --git a/gtsam/base/deprecated/LieVector.h b/gtsam/base/deprecated/LieVector.h deleted file mode 100644 index 745189c3d..000000000 --- a/gtsam/base/deprecated/LieVector.h +++ /dev/null @@ -1,121 +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 LieVector.h - * @brief A wrapper around vector providing Lie compatibility - * @author Alex Cunningham - */ - -#pragma once - -#include -#include - -namespace gtsam { - -/** - * @deprecated: LieVector, LieVector and LieMatrix are obsolete in GTSAM 4.0 as - * we can directly add double, Vector, and Matrix into values now, because of - * gtsam::traits. - */ -struct LieVector : public Vector { - - enum { dimension = Eigen::Dynamic }; - - /** default constructor - should be unnecessary */ - LieVector() {} - - /** initialize from a normal vector */ - LieVector(const Vector& v) : Vector(v) {} - - template - LieVector(const V& v) : Vector(v) {} - -// Currently TMP constructor causes ICE on MSVS 2013 -#if (_MSC_VER < 1800) - /** initialize from a fixed size normal vector */ - template - LieVector(const Eigen::Matrix& v) : Vector(v) {} -#endif - - /** wrap a double */ - LieVector(double d) : Vector((Vector(1) << d).finished()) {} - - /** constructor with size and initial data, row order ! */ - LieVector(size_t m, const double* const data) : Vector(m) { - for (size_t i = 0; i < m; i++) (*this)(i) = data[i]; - } - - /// @name Testable - /// @{ - void print(const std::string& name="") const { - gtsam::print(vector(), name); - } - bool equals(const LieVector& expected, double tol=1e-5) const { - return gtsam::equal(vector(), expected.vector(), tol); - } - /// @} - - /// @name Group - /// @{ - LieVector compose(const LieVector& q) { return (*this)+q;} - LieVector between(const LieVector& q) { return q-(*this);} - LieVector inverse() { return -(*this);} - /// @} - - /// @name Manifold - /// @{ - Vector localCoordinates(const LieVector& q) { return between(q).vector();} - LieVector retract(const Vector& v) {return compose(LieVector(v));} - /// @} - - /// @name Lie Group - /// @{ - static Vector Logmap(const LieVector& p) {return p.vector();} - static LieVector Expmap(const Vector& v) { return LieVector(v);} - /// @} - - /// @name VectorSpace requirements - /// @{ - - /** get the underlying vector */ - Vector vector() const { - return static_cast(*this); - } - - /** Returns dimensionality of the tangent space */ - size_t dim() const { return this->size(); } - - /** identity - NOTE: no known size at compile time - so zero length */ - static LieVector identity() { - throw std::runtime_error("LieVector::identity(): Don't use this function"); - return LieVector(); - } - - /// @} - -private: - - // Serialization function - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("Vector", - boost::serialization::base_object(*this)); - } -}; - - -template<> -struct traits : public internal::VectorSpace {}; - -} // \namespace gtsam diff --git a/gtsam/base/tests/testLieMatrix.cpp b/gtsam/base/tests/testLieMatrix.cpp deleted file mode 100644 index 8c68bf8a0..000000000 --- a/gtsam/base/tests/testLieMatrix.cpp +++ /dev/null @@ -1,70 +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 testLieMatrix.cpp - * @author Richard Roberts - */ - -#include -#include -#include -#include - -using namespace gtsam; - -GTSAM_CONCEPT_TESTABLE_INST(LieMatrix) -GTSAM_CONCEPT_LIE_INST(LieMatrix) - -/* ************************************************************************* */ -TEST( LieMatrix, construction ) { - Matrix m = (Matrix(2,2) << 1.0,2.0, 3.0,4.0).finished(); - LieMatrix lie1(m), lie2(m); - - EXPECT(traits::GetDimension(m) == 4); - EXPECT(assert_equal(m, lie1.matrix())); - EXPECT(assert_equal(lie1, lie2)); -} - -/* ************************************************************************* */ -TEST( LieMatrix, other_constructors ) { - Matrix init = (Matrix(2,2) << 10.0,20.0, 30.0,40.0).finished(); - LieMatrix exp(init); - double data[] = {10,30,20,40}; - LieMatrix b(2,2,data); - EXPECT(assert_equal(exp, b)); -} - -/* ************************************************************************* */ -TEST(LieMatrix, retract) { - LieMatrix init((Matrix(2,2) << 1.0,2.0,3.0,4.0).finished()); - Vector update = (Vector(4) << 3.0, 4.0, 6.0, 7.0).finished(); - - LieMatrix expected((Matrix(2,2) << 4.0, 6.0, 9.0, 11.0).finished()); - LieMatrix actual = traits::Retract(init,update); - - EXPECT(assert_equal(expected, actual)); - - Vector expectedUpdate = update; - Vector actualUpdate = traits::Local(init,actual); - - EXPECT(assert_equal(expectedUpdate, actualUpdate)); - - Vector expectedLogmap = (Vector(4) << 1, 2, 3, 4).finished(); - Vector actualLogmap = traits::Logmap(LieMatrix((Matrix(2,2) << 1.0, 2.0, 3.0, 4.0).finished())); - EXPECT(assert_equal(expectedLogmap, actualLogmap)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ - - diff --git a/gtsam/base/tests/testLieScalar.cpp b/gtsam/base/tests/testLieScalar.cpp deleted file mode 100644 index 74f5e0d41..000000000 --- a/gtsam/base/tests/testLieScalar.cpp +++ /dev/null @@ -1,64 +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 testLieScalar.cpp - * @author Kai Ni - */ - -#include -#include -#include -#include - -using namespace gtsam; - -GTSAM_CONCEPT_TESTABLE_INST(LieScalar) -GTSAM_CONCEPT_LIE_INST(LieScalar) - -const double tol=1e-9; - -//****************************************************************************** -TEST(LieScalar , Concept) { - BOOST_CONCEPT_ASSERT((IsGroup)); - BOOST_CONCEPT_ASSERT((IsManifold)); - BOOST_CONCEPT_ASSERT((IsLieGroup)); -} - -//****************************************************************************** -TEST(LieScalar , Invariants) { - LieScalar lie1(2), lie2(3); - CHECK(check_group_invariants(lie1, lie2)); - CHECK(check_manifold_invariants(lie1, lie2)); -} - -/* ************************************************************************* */ -TEST( testLieScalar, construction ) { - double d = 2.; - LieScalar lie1(d), lie2(d); - - EXPECT_DOUBLES_EQUAL(2., lie1.value(),tol); - EXPECT_DOUBLES_EQUAL(2., lie2.value(),tol); - EXPECT(traits::dimension == 1); - EXPECT(assert_equal(lie1, lie2)); -} - -/* ************************************************************************* */ -TEST( testLieScalar, localCoordinates ) { - LieScalar lie1(1.), lie2(3.); - - Vector1 actual = traits::Local(lie1, lie2); - EXPECT( assert_equal((Vector)(Vector(1) << 2).finished(), actual)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ diff --git a/gtsam/base/tests/testLieVector.cpp b/gtsam/base/tests/testLieVector.cpp deleted file mode 100644 index 76c4fc490..000000000 --- a/gtsam/base/tests/testLieVector.cpp +++ /dev/null @@ -1,66 +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 testLieVector.cpp - * @author Alex Cunningham - */ - -#include -#include -#include -#include - -using namespace gtsam; - -GTSAM_CONCEPT_TESTABLE_INST(LieVector) -GTSAM_CONCEPT_LIE_INST(LieVector) - -//****************************************************************************** -TEST(LieVector , Concept) { - BOOST_CONCEPT_ASSERT((IsGroup)); - BOOST_CONCEPT_ASSERT((IsManifold)); - BOOST_CONCEPT_ASSERT((IsLieGroup)); -} - -//****************************************************************************** -TEST(LieVector , Invariants) { - Vector v = Vector3(1.0, 2.0, 3.0); - LieVector lie1(v), lie2(v); - check_manifold_invariants(lie1, lie2); -} - -//****************************************************************************** -TEST( testLieVector, construction ) { - Vector v = Vector3(1.0, 2.0, 3.0); - LieVector lie1(v), lie2(v); - - EXPECT(lie1.dim() == 3); - EXPECT(assert_equal(v, lie1.vector())); - EXPECT(assert_equal(lie1, lie2)); -} - -//****************************************************************************** -TEST( testLieVector, other_constructors ) { - Vector init = Vector2(10.0, 20.0); - LieVector exp(init); - double data[] = { 10, 20 }; - LieVector b(2, data); - EXPECT(assert_equal(exp, b)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ - diff --git a/gtsam/base/tests/testTestableAssertions.cpp b/gtsam/base/tests/testTestableAssertions.cpp deleted file mode 100644 index 305aa7ca9..000000000 --- a/gtsam/base/tests/testTestableAssertions.cpp +++ /dev/null @@ -1,35 +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 testTestableAssertions - * @author Alex Cunningham - */ - -#include -#include -#include - -using namespace gtsam; - -/* ************************************************************************* */ -TEST( testTestableAssertions, optional ) { - typedef boost::optional OptionalScalar; - LieScalar x(1.0); - OptionalScalar ox(x), dummy = boost::none; - EXPECT(assert_equal(ox, ox)); - EXPECT(assert_equal(x, ox)); - EXPECT(assert_equal(dummy, dummy)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ diff --git a/gtsam/basis/ParameterMatrix.h b/gtsam/basis/ParameterMatrix.h index df2d9f62e..eddcbfeae 100644 --- a/gtsam/basis/ParameterMatrix.h +++ b/gtsam/basis/ParameterMatrix.h @@ -153,7 +153,7 @@ class ParameterMatrix { return matrix_ * other; } - /// @name Vector Space requirements, following LieMatrix + /// @name Vector Space requirements /// @{ /** diff --git a/gtsam/config.h.in b/gtsam/config.h.in index e7623c52b..d47329a62 100644 --- a/gtsam/config.h.in +++ b/gtsam/config.h.in @@ -70,7 +70,7 @@ #cmakedefine GTSAM_THROW_CHEIRALITY_EXCEPTION // Make sure dependent projects that want it can see deprecated functions -#cmakedefine GTSAM_ALLOW_DEPRECATED_SINCE_V41 +#cmakedefine GTSAM_ALLOW_DEPRECATED_SINCE_V42 // Support Metis-based nested dissection #cmakedefine GTSAM_SUPPORT_NESTED_DISSECTION diff --git a/gtsam/geometry/Cal3.h b/gtsam/geometry/Cal3.h index 08ce4c1e6..1690615dd 100644 --- a/gtsam/geometry/Cal3.h +++ b/gtsam/geometry/Cal3.h @@ -170,9 +170,9 @@ class GTSAM_EXPORT Cal3 { return K; } -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 /** @deprecated The following function has been deprecated, use K above */ - Matrix3 matrix() const { return K(); } + Matrix3 GTSAM_DEPRECATED matrix() const { return K(); } #endif /// Return inverted calibration matrix inv(K) diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 0d7c1be9d..b240603fc 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -97,12 +97,12 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 { Vector3 vector() const; -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 /// get parameter u0 - inline double u0() const { return u0_; } + inline double GTSAM_DEPRECATED u0() const { return u0_; } /// get parameter v0 - inline double v0() const { return v0_; } + inline double GTSAM_DEPRECATED v0() const { return v0_; } #endif /** diff --git a/gtsam/geometry/SimpleCamera.cpp b/gtsam/geometry/SimpleCamera.cpp index d1a5ed330..be6a010b2 100644 --- a/gtsam/geometry/SimpleCamera.cpp +++ b/gtsam/geometry/SimpleCamera.cpp @@ -21,8 +21,8 @@ namespace gtsam { -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 - SimpleCamera simpleCamera(const Matrix34& P) { +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 + SimpleCamera GTSAM_DEPRECATED simpleCamera(const Matrix34& P) { // P = [A|a] = s K cRw [I|-T], with s the unknown scale Matrix3 A = P.topLeftCorner(3, 3); diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index 5ff6b9816..f0776c2e2 100644 --- a/gtsam/geometry/SimpleCamera.h +++ b/gtsam/geometry/SimpleCamera.h @@ -37,7 +37,7 @@ namespace gtsam { using PinholeCameraCal3Unified = gtsam::PinholeCamera; using PinholeCameraCal3Fisheye = gtsam::PinholeCamera; -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 /** * @deprecated: SimpleCamera for backwards compatability with GTSAM 3.x * Use PinholeCameraCal3_S2 instead diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp index 18a25c553..173ccf05b 100644 --- a/gtsam/geometry/tests/testSimpleCamera.cpp +++ b/gtsam/geometry/tests/testSimpleCamera.cpp @@ -26,7 +26,7 @@ using namespace std; using namespace gtsam; -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 static const Cal3_S2 K(625, 625, 0, 0, 0); diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index 579eed7f9..c904d2f7f 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -288,8 +288,8 @@ namespace gtsam { FactorGraphType& asDerived() { return static_cast(*this); } public: - #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 - /** \deprecated ordering and orderingType shouldn't both be specified */ + #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 + /** @deprecated ordering and orderingType shouldn't both be specified */ boost::shared_ptr GTSAM_DEPRECATED eliminateSequential( const Ordering& ordering, const Eliminate& function, @@ -298,7 +298,7 @@ namespace gtsam { return eliminateSequential(ordering, function, variableIndex); } - /** \deprecated orderingType specified first for consistency */ + /** @deprecated orderingType specified first for consistency */ boost::shared_ptr GTSAM_DEPRECATED eliminateSequential( const Eliminate& function, OptionalVariableIndex variableIndex = boost::none, @@ -306,7 +306,7 @@ namespace gtsam { return eliminateSequential(orderingType, function, variableIndex); } - /** \deprecated ordering and orderingType shouldn't both be specified */ + /** @deprecated ordering and orderingType shouldn't both be specified */ boost::shared_ptr GTSAM_DEPRECATED eliminateMultifrontal( const Ordering& ordering, const Eliminate& function, @@ -315,7 +315,7 @@ namespace gtsam { return eliminateMultifrontal(ordering, function, variableIndex); } - /** \deprecated orderingType specified first for consistency */ + /** @deprecated orderingType specified first for consistency */ boost::shared_ptr GTSAM_DEPRECATED eliminateMultifrontal( const Eliminate& function, OptionalVariableIndex variableIndex = boost::none, @@ -323,7 +323,7 @@ namespace gtsam { return eliminateMultifrontal(orderingType, function, variableIndex); } - /** \deprecated */ + /** @deprecated */ boost::shared_ptr GTSAM_DEPRECATED marginalMultifrontalBayesNet( boost::variant variables, boost::none_t, @@ -332,7 +332,7 @@ namespace gtsam { return marginalMultifrontalBayesNet(variables, function, variableIndex); } - /** \deprecated */ + /** @deprecated */ boost::shared_ptr GTSAM_DEPRECATED marginalMultifrontalBayesTree( boost::variant variables, boost::none_t, diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index 1ca97cea5..e6a8dcc60 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -153,7 +153,6 @@ typedef FastSet FactorIndexSet; const std::string& s = "Factor", const KeyFormatter& formatter = DefaultKeyFormatter) const; - protected: /// check equality bool equals(const This& other, double tol = 1e-9) const; diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 9297d6461..d87c39eea 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -193,12 +193,15 @@ namespace gtsam { } /* ************************************************************************* */ - void GaussianConditional::scaleFrontalsBySigma(VectorValues& gy) const { +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 + void GTSAM_DEPRECATED + GaussianConditional::scaleFrontalsBySigma(VectorValues& gy) const { DenseIndex vectorPosition = 0; for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { gy[*frontal].array() *= model_->sigmas().segment(vectorPosition, getDim(frontal)).array(); vectorPosition += getDim(frontal); } } +#endif } // namespace gtsam diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 0ea597f99..d93f65b42 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -125,12 +125,11 @@ namespace gtsam { /** Performs transpose backsubstition in place on values */ void solveTransposeInPlace(VectorValues& gy) const; +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 /** Scale the values in \c gy according to the sigmas for the frontal variables in this * conditional. */ - void scaleFrontalsBySigma(VectorValues& gy) const; - - // FIXME: deprecated flag doesn't appear to exist? - // __declspec(deprecated) void scaleFrontalsBySigma(VectorValues& gy) const; + void GTSAM_DEPRECATED scaleFrontalsBySigma(VectorValues& gy) const; +#endif private: /** Serialization function */ diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 5ccb0ce91..7bee4c9fb 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -396,11 +396,11 @@ namespace gtsam { public: -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 - /** \deprecated */ - VectorValues optimize(boost::none_t, - const Eliminate& function = - EliminationTraitsType::DefaultEliminate) const { +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 + /** @deprecated */ + VectorValues GTSAM_DEPRECATED + optimize(boost::none_t, const Eliminate& function = + EliminationTraitsType::DefaultEliminate) const { return optimize(function); } #endif diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 54b9d955f..5c379beb8 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -730,8 +730,8 @@ namespace gtsam { } // namespace noiseModel - /** Note, deliberately not in noiseModel namespace. - * Deprecated. Only for compatibility with previous version. + /** + * Aliases. Deliberately not in noiseModel namespace. */ typedef noiseModel::Base::shared_ptr SharedNoiseModel; typedef noiseModel::Gaussian::shared_ptr SharedGaussian; diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index 7b1ce550f..7acc5bd41 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -467,7 +467,6 @@ virtual class GaussianConditional : gtsam::JacobianFactor { gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues& parents, const gtsam::VectorValues& rhs) const; void solveTransposeInPlace(gtsam::VectorValues& gy) const; - void scaleFrontalsBySigma(gtsam::VectorValues& gy) const; Matrix R() const; Matrix S() const; Vector d() const; diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 4604a55dd..f4db42d0f 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -168,13 +168,12 @@ Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj, } //------------------------------------------------------------------------------ -Rot3 AHRSFactor::Predict( - const Rot3& rot_i, const Vector3& bias, - const PreintegratedAhrsMeasurements preintegratedMeasurements) { - const Vector3 biascorrectedOmega = preintegratedMeasurements.predict(bias); +Rot3 AHRSFactor::Predict(const Rot3& rot_i, const Vector3& bias, + const PreintegratedAhrsMeasurements& pim) { + const Vector3 biascorrectedOmega = pim.predict(bias); // Coriolis term - const Vector3 coriolis = preintegratedMeasurements.integrateCoriolis(rot_i); + const Vector3 coriolis = pim.integrateCoriolis(rot_i); const Vector3 correctedOmega = biascorrectedOmega - coriolis; const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega); @@ -184,27 +183,26 @@ Rot3 AHRSFactor::Predict( //------------------------------------------------------------------------------ AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias, - const PreintegratedMeasurements& pim, + const PreintegratedAhrsMeasurements& pim, const Vector3& omegaCoriolis, const boost::optional& body_P_sensor) - : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), rot_i, rot_j, bias), + : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), rot_i, rot_j, + bias), _PIM_(pim) { - boost::shared_ptr p = - boost::make_shared(pim.p()); + auto p = boost::make_shared(pim.p()); p->body_P_sensor = body_P_sensor; _PIM_.p_ = p; } //------------------------------------------------------------------------------ Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias, - const PreintegratedMeasurements pim, + const PreintegratedAhrsMeasurements& pim, const Vector3& omegaCoriolis, const boost::optional& body_P_sensor) { - boost::shared_ptr p = - boost::make_shared(pim.p()); + auto p = boost::make_shared(pim.p()); p->omegaCoriolis = omegaCoriolis; p->body_P_sensor = body_P_sensor; - PreintegratedMeasurements newPim = pim; + PreintegratedAhrsMeasurements newPim = pim; newPim.p_ = p; return Predict(rot_i, bias, newPim); } diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index 1ab2d7cdc..10c33d101 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -104,11 +104,10 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation static Vector DeltaAngles(const Vector& msr_gyro_t, const double msr_dt, const Vector3& delta_angles); - /// @deprecated constructor + /// @deprecated constructor, but used in tests. PreintegratedAhrsMeasurements(const Vector3& biasHat, const Matrix3& measuredOmegaCovariance) - : PreintegratedRotation(boost::make_shared()), - biasHat_(biasHat) { + : PreintegratedRotation(boost::make_shared()), biasHat_(biasHat) { p_->gyroscopeCovariance = measuredOmegaCovariance; resetIntegration(); } @@ -182,24 +181,26 @@ public: /// predicted states from IMU /// TODO(frank): relationship with PIM predict ?? - static Rot3 Predict( - const Rot3& rot_i, const Vector3& bias, - const PreintegratedAhrsMeasurements preintegratedMeasurements); + static Rot3 Predict(const Rot3& rot_i, const Vector3& bias, + const PreintegratedAhrsMeasurements& pim); + /// @deprecated constructor, but used in tests. + AHRSFactor(Key rot_i, Key rot_j, Key bias, + const PreintegratedAhrsMeasurements& pim, + const Vector3& omegaCoriolis, + const boost::optional& body_P_sensor = boost::none); + + /// @deprecated static function, but used in tests. + static Rot3 predict( + const Rot3& rot_i, const Vector3& bias, + const PreintegratedAhrsMeasurements& pim, const Vector3& omegaCoriolis, + const boost::optional& body_P_sensor = boost::none); + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 /// @deprecated name typedef PreintegratedAhrsMeasurements PreintegratedMeasurements; - /// @deprecated constructor - AHRSFactor(Key rot_i, Key rot_j, Key bias, - const PreintegratedMeasurements& preintegratedMeasurements, - const Vector3& omegaCoriolis, - const boost::optional& body_P_sensor = boost::none); - - /// @deprecated static function - static Rot3 predict(const Rot3& rot_i, const Vector3& bias, - const PreintegratedMeasurements preintegratedMeasurements, - const Vector3& omegaCoriolis, - const boost::optional& body_P_sensor = boost::none); +#endif private: diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index fad952232..7557c4732 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -131,30 +131,30 @@ public: /// @} +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 /// @name Deprecated /// @{ - ConstantBias inverse() { - return -(*this); - } - ConstantBias compose(const ConstantBias& q) { + ConstantBias GTSAM_DEPRECATED inverse() { return -(*this); } + ConstantBias GTSAM_DEPRECATED compose(const ConstantBias& q) { return (*this) + q; } - ConstantBias between(const ConstantBias& q) { + ConstantBias GTSAM_DEPRECATED between(const ConstantBias& q) { return q - (*this); } - Vector6 localCoordinates(const ConstantBias& q) { + Vector6 GTSAM_DEPRECATED localCoordinates(const ConstantBias& q) { return between(q).vector(); } - ConstantBias retract(const Vector6& v) { + ConstantBias GTSAM_DEPRECATED retract(const Vector6& v) { return compose(ConstantBias(v)); } - static Vector6 Logmap(const ConstantBias& p) { + static Vector6 GTSAM_DEPRECATED Logmap(const ConstantBias& p) { return p.vector(); } - static ConstantBias Expmap(const Vector6& v) { + static ConstantBias GTSAM_DEPRECATED Expmap(const Vector6& v) { return ConstantBias(v); } /// @} +#endif private: diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index c2a3bcdb4..6ede1645f 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -18,23 +18,12 @@ class ConstantBias { // Group static gtsam::imuBias::ConstantBias identity(); - gtsam::imuBias::ConstantBias inverse() const; - gtsam::imuBias::ConstantBias compose(const gtsam::imuBias::ConstantBias& b) const; - gtsam::imuBias::ConstantBias between(const gtsam::imuBias::ConstantBias& b) const; // Operator Overloads gtsam::imuBias::ConstantBias operator-() const; gtsam::imuBias::ConstantBias operator+(const gtsam::imuBias::ConstantBias& b) const; gtsam::imuBias::ConstantBias operator-(const gtsam::imuBias::ConstantBias& b) const; - // Manifold - gtsam::imuBias::ConstantBias retract(Vector v) const; - Vector localCoordinates(const gtsam::imuBias::ConstantBias& b) const; - - // Lie Group - static gtsam::imuBias::ConstantBias Expmap(Vector v); - static Vector Logmap(const gtsam::imuBias::ConstantBias& b); - // Standard Interface Vector vector() const; Vector accelerometer() const; diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index a4d06d01a..779f6abcc 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -54,11 +54,11 @@ Rot3 evaluateRotationError(const AHRSFactor& factor, const Rot3 rot_i, return Rot3::Expmap(factor.evaluateError(rot_i, rot_j, bias).tail(3)); } -AHRSFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( +PreintegratedAhrsMeasurements evaluatePreintegratedMeasurements( const Vector3& bias, const list& measuredOmegas, const list& deltaTs, const Vector3& initialRotationRate = Vector3::Zero()) { - AHRSFactor::PreintegratedMeasurements result(bias, I_3x3); + PreintegratedAhrsMeasurements result(bias, I_3x3); list::const_iterator itOmega = measuredOmegas.begin(); list::const_iterator itDeltaT = deltaTs.begin(); @@ -86,10 +86,10 @@ Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) { return Rot3::Logmap(Rot3::Expmap(thetahat).compose(Rot3::Expmap(deltatheta))); } - } + //****************************************************************************** -TEST( AHRSFactor, PreintegratedMeasurements ) { +TEST( AHRSFactor, PreintegratedAhrsMeasurements ) { // Linearization point Vector3 bias(0,0,0); ///< Current estimate of angular rate bias @@ -102,7 +102,7 @@ TEST( AHRSFactor, PreintegratedMeasurements ) { double expectedDeltaT1(0.5); // Actual preintegrated values - AHRSFactor::PreintegratedMeasurements actual1(bias, Z_3x3); + PreintegratedAhrsMeasurements actual1(bias, Z_3x3); actual1.integrateMeasurement(measuredOmega, deltaT); EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()), 1e-6)); @@ -113,7 +113,7 @@ TEST( AHRSFactor, PreintegratedMeasurements ) { double expectedDeltaT2(1); // Actual preintegrated values - AHRSFactor::PreintegratedMeasurements actual2 = actual1; + PreintegratedAhrsMeasurements actual2 = actual1; actual2.integrateMeasurement(measuredOmega, deltaT); EXPECT(assert_equal(expectedDeltaR2, Rot3(actual2.deltaRij()), 1e-6)); @@ -159,7 +159,7 @@ TEST(AHRSFactor, Error) { Vector3 measuredOmega; measuredOmega << M_PI / 100, 0, 0; double deltaT = 1.0; - AHRSFactor::PreintegratedMeasurements pim(bias, Z_3x3); + PreintegratedAhrsMeasurements pim(bias, Z_3x3); pim.integrateMeasurement(measuredOmega, deltaT); // Create factor @@ -217,7 +217,7 @@ TEST(AHRSFactor, ErrorWithBiases) { measuredOmega << 0, 0, M_PI / 10.0 + 0.3; double deltaT = 1.0; - AHRSFactor::PreintegratedMeasurements pim(Vector3(0,0,0), + PreintegratedAhrsMeasurements pim(Vector3(0,0,0), Z_3x3); pim.integrateMeasurement(measuredOmega, deltaT); @@ -360,7 +360,7 @@ TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) { } // Actual preintegrated values - AHRSFactor::PreintegratedMeasurements preintegrated = + PreintegratedAhrsMeasurements preintegrated = evaluatePreintegratedMeasurements(bias, measuredOmegas, deltaTs, Vector3(M_PI / 100.0, 0.0, 0.0)); @@ -397,7 +397,7 @@ TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) { const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)), Point3(1, 0, 0)); - AHRSFactor::PreintegratedMeasurements pim(Vector3::Zero(), kMeasuredAccCovariance); + PreintegratedAhrsMeasurements pim(Vector3::Zero(), kMeasuredAccCovariance); pim.integrateMeasurement(measuredOmega, deltaT); @@ -439,7 +439,7 @@ TEST (AHRSFactor, predictTest) { Vector3 measuredOmega; measuredOmega << 0, 0, M_PI / 10.0; double deltaT = 0.2; - AHRSFactor::PreintegratedMeasurements pim(bias, kMeasuredAccCovariance); + PreintegratedAhrsMeasurements pim(bias, kMeasuredAccCovariance); for (int i = 0; i < 1000; ++i) { pim.integrateMeasurement(measuredOmega, deltaT); } @@ -456,9 +456,9 @@ TEST (AHRSFactor, predictTest) { Rot3 actualRot = factor.predict(x, bias, pim, kZeroOmegaCoriolis); EXPECT(assert_equal(expectedRot, actualRot, 1e-6)); - // AHRSFactor::PreintegratedMeasurements::predict + // PreintegratedAhrsMeasurements::predict Matrix expectedH = numericalDerivative11( - std::bind(&AHRSFactor::PreintegratedMeasurements::predict, + std::bind(&PreintegratedAhrsMeasurements::predict, &pim, std::placeholders::_1, boost::none), bias); // Actual Jacobians @@ -478,7 +478,7 @@ TEST (AHRSFactor, graphTest) { // PreIntegrator Vector3 biasHat(0, 0, 0); - AHRSFactor::PreintegratedMeasurements pim(biasHat, kMeasuredAccCovariance); + PreintegratedAhrsMeasurements pim(biasHat, kMeasuredAccCovariance); // Pre-integrate measurements Vector3 measuredOmega(0, M_PI / 20, 0); diff --git a/gtsam/navigation/tests/testImuBias.cpp b/gtsam/navigation/tests/testImuBias.cpp index b486a4a98..81a1a2ceb 100644 --- a/gtsam/navigation/tests/testImuBias.cpp +++ b/gtsam/navigation/tests/testImuBias.cpp @@ -47,20 +47,19 @@ TEST(ImuBias, Constructor) { } /* ************************************************************************* */ +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 TEST(ImuBias, inverse) { Bias biasActual = bias1.inverse(); Bias biasExpected = Bias(-biasAcc1, -biasGyro1); EXPECT(assert_equal(biasExpected, biasActual)); } -/* ************************************************************************* */ TEST(ImuBias, compose) { Bias biasActual = bias2.compose(bias1); Bias biasExpected = Bias(biasAcc1 + biasAcc2, biasGyro1 + biasGyro2); EXPECT(assert_equal(biasExpected, biasActual)); } -/* ************************************************************************* */ TEST(ImuBias, between) { // p.between(q) == q - p Bias biasActual = bias2.between(bias1); @@ -68,7 +67,6 @@ TEST(ImuBias, between) { EXPECT(assert_equal(biasExpected, biasActual)); } -/* ************************************************************************* */ TEST(ImuBias, localCoordinates) { Vector deltaActual = Vector(bias2.localCoordinates(bias1)); Vector deltaExpected = @@ -76,7 +74,6 @@ TEST(ImuBias, localCoordinates) { EXPECT(assert_equal(deltaExpected, deltaActual)); } -/* ************************************************************************* */ TEST(ImuBias, retract) { Vector6 delta; delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2; @@ -86,14 +83,12 @@ TEST(ImuBias, retract) { EXPECT(assert_equal(biasExpected, biasActual)); } -/* ************************************************************************* */ TEST(ImuBias, Logmap) { Vector deltaActual = bias2.Logmap(bias1); Vector deltaExpected = bias1.vector(); EXPECT(assert_equal(deltaExpected, deltaActual)); } -/* ************************************************************************* */ TEST(ImuBias, Expmap) { Vector6 delta; delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2; @@ -101,6 +96,7 @@ TEST(ImuBias, Expmap) { Bias biasExpected = Bias(delta); EXPECT(assert_equal(biasExpected, biasActual)); } +#endif /* ************************************************************************* */ TEST(ImuBias, operatorSub) { diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index d22690cf3..11bf873e7 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -295,14 +295,14 @@ struct traits> // ExpressionFactorN -#if defined(GTSAM_ALLOW_DEPRECATED_SINCE_V41) +#if defined(GTSAM_ALLOW_DEPRECATED_SINCE_V42) /** * Binary specialization of ExpressionFactor meant as a base class for binary * factors. Enforces an 'expression' method with two keys, and provides * 'evaluateError'. Derived class (a binary factor!) needs to call 'initialize'. * * \sa ExpressionFactorN - * \deprecated Prefer the more general ExpressionFactorN<>. + * @deprecated Prefer the more general ExpressionFactorN<>. */ template class GTSAM_DEPRECATED ExpressionFactor2 : public ExpressionFactorN { diff --git a/gtsam/nonlinear/ExtendedKalmanFilter.h b/gtsam/nonlinear/ExtendedKalmanFilter.h index 77bb1ca6c..df27d16ff 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter.h @@ -51,9 +51,11 @@ class ExtendedKalmanFilter { typedef boost::shared_ptr > shared_ptr; typedef VALUE T; +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 //@deprecated: any NoiseModelFactor will do, as long as they have the right keys typedef NoiseModelFactor2 MotionFactor; typedef NoiseModelFactor1 MeasurementFactor; +#endif protected: T x_; // linearization point diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h index 947274a97..028545d01 100644 --- a/gtsam/nonlinear/Marginals.h +++ b/gtsam/nonlinear/Marginals.h @@ -131,16 +131,16 @@ protected: void computeBayesTree(const Ordering& ordering); public: -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 - /** \deprecated argument order changed due to removing boost::optional */ +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 + /** @deprecated argument order changed due to removing boost::optional */ GTSAM_DEPRECATED Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization, const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {} - /** \deprecated argument order changed due to removing boost::optional */ + /** @deprecated argument order changed due to removing boost::optional */ GTSAM_DEPRECATED Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization, const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {} - /** \deprecated argument order changed due to removing boost::optional */ + /** @deprecated argument order changed due to removing boost::optional */ GTSAM_DEPRECATED Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization, const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {} #endif diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 84b1516e9..160e46924 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -250,25 +250,25 @@ namespace gtsam { public: -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 - /** \deprecated */ +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 + /** @deprecated */ boost::shared_ptr GTSAM_DEPRECATED linearizeToHessianFactor( const Values& values, boost::none_t, const Dampen& dampen = nullptr) const {return linearizeToHessianFactor(values, dampen);} - /** \deprecated */ + /** @deprecated */ Values GTSAM_DEPRECATED updateCholesky(const Values& values, boost::none_t, const Dampen& dampen = nullptr) const {return updateCholesky(values, dampen);} - /** \deprecated */ + /** @deprecated */ void GTSAM_DEPRECATED saveGraph( std::ostream& os, const Values& values = Values(), const GraphvizFormatting& graphvizFormatting = GraphvizFormatting(), const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { dot(os, values, keyFormatter, graphvizFormatting); } - /** \deprecated */ + /** @deprecated */ void GTSAM_DEPRECATED saveGraph(const std::string& filename, const Values& values, const GraphvizFormatting& graphvizFormatting, diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index d7e925bd9..0684063de 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -1304,14 +1304,14 @@ parse3DFactors(const std::string &filename, return parseFactors(filename, model, maxIndex); } -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 -std::map parse3DPoses(const std::string &filename, - size_t maxIndex) { +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 +std::map GTSAM_DEPRECATED +parse3DPoses(const std::string &filename, size_t maxIndex) { return parseVariables(filename, maxIndex); } -std::map parse3DLandmarks(const std::string &filename, - size_t maxIndex) { +std::map GTSAM_DEPRECATED +parse3DLandmarks(const std::string &filename, size_t maxIndex) { return parseVariables(filename, maxIndex); } #endif diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index ec5d6dce9..db5d7d76a 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -172,10 +172,6 @@ GTSAM_EXPORT GraphAndValues load2D(const std::string& filename, false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatAUTO, // KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE); -/// @deprecated load2D now allows for arbitrary models and wrapping a robust kernel -GTSAM_EXPORT GraphAndValues load2D_robust(const std::string& filename, - const noiseModel::Base::shared_ptr& model, size_t maxIndex = 0); - /** save 2d graph */ GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, const Values& config, const noiseModel::Diagonal::shared_ptr model, @@ -504,17 +500,21 @@ parse3DFactors(const std::string &filename, size_t maxIndex = 0); using BinaryMeasurementsUnit3 = std::vector>; -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 -inline boost::optional parseVertex(std::istream &is, - const std::string &tag) { + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 +inline boost::optional GTSAM_DEPRECATED +parseVertex(std::istream& is, const std::string& tag) { return parseVertexPose(is, tag); } -GTSAM_EXPORT std::map parse3DPoses(const std::string &filename, - size_t maxIndex = 0); +GTSAM_EXPORT std::map GTSAM_DEPRECATED +parse3DPoses(const std::string& filename, size_t maxIndex = 0); -GTSAM_EXPORT std::map -parse3DLandmarks(const std::string &filename, size_t maxIndex = 0); +GTSAM_EXPORT std::map GTSAM_DEPRECATED +parse3DLandmarks(const std::string& filename, size_t maxIndex = 0); +GTSAM_EXPORT GraphAndValues GTSAM_DEPRECATED +load2D_robust(const std::string& filename, + const noiseModel::Base::shared_ptr& model, size_t maxIndex = 0); #endif } // namespace gtsam diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index d276c4f2e..a0a7329dd 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -264,8 +264,6 @@ pair load2D( pair load2D( string filename, gtsam::noiseModel::Diagonal* model); pair load2D(string filename); -pair load2D_robust( - string filename, gtsam::noiseModel::Base* model, int maxIndex); void save2D(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config, gtsam::noiseModel::Diagonal* model, string filename); diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h index 572935da3..9c19bae8c 100644 --- a/gtsam_unstable/slam/BetweenFactorEM.h +++ b/gtsam_unstable/slam/BetweenFactorEM.h @@ -421,4 +421,8 @@ private: }; // \class BetweenFactorEM +/// traits +template +struct traits > : public Testable > {}; + } // namespace gtsam diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h index 0e2aebd7f..b053b13f8 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h @@ -372,15 +372,15 @@ public: Matrix Z_3x3 = Z_3x3; Matrix I_3x3 = I_3x3; - Matrix H_pos_pos = numericalDerivative11(std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0); - Matrix H_pos_vel = numericalDerivative11(std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0); + Matrix H_pos_pos = numericalDerivative11(std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0); + Matrix H_pos_vel = numericalDerivative11(std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0); Matrix H_pos_angles = Z_3x3; - Matrix H_vel_vel = numericalDerivative11(std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor), delta_vel_in_t0); - Matrix H_vel_angles = numericalDerivative11(std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor), delta_angles); + Matrix H_vel_vel = numericalDerivative11(std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor), delta_vel_in_t0); + Matrix H_vel_angles = numericalDerivative11(std::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor), delta_angles); Matrix H_vel_pos = Z_3x3; - Matrix H_angles_angles = numericalDerivative11(std::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor), delta_angles); + Matrix H_angles_angles = numericalDerivative11(std::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor), delta_angles); Matrix H_angles_pos = Z_3x3; Matrix H_angles_vel = Z_3x3; diff --git a/gtsam_unstable/slam/serialization.cpp b/gtsam_unstable/slam/serialization.cpp index 88a94fd51..d87ca6f2d 100644 --- a/gtsam_unstable/slam/serialization.cpp +++ b/gtsam_unstable/slam/serialization.cpp @@ -5,8 +5,6 @@ * @author Alex Cunningham */ -#include -#include #include #include @@ -31,8 +29,6 @@ using namespace gtsam; // Creating as many permutations of factors as possible -typedef PriorFactor PriorFactorLieVector; -typedef PriorFactor PriorFactorLieMatrix; typedef PriorFactor PriorFactorPoint2; typedef PriorFactor PriorFactorStereoPoint2; typedef PriorFactor PriorFactorPoint3; @@ -46,8 +42,6 @@ typedef PriorFactor PriorFactorCalibratedCamera; typedef PriorFactor PriorFactorPinholeCameraCal3_S2; typedef PriorFactor PriorFactorStereoCamera; -typedef BetweenFactor BetweenFactorLieVector; -typedef BetweenFactor BetweenFactorLieMatrix; typedef BetweenFactor BetweenFactorPoint2; typedef BetweenFactor BetweenFactorPoint3; typedef BetweenFactor BetweenFactorRot2; @@ -55,8 +49,6 @@ typedef BetweenFactor BetweenFactorRot3; typedef BetweenFactor BetweenFactorPose2; typedef BetweenFactor BetweenFactorPose3; -typedef NonlinearEquality NonlinearEqualityLieVector; -typedef NonlinearEquality NonlinearEqualityLieMatrix; typedef NonlinearEquality NonlinearEqualityPoint2; typedef NonlinearEquality NonlinearEqualityStereoPoint2; typedef NonlinearEquality NonlinearEqualityPoint3; @@ -112,8 +104,6 @@ BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); /* Create GUIDs for geometry */ /* ************************************************************************* */ -GTSAM_VALUE_EXPORT(gtsam::LieVector); -GTSAM_VALUE_EXPORT(gtsam::LieMatrix); GTSAM_VALUE_EXPORT(gtsam::Point2); GTSAM_VALUE_EXPORT(gtsam::StereoPoint2); GTSAM_VALUE_EXPORT(gtsam::Point3); @@ -133,8 +123,6 @@ GTSAM_VALUE_EXPORT(gtsam::StereoCamera); BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor"); -BOOST_CLASS_EXPORT_GUID(PriorFactorLieVector, "gtsam::PriorFactorLieVector"); -BOOST_CLASS_EXPORT_GUID(PriorFactorLieMatrix, "gtsam::PriorFactorLieMatrix"); BOOST_CLASS_EXPORT_GUID(PriorFactorPoint2, "gtsam::PriorFactorPoint2"); BOOST_CLASS_EXPORT_GUID(PriorFactorStereoPoint2, "gtsam::PriorFactorStereoPoint2"); BOOST_CLASS_EXPORT_GUID(PriorFactorPoint3, "gtsam::PriorFactorPoint3"); @@ -147,8 +135,6 @@ BOOST_CLASS_EXPORT_GUID(PriorFactorCal3DS2, "gtsam::PriorFactorCal3DS2"); BOOST_CLASS_EXPORT_GUID(PriorFactorCalibratedCamera, "gtsam::PriorFactorCalibratedCamera"); BOOST_CLASS_EXPORT_GUID(PriorFactorStereoCamera, "gtsam::PriorFactorStereoCamera"); -BOOST_CLASS_EXPORT_GUID(BetweenFactorLieVector, "gtsam::BetweenFactorLieVector"); -BOOST_CLASS_EXPORT_GUID(BetweenFactorLieMatrix, "gtsam::BetweenFactorLieMatrix"); BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint2, "gtsam::BetweenFactorPoint2"); BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint3, "gtsam::BetweenFactorPoint3"); BOOST_CLASS_EXPORT_GUID(BetweenFactorRot2, "gtsam::BetweenFactorRot2"); @@ -156,8 +142,6 @@ BOOST_CLASS_EXPORT_GUID(BetweenFactorRot3, "gtsam::BetweenFactorRot3"); BOOST_CLASS_EXPORT_GUID(BetweenFactorPose2, "gtsam::BetweenFactorPose2"); BOOST_CLASS_EXPORT_GUID(BetweenFactorPose3, "gtsam::BetweenFactorPose3"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityLieVector, "gtsam::NonlinearEqualityLieVector"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityLieMatrix, "gtsam::NonlinearEqualityLieMatrix"); BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint2, "gtsam::NonlinearEqualityPoint2"); BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoPoint2, "gtsam::NonlinearEqualityStereoPoint2"); BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint3, "gtsam::NonlinearEqualityPoint3"); @@ -189,7 +173,7 @@ BOOST_CLASS_EXPORT_GUID(GeneralSFMFactor2Cal3_S2, "gtsam::GeneralSFMFactor2Cal3_ BOOST_CLASS_EXPORT_GUID(GenericStereoFactor3D, "gtsam::GenericStereoFactor3D"); -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 typedef PriorFactor PriorFactorSimpleCamera; typedef NonlinearEquality NonlinearEqualitySimpleCamera; diff --git a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp index 4d6e1912a..f43ae293e 100644 --- a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp +++ b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp @@ -11,6 +11,7 @@ #include #include #include +#include #include @@ -21,26 +22,24 @@ using namespace gtsam; // Disabled this test because it is currently failing - remove the lines "#if 0" and "#endif" below // to reenable the test. -#if 0 +// #if 0 /* ************************************************************************* */ -LieVector predictionError(const Pose2& p1, const Pose2& p2, const gtsam::Key& key1, const gtsam::Key& key2, const BetweenFactorEM& factor){ +Vector predictionError(const Pose2& p1, const Pose2& p2, const gtsam::Key& key1, const gtsam::Key& key2, const BetweenFactorEM& factor){ gtsam::Values values; values.insert(key1, p1); values.insert(key2, p2); - // LieVector err = factor.whitenedError(values); - // return err; - return LieVector::Expmap(factor.whitenedError(values)); + return factor.whitenedError(values); } /* ************************************************************************* */ -LieVector predictionError_standard(const Pose2& p1, const Pose2& p2, const gtsam::Key& key1, const gtsam::Key& key2, const BetweenFactor& factor){ +Vector predictionError_standard(const Pose2& p1, const Pose2& p2, const gtsam::Key& key1, const gtsam::Key& key2, const BetweenFactor& factor){ gtsam::Values values; values.insert(key1, p1); values.insert(key2, p2); - // LieVector err = factor.whitenedError(values); + // Vector err = factor.whitenedError(values); // return err; - return LieVector::Expmap(factor.whitenedError(values)); + return factor.whitenedError(values); } /* ************************************************************************* */ @@ -99,8 +98,8 @@ TEST( BetweenFactorEM, EvaluateError) Vector actual_err_wh = f.whitenedError(values); - Vector actual_err_wh_inlier = (Vector(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); - Vector actual_err_wh_outlier = (Vector(3) << actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]); + Vector3 actual_err_wh_inlier = Vector3(actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); + Vector3 actual_err_wh_outlier = Vector3(actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]); // cout << "Inlier test. norm of actual_err_wh_inlier, actual_err_wh_outlier: "< h_EM(key1, key2, rel_pose_msr, model_inlier, model_outlier, prior_inlier, prior_outlier); actual_err_wh = h_EM.whitenedError(values); - actual_err_wh_inlier = (Vector(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); + actual_err_wh_inlier = Vector3(actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); BetweenFactor h(key1, key2, rel_pose_msr, model_inlier ); Vector actual_err_wh_stnd = h.whitenedError(values); @@ -178,7 +177,7 @@ TEST (BetweenFactorEM, jacobian ) { // compare to standard between factor BetweenFactor h(key1, key2, rel_pose_msr, model_inlier ); Vector actual_err_wh_stnd = h.whitenedError(values); - Vector actual_err_wh_inlier = (Vector(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); + Vector actual_err_wh_inlier = Vector3(actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); // CHECK( assert_equal(actual_err_wh_stnd, actual_err_wh_inlier, 1e-8)); std::vector H_actual_stnd_unwh(2); (void)h.unwhitenedError(values, H_actual_stnd_unwh); @@ -190,12 +189,13 @@ TEST (BetweenFactorEM, jacobian ) { // CHECK( assert_equal(H2_actual_stnd, H2_actual, 1e-8)); double stepsize = 1.0e-9; - Matrix H1_expected = gtsam::numericalDerivative11(std::bind(&predictionError, _1, p2, key1, key2, f), p1, stepsize); - Matrix H2_expected = gtsam::numericalDerivative11(std::bind(&predictionError, p1, _1, key1, key2, f), p2, stepsize); + using std::placeholders::_1; + Matrix H1_expected = gtsam::numericalDerivative11(std::bind(&predictionError, _1, p2, key1, key2, f), p1, stepsize); + Matrix H2_expected = gtsam::numericalDerivative11(std::bind(&predictionError, p1, _1, key1, key2, f), p2, stepsize); // try to check numerical derivatives of a standard between factor - Matrix H1_expected_stnd = gtsam::numericalDerivative11(std::bind(&predictionError_standard, _1, p2, key1, key2, h), p1, stepsize); + Matrix H1_expected_stnd = gtsam::numericalDerivative11(std::bind(&predictionError_standard, _1, p2, key1, key2, h), p1, stepsize); // CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5)); // // @@ -240,8 +240,8 @@ TEST( BetweenFactorEM, CaseStudy) Vector actual_err_unw = f.unwhitenedError(values); Vector actual_err_wh = f.whitenedError(values); - Vector actual_err_wh_inlier = (Vector(3) << actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); - Vector actual_err_wh_outlier = (Vector(3) << actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]); + Vector3 actual_err_wh_inlier = Vector3(actual_err_wh[0], actual_err_wh[1], actual_err_wh[2]); + Vector3 actual_err_wh_outlier = Vector3(actual_err_wh[3], actual_err_wh[4], actual_err_wh[5]); if (debug){ cout << "p_inlier_outler: "<print("model_inlier:"); - model_outlier->print("model_outlier:"); - model_inlier_new->print("model_inlier_new:"); - model_outlier_new->print("model_outlier_new:"); + // model_inlier->print("model_inlier:"); + // model_outlier->print("model_outlier:"); + // model_inlier_new->print("model_inlier_new:"); + // model_outlier_new->print("model_outlier_new:"); } -#endif +// #endif /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} diff --git a/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp index 8692cf584..ed4092c60 100644 --- a/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp +++ b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp @@ -16,22 +16,23 @@ * @date Jan 17, 2012 */ -#include -#include -#include -#include #include -#include +#include +#include +#include +#include +#include using namespace std::placeholders; using namespace std; using namespace gtsam; //! Factors -typedef GaussMarkov1stOrderFactor GaussMarkovFactor; +typedef GaussMarkov1stOrderFactor GaussMarkovFactor; /* ************************************************************************* */ -LieVector predictionError(const LieVector& v1, const LieVector& v2, const GaussMarkovFactor factor) { +Vector predictionError(const Vector& v1, const Vector& v2, + const GaussMarkovFactor factor) { return factor.evaluateError(v1, v2); } @@ -58,29 +59,29 @@ TEST( GaussMarkovFactor, error ) Key x1(1); Key x2(2); double delta_t = 0.10; - Vector tau = Vector3(100.0, 150.0, 10.0); + Vector3 tau(100.0, 150.0, 10.0); SharedGaussian model = noiseModel::Isotropic::Sigma(3, 1.0); - LieVector v1 = LieVector(Vector3(10.0, 12.0, 13.0)); - LieVector v2 = LieVector(Vector3(10.0, 15.0, 14.0)); + Vector3 v1(10.0, 12.0, 13.0); + Vector3 v2(10.0, 15.0, 14.0); // Create two nodes linPoint.insert(x1, v1); linPoint.insert(x2, v2); GaussMarkovFactor factor(x1, x2, delta_t, tau, model); - Vector Err1( factor.evaluateError(v1, v2) ); + Vector3 error1 = factor.evaluateError(v1, v2); // Manually calculate the error - Vector alpha(tau.size()); - Vector alpha_v1(tau.size()); + Vector3 alpha(tau.size()); + Vector3 alpha_v1(tau.size()); for(int i=0; i #include -#include +#include #include #include diff --git a/matlab/+gtsam/Contents.m b/matlab/+gtsam/Contents.m index fb6d3081e..77536e5c9 100644 --- a/matlab/+gtsam/Contents.m +++ b/matlab/+gtsam/Contents.m @@ -49,9 +49,6 @@ % Ordering - class Ordering, see Doxygen page for details % Value - class Value, see Doxygen page for details % Values - class Values, see Doxygen page for details -% LieScalar - class LieScalar, see Doxygen page for details -% LieVector - class LieVector, see Doxygen page for details -% LieMatrix - class LieMatrix, see Doxygen page for details % NonlinearFactor - class NonlinearFactor, see Doxygen page for details % NonlinearFactorGraph - class NonlinearFactorGraph, see Doxygen page for details % @@ -101,9 +98,6 @@ % BearingFactor2D - class BearingFactor2D, see Doxygen page for details % BearingFactor3D - class BearingFactor3D, see Doxygen page for details % BearingRangeFactor2D - class BearingRangeFactor2D, see Doxygen page for details -% BetweenFactorLieMatrix - class BetweenFactorLieMatrix, see Doxygen page for details -% BetweenFactorLieScalar - class BetweenFactorLieScalar, see Doxygen page for details -% BetweenFactorLieVector - class BetweenFactorLieVector, see Doxygen page for details % BetweenFactorPoint2 - class BetweenFactorPoint2, see Doxygen page for details % BetweenFactorPoint3 - class BetweenFactorPoint3, see Doxygen page for details % BetweenFactorPose2 - class BetweenFactorPose2, see Doxygen page for details @@ -116,9 +110,6 @@ % GenericStereoFactor3D - class GenericStereoFactor3D, see Doxygen page for details % NonlinearEqualityCal3_S2 - class NonlinearEqualityCal3_S2, see Doxygen page for details % NonlinearEqualityCalibratedCamera - class NonlinearEqualityCalibratedCamera, see Doxygen page for details -% NonlinearEqualityLieMatrix - class NonlinearEqualityLieMatrix, see Doxygen page for details -% NonlinearEqualityLieScalar - class NonlinearEqualityLieScalar, see Doxygen page for details -% NonlinearEqualityLieVector - class NonlinearEqualityLieVector, see Doxygen page for details % NonlinearEqualityPoint2 - class NonlinearEqualityPoint2, see Doxygen page for details % NonlinearEqualityPoint3 - class NonlinearEqualityPoint3, see Doxygen page for details % NonlinearEqualityPose2 - class NonlinearEqualityPose2, see Doxygen page for details @@ -129,9 +120,6 @@ % NonlinearEqualityStereoPoint2 - class NonlinearEqualityStereoPoint2, see Doxygen page for details % PriorFactorCal3_S2 - class PriorFactorCal3_S2, see Doxygen page for details % PriorFactorCalibratedCamera - class PriorFactorCalibratedCamera, see Doxygen page for details -% PriorFactorLieMatrix - class PriorFactorLieMatrix, see Doxygen page for details -% PriorFactorLieScalar - class PriorFactorLieScalar, see Doxygen page for details -% PriorFactorLieVector - class PriorFactorLieVector, see Doxygen page for details % PriorFactorPoint2 - class PriorFactorPoint2, see Doxygen page for details % PriorFactorPoint3 - class PriorFactorPoint3, see Doxygen page for details % PriorFactorPose2 - class PriorFactorPose2, see Doxygen page for details diff --git a/matlab/unstable_examples/+imuSimulator/IMUComparison.m b/matlab/unstable_examples/+imuSimulator/IMUComparison.m index 871f023ef..ccc975d84 100644 --- a/matlab/unstable_examples/+imuSimulator/IMUComparison.m +++ b/matlab/unstable_examples/+imuSimulator/IMUComparison.m @@ -51,13 +51,13 @@ isam = gtsam.ISAM2(isamParams); initialValues = Values; initialValues.insert(symbol('x',0), currentPoseGlobal); -initialValues.insert(symbol('v',0), LieVector(currentVelocityGlobal)); +initialValues.insert(symbol('v',0), currentVelocityGlobal); initialValues.insert(symbol('b',0), imuBias.ConstantBias([0;0;0],[0;0;0])); initialFactors = NonlinearFactorGraph; initialFactors.add(PriorFactorPose3(symbol('x',0), ... currentPoseGlobal, noiseModel.Isotropic.Sigma(6, 1.0))); -initialFactors.add(PriorFactorLieVector(symbol('v',0), ... - LieVector(currentVelocityGlobal), noiseModel.Isotropic.Sigma(3, 1.0))); +initialFactors.add(PriorFactorVector(symbol('v',0), ... + currentVelocityGlobal, noiseModel.Isotropic.Sigma(3, 1.0))); initialFactors.add(PriorFactorConstantBias(symbol('b',0), ... imuBias.ConstantBias([0;0;0],[0;0;0]), noiseModel.Isotropic.Sigma(6, 1.0))); @@ -96,7 +96,7 @@ for t = times initialVel = isam.calculateEstimate(symbol('v',lastSummaryIndex)); else initialPose = Pose3; - initialVel = LieVector(velocity); + initialVel = velocity; end initialValues.insert(symbol('x',lastSummaryIndex+1), initialPose); initialValues.insert(symbol('v',lastSummaryIndex+1), initialVel); diff --git a/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m b/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m index 450697de0..6adc8e9dc 100644 --- a/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m +++ b/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m @@ -43,15 +43,15 @@ sigma_init_b = 1.0; initialValues = Values; initialValues.insert(symbol('x',0), currentPoseGlobal); -initialValues.insert(symbol('v',0), LieVector(currentVelocityGlobal)); +initialValues.insert(symbol('v',0), currentVelocityGlobal); initialValues.insert(symbol('b',0), imuBias.ConstantBias([0;0;0],[0;0;0])); initialFactors = NonlinearFactorGraph; % Prior on initial pose initialFactors.add(PriorFactorPose3(symbol('x',0), ... currentPoseGlobal, noiseModel.Isotropic.Sigma(6, sigma_init_x))); % Prior on initial velocity -initialFactors.add(PriorFactorLieVector(symbol('v',0), ... - LieVector(currentVelocityGlobal), noiseModel.Isotropic.Sigma(3, sigma_init_v))); +initialFactors.add(PriorFactorVector(symbol('v',0), ... + currentVelocityGlobal, noiseModel.Isotropic.Sigma(3, sigma_init_v))); % Prior on initial bias initialFactors.add(PriorFactorConstantBias(symbol('b',0), ... imuBias.ConstantBias([0;0;0],[0;0;0]), noiseModel.Isotropic.Sigma(6, sigma_init_b))); @@ -91,7 +91,7 @@ for t = times initialVel = isam.calculateEstimate(symbol('v',lastSummaryIndex)); else initialPose = Pose3; - initialVel = LieVector(velocity); + initialVel = velocity; end initialValues.insert(symbol('x',lastSummaryIndex+1), initialPose); initialValues.insert(symbol('v',lastSummaryIndex+1), initialVel); diff --git a/matlab/unstable_examples/+imuSimulator/coriolisExample.m b/matlab/unstable_examples/+imuSimulator/coriolisExample.m index ee4deb433..61dc78d96 100644 --- a/matlab/unstable_examples/+imuSimulator/coriolisExample.m +++ b/matlab/unstable_examples/+imuSimulator/coriolisExample.m @@ -175,9 +175,9 @@ for i = 1:length(times) % known initial conditions currentPoseEstimate = currentPoseFixedGT; if navFrameRotating == 1 - currentVelocityEstimate = LieVector(currentVelocityRotatingGT); + currentVelocityEstimate = currentVelocityRotatingGT; else - currentVelocityEstimate = LieVector(currentVelocityFixedGT); + currentVelocityEstimate = currentVelocityFixedGT; end % Set Priors @@ -186,7 +186,7 @@ for i = 1:length(times) newValues.insert(currentBiasKey, zeroBias); % Initial values, same for IMU types 1 and 2 newFactors.add(PriorFactorPose3(currentPoseKey, currentPoseEstimate, sigma_init_x)); - newFactors.add(PriorFactorLieVector(currentVelKey, currentVelocityEstimate, sigma_init_v)); + newFactors.add(PriorFactorVector(currentVelKey, currentVelocityEstimate, sigma_init_v)); newFactors.add(PriorFactorConstantBias(currentBiasKey, zeroBias, sigma_init_b)); % Store data diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m index 07f146dcb..037065ac5 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -27,7 +27,7 @@ for i=0:length(measurements) if options.includeIMUFactors == 1 currentVelKey = symbol('v', 0); currentVel = values.atPoint3(currentVelKey); - graph.add(PriorFactorLieVector(currentVelKey, LieVector(currentVel), noiseModels.noiseVel)); + graph.add(PriorFactorVector(currentVelKey, currentVel, noiseModels.noiseVel)); currentBiasKey = symbol('b', 0); currentBias = values.atPoint3(currentBiasKey); diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m index 3d8a9b5d2..5fb6589d6 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m @@ -82,7 +82,7 @@ if options.useRealData == 1 end % Add Values: velocity and bias - values.insert(currentVelKey, LieVector(currentVel)); + values.insert(currentVelKey, currentVel); values.insert(currentBiasKey, metadata.imu.zeroBias); end diff --git a/matlab/unstable_examples/FlightCameraTransformIMU.m b/matlab/unstable_examples/FlightCameraTransformIMU.m index d2f2bc34d..aeac2e243 100644 --- a/matlab/unstable_examples/FlightCameraTransformIMU.m +++ b/matlab/unstable_examples/FlightCameraTransformIMU.m @@ -167,7 +167,7 @@ for i=1:size(trajectory)-1 %% priors on first two poses if i < 3 - % fg.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v)); + % fg.add(PriorFactorVector(currentVelKey, currentVelocityGlobal, sigma_init_v)); fg.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b)); end diff --git a/matlab/unstable_examples/IMUKittiExampleVO.m b/matlab/unstable_examples/IMUKittiExampleVO.m index 6434e750a..f35d36512 100644 --- a/matlab/unstable_examples/IMUKittiExampleVO.m +++ b/matlab/unstable_examples/IMUKittiExampleVO.m @@ -46,7 +46,7 @@ clear logposes relposes %% Get initial conditions for the estimated trajectory currentPoseGlobal = Pose3; -currentVelocityGlobal = LieVector([0;0;0]); % the vehicle is stationary at the beginning +currentVelocityGlobal = [0;0;0]; % the vehicle is stationary at the beginning currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); sigma_init_x = noiseModel.Isotropic.Sigmas([ 1.0; 1.0; 0.01; 0.01; 0.01; 0.01 ]); sigma_init_v = noiseModel.Isotropic.Sigma(3, 1000.0); @@ -88,7 +88,7 @@ for measurementIndex = 1:length(timestamps) newValues.insert(currentVelKey, currentVelocityGlobal); newValues.insert(currentBiasKey, currentBias); newFactors.add(PriorFactorPose3(currentPoseKey, currentPoseGlobal, sigma_init_x)); - newFactors.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v)); + newFactors.add(PriorFactorVector(currentVelKey, currentVelocityGlobal, sigma_init_v)); newFactors.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b)); else t_previous = timestamps(measurementIndex-1, 1); diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index d3b20e312..b39e067b0 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -33,8 +33,6 @@ add_custom_target(gtsam_unstable_header DEPENDS "${PROJECT_SOURCE_DIR}/gtsam_uns set(ignore gtsam::Point2 gtsam::Point3 - gtsam::LieVector - gtsam::LieMatrix gtsam::ISAM2ThresholdMapValue gtsam::FactorIndices gtsam::FactorIndexSet @@ -116,8 +114,6 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) set(ignore gtsam::Point2 gtsam::Point3 - gtsam::LieVector - gtsam::LieMatrix gtsam::ISAM2ThresholdMapValue gtsam::FactorIndices gtsam::FactorIndexSet