From 55f31ab2d73e0b775caae61ecb826d054e92de5b Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 2 Jan 2022 14:38:20 -0500 Subject: [PATCH 01/10] Revive BetweenFactorEM, without LieVector --- gtsam_unstable/slam/BetweenFactorEM.h | 4 ++ .../slam/tests/testBetweenFactorEM.cpp | 52 +++++++++---------- 2 files changed, 30 insertions(+), 26 deletions(-) diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h index 572935da3f..9c19bae8c5 100644 --- a/gtsam_unstable/slam/BetweenFactorEM.h +++ b/gtsam_unstable/slam/BetweenFactorEM.h @@ -421,4 +421,8 @@ class BetweenFactorEM: public NonlinearFactor { }; // \class BetweenFactorEM +/// traits +template +struct traits > : public Testable > {}; + } // namespace gtsam diff --git a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp index 4d6e1912a1..f43ae293ef 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);} From 70092ca18aade8399f2173ba2c212eb2cc6c2eff Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 2 Jan 2022 14:43:48 -0500 Subject: [PATCH 02/10] Remove Deprecated Lie* classes --- Using-GTSAM-EXPORT.md | 2 +- gtsam/base/CMakeLists.txt | 3 - gtsam/base/LieMatrix.h | 26 ---- gtsam/base/LieScalar.h | 26 ---- gtsam/base/LieVector.h | 26 ---- gtsam/base/deprecated/LieMatrix.h | 152 -------------------- gtsam/base/deprecated/LieScalar.h | 88 ------------ gtsam/base/deprecated/LieVector.h | 121 ---------------- gtsam/base/tests/testLieMatrix.cpp | 70 --------- gtsam/base/tests/testLieScalar.cpp | 64 --------- gtsam/base/tests/testLieVector.cpp | 66 --------- gtsam/base/tests/testTestableAssertions.cpp | 35 ----- 12 files changed, 1 insertion(+), 678 deletions(-) delete mode 100644 gtsam/base/LieMatrix.h delete mode 100644 gtsam/base/LieScalar.h delete mode 100644 gtsam/base/LieVector.h delete mode 100644 gtsam/base/deprecated/LieMatrix.h delete mode 100644 gtsam/base/deprecated/LieScalar.h delete mode 100644 gtsam/base/deprecated/LieVector.h delete mode 100644 gtsam/base/tests/testLieMatrix.cpp delete mode 100644 gtsam/base/tests/testLieScalar.cpp delete mode 100644 gtsam/base/tests/testLieVector.cpp delete mode 100644 gtsam/base/tests/testTestableAssertions.cpp diff --git a/Using-GTSAM-EXPORT.md b/Using-GTSAM-EXPORT.md index cae1d499c6..faeebc97f3 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/gtsam/base/CMakeLists.txt b/gtsam/base/CMakeLists.txt index 99984e7b38..66d3ec7218 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 210bdcc735..0000000000 --- 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 e159ffa873..0000000000 --- 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 a7491d8042..0000000000 --- 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/deprecated/LieMatrix.h b/gtsam/base/deprecated/LieMatrix.h deleted file mode 100644 index a3d0a43289..0000000000 --- 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 6c9a5f766d..0000000000 --- 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 745189c3de..0000000000 --- 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 8c68bf8a00..0000000000 --- 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 74f5e0d414..0000000000 --- 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 76c4fc490a..0000000000 --- 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 305aa7ca93..0000000000 --- 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); } -/* ************************************************************************* */ From 8dbe5ee1790af2be6ceef6e34d41706b5f607bfc Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 2 Jan 2022 14:46:39 -0500 Subject: [PATCH 03/10] "fixed" Unstable MATLAB examples - untested --- matlab/+gtsam/Contents.m | 12 ------------ .../unstable_examples/+imuSimulator/IMUComparison.m | 8 ++++---- .../+imuSimulator/IMUComparison_with_cov.m | 8 ++++---- .../+imuSimulator/coriolisExample.m | 6 +++--- .../covarianceAnalysisCreateFactorGraph.m | 2 +- .../covarianceAnalysisCreateTrajectory.m | 2 +- matlab/unstable_examples/FlightCameraTransformIMU.m | 2 +- matlab/unstable_examples/IMUKittiExampleVO.m | 4 ++-- 8 files changed, 16 insertions(+), 28 deletions(-) diff --git a/matlab/+gtsam/Contents.m b/matlab/+gtsam/Contents.m index fb6d3081e8..77536e5c9d 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 871f023ef7..ccc975d849 100644 --- a/matlab/unstable_examples/+imuSimulator/IMUComparison.m +++ b/matlab/unstable_examples/+imuSimulator/IMUComparison.m @@ -51,13 +51,13 @@ 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 @@ 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 450697de05..6adc8e9dc2 100644 --- a/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m +++ b/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m @@ -43,15 +43,15 @@ 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 @@ 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 ee4deb4335..61dc78d968 100644 --- a/matlab/unstable_examples/+imuSimulator/coriolisExample.m +++ b/matlab/unstable_examples/+imuSimulator/coriolisExample.m @@ -175,9 +175,9 @@ % 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 @@ 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 07f146dcb8..037065ac54 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -27,7 +27,7 @@ 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 3d8a9b5d28..5fb6589d6e 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m @@ -82,7 +82,7 @@ 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 d2f2bc34d7..aeac2e2431 100644 --- a/matlab/unstable_examples/FlightCameraTransformIMU.m +++ b/matlab/unstable_examples/FlightCameraTransformIMU.m @@ -167,7 +167,7 @@ %% 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 6434e750ae..f35d365127 100644 --- a/matlab/unstable_examples/IMUKittiExampleVO.m +++ b/matlab/unstable_examples/IMUKittiExampleVO.m @@ -46,7 +46,7 @@ %% 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 @@ 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); From 9518346161c21715d407bac5ff553f054b6fc876 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 2 Jan 2022 14:59:28 -0500 Subject: [PATCH 04/10] Fix unstable c++ examples --- .../EquivInertialNavFactor_GlobalVel_NoBias.h | 10 ++--- gtsam_unstable/slam/serialization.cpp | 16 -------- .../tests/testGaussMarkov1stOrderFactor.cpp | 41 ++++++++++--------- .../slam/tests/testSerialization.cpp | 2 +- 4 files changed, 27 insertions(+), 42 deletions(-) diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h index 0e2aebd7fe..b053b13f82 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h @@ -372,15 +372,15 @@ class EquivInertialNavFactor_GlobalVel_NoBias : public NoiseModelFactor4(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 88a94fd512..bed11e5356 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"); diff --git a/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp b/gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp index 8692cf5841..ed4092c600 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 From a38de285357d5628a75a071fdff293b00079e9a2 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 2 Jan 2022 15:00:49 -0500 Subject: [PATCH 05/10] Tested python wrapper without Lie* --- gtsam/basis/ParameterMatrix.h | 2 +- python/CMakeLists.txt | 4 ---- 2 files changed, 1 insertion(+), 5 deletions(-) diff --git a/gtsam/basis/ParameterMatrix.h b/gtsam/basis/ParameterMatrix.h index df2d9f62e9..eddcbfeaec 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/python/CMakeLists.txt b/python/CMakeLists.txt index d3b20e3126..b39e067b07 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 From 6d0c55901cbe9aeb02f131d73bb3167da8b4e500 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 2 Jan 2022 15:49:47 -0500 Subject: [PATCH 06/10] Global replace to V42 --- .github/scripts/python.sh | 2 +- .github/scripts/unix.sh | 2 +- .github/workflows/build-special.yml | 2 +- README.md | 4 ++-- cmake/HandleGeneralOptions.cmake | 2 +- cmake/HandlePrintConfiguration.cmake | 2 +- .../Eigen/Eigen/src/Core/CwiseNullaryOp.h | 4 ++-- .../3rdparty/Eigen/Eigen/src/Core/DenseBase.h | 4 ++-- .../3rdparty/Eigen/Eigen/src/Core/EigenBase.h | 2 +- .../Eigen/Eigen/src/Core/TriangularMatrix.h | 6 ++--- .../Eigen/Eigen/src/Core/util/Constants.h | 4 ++-- .../Eigen/Eigen/src/Geometry/AlignedBox.h | 4 ++-- .../Eigen/src/Geometry/ParametrizedLine.h | 2 +- .../Eigen/Eigen/src/Geometry/Scaling.h | 8 +++---- .../src/SparseCholesky/SimplicialCholesky.h | 2 +- .../Eigen/src/SparseCore/MappedSparseMatrix.h | 2 +- .../src/SparseExtra/DynamicSparseMatrix.h | 10 ++++---- .../include/GeographicLib/NormalGravity.hpp | 2 +- .../include/GeographicLib/Utility.hpp | 2 +- gtsam/base/TestableAssertions.h | 4 +++- gtsam/base/Vector.h | 6 +++-- gtsam/config.h.in | 2 +- gtsam/geometry/Cal3.h | 2 +- gtsam/geometry/Cal3Bundler.h | 2 +- gtsam/geometry/SimpleCamera.cpp | 2 +- gtsam/geometry/SimpleCamera.h | 2 +- gtsam/geometry/tests/testSimpleCamera.cpp | 2 +- gtsam/inference/EliminateableFactorGraph.h | 14 +++++------ gtsam/inference/Factor.h | 1 - gtsam/linear/GaussianConditional.h | 7 +++--- gtsam/linear/GaussianFactorGraph.h | 10 ++++---- gtsam/linear/NoiseModel.h | 2 ++ gtsam/navigation/AHRSFactor.h | 23 +++++++++++-------- gtsam/navigation/ImuBias.h | 18 +++++++-------- gtsam/nonlinear/ExpressionFactor.h | 4 ++-- gtsam/nonlinear/ExtendedKalmanFilter.h | 2 ++ gtsam/nonlinear/Marginals.h | 8 +++---- gtsam/nonlinear/NonlinearFactorGraph.h | 10 ++++---- gtsam/slam/dataset.cpp | 10 ++++---- gtsam/slam/dataset.h | 22 +++++++++--------- gtsam_unstable/slam/serialization.cpp | 2 +- 41 files changed, 116 insertions(+), 105 deletions(-) diff --git a/.github/scripts/python.sh b/.github/scripts/python.sh index 3f5701281c..6cc62d2b06 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 9689d346ca..d890577b65 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 647b9c0f18..d357b9a340 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 ee5746e1cf..52ac0a5d88 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/cmake/HandleGeneralOptions.cmake b/cmake/HandleGeneralOptions.cmake index 64c239f393..7c8f8533f3 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 ad6ac5c5cf..43ee5b57b4 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/CwiseNullaryOp.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseNullaryOp.h index ddd607e383..4ffd0057b1 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseNullaryOp.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseNullaryOp.h @@ -215,7 +215,7 @@ DenseBase::Constant(const Scalar& value) return DenseBase::NullaryExpr(RowsAtCompileTime, ColsAtCompileTime, internal::scalar_constant_op(value)); } -/** \deprecated because of accuracy loss. In Eigen 3.3, it is an alias for LinSpaced(Index,const Scalar&,const Scalar&) +/** @deprecated because of accuracy loss. In Eigen 3.3, it is an alias for LinSpaced(Index,const Scalar&,const Scalar&) * * \sa LinSpaced(Index,Scalar,Scalar), setLinSpaced(Index,const Scalar&,const Scalar&) */ @@ -227,7 +227,7 @@ DenseBase::LinSpaced(Sequential_t, Index size, const Scalar& low, const return DenseBase::NullaryExpr(size, internal::linspaced_op(low,high,size)); } -/** \deprecated because of accuracy loss. In Eigen 3.3, it is an alias for LinSpaced(const Scalar&,const Scalar&) +/** @deprecated because of accuracy loss. In Eigen 3.3, it is an alias for LinSpaced(const Scalar&,const Scalar&) * * \sa LinSpaced(Scalar,Scalar) */ diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h index 90066ae73f..8b2733814e 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h @@ -298,7 +298,7 @@ template class DenseBase /** \internal * Copies \a other into *this without evaluating other. \returns a reference to *this. - * \deprecated */ + * @deprecated */ template EIGEN_DEVICE_FUNC Derived& lazyAssign(const DenseBase& other); @@ -306,7 +306,7 @@ template class DenseBase EIGEN_DEVICE_FUNC CommaInitializer operator<< (const Scalar& s); - /** \deprecated it now returns \c *this */ + /** @deprecated it now returns \c *this */ template EIGEN_DEPRECATED const Derived& flagged() const diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/EigenBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/EigenBase.h index b195506a91..bf30e03ffd 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/EigenBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/EigenBase.h @@ -32,7 +32,7 @@ template struct EigenBase /** \brief The interface type of indices * \details To change this, \c \#define the preprocessor symbol \c EIGEN_DEFAULT_DENSE_INDEX_TYPE. - * \deprecated Since Eigen 3.3, its usage is deprecated. Use Eigen::Index instead. + * @deprecated Since Eigen 3.3, its usage is deprecated. Use Eigen::Index instead. * \sa StorageIndex, \ref TopicPreprocessorDirectives. */ typedef Eigen::Index Index; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h index 667ef09dc1..9c76906fec 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h @@ -435,12 +435,12 @@ template class TriangularViewImpl<_Mat TriangularViewType& operator=(const TriangularViewImpl& other) { return *this = other.derived().nestedExpression(); } - /** \deprecated */ + /** @deprecated */ template 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/3rdparty/Eigen/Eigen/src/Core/util/Constants.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Constants.h index 7587d68424..9602332c8a 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Constants.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Constants.h @@ -65,7 +65,7 @@ const unsigned int RowMajorBit = 0x1; const unsigned int EvalBeforeNestingBit = 0x2; /** \ingroup flags - * \deprecated + * @deprecated * means the expression should be evaluated before any assignment */ EIGEN_DEPRECATED const unsigned int EvalBeforeAssigningBit = 0x4; // FIXME deprecated @@ -149,7 +149,7 @@ const unsigned int LvalueBit = 0x20; */ const unsigned int DirectAccessBit = 0x40; -/** \deprecated \ingroup flags +/** @deprecated \ingroup flags * * means the first coefficient packet is guaranteed to be aligned. * An expression cannot has the AlignedBit without the PacketAccessBit flag. diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/AlignedBox.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/AlignedBox.h index 066eae4f92..96c100245e 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/AlignedBox.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/AlignedBox.h @@ -84,10 +84,10 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) /** \returns the dimension in which the box holds */ EIGEN_DEVICE_FUNC inline Index dim() const { return AmbientDimAtCompileTime==Dynamic ? m_min.size() : Index(AmbientDimAtCompileTime); } - /** \deprecated use isEmpty() */ + /** @deprecated use isEmpty() */ EIGEN_DEVICE_FUNC inline bool isNull() const { return isEmpty(); } - /** \deprecated use setEmpty() */ + /** @deprecated use setEmpty() */ EIGEN_DEVICE_FUNC inline void setNull() { setEmpty(); } /** \returns true if the box is empty. diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/ParametrizedLine.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/ParametrizedLine.h index 1e985d8cde..60ca9b7f37 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/ParametrizedLine.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/ParametrizedLine.h @@ -170,7 +170,7 @@ EIGEN_DEVICE_FUNC inline _Scalar ParametrizedLine<_Scalar, _AmbientDim,_Options> } -/** \deprecated use intersectionParameter() +/** @deprecated use intersectionParameter() * \returns the parameter value of the intersection between \c *this and the given \a hyperplane */ template diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Scaling.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Scaling.h index f58ca03d94..81a5bcafce 100755 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Scaling.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Scaling.h @@ -142,13 +142,13 @@ template inline const DiagonalWrapper Scaling(const MatrixBase& coeffs) { return coeffs.asDiagonal(); } -/** \deprecated */ +/** @deprecated */ typedef DiagonalMatrix AlignedScaling2f; -/** \deprecated */ +/** @deprecated */ typedef DiagonalMatrix AlignedScaling2d; -/** \deprecated */ +/** @deprecated */ typedef DiagonalMatrix AlignedScaling3f; -/** \deprecated */ +/** @deprecated */ typedef DiagonalMatrix AlignedScaling3d; //@} diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky.h index 2907f65296..f2693fc81b 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky.h @@ -493,7 +493,7 @@ template } }; -/** \deprecated use SimplicialLDLT or class SimplicialLLT +/** @deprecated use SimplicialLDLT or class SimplicialLLT * \ingroup SparseCholesky_Module * \class SimplicialCholesky * diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/MappedSparseMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/MappedSparseMatrix.h index 67718c85be..25ca27fe59 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/MappedSparseMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/MappedSparseMatrix.h @@ -12,7 +12,7 @@ namespace Eigen { -/** \deprecated Use Map > +/** @deprecated Use Map > * \class MappedSparseMatrix * * \brief Sparse matrix diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/DynamicSparseMatrix.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/DynamicSparseMatrix.h index 0ffbc43d23..1f0379c152 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/DynamicSparseMatrix.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/DynamicSparseMatrix.h @@ -12,7 +12,7 @@ namespace Eigen { -/** \deprecated use a SparseMatrix in an uncompressed mode +/** @deprecated use a SparseMatrix in an uncompressed mode * * \class DynamicSparseMatrix * @@ -291,7 +291,7 @@ template public: - /** \deprecated + /** @deprecated * Set the matrix to zero and reserve the memory for \a reserveSize nonzero coefficients. */ EIGEN_DEPRECATED void startFill(Index reserveSize = 1000) { @@ -299,7 +299,7 @@ template reserve(reserveSize); } - /** \deprecated use insert() + /** @deprecated use insert() * inserts a nonzero coefficient at given coordinates \a row, \a col and returns its reference assuming that: * 1 - the coefficient does not exist yet * 2 - this the coefficient with greater inner coordinate for the given outer coordinate. @@ -315,7 +315,7 @@ template return insertBack(outer,inner); } - /** \deprecated use insert() + /** @deprecated use insert() * Like fill() but with random inner coordinates. * Compared to the generic coeffRef(), the unique limitation is that we assume * the coefficient does not exist yet. @@ -325,7 +325,7 @@ template return insert(row,col); } - /** \deprecated use finalize() + /** @deprecated use finalize() * Does nothing. Provided for compatibility with SparseMatrix. */ EIGEN_DEPRECATED void endFill() {} diff --git a/gtsam/3rdparty/GeographicLib/include/GeographicLib/NormalGravity.hpp b/gtsam/3rdparty/GeographicLib/include/GeographicLib/NormalGravity.hpp index 2c4955f745..13a21c657c 100644 --- a/gtsam/3rdparty/GeographicLib/include/GeographicLib/NormalGravity.hpp +++ b/gtsam/3rdparty/GeographicLib/include/GeographicLib/NormalGravity.hpp @@ -142,7 +142,7 @@ namespace GeographicLib { NormalGravity(real a, real GM, real omega, real f_J2, bool geometricp = true); /** - * \deprecated Old constructor for the normal gravity. + * @deprecated Old constructor for the normal gravity. * * @param[in] a equatorial radius (meters). * @param[in] GM mass constant of the ellipsoid diff --git a/gtsam/3rdparty/GeographicLib/include/GeographicLib/Utility.hpp b/gtsam/3rdparty/GeographicLib/include/GeographicLib/Utility.hpp index 9990e47e91..a9b0129d47 100644 --- a/gtsam/3rdparty/GeographicLib/include/GeographicLib/Utility.hpp +++ b/gtsam/3rdparty/GeographicLib/include/GeographicLib/Utility.hpp @@ -380,7 +380,7 @@ namespace GeographicLib { return x; } /** - * \deprecated An old name for val(s). + * @deprecated An old name for val(s). **********************************************************************/ template // GEOGRAPHICLIB_DEPRECATED("Use new Utility::val(s)") diff --git a/gtsam/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h index c86fbb6d22..e5bd34d19c 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 a057da46b4..36dc2288da 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/config.h.in b/gtsam/config.h.in index e7623c52b7..d47329a627 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 08ce4c1e67..128c217f93 100644 --- a/gtsam/geometry/Cal3.h +++ b/gtsam/geometry/Cal3.h @@ -170,7 +170,7 @@ 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(); } #endif diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 0d7c1be9d5..2285b2dbbe 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -97,7 +97,7 @@ 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_; } diff --git a/gtsam/geometry/SimpleCamera.cpp b/gtsam/geometry/SimpleCamera.cpp index d1a5ed3308..3b871b4686 100644 --- a/gtsam/geometry/SimpleCamera.cpp +++ b/gtsam/geometry/SimpleCamera.cpp @@ -21,7 +21,7 @@ namespace gtsam { -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 SimpleCamera simpleCamera(const Matrix34& P) { // P = [A|a] = s K cRw [I|-T], with s the unknown scale diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index 5ff6b9816f..f0776c2e2a 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 18a25c5534..173ccf05b8 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 579eed7f9d..c904d2f7ff 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 1ca97cea51..e6a8dcc604 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.h b/gtsam/linear/GaussianConditional.h index 0ea597f99a..d93f65b425 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 5ccb0ce91b..7bee4c9fb5 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 54b9d955fb..ba9c97efbd 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -730,6 +730,7 @@ namespace gtsam { } // namespace noiseModel +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 /** Note, deliberately not in noiseModel namespace. * Deprecated. Only for compatibility with previous version. */ @@ -738,6 +739,7 @@ namespace gtsam { typedef noiseModel::Diagonal::shared_ptr SharedDiagonal; typedef noiseModel::Constrained::shared_ptr SharedConstrained; typedef noiseModel::Isotropic::shared_ptr SharedIsotropic; +#endif /// traits template<> struct traits : public Testable {}; diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index 1ab2d7cdc6..94a9665542 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -104,14 +104,15 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation static Vector DeltaAngles(const Vector& msr_gyro_t, const double msr_dt, const Vector3& delta_angles); +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 /// @deprecated constructor - PreintegratedAhrsMeasurements(const Vector3& biasHat, - const Matrix3& measuredOmegaCovariance) - : PreintegratedRotation(boost::make_shared()), - biasHat_(biasHat) { + GTSAM_DEPRECATED PreintegratedAhrsMeasurements( + const Vector3& biasHat, const Matrix3& measuredOmegaCovariance) + : PreintegratedRotation(boost::make_shared()), biasHat_(biasHat) { p_->gyroscopeCovariance = measuredOmegaCovariance; resetIntegration(); } +#endif private: @@ -186,20 +187,24 @@ class GTSAM_EXPORT AHRSFactor: public NoiseModelFactor3 { const Rot3& rot_i, const Vector3& bias, const PreintegratedAhrsMeasurements preintegratedMeasurements); +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 /// @deprecated name typedef PreintegratedAhrsMeasurements PreintegratedMeasurements; /// @deprecated constructor - AHRSFactor(Key rot_i, Key rot_j, Key bias, + GTSAM_DEPRECATED 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); + static Rot3 GTSAM_DEPRECATED + 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 fad952232f..7557c47324 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -131,30 +131,30 @@ class GTSAM_EXPORT ConstantBias { /// @} +#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/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index d22690cf39..11bf873e7f 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 77bb1ca6ca..df27d16ff0 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 947274a97e..028545d019 100644 --- a/gtsam/nonlinear/Marginals.h +++ b/gtsam/nonlinear/Marginals.h @@ -131,16 +131,16 @@ class GTSAM_EXPORT Marginals { 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 84b1516e95..160e469241 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 d7e925bd95..0684063de2 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 ec5d6dce9d..db5d7d76ad 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_unstable/slam/serialization.cpp b/gtsam_unstable/slam/serialization.cpp index bed11e5356..d87ca6f2d3 100644 --- a/gtsam_unstable/slam/serialization.cpp +++ b/gtsam_unstable/slam/serialization.cpp @@ -173,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; From 906176291f7523534fdc864a98df3d14a9a80896 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 2 Jan 2022 16:19:17 -0500 Subject: [PATCH 07/10] Fix everything to work with no deprecated methods allowed. --- gtsam/linear/GaussianConditional.cpp | 2 ++ gtsam/linear/GaussianConditional.h | 2 +- gtsam/linear/NoiseModel.h | 6 ++-- gtsam/navigation/AHRSFactor.cpp | 24 +++++++-------- gtsam/navigation/AHRSFactor.h | 36 ++++++++++------------- gtsam/navigation/tests/testAHRSFactor.cpp | 28 +++++++++--------- gtsam/navigation/tests/testImuBias.cpp | 8 ++--- 7 files changed, 48 insertions(+), 58 deletions(-) diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 9297d6461b..e5016c6240 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -193,6 +193,7 @@ namespace gtsam { } /* ************************************************************************* */ +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 void GaussianConditional::scaleFrontalsBySigma(VectorValues& gy) const { DenseIndex vectorPosition = 0; for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { @@ -200,5 +201,6 @@ namespace gtsam { vectorPosition += getDim(frontal); } } +#endif } // namespace gtsam diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index d93f65b425..958f411f86 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -128,7 +128,7 @@ namespace gtsam { #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 /** Scale the values in \c gy according to the sigmas for the frontal variables in this * conditional. */ - void GTSAM_DEPRECATED scaleFrontalsBySigma(VectorValues& gy) const; + void scaleFrontalsBySigma(VectorValues& gy) const; #endif private: diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index ba9c97efbd..5c379beb8d 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -730,16 +730,14 @@ namespace gtsam { } // namespace noiseModel -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 - /** 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; typedef noiseModel::Diagonal::shared_ptr SharedDiagonal; typedef noiseModel::Constrained::shared_ptr SharedConstrained; typedef noiseModel::Isotropic::shared_ptr SharedIsotropic; -#endif /// traits template<> struct traits : public Testable {}; diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 4604a55dd1..f4db42d0fd 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 94a9665542..10c33d101d 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -104,15 +104,13 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation static Vector DeltaAngles(const Vector& msr_gyro_t, const double msr_dt, const Vector3& delta_angles); -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 - /// @deprecated constructor - GTSAM_DEPRECATED PreintegratedAhrsMeasurements( - const Vector3& biasHat, const Matrix3& measuredOmegaCovariance) + /// @deprecated constructor, but used in tests. + PreintegratedAhrsMeasurements(const Vector3& biasHat, + const Matrix3& measuredOmegaCovariance) : PreintegratedRotation(boost::make_shared()), biasHat_(biasHat) { p_->gyroscopeCovariance = measuredOmegaCovariance; resetIntegration(); } -#endif private: @@ -183,27 +181,25 @@ class GTSAM_EXPORT AHRSFactor: public NoiseModelFactor3 { /// predicted states from IMU /// TODO(frank): relationship with PIM predict ?? - static Rot3 Predict( + 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 preintegratedMeasurements); + 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 - GTSAM_DEPRECATED 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 GTSAM_DEPRECATED - 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/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index a4d06d01a0..779f6abccf 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 b486a4a98b..81a1a2ceb9 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) { From a25e3f6d38f853d53fb6935511a74546efc9ba1d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 2 Jan 2022 16:23:56 -0500 Subject: [PATCH 08/10] Remove deprecated methods from wrapper --- gtsam/linear/GaussianConditional.h | 2 +- gtsam/linear/linear.i | 1 - gtsam/navigation/navigation.i | 11 ----------- gtsam/slam/slam.i | 2 -- 4 files changed, 1 insertion(+), 15 deletions(-) diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 958f411f86..d93f65b425 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -128,7 +128,7 @@ namespace gtsam { #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; + void GTSAM_DEPRECATED scaleFrontalsBySigma(VectorValues& gy) const; #endif private: diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index 7b1ce550f0..7acc5bd416 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/navigation.i b/gtsam/navigation/navigation.i index c2a3bcdb42..6ede1645f3 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/slam/slam.i b/gtsam/slam/slam.i index d276c4f2ec..a0a7329dd8 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); From 754d03696de094b23d5085c6a346f8076771c13a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 3 Jan 2022 10:27:42 -0500 Subject: [PATCH 09/10] Undo changes in 3dparty --- gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseNullaryOp.h | 4 ++-- gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h | 4 ++-- gtsam/3rdparty/Eigen/Eigen/src/Core/EigenBase.h | 2 +- gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h | 2 +- gtsam/3rdparty/Eigen/Eigen/src/Core/util/Constants.h | 4 ++-- gtsam/3rdparty/Eigen/Eigen/src/Geometry/AlignedBox.h | 4 ++-- .../Eigen/Eigen/src/Geometry/ParametrizedLine.h | 2 +- gtsam/3rdparty/Eigen/Eigen/src/Geometry/Scaling.h | 8 ++++---- .../Eigen/src/SparseCholesky/SimplicialCholesky.h | 2 +- .../Eigen/Eigen/src/SparseCore/MappedSparseMatrix.h | 2 +- .../Eigen/src/SparseExtra/DynamicSparseMatrix.h | 10 +++++----- .../include/GeographicLib/NormalGravity.hpp | 2 +- .../GeographicLib/include/GeographicLib/Utility.hpp | 2 +- 13 files changed, 24 insertions(+), 24 deletions(-) diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseNullaryOp.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseNullaryOp.h index 4ffd0057b1..ddd607e383 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseNullaryOp.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseNullaryOp.h @@ -215,7 +215,7 @@ DenseBase::Constant(const Scalar& value) return DenseBase::NullaryExpr(RowsAtCompileTime, ColsAtCompileTime, internal::scalar_constant_op(value)); } -/** @deprecated because of accuracy loss. In Eigen 3.3, it is an alias for LinSpaced(Index,const Scalar&,const Scalar&) +/** \deprecated because of accuracy loss. In Eigen 3.3, it is an alias for LinSpaced(Index,const Scalar&,const Scalar&) * * \sa LinSpaced(Index,Scalar,Scalar), setLinSpaced(Index,const Scalar&,const Scalar&) */ @@ -227,7 +227,7 @@ DenseBase::LinSpaced(Sequential_t, Index size, const Scalar& low, const return DenseBase::NullaryExpr(size, internal::linspaced_op(low,high,size)); } -/** @deprecated because of accuracy loss. In Eigen 3.3, it is an alias for LinSpaced(const Scalar&,const Scalar&) +/** \deprecated because of accuracy loss. In Eigen 3.3, it is an alias for LinSpaced(const Scalar&,const Scalar&) * * \sa LinSpaced(Scalar,Scalar) */ diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h index 8b2733814e..90066ae73f 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h @@ -298,7 +298,7 @@ template class DenseBase /** \internal * Copies \a other into *this without evaluating other. \returns a reference to *this. - * @deprecated */ + * \deprecated */ template EIGEN_DEVICE_FUNC Derived& lazyAssign(const DenseBase& other); @@ -306,7 +306,7 @@ template class DenseBase EIGEN_DEVICE_FUNC CommaInitializer operator<< (const Scalar& s); - /** @deprecated it now returns \c *this */ + /** \deprecated it now returns \c *this */ template EIGEN_DEPRECATED const Derived& flagged() const diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/EigenBase.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/EigenBase.h index bf30e03ffd..b195506a91 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/EigenBase.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/EigenBase.h @@ -32,7 +32,7 @@ template struct EigenBase /** \brief The interface type of indices * \details To change this, \c \#define the preprocessor symbol \c EIGEN_DEFAULT_DENSE_INDEX_TYPE. - * @deprecated Since Eigen 3.3, its usage is deprecated. Use Eigen::Index instead. + * \deprecated Since Eigen 3.3, its usage is deprecated. Use Eigen::Index instead. * \sa StorageIndex, \ref TopicPreprocessorDirectives. */ typedef Eigen::Index Index; diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h index 9c76906fec..9db32744e6 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h @@ -435,7 +435,7 @@ template class TriangularViewImpl<_Mat TriangularViewType& operator=(const TriangularViewImpl& other) { return *this = other.derived().nestedExpression(); } - /** @deprecated */ + /** \deprecated */ template EIGEN_DEVICE_FUNC void lazyAssign(const TriangularBase& other); diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Constants.h b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Constants.h index 9602332c8a..7587d68424 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Constants.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Core/util/Constants.h @@ -65,7 +65,7 @@ const unsigned int RowMajorBit = 0x1; const unsigned int EvalBeforeNestingBit = 0x2; /** \ingroup flags - * @deprecated + * \deprecated * means the expression should be evaluated before any assignment */ EIGEN_DEPRECATED const unsigned int EvalBeforeAssigningBit = 0x4; // FIXME deprecated @@ -149,7 +149,7 @@ const unsigned int LvalueBit = 0x20; */ const unsigned int DirectAccessBit = 0x40; -/** @deprecated \ingroup flags +/** \deprecated \ingroup flags * * means the first coefficient packet is guaranteed to be aligned. * An expression cannot has the AlignedBit without the PacketAccessBit flag. diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/AlignedBox.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/AlignedBox.h index 96c100245e..066eae4f92 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/AlignedBox.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/AlignedBox.h @@ -84,10 +84,10 @@ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(_Scalar,_AmbientDim) /** \returns the dimension in which the box holds */ EIGEN_DEVICE_FUNC inline Index dim() const { return AmbientDimAtCompileTime==Dynamic ? m_min.size() : Index(AmbientDimAtCompileTime); } - /** @deprecated use isEmpty() */ + /** \deprecated use isEmpty() */ EIGEN_DEVICE_FUNC inline bool isNull() const { return isEmpty(); } - /** @deprecated use setEmpty() */ + /** \deprecated use setEmpty() */ EIGEN_DEVICE_FUNC inline void setNull() { setEmpty(); } /** \returns true if the box is empty. diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/ParametrizedLine.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/ParametrizedLine.h index 60ca9b7f37..1e985d8cde 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/ParametrizedLine.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/ParametrizedLine.h @@ -170,7 +170,7 @@ EIGEN_DEVICE_FUNC inline _Scalar ParametrizedLine<_Scalar, _AmbientDim,_Options> } -/** @deprecated use intersectionParameter() +/** \deprecated use intersectionParameter() * \returns the parameter value of the intersection between \c *this and the given \a hyperplane */ template diff --git a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Scaling.h b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Scaling.h index 81a5bcafce..f58ca03d94 100755 --- a/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Scaling.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/Geometry/Scaling.h @@ -142,13 +142,13 @@ template inline const DiagonalWrapper Scaling(const MatrixBase& coeffs) { return coeffs.asDiagonal(); } -/** @deprecated */ +/** \deprecated */ typedef DiagonalMatrix AlignedScaling2f; -/** @deprecated */ +/** \deprecated */ typedef DiagonalMatrix AlignedScaling2d; -/** @deprecated */ +/** \deprecated */ typedef DiagonalMatrix AlignedScaling3f; -/** @deprecated */ +/** \deprecated */ typedef DiagonalMatrix AlignedScaling3d; //@} diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky.h index f2693fc81b..2907f65296 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCholesky/SimplicialCholesky.h @@ -493,7 +493,7 @@ template } }; -/** @deprecated use SimplicialLDLT or class SimplicialLLT +/** \deprecated use SimplicialLDLT or class SimplicialLLT * \ingroup SparseCholesky_Module * \class SimplicialCholesky * diff --git a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/MappedSparseMatrix.h b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/MappedSparseMatrix.h index 25ca27fe59..67718c85be 100644 --- a/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/MappedSparseMatrix.h +++ b/gtsam/3rdparty/Eigen/Eigen/src/SparseCore/MappedSparseMatrix.h @@ -12,7 +12,7 @@ namespace Eigen { -/** @deprecated Use Map > +/** \deprecated Use Map > * \class MappedSparseMatrix * * \brief Sparse matrix diff --git a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/DynamicSparseMatrix.h b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/DynamicSparseMatrix.h index 1f0379c152..0ffbc43d23 100644 --- a/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/DynamicSparseMatrix.h +++ b/gtsam/3rdparty/Eigen/unsupported/Eigen/src/SparseExtra/DynamicSparseMatrix.h @@ -12,7 +12,7 @@ namespace Eigen { -/** @deprecated use a SparseMatrix in an uncompressed mode +/** \deprecated use a SparseMatrix in an uncompressed mode * * \class DynamicSparseMatrix * @@ -291,7 +291,7 @@ template public: - /** @deprecated + /** \deprecated * Set the matrix to zero and reserve the memory for \a reserveSize nonzero coefficients. */ EIGEN_DEPRECATED void startFill(Index reserveSize = 1000) { @@ -299,7 +299,7 @@ template reserve(reserveSize); } - /** @deprecated use insert() + /** \deprecated use insert() * inserts a nonzero coefficient at given coordinates \a row, \a col and returns its reference assuming that: * 1 - the coefficient does not exist yet * 2 - this the coefficient with greater inner coordinate for the given outer coordinate. @@ -315,7 +315,7 @@ template return insertBack(outer,inner); } - /** @deprecated use insert() + /** \deprecated use insert() * Like fill() but with random inner coordinates. * Compared to the generic coeffRef(), the unique limitation is that we assume * the coefficient does not exist yet. @@ -325,7 +325,7 @@ template return insert(row,col); } - /** @deprecated use finalize() + /** \deprecated use finalize() * Does nothing. Provided for compatibility with SparseMatrix. */ EIGEN_DEPRECATED void endFill() {} diff --git a/gtsam/3rdparty/GeographicLib/include/GeographicLib/NormalGravity.hpp b/gtsam/3rdparty/GeographicLib/include/GeographicLib/NormalGravity.hpp index 13a21c657c..2c4955f745 100644 --- a/gtsam/3rdparty/GeographicLib/include/GeographicLib/NormalGravity.hpp +++ b/gtsam/3rdparty/GeographicLib/include/GeographicLib/NormalGravity.hpp @@ -142,7 +142,7 @@ namespace GeographicLib { NormalGravity(real a, real GM, real omega, real f_J2, bool geometricp = true); /** - * @deprecated Old constructor for the normal gravity. + * \deprecated Old constructor for the normal gravity. * * @param[in] a equatorial radius (meters). * @param[in] GM mass constant of the ellipsoid diff --git a/gtsam/3rdparty/GeographicLib/include/GeographicLib/Utility.hpp b/gtsam/3rdparty/GeographicLib/include/GeographicLib/Utility.hpp index a9b0129d47..9990e47e91 100644 --- a/gtsam/3rdparty/GeographicLib/include/GeographicLib/Utility.hpp +++ b/gtsam/3rdparty/GeographicLib/include/GeographicLib/Utility.hpp @@ -380,7 +380,7 @@ namespace GeographicLib { return x; } /** - * @deprecated An old name for val(s). + * \deprecated An old name for val(s). **********************************************************************/ template // GEOGRAPHICLIB_DEPRECATED("Use new Utility::val(s)") From cf50d10fab3c345cea516961d85c51cefbd66678 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 3 Jan 2022 10:32:55 -0500 Subject: [PATCH 10/10] Added more GTSAM_DEPRECATED directives --- gtsam/geometry/Cal3.h | 2 +- gtsam/geometry/Cal3Bundler.h | 4 ++-- gtsam/geometry/SimpleCamera.cpp | 2 +- gtsam/linear/GaussianConditional.cpp | 3 ++- 4 files changed, 6 insertions(+), 5 deletions(-) diff --git a/gtsam/geometry/Cal3.h b/gtsam/geometry/Cal3.h index 128c217f93..1690615ddf 100644 --- a/gtsam/geometry/Cal3.h +++ b/gtsam/geometry/Cal3.h @@ -172,7 +172,7 @@ class GTSAM_EXPORT Cal3 { #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 2285b2dbbe..b240603fc8 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -99,10 +99,10 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 { #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 3b871b4686..be6a010b20 100644 --- a/gtsam/geometry/SimpleCamera.cpp +++ b/gtsam/geometry/SimpleCamera.cpp @@ -22,7 +22,7 @@ namespace gtsam { #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 - SimpleCamera simpleCamera(const Matrix34& P) { + 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/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index e5016c6240..d87c39eeaf 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -194,7 +194,8 @@ namespace gtsam { /* ************************************************************************* */ #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 - void GaussianConditional::scaleFrontalsBySigma(VectorValues& gy) const { + 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();