diff --git a/.github/scripts/python.sh b/.github/scripts/python.sh index 718eee5eaf..99fddda683 100644 --- a/.github/scripts/python.sh +++ b/.github/scripts/python.sh @@ -79,7 +79,7 @@ function build() -DGTSAM_UNSTABLE_BUILD_PYTHON=${GTSAM_BUILD_UNSTABLE:-ON} \ -DGTSAM_PYTHON_VERSION=$PYTHON_VERSION \ -DPYTHON_EXECUTABLE:FILEPATH=$(which $PYTHON) \ - -DGTSAM_ALLOW_DEPRECATED_SINCE_V42=OFF \ + -DGTSAM_ALLOW_DEPRECATED_SINCE_V43=OFF \ -DCMAKE_INSTALL_PREFIX=$GITHUB_WORKSPACE/gtsam_install diff --git a/.github/scripts/unix.sh b/.github/scripts/unix.sh index b5a559df59..87b0a3100b 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_V42=${GTSAM_ALLOW_DEPRECATED_SINCE_V42:-OFF} \ + -DGTSAM_ALLOW_DEPRECATED_SINCE_V43=${GTSAM_ALLOW_DEPRECATED_SINCE_V43:-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 ef7d7723d9..458394211c 100644 --- a/.github/workflows/build-special.yml +++ b/.github/workflows/build-special.yml @@ -110,8 +110,8 @@ jobs: - name: Set Allow Deprecated Flag if: matrix.flag == 'deprecated' run: | - echo "GTSAM_ALLOW_DEPRECATED_SINCE_V42=ON" >> $GITHUB_ENV - echo "Allow deprecated since version 4.1" + echo "GTSAM_ALLOW_DEPRECATED_SINCE_V43=ON" >> $GITHUB_ENV + echo "Allow deprecated since version 4.3" - name: Set Use Quaternions Flag if: matrix.flag == 'quaternions' diff --git a/CMakeLists.txt b/CMakeLists.txt index 9be0d89a99..52b34101f3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,8 +6,6 @@ if(NOT DEFINED CMAKE_MACOSX_RPATH) set(CMAKE_MACOSX_RPATH 0) endif() -set(CMAKE_CXX_STANDARD 17) - # Set the version number for the library set (GTSAM_VERSION_MAJOR 4) set (GTSAM_VERSION_MINOR 3) diff --git a/README.md b/README.md index 87d79d5576..10a239f972 100644 --- a/README.md +++ b/README.md @@ -55,11 +55,6 @@ Optional prerequisites - used automatically if findable by CMake: GTSAM 4 introduces several new features, most notably Expressions and a Python toolbox. It also introduces traits, a C++ technique that allows optimizing with non-GTSAM types. That opens the door to retiring geometric types such as Point2 and Point3 to pure Eigen types, which we also do. A significant change which will not trigger a compile error is that zero-initializing of Point2 and Point3 is deprecated, so please be aware that this might render functions using their default constructor incorrect. -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_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 We provide support for [MATLAB](matlab/README.md) and [Python](python/README.md) wrappers for GTSAM. Please refer to the linked documents for more details. diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index 0db9538f2c..e8f01a1f0d 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -142,17 +142,14 @@ endif() if (NOT CMAKE_VERSION VERSION_LESS 3.8) set(GTSAM_COMPILE_FEATURES_PUBLIC "cxx_std_17" CACHE STRING "CMake compile features property for all gtsam targets.") # See: https://cmake.org/cmake/help/latest/prop_tgt/CXX_EXTENSIONS.html - # TODO(dellaert): is following line still needed or was that only for c++11? set(CMAKE_CXX_EXTENSIONS OFF) if (MSVC) # NOTE(jlblanco): seems to be required in addition to the cxx_std_17 above? - # TODO(dellaert): is this the right syntax below? list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC /std:c++latest) endif() else() # Old cmake versions: if (NOT MSVC) - # TODO(dellaert): I just changed 11 to 17 below, hopefully that works list_append_cache(GTSAM_COMPILE_OPTIONS_PUBLIC $<$:-std=c++17>) endif() endif() diff --git a/cmake/HandleGeneralOptions.cmake b/cmake/HandleGeneralOptions.cmake index 7c8f8533f3..64fa6b407b 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_V42 "Allow use of methods/functions deprecated in GTSAM 4.1" ON) +option(GTSAM_ALLOW_DEPRECATED_SINCE_V43 "Allow use of methods/functions deprecated in GTSAM 4.3" 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 04d27c27f4..dd3f6fc1fc 100644 --- a/cmake/HandlePrintConfiguration.cmake +++ b/cmake/HandlePrintConfiguration.cmake @@ -87,7 +87,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_V42} "Allow features deprecated in GTSAM 4.1") +print_enabled_config(${GTSAM_ALLOW_DEPRECATED_SINCE_V43} "Allow features deprecated in GTSAM 4.3") print_enabled_config(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ") print_enabled_config(${GTSAM_TANGENT_PREINTEGRATION} "Use tangent-space preintegration") diff --git a/examples/StereoVOExample_large.cpp b/examples/StereoVOExample_large.cpp index af81db7030..c4a3a8de42 100644 --- a/examples/StereoVOExample_large.cpp +++ b/examples/StereoVOExample_large.cpp @@ -27,6 +27,7 @@ #include #include #include +#include #include #include #include @@ -113,7 +114,7 @@ int main(int argc, char** argv) { Values result = optimizer.optimize(); cout << "Final result sample:" << endl; - Values pose_values = result.filter(); + Values pose_values = utilities::allPose3s(result); pose_values.print("Final camera poses:\n"); return 0; diff --git a/gtsam/base/TestableAssertions.h b/gtsam/base/TestableAssertions.h index e5bd34d19c..638096c955 100644 --- a/gtsam/base/TestableAssertions.h +++ b/gtsam/base/TestableAssertions.h @@ -80,37 +80,6 @@ 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 - */ -template -bool GTSAM_DEPRECATED assert_equal(const std::vector& expected, const std::vector& actual, double tol = 1e-9) { - bool match = true; - if (expected.size() != actual.size()) - match = false; - if(match) { - size_t i = 0; - for(const V& a: expected) { - if (!assert_equal(a, actual[i++], tol)) { - match = false; - break; - } - } - } - if(!match) { - std::cout << "expected: " << std::endl; - for(const V& a: expected) { std::cout << a << " "; } - std::cout << "\nactual: " << std::endl; - for(const V& a: actual) { std::cout << a << " "; } - std::cout << std::endl; - return false; - } - return true; -} -#endif - /** * Function for comparing maps of testable->testable * TODO: replace with more generalized version diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index f7923ff88c..f9a5cf0792 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -204,28 +204,6 @@ 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 - */ -inline void GTSAM_DEPRECATED scal(double alpha, Vector& x) { x *= alpha; } - -/** - * BLAS Level 1 axpy: y <- alpha*x + y - * @deprecated: use operators instead - */ -template -inline void GTSAM_DEPRECATED axpy(double alpha, const V1& x, V2& y) { - assert (y.size()==x.size()); - y += alpha * x; -} -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 * from x, such that the corresponding Householder reflection zeroes out diff --git a/gtsam/config.h.in b/gtsam/config.h.in index d47329a627..7f8936d1e3 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_V42 +#cmakedefine GTSAM_ALLOW_DEPRECATED_SINCE_V43 // Support Metis-based nested dissection #cmakedefine GTSAM_SUPPORT_NESTED_DISSECTION diff --git a/gtsam/discrete/DiscreteBayesNet.cpp b/gtsam/discrete/DiscreteBayesNet.cpp index 7cd190cc26..97734c3bbc 100644 --- a/gtsam/discrete/DiscreteBayesNet.cpp +++ b/gtsam/discrete/DiscreteBayesNet.cpp @@ -51,26 +51,6 @@ double DiscreteBayesNet::evaluate(const DiscreteValues& values) const { return result; } -/* ************************************************************************* */ -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 -DiscreteValues DiscreteBayesNet::optimize() const { - DiscreteValues result; - return optimize(result); -} - -DiscreteValues DiscreteBayesNet::optimize(DiscreteValues result) const { - // solve each node in turn in topological sort order (parents first) -#ifdef _MSC_VER -#pragma message("DiscreteBayesNet::optimize (deprecated) does not compute MPE!") -#else -#warning "DiscreteBayesNet::optimize (deprecated) does not compute MPE!" -#endif - for (auto conditional : boost::adaptors::reverse(*this)) - conditional->solveInPlace(&result); - return result; -} -#endif - /* ************************************************************************* */ DiscreteValues DiscreteBayesNet::sample() const { DiscreteValues result; diff --git a/gtsam/discrete/DiscreteBayesNet.h b/gtsam/discrete/DiscreteBayesNet.h index 700f81d7e5..f3b4a58f50 100644 --- a/gtsam/discrete/DiscreteBayesNet.h +++ b/gtsam/discrete/DiscreteBayesNet.h @@ -149,15 +149,6 @@ class GTSAM_EXPORT DiscreteBayesNet: public BayesNet { /// @} -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 - /// @name Deprecated functionality - /// @{ - - DiscreteValues GTSAM_DEPRECATED optimize() const; - DiscreteValues GTSAM_DEPRECATED optimize(DiscreteValues given) const; - /// @} -#endif - private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index 214bc64da2..b4f58a5bd9 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -235,57 +235,6 @@ DecisionTreeFactor::shared_ptr DiscreteConditional::likelihood( return likelihood(values); } -/* ************************************************************************** */ -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 -void DiscreteConditional::solveInPlace(DiscreteValues* values) const { - ADT pFS = choose(*values, true); // P(F|S=parentsValues) - - // Initialize - DiscreteValues mpe; - double maxP = 0; - - // Get all Possible Configurations - const auto allPosbValues = frontalAssignments(); - - // Find the maximum - for (const auto& frontalVals : allPosbValues) { - double pValueS = pFS(frontalVals); // P(F=value|S=parentsValues) - // Update maximum solution if better - if (pValueS > maxP) { - maxP = pValueS; - mpe = frontalVals; - } - } - - // set values (inPlace) to maximum - for (Key j : frontals()) { - (*values)[j] = mpe[j]; - } -} - -/* ************************************************************************** */ -size_t DiscreteConditional::solve(const DiscreteValues& parentsValues) const { - ADT pFS = choose(parentsValues, true); // P(F|S=parentsValues) - - // Then, find the max over all remaining - size_t max = 0; - double maxP = 0; - DiscreteValues frontals; - assert(nrFrontals() == 1); - Key j = (firstFrontalKey()); - for (size_t value = 0; value < cardinality(j); value++) { - frontals[j] = value; - double pValueS = pFS(frontals); // P(F=value|S=parentsValues) - // Update solution if better - if (pValueS > maxP) { - maxP = pValueS; - max = value; - } - } - return max; -} -#endif - /* ************************************************************************** */ size_t DiscreteConditional::argmax() const { size_t maxValue = 0; diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index a4e1f011b7..03c44649fc 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -263,14 +263,6 @@ class GTSAM_EXPORT DiscreteConditional /// @} -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 - /// @name Deprecated functionality - /// @{ - size_t GTSAM_DEPRECATED solve(const DiscreteValues& parentsValues) const; - void GTSAM_DEPRECATED solveInPlace(DiscreteValues* parentsValues) const; - /// @} -#endif - protected: /// Internal version of choose DiscreteConditional::ADT choose(const DiscreteValues& given, diff --git a/gtsam/discrete/DiscreteDistribution.h b/gtsam/discrete/DiscreteDistribution.h index bf3f1c9ae9..4b690da156 100644 --- a/gtsam/discrete/DiscreteDistribution.h +++ b/gtsam/discrete/DiscreteDistribution.h @@ -93,12 +93,6 @@ class GTSAM_EXPORT DiscreteDistribution : public DiscreteConditional { std::vector pmf() const; /// @} -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 - /// @name Deprecated functionality - /// @{ - size_t GTSAM_DEPRECATED solve() const { return Base::solve({}); } - /// @} -#endif }; // DiscreteDistribution diff --git a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp index 2d79b9de9e..9439ff4178 100644 --- a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp +++ b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp @@ -209,13 +209,6 @@ TEST(DiscreteFactorGraph, marginalIsNotMPE) { auto actualMPE = graph.optimize(); EXPECT(assert_equal(mpe, actualMPE)); EXPECT_DOUBLES_EQUAL(0.315789, graph(mpe), 1e-5); // regression - -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 - // Optimize on BayesNet maximizes marginal, then the conditional marginals: - auto notOptimal = bayesNet.optimize(); - EXPECT(graph(notOptimal) < graph(mpe)); - EXPECT_DOUBLES_EQUAL(0.263158, graph(notOptimal), 1e-5); // regression -#endif } /* ************************************************************************* */ @@ -246,10 +239,6 @@ TEST(DiscreteFactorGraph, testMPE_Darwiche09book_p244) { ordering += Key(0), Key(1), Key(2), Key(3), Key(4); auto chordal = graph.eliminateSequential(ordering); EXPECT_LONGS_EQUAL(5, chordal->size()); -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 - auto notOptimal = chordal->optimize(); // not MPE ! - EXPECT(graph(notOptimal) < graph(mpe)); -#endif // Let us create the Bayes tree here, just for fun, because we don't use it DiscreteBayesTree::shared_ptr bayesTree = diff --git a/gtsam/geometry/Cal3.h b/gtsam/geometry/Cal3.h index 25acc11f8e..e3cf8f79c3 100644 --- a/gtsam/geometry/Cal3.h +++ b/gtsam/geometry/Cal3.h @@ -170,11 +170,6 @@ class GTSAM_EXPORT Cal3 { return K; } -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 - /** @deprecated The following function has been deprecated, use K above */ - Matrix3 GTSAM_DEPRECATED matrix() const { return K(); } -#endif - /// Return inverted calibration matrix inv(K) Matrix3 inverse() const; diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index c8e0fe381a..46dae5f9be 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -100,14 +100,6 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 { Vector3 vector() const; -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 - /// get parameter u0 - inline double GTSAM_DEPRECATED u0() const { return u0_; } - - /// get parameter v0 - inline double GTSAM_DEPRECATED v0() const { return v0_; } -#endif - /** * @brief: convert intrinsic coordinates xy to image coordinates uv * Version of uncalibrate with derivatives diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index b37674b925..47f50cf7e7 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -371,15 +371,5 @@ boost::optional Pose2::Align(const Matrix& a, const Matrix& b) { return Pose2::Align(ab_pairs); } -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 -boost::optional align(const Point2Pairs& ba_pairs) { - Point2Pairs ab_pairs; - for (const Point2Pair &baPair : ba_pairs) { - ab_pairs.emplace_back(baPair.second, baPair.first); - } - return Pose2::Align(ab_pairs); -} -#endif - /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 0ecd95e1cb..dc2f87b9fd 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -343,16 +343,6 @@ inline Matrix wedge(const Vector& xi) { return Matrix(Pose2::wedge(xi(0),xi(1),xi(2))).eval(); } -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 -/** - * @deprecated Use static constructor (with reversed pairs!) - * Calculate pose between a vector of 2D point correspondences (p,q) - * where q = Pose2::transformFrom(p) = t + R*p - */ -GTSAM_EXPORT boost::optional -GTSAM_DEPRECATED align(const Point2Pairs& pairs); -#endif - // Convenience typedef using Pose2Pair = std::pair; using Pose2Pairs = std::vector; diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 2652fc0730..b018bb8ccb 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -478,16 +478,6 @@ boost::optional Pose3::Align(const Matrix& a, const Matrix& b) { return Pose3::Align(abPointPairs); } -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 -boost::optional align(const Point3Pairs &baPointPairs) { - Point3Pairs abPointPairs; - for (const Point3Pair &baPair : baPointPairs) { - abPointPairs.emplace_back(baPair.second, baPair.first); - } - return Pose3::Align(abPointPairs); -} -#endif - /* ************************************************************************* */ Pose3 Pose3::slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx, OptionalJacobian<6, 6> Hy) const { return interpolate(*this, other, t, Hx, Hy); diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 6db5e19195..e854d03b77 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -227,19 +227,6 @@ double Rot3::yaw(OptionalJacobian<1, 3> H) const { return y; } -/* ************************************************************************* */ -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 -Vector Rot3::quaternion() const { - gtsam::Quaternion q = toQuaternion(); - Vector v(4); - v(0) = q.w(); - v(1) = q.x(); - v(2) = q.y(); - v(3) = q.z(); - return v; -} -#endif - /* ************************************************************************* */ pair Rot3::axisAngle() const { const Vector3 omega = Rot3::Logmap(*this); diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 65f3d4e5f5..c3ca87d21a 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -515,17 +515,6 @@ class GTSAM_EXPORT Rot3 : public LieGroup { */ gtsam::Quaternion toQuaternion() const; -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 - /** - * Converts to a generic matrix to allow for use with matlab - * In format: w x y z - * @deprecated: use Rot3::toQuaternion() instead. - * If still using this API, remind that in the returned Vector `V`, - * `V.x()` denotes the actual `qw`, `V.y()` denotes 'qx', `V.z()` denotes `qy`, and `V.w()` denotes 'qz'. - */ - Vector GTSAM_DEPRECATED quaternion() const; -#endif - /** * @brief Spherical Linear intERPolation between *this and other * @param t a value between 0 and 1 diff --git a/gtsam/geometry/SimpleCamera.cpp b/gtsam/geometry/SimpleCamera.cpp deleted file mode 100644 index be6a010b20..0000000000 --- a/gtsam/geometry/SimpleCamera.cpp +++ /dev/null @@ -1,51 +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 SimpleCamera.cpp - * @brief A simple camera class with a Cal3_S2 calibration - * @date June 30, 2012 - * @author Frank Dellaert - */ - -#include -#include - -namespace gtsam { - -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 - SimpleCamera GTSAM_DEPRECATED simpleCamera(const Matrix34& P) { - - // P = [A|a] = s K cRw [I|-T], with s the unknown scale - Matrix3 A = P.topLeftCorner(3, 3); - Vector3 a = P.col(3); - - // do RQ decomposition to get s*K and cRw angles - Matrix3 sK; - Vector3 xyz; - boost::tie(sK, xyz) = RQ(A); - - // Recover scale factor s and K - double s = sK(2, 2); - Matrix3 K = sK / s; - - // Recover cRw itself, and its inverse - Rot3 cRw = Rot3::RzRyRx(xyz); - Rot3 wRc = cRw.inverse(); - - // Now, recover T from a = - s K cRw T = - A T - Vector3 T = -(A.inverse() * a); - return SimpleCamera(Pose3(wRc, T), - Cal3_S2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2))); - } -#endif - -} diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index f0776c2e2a..7944514423 100644 --- a/gtsam/geometry/SimpleCamera.h +++ b/gtsam/geometry/SimpleCamera.h @@ -37,120 +37,4 @@ namespace gtsam { using PinholeCameraCal3Unified = gtsam::PinholeCamera; using PinholeCameraCal3Fisheye = gtsam::PinholeCamera; -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 -/** - * @deprecated: SimpleCamera for backwards compatability with GTSAM 3.x - * Use PinholeCameraCal3_S2 instead - */ -class GTSAM_EXPORT SimpleCamera : public PinholeCameraCal3_S2 { - - typedef PinholeCamera Base; - typedef boost::shared_ptr shared_ptr; - -public: - - /// @name Standard Constructors - /// @{ - - /** default constructor */ - SimpleCamera() : - Base() { - } - - /** constructor with pose */ - explicit SimpleCamera(const Pose3& pose) : - Base(pose) { - } - - /** constructor with pose and calibration */ - SimpleCamera(const Pose3& pose, const Cal3_S2& K) : - Base(pose, K) { - } - - /// @} - /// @name Named Constructors - /// @{ - - /** - * Create a level camera at the given 2D pose and height - * @param K the calibration - * @param pose2 specifies the location and viewing direction - * (theta 0 = looking in direction of positive X axis) - * @param height camera height - */ - static SimpleCamera Level(const Cal3_S2 &K, const Pose2& pose2, - double height) { - return SimpleCamera(Base::LevelPose(pose2, height), K); - } - - /// PinholeCamera::level with default calibration - static SimpleCamera Level(const Pose2& pose2, double height) { - return SimpleCamera::Level(Cal3_S2(), pose2, height); - } - - /** - * Create a camera at the given eye position looking at a target point in the scene - * with the specified up direction vector. - * @param eye specifies the camera position - * @param target the point to look at - * @param upVector specifies the camera up direction vector, - * doesn't need to be on the image plane nor orthogonal to the viewing axis - * @param K optional calibration parameter - */ - static SimpleCamera Lookat(const Point3& eye, const Point3& target, - const Point3& upVector, const Cal3_S2& K = Cal3_S2()) { - return SimpleCamera(Base::LookatPose(eye, target, upVector), K); - } - - /// @} - /// @name Advanced Constructors - /// @{ - - /// Init from vector, can be 6D (default calibration) or dim - explicit SimpleCamera(const Vector &v) : - Base(v) { - } - - /// Init from Vector and calibration - SimpleCamera(const Vector &v, const Vector &K) : - Base(v, K) { - } - - /// Copy this object as its actual derived type. - SimpleCamera::shared_ptr clone() const { return boost::make_shared(*this); } - - - /// @} - /// @name Manifold - /// @{ - - /// move a cameras according to d - SimpleCamera retract(const Vector& d) const { - if ((size_t) d.size() == 6) - return SimpleCamera(this->pose().retract(d), calibration()); - else - return SimpleCamera(this->pose().retract(d.head(6)), - calibration().retract(d.tail(calibration().dim()))); - } - - /// @} - -}; - -/// Recover camera from 3*4 camera matrix -GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix34& P); - -// manifold traits -template <> -struct traits : public internal::Manifold {}; - -template <> -struct traits : public internal::Manifold {}; - -// range traits, used in RangeFactor -template -struct Range : HasRange {}; - -#endif - } // namespace gtsam diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp deleted file mode 100644 index 173ccf05b8..0000000000 --- a/gtsam/geometry/tests/testSimpleCamera.cpp +++ /dev/null @@ -1,160 +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 testSimpleCamera.cpp - * @author Frank Dellaert - * @brief test SimpleCamera class - */ - -#include -#include -#include -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; - -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 - -static const Cal3_S2 K(625, 625, 0, 0, 0); - -static const Pose3 pose1(Rot3(Vector3(1, -1, -1).asDiagonal()), - Point3(0, 0, 0.5)); - -static const SimpleCamera camera(pose1, K); - -static const Point3 point1(-0.08,-0.08, 0.0); -static const Point3 point2(-0.08, 0.08, 0.0); -static const Point3 point3( 0.08, 0.08, 0.0); -static const Point3 point4( 0.08,-0.08, 0.0); - -/* ************************************************************************* */ -TEST( SimpleCamera, constructor) -{ - CHECK(assert_equal( camera.calibration(), K)); - CHECK(assert_equal( camera.pose(), pose1)); -} - -/* ************************************************************************* */ -TEST( SimpleCamera, level2) -{ - // Create a level camera, looking in Y-direction - Pose2 pose2(0.4,0.3,M_PI/2.0); - SimpleCamera camera = SimpleCamera::Level(K, pose2, 0.1); - - // expected - Point3 x(1,0,0),y(0,0,-1),z(0,1,0); - Rot3 wRc(x,y,z); - Pose3 expected(wRc,Point3(0.4,0.3,0.1)); - CHECK(assert_equal( camera.pose(), expected)); -} - -/* ************************************************************************* */ -TEST( SimpleCamera, lookat) -{ - // Create a level camera, looking in Y-direction - Point3 C(10,0,0); - SimpleCamera camera = SimpleCamera::Lookat(C, Point3(0,0,0), Point3(0,0,1)); - - // expected - Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0); - Pose3 expected(Rot3(xc,yc,zc),C); - CHECK(assert_equal( camera.pose(), expected)); - - Point3 C2(30,0,10); - SimpleCamera camera2 = SimpleCamera::Lookat(C2, Point3(0,0,0), Point3(0,0,1)); - - Matrix R = camera2.pose().rotation().matrix(); - Matrix I = trans(R)*R; - CHECK(assert_equal(I, I_3x3)); -} - -/* ************************************************************************* */ -TEST( SimpleCamera, project) -{ - CHECK(assert_equal( camera.project(point1), Point2(-100, 100) )); - CHECK(assert_equal( camera.project(point2), Point2(-100, -100) )); - CHECK(assert_equal( camera.project(point3), Point2( 100, -100) )); - CHECK(assert_equal( camera.project(point4), Point2( 100, 100) )); -} - -/* ************************************************************************* */ -TEST( SimpleCamera, backproject) -{ - CHECK(assert_equal( camera.backproject(Point2(-100, 100), 0.5), point1)); - CHECK(assert_equal( camera.backproject(Point2(-100, -100), 0.5), point2)); - CHECK(assert_equal( camera.backproject(Point2( 100, -100), 0.5), point3)); - CHECK(assert_equal( camera.backproject(Point2( 100, 100), 0.5), point4)); -} - -/* ************************************************************************* */ -TEST( SimpleCamera, backproject2) -{ - Point3 origin(0,0,0); - Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera looking down - SimpleCamera camera(Pose3(rot, origin), K); - - Point3 actual = camera.backproject(Point2(0,0), 1.); - Point3 expected(0., 1., 0.); - pair x = camera.projectSafe(expected); - - CHECK(assert_equal(expected, actual)); - CHECK(assert_equal(Point2(0,0), x.first)); - CHECK(x.second); -} - -/* ************************************************************************* */ -static Point2 project2(const Pose3& pose, const Point3& point) { - return SimpleCamera(pose,K).project(point); -} - -TEST( SimpleCamera, Dproject_point_pose) -{ - Matrix Dpose, Dpoint; - Point2 result = camera.project(point1, Dpose, Dpoint, boost::none); - Matrix numerical_pose = numericalDerivative21(project2, pose1, point1); - Matrix numerical_point = numericalDerivative22(project2, pose1, point1); - CHECK(assert_equal(result, Point2(-100, 100) )); - CHECK(assert_equal(Dpose, numerical_pose, 1e-7)); - CHECK(assert_equal(Dpoint, numerical_point,1e-7)); -} - -/* ************************************************************************* */ -TEST( SimpleCamera, simpleCamera) -{ - Cal3_S2 K(468.2,427.2,91.2,300,200); - Rot3 R( - 0.41380,0.90915,0.04708, - -0.57338,0.22011,0.78917, - 0.70711,-0.35355,0.61237); - Point3 T(1000,2000,1500); - SimpleCamera expected(Pose3(R.inverse(),T),K); - // H&Z example, 2nd edition, page 163 - Matrix P = (Matrix(3,4) << - 3.53553e2, 3.39645e2, 2.77744e2, -1.44946e6, - -1.03528e2, 2.33212e1, 4.59607e2, -6.32525e5, - 7.07107e-1, -3.53553e-1,6.12372e-1, -9.18559e2).finished(); - SimpleCamera actual = simpleCamera(P); - // Note precision of numbers given in book - CHECK(assert_equal(expected, actual,1e-1)); -} - -#endif - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ - - diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index 900346f7fb..7629127438 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -286,61 +286,6 @@ namespace gtsam { // Access the derived factor graph class FactorGraphType& asDerived() { return static_cast(*this); } - - public: - #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, - OptionalVariableIndex variableIndex, - OptionalOrderingType orderingType) const { - return eliminateSequential(ordering, function, variableIndex); - } - - /** @deprecated orderingType specified first for consistency */ - boost::shared_ptr GTSAM_DEPRECATED eliminateSequential( - const Eliminate& function, - OptionalVariableIndex variableIndex = boost::none, - OptionalOrderingType orderingType = boost::none) const { - return eliminateSequential(orderingType, function, variableIndex); - } - - /** @deprecated ordering and orderingType shouldn't both be specified */ - boost::shared_ptr GTSAM_DEPRECATED eliminateMultifrontal( - const Ordering& ordering, - const Eliminate& function, - OptionalVariableIndex variableIndex, - OptionalOrderingType orderingType) const { - return eliminateMultifrontal(ordering, function, variableIndex); - } - - /** @deprecated orderingType specified first for consistency */ - boost::shared_ptr GTSAM_DEPRECATED eliminateMultifrontal( - const Eliminate& function, - OptionalVariableIndex variableIndex = boost::none, - OptionalOrderingType orderingType = boost::none) const { - return eliminateMultifrontal(orderingType, function, variableIndex); - } - - /** @deprecated */ - boost::shared_ptr GTSAM_DEPRECATED marginalMultifrontalBayesNet( - boost::variant variables, - boost::none_t, - const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none) const { - return marginalMultifrontalBayesNet(variables, function, variableIndex); - } - - /** @deprecated */ - boost::shared_ptr GTSAM_DEPRECATED marginalMultifrontalBayesTree( - boost::variant variables, - boost::none_t, - const Eliminate& function = EliminationTraitsType::DefaultEliminate, - OptionalVariableIndex variableIndex = boost::none) const { - return marginalMultifrontalBayesTree(variables, function, variableIndex); - } - #endif }; } diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index cb8726d7a4..8118932677 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -371,15 +371,5 @@ namespace gtsam { } /* ************************************************************************ */ -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 - void GTSAM_DEPRECATED - GaussianConditional::scaleFrontalsBySigma(VectorValues& gy) const { - DenseIndex vectorPosition = 0; - for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { - gy[*frontal].array() *= model_->sigmas().segment(vectorPosition, getDim(frontal)).array(); - vectorPosition += getDim(frontal); - } - } -#endif } // namespace gtsam diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 4611e30d06..8bb65d8a93 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -273,16 +273,6 @@ namespace gtsam { /// @} - -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 - /// @name Deprecated - /// @{ - /** Scale the values in \c gy according to the sigmas for the frontal variables in this - * conditional. */ - void GTSAM_DEPRECATED scaleFrontalsBySigma(VectorValues& gy) const; - /// @} -#endif - private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index fb5e1ea362..55f8ed25dc 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -408,18 +408,6 @@ namespace gtsam { void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } - - public: - -#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/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index e19e53a1cb..3f37626825 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -202,12 +202,6 @@ class GTSAM_EXPORT AHRSFactor: public NoiseModelFactorN { 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; - -#endif - private: /** Serialization function */ diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index d9080eb273..261436d2d2 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -135,31 +135,6 @@ class GTSAM_EXPORT ConstantBias { /// @} -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 - /// @name Deprecated - /// @{ - ConstantBias GTSAM_DEPRECATED inverse() { return -(*this); } - ConstantBias GTSAM_DEPRECATED compose(const ConstantBias& q) { - return (*this) + q; - } - ConstantBias GTSAM_DEPRECATED between(const ConstantBias& q) { - return q - (*this); - } - Vector6 GTSAM_DEPRECATED localCoordinates(const ConstantBias& q) { - return (q - (*this)).vector(); - } - ConstantBias GTSAM_DEPRECATED retract(const Vector6& v) { - return (*this) + ConstantBias(v); - } - static Vector6 GTSAM_DEPRECATED Logmap(const ConstantBias& p) { - return p.vector(); - } - static ConstantBias GTSAM_DEPRECATED Expmap(const Vector6& v) { - return ConstantBias(v); - } - /// @} -#endif - private: /// @name Advanced Interface diff --git a/gtsam/navigation/tests/testImuBias.cpp b/gtsam/navigation/tests/testImuBias.cpp index 81a1a2ceb9..bd6cd7234e 100644 --- a/gtsam/navigation/tests/testImuBias.cpp +++ b/gtsam/navigation/tests/testImuBias.cpp @@ -46,58 +46,6 @@ TEST(ImuBias, Constructor) { Bias bias3(bias2); } -/* ************************************************************************* */ -#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); - Bias biasExpected = Bias(biasAcc1 - biasAcc2, biasGyro1 - biasGyro2); - EXPECT(assert_equal(biasExpected, biasActual)); -} - -TEST(ImuBias, localCoordinates) { - Vector deltaActual = Vector(bias2.localCoordinates(bias1)); - Vector deltaExpected = - (Bias(biasAcc1 - biasAcc2, biasGyro1 - biasGyro2)).vector(); - EXPECT(assert_equal(deltaExpected, deltaActual)); -} - -TEST(ImuBias, retract) { - Vector6 delta; - delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2; - Bias biasActual = bias2.retract(delta); - Bias biasExpected = Bias(biasAcc2 + delta.block<3, 1>(0, 0), - biasGyro2 + delta.block<3, 1>(3, 0)); - 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; - Bias biasActual = bias2.Expmap(delta); - Bias biasExpected = Bias(delta); - EXPECT(assert_equal(biasExpected, biasActual)); -} -#endif - /* ************************************************************************* */ TEST(ImuBias, operatorSub) { Bias biasActual = -bias1; diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index 16d7eff8b2..797f47002a 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -300,55 +300,4 @@ struct traits> : public Testable> {}; // ExpressionFactorN - -#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<>. - */ -template -class GTSAM_DEPRECATED ExpressionFactor2 : public ExpressionFactorN { -public: - /// Destructor - ~ExpressionFactor2() override {} - - /// Backwards compatible evaluateError, to make existing tests compile - Vector evaluateError(const A1& a1, const A2& a2, OptionalMatrixType H1 = OptionalNone, - OptionalMatrixType H2 = OptionalNone) const { - Values values; - values.insert(this->keys_[0], a1); - values.insert(this->keys_[1], a2); - std::vector H(2); - Vector error = ExpressionFactor::unwhitenedError(values, H); - if (H1) (*H1) = H[0]; - if (H2) (*H2) = H[1]; - return error; - } - - - /// Recreate expression from given keys_ and measured_, used in load - /// Needed to deserialize a derived factor - virtual Expression expression(Key key1, Key key2) const { - throw std::runtime_error("ExpressionFactor2::expression not provided: cannot deserialize."); - } - Expression expression(const typename ExpressionFactorN::ArrayNKeys& keys) const override { - return expression(keys[0], keys[1]); - } - -protected: - /// Default constructor, for serialization - ExpressionFactor2() {} - - /// Constructor takes care of keys, but still need to call initialize - ExpressionFactor2(Key key1, Key key2, const SharedNoiseModel& noiseModel, - const T &measurement) - : ExpressionFactorN({key1, key2}, noiseModel, measurement) {} -}; -// ExpressionFactor2 -#endif - } // namespace gtsam diff --git a/gtsam/nonlinear/ExtendedKalmanFilter.h b/gtsam/nonlinear/ExtendedKalmanFilter.h index d76a6ea7e6..a5d6215c10 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter.h @@ -51,12 +51,6 @@ 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 NoiseModelFactorN MotionFactor; - typedef NoiseModelFactorN MeasurementFactor; -#endif - protected: T x_; // linearization point JacobianFactor::shared_ptr priorFactor_; // Gaussian density on x_ diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h index 3c5aa9cabc..41707ea82d 100644 --- a/gtsam/nonlinear/Marginals.h +++ b/gtsam/nonlinear/Marginals.h @@ -129,22 +129,6 @@ class GTSAM_EXPORT Marginals { /** Compute the Bayes Tree as a helper function to the constructor */ void computeBayesTree(const Ordering& ordering); - -public: -#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 */ - 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 */ - 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 3237d7c1e0..cf0f5aa7f5 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -260,39 +260,6 @@ namespace gtsam { ar & boost::serialization::make_nvp("NonlinearFactorGraph", boost::serialization::base_object(*this)); } - - public: - -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 - /// @name Deprecated - /// @{ - /** @deprecated */ - boost::shared_ptr GTSAM_DEPRECATED linearizeToHessianFactor( - const Values& values, boost::none_t, const Dampen& dampen = nullptr) const - {return linearizeToHessianFactor(values, dampen);} - - /** @deprecated */ - Values GTSAM_DEPRECATED updateCholesky(const Values& values, boost::none_t, - const Dampen& dampen = nullptr) const - {return updateCholesky(values, dampen);} - - /** @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 */ - void GTSAM_DEPRECATED - saveGraph(const std::string& filename, const Values& values, - const GraphvizFormatting& graphvizFormatting, - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - saveGraph(filename, values, keyFormatter, graphvizFormatting); - } - /// @} -#endif - }; /// traits diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 283f8d3a0d..4f1577f612 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -107,172 +107,6 @@ namespace gtsam { return result; } -/* ************************************************************************* */ -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 - template - class Values::Filtered { - public: - /** A key-value pair, with the value a specific derived Value type. */ - typedef _ValuesKeyValuePair KeyValuePair; - typedef _ValuesConstKeyValuePair ConstKeyValuePair; - typedef KeyValuePair value_type; - - typedef - boost::transform_iterator< - KeyValuePair(*)(Values::KeyValuePair), - boost::filter_iterator< - std::function, - Values::iterator> > - iterator; - - typedef iterator const_iterator; - - typedef - boost::transform_iterator< - ConstKeyValuePair(*)(Values::ConstKeyValuePair), - boost::filter_iterator< - std::function, - Values::const_iterator> > - const_const_iterator; - - iterator begin() { return begin_; } - iterator end() { return end_; } - const_iterator begin() const { return begin_; } - const_iterator end() const { return end_; } - const_const_iterator beginConst() const { return constBegin_; } - const_const_iterator endConst() const { return constEnd_; } - - /** Returns the number of values in this view */ - size_t size() const { - size_t i = 0; - for (const_const_iterator it = beginConst(); it != endConst(); ++it) - ++i; - return i; - } - - private: - Filtered( - const std::function& filter, - Values& values) : - begin_( - boost::make_transform_iterator( - boost::make_filter_iterator(filter, values.begin(), values.end()), - &ValuesCastHelper::cast)), end_( - boost::make_transform_iterator( - boost::make_filter_iterator(filter, values.end(), values.end()), - &ValuesCastHelper::cast)), constBegin_( - boost::make_transform_iterator( - boost::make_filter_iterator(filter, - ((const Values&) values).begin(), - ((const Values&) values).end()), - &ValuesCastHelper::cast)), constEnd_( - boost::make_transform_iterator( - boost::make_filter_iterator(filter, - ((const Values&) values).end(), - ((const Values&) values).end()), - &ValuesCastHelper::cast)) { - } - - friend class Values; - iterator begin_; - iterator end_; - const_const_iterator constBegin_; - const_const_iterator constEnd_; - }; - - template - class Values::ConstFiltered { - public: - /** A const key-value pair, with the value a specific derived Value type. */ - typedef _ValuesConstKeyValuePair KeyValuePair; - typedef KeyValuePair value_type; - - typedef typename Filtered::const_const_iterator iterator; - typedef typename Filtered::const_const_iterator const_iterator; - - /** Conversion from Filtered to ConstFiltered */ - ConstFiltered(const Filtered& rhs) : - begin_(rhs.beginConst()), - end_(rhs.endConst()) {} - - iterator begin() { return begin_; } - iterator end() { return end_; } - const_iterator begin() const { return begin_; } - const_iterator end() const { return end_; } - - /** Returns the number of values in this view */ - size_t size() const { - size_t i = 0; - for (const_iterator it = begin(); it != end(); ++it) - ++i; - return i; - } - - FastList keys() const { - FastList result; - for(const_iterator it = begin(); it != end(); ++it) - result.push_back(it->key); - return result; - } - - private: - friend class Values; - const_iterator begin_; - const_iterator end_; - ConstFiltered( - const std::function& filter, - const Values& values) { - // We remove the const from values to create a non-const Filtered - // view, then pull the const_iterators out of it. - const Filtered filtered(filter, const_cast(values)); - begin_ = filtered.beginConst(); - end_ = filtered.endConst(); - } - }; - - template - Values::Values(const Values::Filtered& view) { - for(const auto key_value: view) { - Key key = key_value.key; - insert(key, static_cast(key_value.value)); - } - } - - template - Values::Values(const Values::ConstFiltered& view) { - for(const auto key_value: view) { - Key key = key_value.key; - insert(key, static_cast(key_value.value)); - } - } - - Values::Filtered - inline Values::filter(const std::function& filterFcn) { - return filter(filterFcn); - } - - template - Values::Filtered - Values::filter(const std::function& filterFcn) { - return Filtered(std::bind(&filterHelper, filterFcn, - std::placeholders::_1), *this); - } - - Values::ConstFiltered - inline Values::filter(const std::function& filterFcn) const { - return filter(filterFcn); - } - - template - Values::ConstFiltered - Values::filter(const std::function& filterFcn) const { - return ConstFiltered(std::bind(&filterHelper, - filterFcn, std::placeholders::_1), *this); - } -#endif - /* ************************************************************************* */ template<> inline bool Values::filterHelper(const std::function filter, diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index d75967eef8..4ea3309528 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -31,9 +31,6 @@ #include #include #include -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 -#include -#endif #include #include @@ -342,42 +339,6 @@ namespace gtsam { std::map // , std::less, Eigen::aligned_allocator extract(const std::function& filterFcn = &_truePredicate) const; -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 - /** A filtered view of a Values, returned from Values::filter. */ - template - class Filtered; - - /** A filtered view of a const Values, returned from Values::filter. */ - template - class ConstFiltered; - - /** Constructor from a Filtered view copies out all values */ - template - Values(const Filtered& view); - - /** Constructor from a Filtered or ConstFiltered view */ - template - Values(const ConstFiltered& view); - - /// A filtered view of the original Values class. - Filtered GTSAM_DEPRECATED - filter(const std::function& filterFcn); - - /// A filtered view of the original Values class, also filter on type. - template - Filtered GTSAM_DEPRECATED - filter(const std::function& filterFcn = &_truePredicate); - - /// A filtered view of the original Values class, const version. - ConstFiltered GTSAM_DEPRECATED - filter(const std::function& filterFcn) const; - - /// A filtered view of the original Values class, also on type, const. - template - ConstFiltered GTSAM_DEPRECATED filter( - const std::function& filterFcn = &_truePredicate) const; -#endif - private: // Filters based on ValueType (if not Value) and also based on the user- // supplied \c filter function. diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index ee7fcd1897..2db5d83ff4 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -343,68 +343,6 @@ TEST(Values, filter) { values.insert(2, pose2); values.insert(3, pose3); -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 - // Filter by key - int i = 0; - Values::Filtered filtered = values.filter(std::bind(std::greater_equal(), std::placeholders::_1, 2)); - EXPECT_LONGS_EQUAL(2, (long)filtered.size()); - for(const auto key_value: filtered) { - if(i == 0) { - LONGS_EQUAL(2, (long)key_value.key); - try {key_value.value.cast();} catch (const std::bad_cast& e) { FAIL("can't cast Value to Pose2");} - THROWS_EXCEPTION(key_value.value.cast()); - EXPECT(assert_equal(pose2, key_value.value.cast())); - } else if(i == 1) { - LONGS_EQUAL(3, (long)key_value.key); - try {key_value.value.cast();} catch (const std::bad_cast& e) { FAIL("can't cast Value to Pose3");} - THROWS_EXCEPTION(key_value.value.cast()); - EXPECT(assert_equal(pose3, key_value.value.cast())); - } else { - EXPECT(false); - } - ++ i; - } - EXPECT_LONGS_EQUAL(2, (long)i); - - // construct a values with the view - Values actualSubValues1(filtered); - Values expectedSubValues1; - expectedSubValues1.insert(2, pose2); - expectedSubValues1.insert(3, pose3); - EXPECT(assert_equal(expectedSubValues1, actualSubValues1)); - - // ConstFilter by Key - Values::ConstFiltered constfiltered = values.filter(std::bind(std::greater_equal(), std::placeholders::_1, 2)); - EXPECT_LONGS_EQUAL(2, (long)constfiltered.size()); - Values fromconstfiltered(constfiltered); - EXPECT(assert_equal(expectedSubValues1, fromconstfiltered)); - - // Filter by type - i = 0; - Values::ConstFiltered pose_filtered = values.filter(); - EXPECT_LONGS_EQUAL(2, (long)pose_filtered.size()); - for(const auto key_value: pose_filtered) { - if(i == 0) { - EXPECT_LONGS_EQUAL(1, (long)key_value.key); - EXPECT(assert_equal(pose1, key_value.value)); - } else if(i == 1) { - EXPECT_LONGS_EQUAL(3, (long)key_value.key); - EXPECT(assert_equal(pose3, key_value.value)); - } else { - EXPECT(false); - } - ++ i; - } - EXPECT_LONGS_EQUAL(2, (long)i); - - // construct a values with the view - Values actualSubValues2(pose_filtered); - Values expectedSubValues2; - expectedSubValues2.insert(1, pose1); - expectedSubValues2.insert(3, pose3); - EXPECT(assert_equal(expectedSubValues2, actualSubValues2)); -#endif - // Test counting by type. EXPECT_LONGS_EQUAL(2, (long)values.count()); EXPECT_LONGS_EQUAL(2, (long)values.count()); @@ -412,7 +350,6 @@ TEST(Values, filter) { // Filter by type using extract. auto extracted_pose3s = values.extract(); EXPECT_LONGS_EQUAL(2, (long)extracted_pose3s.size()); - } /* ************************************************************************* */ @@ -428,27 +365,9 @@ TEST(Values, Symbol_filter) { values.insert(X(2), pose2); values.insert(Symbol('y', 3), pose3); -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 - int i = 0; - for(const auto key_value: values.filter(Symbol::ChrTest('y'))) { - if(i == 0) { - LONGS_EQUAL(Symbol('y', 1), (long)key_value.key); - EXPECT(assert_equal(pose1, key_value.value.cast())); - } else if(i == 1) { - LONGS_EQUAL(Symbol('y', 3), (long)key_value.key); - EXPECT(assert_equal(pose3, key_value.value.cast())); - } else { - EXPECT(false); - } - ++ i; - } - LONGS_EQUAL(2, (long)i); -#endif - -// Test extract with filter on symbol: + // Test extract with filter on symbol: auto extracted_pose3s = values.extract(Symbol::ChrTest('y')); EXPECT_LONGS_EQUAL(2, (long)extracted_pose3s.size()); - } /* ************************************************************************* */ diff --git a/gtsam/sfm/SfmData.cpp b/gtsam/sfm/SfmData.cpp index 6c2676e487..246c65c4a2 100644 --- a/gtsam/sfm/SfmData.cpp +++ b/gtsam/sfm/SfmData.cpp @@ -322,25 +322,6 @@ bool writeBAL(const std::string &filename, const SfmData &data) { return true; } -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 -bool readBundler(const std::string &filename, SfmData &data) { - try { - data = SfmData::FromBundlerFile(filename); - return true; - } catch (const std::exception & /* e */) { - return false; - } -} -bool readBAL(const std::string &filename, SfmData &data) { - try { - data = SfmData::FromBalFile(filename); - return true; - } catch (const std::exception & /* e */) { - return false; - } -} -#endif - SfmData readBal(const std::string &filename) { return SfmData::FromBalFile(filename); } diff --git a/gtsam/sfm/SfmData.h b/gtsam/sfm/SfmData.h index f445d74da8..cc9bd86f6e 100644 --- a/gtsam/sfm/SfmData.h +++ b/gtsam/sfm/SfmData.h @@ -122,17 +122,6 @@ struct GTSAM_EXPORT SfmData { bool equals(const SfmData& sfmData, double tol = 1e-9) const; /// @} -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 - /// @name Deprecated - /// @{ - void GTSAM_DEPRECATED add_track(const SfmTrack& t) { tracks.push_back(t); } - void GTSAM_DEPRECATED add_camera(const SfmCamera& cam) { - cameras.push_back(cam); - } - size_t GTSAM_DEPRECATED number_tracks() const { return tracks.size(); } - size_t GTSAM_DEPRECATED number_cameras() const { return cameras.size(); } - /// @} -#endif /// @name Serialization /// @{ @@ -151,13 +140,6 @@ struct GTSAM_EXPORT SfmData { template <> struct traits : public Testable {}; -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 -GTSAM_EXPORT bool GTSAM_DEPRECATED readBundler(const std::string& filename, - SfmData& data); -GTSAM_EXPORT bool GTSAM_DEPRECATED readBAL(const std::string& filename, - SfmData& data); -#endif - /** * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and * returns the data as a SfmData structure. Mainly used by wrapped code. diff --git a/gtsam/sfm/SfmTrack.h b/gtsam/sfm/SfmTrack.h index c75199374f..60699bc314 100644 --- a/gtsam/sfm/SfmTrack.h +++ b/gtsam/sfm/SfmTrack.h @@ -158,18 +158,6 @@ struct GTSAM_EXPORT SfmTrack : SfmTrack2d { bool equals(const SfmTrack& sfmTrack, double tol = 1e-9) const; /// @} -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 - /// @name Deprecated - /// @{ - void GTSAM_DEPRECATED add_measurement(size_t idx, const gtsam::Point2& m) { - measurements.emplace_back(idx, m); - } - - size_t GTSAM_DEPRECATED number_measurements() const { - return measurements.size(); - } - /// @} -#endif /// @name Serialization /// @{ diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index db79fcd3c0..5eaa007202 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -962,15 +962,4 @@ parse3DFactors(const std::string &filename, return parseFactors(filename, model, 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 GTSAM_DEPRECATED -parse3DLandmarks(const std::string &filename, size_t maxIndex) { - return parseVariables(filename, maxIndex); -} -#endif } // namespace gtsam diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 95e750674d..fcfe4b96c7 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -225,21 +225,4 @@ parse3DFactors(const std::string &filename, using BinaryMeasurementsUnit3 = std::vector>; using BinaryMeasurementsPoint3 = std::vector>; using BinaryMeasurementsRot3 = std::vector>; - -#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 GTSAM_DEPRECATED -parse3DPoses(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/examples/ConcurrentCalibration.cpp b/gtsam_unstable/examples/ConcurrentCalibration.cpp index 95629fb43b..75dc49eeec 100644 --- a/gtsam_unstable/examples/ConcurrentCalibration.cpp +++ b/gtsam_unstable/examples/ConcurrentCalibration.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include #include @@ -35,12 +36,15 @@ using namespace std; using namespace gtsam; +using symbol_shorthand::K; +using symbol_shorthand::L; +using symbol_shorthand::X; int main(int argc, char** argv){ Values initial_estimate; NonlinearFactorGraph graph; - const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2,1); + const auto model = noiseModel::Isotropic::Sigma(2,1); string calibration_loc = findExampleDataFile("VO_calibration00s.txt"); string pose_loc = findExampleDataFile("VO_camera_poses00s.txt"); @@ -54,13 +58,13 @@ int main(int argc, char** argv){ calibration_file >> fx >> fy >> s >> u0 >> v0 >> b; //create stereo camera calibration object - const Cal3_S2::shared_ptr K(new Cal3_S2(fx,fy,s,u0,v0)); - const Cal3_S2::shared_ptr noisy_K(new Cal3_S2(fx*1.2,fy*1.2,s,u0-10,v0+10)); + const Cal3_S2 true_K(fx,fy,s,u0,v0); + const Cal3_S2 noisy_K(fx*1.2,fy*1.2,s,u0-10,v0+10); - initial_estimate.insert(Symbol('K', 0), *noisy_K); + initial_estimate.insert(K(0), noisy_K); - noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 1e-5, 100, 100).finished()); - graph.addPrior(Symbol('K', 0), *noisy_K, calNoise); + auto calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 1e-5, 100, 100).finished()); + graph.addPrior(K(0), noisy_K, calNoise); ifstream pose_file(pose_loc.c_str()); @@ -75,7 +79,7 @@ int main(int argc, char** argv){ initial_estimate.insert(Symbol('x', pose_id), Pose3(m)); } - noiseModel::Isotropic::shared_ptr poseNoise = noiseModel::Isotropic::Sigma(6, 0.01); + auto poseNoise = noiseModel::Isotropic::Sigma(6, 0.01); graph.addPrior(Symbol('x', pose_id), Pose3(m), poseNoise); // camera and landmark keys @@ -83,22 +87,22 @@ int main(int argc, char** argv){ // pixel coordinates uL, uR, v (same for left/right images due to rectification) // landmark coordinates X, Y, Z in camera frame, resulting from triangulation - double uL, uR, v, X, Y, Z; + double uL, uR, v, _X, Y, Z; ifstream factor_file(factor_loc.c_str()); cout << "Reading stereo factors" << endl; //read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation - while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) { -// graph.emplace_shared >(StereoPoint2(uL, uR, v), model, Symbol('x', x), Symbol('l', l), K); + while (factor_file >> x >> l >> uL >> uR >> v >> _X >> Y >> Z) { +// graph.emplace_shared >(StereoPoint2(uL, uR, v), model, X(x), L(l), K); - graph.emplace_shared >(Point2(uL,v), model, Symbol('x', x), Symbol('l', l), Symbol('K', 0)); + graph.emplace_shared >(Point2(uL,v), model, X(x), L(l), K(0)); //if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it - if (!initial_estimate.exists(Symbol('l', l))) { - Pose3 camPose = initial_estimate.at(Symbol('x', x)); + if (!initial_estimate.exists(L(l))) { + Pose3 camPose = initial_estimate.at(X(x)); //transformFrom() transforms the input Point3 from the camera pose space, camPose, to the global space - Point3 worldPoint = camPose.transformFrom(Point3(X, Y, Z)); - initial_estimate.insert(Symbol('l', l), worldPoint); + Point3 worldPoint = camPose.transformFrom(Point3(_X, Y, Z)); + initial_estimate.insert(L(l), worldPoint); } } @@ -123,7 +127,7 @@ int main(int argc, char** argv){ double currentError; - stream_K << optimizer.iterations() << " " << optimizer.values().at(Symbol('K',0)).vector().transpose() << endl; + stream_K << optimizer.iterations() << " " << optimizer.values().at(K(0)).vector().transpose() << endl; // Iterative loop @@ -132,7 +136,7 @@ int main(int argc, char** argv){ currentError = optimizer.error(); optimizer.iterate(); - stream_K << optimizer.iterations() << " " << optimizer.values().at(Symbol('K',0)).vector().transpose() << endl; + stream_K << optimizer.iterations() << " " << optimizer.values().at(K(0)).vector().transpose() << endl; if(params.verbosity >= NonlinearOptimizerParams::ERROR) cout << "newError: " << optimizer.error() << endl; } while(optimizer.iterations() < params.maxIterations && @@ -142,13 +146,13 @@ int main(int argc, char** argv){ Values result = optimizer.values(); cout << "Final result sample:" << endl; - Values pose_values = result.filter(); + Values pose_values = utilities::allPose3s(result); pose_values.print("Final camera poses:\n"); - Values(result.filter()).print("Final K\n"); + result.at(K(0)).print("Final K\n"); - noisy_K->print("Initial noisy K\n"); - K->print("Initial correct K\n"); + noisy_K.print("Initial noisy K\n"); + true_K.print("Initial correct K\n"); return 0; } diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp index e290cef7e5..b3b04cca32 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp @@ -30,6 +30,7 @@ #include #include #include +#include #include #include #include @@ -114,7 +115,7 @@ int main(int argc, char** argv){ Values result = optimizer.optimize(); cout << "Final result sample:" << endl; - Values pose_values = result.filter(); + Values pose_values = utilities::allPose3s(result); pose_values.print("Final camera poses:\n"); return 0; diff --git a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp index 64afa80309..2fccf6b18d 100644 --- a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp +++ b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp @@ -234,9 +234,8 @@ int main(int argc, char** argv) { } } countK = 0; - for(const auto it: result.filter()) - os2 << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t1" - << endl; + for (const auto& [key, point] : result.extract()) + os2 << key << "\t" << point.x() << "\t" << point.y() << "\t1" << endl; if (smart) { for(size_t jj: ids) { Point2 landmark = smartFactors[jj]->triangulate(result); @@ -257,9 +256,8 @@ int main(int argc, char** argv) { // Write result to file Values result = isam.calculateEstimate(); ofstream os("rangeResult.txt"); - for(const auto it: result.filter()) - os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t" - << it.value.theta() << endl; + for (const auto& [key, pose] : result.extract()) + os << key << "\t" << pose.x() << "\t" << pose.y() << "\t" << pose.theta() << endl; exit(0); } diff --git a/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp b/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp index 95aff85a7f..1e6f77b31e 100644 --- a/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp +++ b/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp @@ -202,13 +202,11 @@ int main(int argc, char** argv) { // Write result to file Values result = isam.calculateEstimate(); ofstream os2("rangeResultLM.txt"); - for(const auto it: result.filter()) - os2 << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t1" - << endl; + for (const auto& [key, point] : result.extract()) + os2 << key << "\t" << point.x() << "\t" << point.y() << "\t1" << endl; ofstream os("rangeResult.txt"); - for(const auto it: result.filter()) - os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t" - << it.value.theta() << endl; + for (const auto& [key, pose] : result.extract()) + os << key << "\t" << pose.x() << "\t" << pose.y() << "\t" << pose.theta() << endl; exit(0); } diff --git a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp index 2426ca2012..804e4cf5b8 100644 --- a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp @@ -29,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -43,6 +44,7 @@ using namespace std; using namespace gtsam; +using symbol_shorthand::X; int main(int argc, char** argv){ @@ -84,16 +86,16 @@ int main(int argc, char** argv){ if(add_initial_noise){ m(1,3) += (pose_id % 10)/10.0; } - initial_estimate.insert(Symbol('x', pose_id), Pose3(m)); + initial_estimate.insert(X(pose_id), Pose3(m)); } - Values initial_pose_values = initial_estimate.filter(); + const auto initialPoses = initial_estimate.extract(); if (output_poses) { init_pose3Out.open(init_poseOutput.c_str(), ios::out); - for (size_t i = 1; i <= initial_pose_values.size(); i++) { + for (size_t i = 1; i <= initialPoses.size(); i++) { init_pose3Out << i << " " - << initial_pose_values.at(Symbol('x', i)).matrix().format( + << initialPoses.at(X(i)).matrix().format( Eigen::IOFormat(Eigen::StreamPrecision, 0, " ", " ")) << endl; } } @@ -103,7 +105,7 @@ int main(int argc, char** argv){ // pixel coordinates uL, uR, v (same for left/right images due to rectification) // landmark coordinates X, Y, Z in camera frame, resulting from triangulation - double uL, uR, v, X, Y, Z; + double uL, uR, v, _X, Y, Z; ifstream factor_file(factor_loc.c_str()); cout << "Reading stereo factors" << endl; @@ -112,21 +114,21 @@ int main(int argc, char** argv){ SmartFactor::shared_ptr factor(new SmartFactor(model)); size_t current_l = 3; // hardcoded landmark ID from first measurement - while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) { + while (factor_file >> x >> l >> uL >> uR >> v >> _X >> Y >> Z) { if(current_l != l) { graph.push_back(factor); factor = SmartFactor::shared_ptr(new SmartFactor(model)); current_l = l; } - factor->add(StereoPoint2(uL,uR,v), Symbol('x',x), K); + factor->add(StereoPoint2(uL,uR,v), X(x), K); } - Pose3 first_pose = initial_estimate.at(Symbol('x',1)); + Pose3 first_pose = initial_estimate.at(X(1)); //constrain the first pose such that it cannot change from its original value during optimization // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky // QR is much slower than Cholesky, but numerically more stable - graph.emplace_shared >(Symbol('x',1),first_pose); + graph.emplace_shared >(X(1),first_pose); LevenbergMarquardtParams params; params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; @@ -138,13 +140,13 @@ int main(int argc, char** argv){ Values result = optimizer.optimize(); cout << "Final result sample:" << endl; - Values pose_values = result.filter(); + Values pose_values = utilities::allPose3s(result); pose_values.print("Final camera poses:\n"); if(output_poses){ pose3Out.open(poseOutput.c_str(),ios::out); for(size_t i = 1; i<=pose_values.size(); i++){ - pose3Out << i << " " << pose_values.at(Symbol('x',i)).matrix().format(Eigen::IOFormat(Eigen::StreamPrecision, 0, + pose3Out << i << " " << pose_values.at(X(i)).matrix().format(Eigen::IOFormat(Eigen::StreamPrecision, 0, " ", " ")) << endl; } cout << "Writing output" << endl; diff --git a/gtsam_unstable/slam/serialization.cpp b/gtsam_unstable/slam/serialization.cpp index d87ca6f2d3..4b1ecae54d 100644 --- a/gtsam_unstable/slam/serialization.cpp +++ b/gtsam_unstable/slam/serialization.cpp @@ -173,22 +173,6 @@ BOOST_CLASS_EXPORT_GUID(GeneralSFMFactor2Cal3_S2, "gtsam::GeneralSFMFactor2Cal3_ BOOST_CLASS_EXPORT_GUID(GenericStereoFactor3D, "gtsam::GenericStereoFactor3D"); -#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 - -typedef PriorFactor PriorFactorSimpleCamera; -typedef NonlinearEquality NonlinearEqualitySimpleCamera; -typedef RangeFactor RangeFactorSimpleCameraPoint; -typedef RangeFactor RangeFactorSimpleCamera; - -GTSAM_VALUE_EXPORT(gtsam::SimpleCamera); -BOOST_CLASS_EXPORT_GUID(PriorFactorSimpleCamera, "gtsam::PriorFactorSimpleCamera"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualitySimpleCamera, "gtsam::NonlinearEqualitySimpleCamera"); -BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCameraPoint, "gtsam::RangeFactorSimpleCameraPoint"); -BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCamera, "gtsam::RangeFactorSimpleCamera"); - -#endif - - /* ************************************************************************* */ // Actual implementations of functions /* ************************************************************************* */ diff --git a/timing/timeLago.cpp b/timing/timeLago.cpp index c3ee6ff4bc..bb506de36e 100644 --- a/timing/timeLago.cpp +++ b/timing/timeLago.cpp @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file timeVirtual.cpp - * @brief Time the overhead of using virtual destructors and methods + * @file timeLago.cpp + * @brief Time the LAGO initialization method * @author Richard Roberts * @date Dec 3, 2010 */ @@ -36,16 +36,15 @@ int main(int argc, char *argv[]) { Values::shared_ptr solution; NonlinearFactorGraph::shared_ptr g; string inputFile = findExampleDataFile("w10000"); - SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0).finished()); + auto model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0).finished()); boost::tie(g, solution) = load2D(inputFile, model); // add noise to create initial estimate Values initial; - Values::ConstFiltered poses = solution->filter(); - SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 15.0 * M_PI / 180.0).finished()); + auto noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 15.0 * M_PI / 180.0).finished()); Sampler sampler(noise); - for(const Values::ConstFiltered::KeyValuePair& it: poses) - initial.insert(it.key, it.value.retract(sampler.sample())); + for(const auto& [key,pose]: solution->extract()) + initial.insert(key, pose.retract(sampler.sample())); // Add prior on the pose having index (key) = 0 noiseModel::Diagonal::shared_ptr priorModel = //