diff --git a/.github/workflows/build-special.yml b/.github/workflows/build-special.yml index 211ef3cbd..7582bf41c 100644 --- a/.github/workflows/build-special.yml +++ b/.github/workflows/build-special.yml @@ -15,7 +15,7 @@ jobs: BOOST_VERSION: 1.67.0 strategy: - fail-fast: true + fail-fast: false matrix: # Github Actions requires a single row to be added to the build matrix. @@ -27,6 +27,7 @@ jobs: ubuntu-clang-tbb, ubuntu-clang-cayleymap, ubuntu-clang-system-libs, + ubuntu-no-boost, ] build_type: [Debug, Release] @@ -62,6 +63,12 @@ jobs: version: "14" flag: system + - name: ubuntu-no-boost + os: ubuntu-22.04 + compiler: clang + version: "14" + flag: no_boost + steps: - name: Checkout uses: actions/checkout@v3 @@ -137,6 +144,13 @@ jobs: # sudo apt-get install metis # echo "GTSAM_USE_SYSTEM_METIS=ON" >> $GITHUB_ENV + - name: Turn off boost + if: matrix.flag == 'no_boost' + run: | + echo "GTSAM_ENABLE_BOOST_SERIALIZATION=OFF" >> $GITHUB_ENV + echo "GTSAM_USE_BOOST_FEATURES=OFF" >> $GITHUB_ENV + echo "GTSAM will not use BOOST" + - name: Build & Test run: | bash .github/scripts/unix.sh -t diff --git a/CMakeLists.txt b/CMakeLists.txt index 52b34101f..ebe27443a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -50,8 +50,24 @@ endif() include(cmake/HandleGeneralOptions.cmake) # CMake build options -# Libraries: -include(cmake/HandleBoost.cmake) # Boost +############### Decide on BOOST ###################################### +# Enable or disable serialization with GTSAM_ENABLE_BOOST_SERIALIZATION +option(GTSAM_ENABLE_BOOST_SERIALIZATION "Enable Boost serialization" ON) +if(GTSAM_ENABLE_BOOST_SERIALIZATION) + add_definitions(-DGTSAM_ENABLE_BOOST_SERIALIZATION) +endif() + +option(GTSAM_USE_BOOST_FEATURES "Enable Features that use Boost" ON) +if(GTSAM_USE_BOOST_FEATURES) + add_definitions(-DGTSAM_USE_BOOST_FEATURES) +endif() + +if(GTSAM_ENABLE_BOOST_SERIALIZATION OR GTSAM_USE_BOOST_FEATURES) +include(cmake/HandleBoost.cmake) +endif() +###################################################################### + +# Other Libraries: include(cmake/HandleCCache.cmake) # ccache include(cmake/HandleCPack.cmake) # CPack include(cmake/HandleEigen.cmake) # Eigen3 diff --git a/CppUnitLite/CMakeLists.txt b/CppUnitLite/CMakeLists.txt index ab884ec1d..cbffa79d1 100644 --- a/CppUnitLite/CMakeLists.txt +++ b/CppUnitLite/CMakeLists.txt @@ -6,7 +6,6 @@ file(GLOB cppunitlite_src "*.cpp") add_library(CppUnitLite STATIC ${cppunitlite_src} ${cppunitlite_headers}) list(APPEND GTSAM_EXPORTED_TARGETS CppUnitLite) set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) -target_link_libraries(CppUnitLite PUBLIC Boost::boost) # boost/lexical_cast.h gtsam_assign_source_folders("${cppunitlite_headers};${cppunitlite_src}") # MSVC project structure diff --git a/CppUnitLite/Test.cpp b/CppUnitLite/Test.cpp index 78995a219..f5bea8819 100644 --- a/CppUnitLite/Test.cpp +++ b/CppUnitLite/Test.cpp @@ -15,8 +15,6 @@ #include "TestResult.h" #include "Failure.h" -#include - Test::Test (const std::string& testName) : name_ (testName), next_(0), lineNumber_(-1), safeCheck_(true) { @@ -47,10 +45,10 @@ bool Test::check(long expected, long actual, TestResult& result, const std::stri result.addFailure ( Failure ( name_, - boost::lexical_cast (__FILE__), + std::string(__FILE__), __LINE__, - boost::lexical_cast (expected), - boost::lexical_cast (actual))); + std::to_string(expected), + std::to_string(actual))); return false; @@ -64,7 +62,7 @@ bool Test::check(const std::string& expected, const std::string& actual, TestRes result.addFailure ( Failure ( name_, - boost::lexical_cast (__FILE__), + std::string(__FILE__), __LINE__, expected, actual)); diff --git a/CppUnitLite/Test.h b/CppUnitLite/Test.h index a898c83ef..422427251 100644 --- a/CppUnitLite/Test.h +++ b/CppUnitLite/Test.h @@ -23,7 +23,7 @@ #include -#include +#include class TestResult; @@ -112,17 +112,17 @@ protected: #define THROWS_EXCEPTION(condition)\ { try { condition; \ - result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Didn't throw: ") + boost::lexical_cast(#condition))); \ + result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Didn't throw: ") + std::string(#condition))); \ return; } \ catch (...) {} } #define CHECK_EXCEPTION(condition, exception_name)\ { try { condition; \ - result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Didn't throw: ") + boost::lexical_cast(#condition))); \ + result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Didn't throw: ") + std::string(#condition))); \ return; } \ catch (exception_name&) {} \ catch (...) { \ - result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Wrong exception: ") + boost::lexical_cast(#condition) + boost::lexical_cast(", expected: ") + boost::lexical_cast(#exception_name))); \ + result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Wrong exception: ") + std::string(#condition) + std::string(", expected: ") + std::string(#exception_name))); \ return; } } #define EQUALITY(expected,actual)\ @@ -130,21 +130,21 @@ protected: result_.addFailure(Failure(name_, __FILE__, __LINE__, #expected, #actual)); } #define CHECK_EQUAL(expected,actual)\ -{ if ((expected) == (actual)) return; result_.addFailure(Failure(name_, __FILE__, __LINE__, boost::lexical_cast(expected), boost::lexical_cast(actual))); } +{ if ((expected) == (actual)) return; result_.addFailure(Failure(name_, __FILE__, __LINE__, std::to_string(expected), std::to_string(actual))); } #define LONGS_EQUAL(expected,actual)\ { long actualTemp = actual; \ long expectedTemp = expected; \ if ((expectedTemp) != (actualTemp)) \ -{ result_.addFailure (Failure (name_, __FILE__, __LINE__, boost::lexical_cast(expectedTemp), \ -boost::lexical_cast(actualTemp))); return; } } +{ result_.addFailure (Failure (name_, __FILE__, __LINE__, std::to_string(expectedTemp), \ +std::to_string(actualTemp))); return; } } #define DOUBLES_EQUAL(expected,actual,threshold)\ { double actualTemp = actual; \ double expectedTemp = expected; \ if (!std::isfinite(actualTemp) || !std::isfinite(expectedTemp) || fabs ((expectedTemp)-(actualTemp)) > threshold) \ { result_.addFailure (Failure (name_, __FILE__, __LINE__, \ -boost::lexical_cast((double)expectedTemp), boost::lexical_cast((double)actualTemp))); return; } } +std::to_string((double)expectedTemp), std::to_string((double)actualTemp))); return; } } /* EXPECTs: tests will continue running after a failure */ @@ -156,15 +156,15 @@ boost::lexical_cast((double)expectedTemp), boost::lexical_cast(expectedTemp), \ -boost::lexical_cast(actualTemp))); } } +{ result_.addFailure (Failure (name_, __FILE__, __LINE__, std::to_string(expectedTemp), \ +std::to_string(actualTemp))); } } #define EXPECT_DOUBLES_EQUAL(expected,actual,threshold)\ { double actualTemp = actual; \ double expectedTemp = expected; \ if (!std::isfinite(actualTemp) || !std::isfinite(expectedTemp) || fabs ((expectedTemp)-(actualTemp)) > threshold) \ { result_.addFailure (Failure (name_, __FILE__, __LINE__, \ -boost::lexical_cast((double)expectedTemp), boost::lexical_cast((double)actualTemp))); } } +std::to_string((double)expectedTemp), std::to_string((double)actualTemp))); } } #define FAIL(text) \ diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md index 953357ede..9911b3764 100644 --- a/GTSAM-Concepts.md +++ b/GTSAM-Concepts.md @@ -166,7 +166,7 @@ Concept Checks Boost provides a nice way to check whether a given type satisfies a concept. For example, the following - BOOST_CONCEPT_ASSERT(IsVectorSpace) + GTSAM_CONCEPT_ASSERT(IsVectorSpace) asserts that Point2 indeed is a model for the VectorSpace concept. diff --git a/cmake/example_cmake_find_gtsam/CMakeLists.txt b/cmake/example_cmake_find_gtsam/CMakeLists.txt index 9a4be4d70..d020f7032 100644 --- a/cmake/example_cmake_find_gtsam/CMakeLists.txt +++ b/cmake/example_cmake_find_gtsam/CMakeLists.txt @@ -12,6 +12,6 @@ add_executable(example ) # By using CMake exported targets, a simple "link" dependency introduces the -# include directories (-I) flags, links against Boost, and add any other +# include directories (-I) flags, and add any other # required build flags (e.g. C++11, etc.) target_link_libraries(example PRIVATE gtsam) diff --git a/cmake/obsolete/GtsamTestingObsolete.cmake b/cmake/obsolete/GtsamTestingObsolete.cmake index c90abfa6c..be9de93bd 100644 --- a/cmake/obsolete/GtsamTestingObsolete.cmake +++ b/cmake/obsolete/GtsamTestingObsolete.cmake @@ -2,7 +2,7 @@ # Macro for adding categorized tests in a "tests" folder, with # optional exclusion of tests and convenience library linking options # -# By default, all tests are linked with CppUnitLite and boost +# By default, all tests are linked with CppUnitLite # Arguments: # - subdir The name of the category for this test # - local_libs A list of convenience libraries to use (if GTSAM_BUILD_CONVENIENCE_LIBRARIES is true) @@ -32,7 +32,6 @@ endfunction() # Macro for adding categorized timing scripts in a "tests" folder, with # optional exclusion of tests and convenience library linking options # -# By default, all tests are linked with boost # Arguments: # - subdir The name of the category for this timing script # - local_libs A list of convenience libraries to use (if GTSAM_BUILD_CONVENIENCE_LIBRARIES is true) @@ -51,8 +50,7 @@ macro(gtsam_add_subdir_timing subdir local_libs full_libs excluded_srcs) endmacro() # Macro for adding executables matching a pattern - builds one executable for -# each file matching the pattern. These exectuables are automatically linked -# with boost. +# each file matching the pattern. # Arguments: # - pattern The glob pattern to match source files # - local_libs A list of convenience libraries to use (if GTSAM_BUILD_CONVENIENCE_LIBRARIES is true) @@ -138,9 +136,9 @@ macro(gtsam_add_grouped_scripts group pattern target_prefix pretty_prefix_name l # Linking and dependendencies if (GTSAM_BUILD_CONVENIENCE_LIBRARIES) - target_link_libraries(${script_bin} ${local_libs} ${GTSAM_BOOST_LIBRARIES}) + target_link_libraries(${script_bin} ${local_libs}) else() - target_link_libraries(${script_bin} ${full_libs} ${GTSAM_BOOST_LIBRARIES}) + target_link_libraries(${script_bin} ${full_libs}) endif() # Add .run target diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 1584a3a35..4b4a81c59 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -1,13 +1,23 @@ set (excluded_examples - elaboratePoint2KalmanFilter.cpp + "elaboratePoint2KalmanFilter.cpp" ) # if GTSAM_ENABLE_BOOST_SERIALIZATION is not set then SolverComparer.cpp will not compile if (NOT GTSAM_ENABLE_BOOST_SERIALIZATION) - set (excluded_examples - ${excluded_examples} - SolverComparer.cpp + list (APPEND excluded_examples + "SolverComparer.cpp" ) endif() +# Add examples to exclude if GTSAM_USE_BOOST_FEATURES is not set +if (NOT GTSAM_USE_BOOST_FEATURES) + # add to excluded examples + list (APPEND excluded_examples + "CombinedImuFactorsExample.cpp" + "ImuFactorsExample.cpp" + "ShonanAveragingCLI.cpp" + "SolverComparer.cpp" + ) +endif() + gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam;gtsam_unstable;${Boost_PROGRAM_OPTIONS_LIBRARY}") diff --git a/examples/DiscreteBayesNetExample.cpp b/examples/DiscreteBayesNetExample.cpp index dfd7beb63..09ee8baa6 100644 --- a/examples/DiscreteBayesNetExample.cpp +++ b/examples/DiscreteBayesNetExample.cpp @@ -51,8 +51,7 @@ int main(int argc, char **argv) { DiscreteFactorGraph fg(asia); // Create solver and eliminate - Ordering ordering; - ordering += Key(0), Key(1), Key(2), Key(3), Key(4), Key(5), Key(6), Key(7); + const Ordering ordering{0, 1, 2, 3, 4, 5, 6, 7}; // solve auto mpe = fg.optimize(); diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 03ffb54bc..a351d0d48 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -48,6 +48,23 @@ else() set(excluded_sources ${excluded_sources} "${CMAKE_CURRENT_SOURCE_DIR}/geometry/Rot3Q.cpp") endif() +# if GTSAM_ENABLE_BOOST_SERIALIZATION is not set, then we need to exclude the following: +if(NOT GTSAM_ENABLE_BOOST_SERIALIZATION) + list (APPEND excluded_headers + "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/base/serializationTestHelpers.h" + "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/base/serialization.h" + ) +endif() + +# if GTSAM_USE_BOOST_FEATURES is not set, then we need to exclude the following: +if(NOT GTSAM_USE_BOOST_FEATURES) + list (APPEND excluded_sources + "${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/GncOptimizer.h" + "${CMAKE_CURRENT_SOURCE_DIR}/inference/graph.h" + "${CMAKE_CURRENT_SOURCE_DIR}/inference/graph-inl.h" + ) +endif() + # Common headers file(GLOB gtsam_core_headers "*.h") install(FILES ${gtsam_core_headers} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam) diff --git a/gtsam/base/ConcurrentMap.h b/gtsam/base/ConcurrentMap.h index 66d06d324..372f02d06 100644 --- a/gtsam/base/ConcurrentMap.h +++ b/gtsam/base/ConcurrentMap.h @@ -52,7 +52,6 @@ using ConcurrentMapBase = gtsam::FastMap; #include #include #endif -#include #include diff --git a/gtsam/base/FastList.h b/gtsam/base/FastList.h index 2b0b9945b..414c1e83f 100644 --- a/gtsam/base/FastList.h +++ b/gtsam/base/FastList.h @@ -20,7 +20,6 @@ #include #include -#include #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION #include #include diff --git a/gtsam/base/FastSet.h b/gtsam/base/FastSet.h index b7c00801b..a1be08d80 100644 --- a/gtsam/base/FastSet.h +++ b/gtsam/base/FastSet.h @@ -18,8 +18,8 @@ #pragma once -#include #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#include #if BOOST_VERSION >= 107400 #include #endif @@ -51,7 +51,7 @@ template class FastSet: public std::set, typename internal::FastDefaultAllocator::type> { - BOOST_CONCEPT_ASSERT ((IsTestable )); + GTSAM_CONCEPT_ASSERT(IsTestable); public: diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index bbf0f57ad..bb92b6b2e 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -23,8 +23,6 @@ #include #include -#include - #include #include #include // operator typeid diff --git a/gtsam/base/Group.h b/gtsam/base/Group.h index 5c0f92b4e..3430a06ae 100644 --- a/gtsam/base/Group.h +++ b/gtsam/base/Group.h @@ -22,10 +22,13 @@ #include +#ifdef GTSAM_USE_BOOST_FEATURES #include #include #include #include +#endif + #include namespace gtsam { @@ -50,7 +53,7 @@ public: //typedef typename traits::identity::value_type identity_value_type; BOOST_CONCEPT_USAGE(IsGroup) { - BOOST_STATIC_ASSERT_MSG( + static_assert( (std::is_base_of::value), "This type's structure_category trait does not assert it as a group (or derived)"); e = traits::Identity(); @@ -79,7 +82,7 @@ private: /// Check invariants template -BOOST_CONCEPT_REQUIRES(((IsGroup)),(bool)) // +GTSAM_CONCEPT_REQUIRES(IsGroup,bool) // check_group_invariants(const G& a, const G& b, double tol = 1e-9) { G e = traits::Identity(); return traits::Equals(traits::Compose(a, traits::Inverse(a)), e, tol) @@ -125,7 +128,7 @@ struct AdditiveGroup : AdditiveGroupTraits, Testable {}; /// compose multiple times template -BOOST_CONCEPT_REQUIRES(((IsGroup)),(G)) // +GTSAM_CONCEPT_REQUIRES(IsGroup,G) // compose_pow(const G& g, size_t n) { if (n == 0) return traits::Identity(); else if (n == 1) return g; @@ -136,8 +139,8 @@ compose_pow(const G& g, size_t n) { /// Assumes nothing except group structure and Testable from G and H template class DirectProduct: public std::pair { - BOOST_CONCEPT_ASSERT((IsGroup)); - BOOST_CONCEPT_ASSERT((IsGroup)); + GTSAM_CONCEPT_ASSERT1(IsGroup); + GTSAM_CONCEPT_ASSERT2(IsGroup); public: /// Default constructor yields identity @@ -167,8 +170,8 @@ struct traits > : /// Assumes existence of three additive operators for both groups template class DirectSum: public std::pair { - BOOST_CONCEPT_ASSERT((IsGroup)); // TODO(frank): check additive - BOOST_CONCEPT_ASSERT((IsGroup)); // TODO(frank): check additive + GTSAM_CONCEPT_ASSERT1(IsGroup); // TODO(frank): check additive + GTSAM_CONCEPT_ASSERT2(IsGroup); // TODO(frank): check additive const G& g() const { return this->first; } const H& h() const { return this->second;} diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 3ea5e94a8..ce021e10e 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -265,7 +265,7 @@ public: typedef typename traits::ChartJacobian ChartJacobian; BOOST_CONCEPT_USAGE(IsLieGroup) { - BOOST_STATIC_ASSERT_MSG( + static_assert( (std::is_base_of::value), "This type's trait does not assert it is a Lie group (or derived)"); diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 55aebd10f..815c8b288 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -22,10 +22,7 @@ #include #include #include - -#include -#include -#include +#include namespace gtsam { @@ -95,7 +92,7 @@ template struct ManifoldTraits: GetDimensionImpl { // Check that Class has the necessary machinery - BOOST_CONCEPT_ASSERT((HasManifoldPrereqs)); + GTSAM_CONCEPT_ASSERT(HasManifoldPrereqs); // Dimension of the manifold enum { dimension = Class::dimension }; @@ -123,7 +120,7 @@ template struct Manifold: ManifoldTraits, Testable {} /// Check invariants for Manifold type template -BOOST_CONCEPT_REQUIRES(((IsTestable)),(bool)) // +GTSAM_CONCEPT_REQUIRES(IsTestable, bool) // check_manifold_invariants(const T& a, const T& b, double tol=1e-9) { typename traits::TangentVector v0 = traits::Local(a,a); typename traits::TangentVector v = traits::Local(a,b); @@ -143,10 +140,10 @@ public: typedef typename traits::TangentVector TangentVector; BOOST_CONCEPT_USAGE(IsManifold) { - BOOST_STATIC_ASSERT_MSG( + static_assert( (std::is_base_of::value), "This type's structure_category trait does not assert it as a manifold (or derived)"); - BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim); + static_assert(TangentVector::SizeAtCompileTime == dim); // make sure Chart methods are defined v = traits::Local(p, q); @@ -164,7 +161,7 @@ template struct FixedDimension { typedef const int value_type; static const int value = traits::dimension; - BOOST_STATIC_ASSERT_MSG(value != Eigen::Dynamic, + static_assert(value != Eigen::Dynamic, "FixedDimension instantiated for dymanically-sized type."); }; } // \ namespace gtsam diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 60fed671a..6dba9cb05 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -279,7 +279,7 @@ struct Reshape { template inline typename Reshape::ReshapedType reshape(const Eigen::Matrix & m){ - BOOST_STATIC_ASSERT(InM * InN == OutM * OutN); + static_assert(InM * InN == OutM * OutN); return Reshape::reshape(m); } diff --git a/gtsam/base/ProductLieGroup.h b/gtsam/base/ProductLieGroup.h index 831968bc8..a8bb2ef98 100644 --- a/gtsam/base/ProductLieGroup.h +++ b/gtsam/base/ProductLieGroup.h @@ -27,8 +27,8 @@ namespace gtsam { /// Assumes Lie group structure for G and H template class ProductLieGroup: public std::pair { - BOOST_CONCEPT_ASSERT((IsLieGroup)); - BOOST_CONCEPT_ASSERT((IsLieGroup)); + GTSAM_CONCEPT_ASSERT1(IsLieGroup); + GTSAM_CONCEPT_ASSERT2(IsLieGroup); typedef std::pair Base; protected: diff --git a/gtsam/base/Testable.h b/gtsam/base/Testable.h index b68d6a97c..8eb6326c0 100644 --- a/gtsam/base/Testable.h +++ b/gtsam/base/Testable.h @@ -33,7 +33,8 @@ #pragma once -#include +#include + #include #include #include @@ -151,7 +152,7 @@ namespace gtsam { struct Testable { // Check that T has the necessary methods - BOOST_CONCEPT_ASSERT((HasTestablePrereqs)); + GTSAM_CONCEPT_ASSERT(HasTestablePrereqs); static void Print(const T& m, const std::string& str = "") { m.print(str); @@ -170,7 +171,7 @@ namespace gtsam { * * NOTE: intentionally not in the gtsam namespace to allow for classes not in * the gtsam namespace to be more easily enforced as testable - * @deprecated please use BOOST_CONCEPT_ASSERT and + * @deprecated please use GTSAM_CONCEPT_ASSERT and */ #define GTSAM_CONCEPT_TESTABLE_INST(T) template class gtsam::IsTestable; #define GTSAM_CONCEPT_TESTABLE_TYPE(T) using _gtsam_Testable_##T = gtsam::IsTestable; diff --git a/gtsam/base/VectorSpace.h b/gtsam/base/VectorSpace.h index 9246ad871..f4e3d3020 100644 --- a/gtsam/base/VectorSpace.h +++ b/gtsam/base/VectorSpace.h @@ -185,7 +185,7 @@ template struct VectorSpaceTraits: VectorSpaceImpl { // Check that Class has the necessary machinery - BOOST_CONCEPT_ASSERT((HasVectorSpacePrereqs)); + GTSAM_CONCEPT_ASSERT(HasVectorSpacePrereqs); typedef vector_space_tag structure_category; @@ -473,7 +473,7 @@ public: typedef typename traits::structure_category structure_category_tag; BOOST_CONCEPT_USAGE(IsVectorSpace) { - BOOST_STATIC_ASSERT_MSG( + static_assert( (std::is_base_of::value), "This type's trait does not assert it as a vector space (or derived)"); r = p + q; diff --git a/gtsam/base/chartTesting.h b/gtsam/base/chartTesting.h index 8f5213f91..05b06a489 100644 --- a/gtsam/base/chartTesting.h +++ b/gtsam/base/chartTesting.h @@ -39,7 +39,7 @@ void testDefaultChart(TestResult& result_, // First, check the basic chart concept. This checks that the interface is satisfied. // The rest of the function is even more detailed, checking the correctness of the chart. - BOOST_CONCEPT_ASSERT((ChartConcept)); + GTSAM_CONCEPT_ASSERT(ChartConcept); T other = value; diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index da872d006..242e681da 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -8,25 +8,27 @@ #pragma once -// This is a helper to ease the transition to the new traits defined in this file. -// Uncomment this if you want all methods that are tagged as not implemented -// to cause compilation errors. -#ifdef COMPILE_ERROR_NOT_IMPLEMENTED - -#include -#define CONCEPT_NOT_IMPLEMENTED BOOST_STATIC_ASSERT_MSG(boost::false_type, \ -"This method is required by the new concepts framework but has not been implemented yet."); - -#else - -#include -#define CONCEPT_NOT_IMPLEMENTED \ - throw std::runtime_error("This method is required by the new concepts framework but has not been implemented yet."); - +#ifdef GTSAM_USE_BOOST_FEATURES +#include +#include +#include +#include +#define GTSAM_CONCEPT_ASSERT(concept) BOOST_CONCEPT_ASSERT((concept)) +#define GTSAM_CONCEPT_ASSERT1(concept) BOOST_CONCEPT_ASSERT((concept)) +#define GTSAM_CONCEPT_ASSERT2(concept) BOOST_CONCEPT_ASSERT((concept)) +#define GTSAM_CONCEPT_ASSERT3(concept) BOOST_CONCEPT_ASSERT((concept)) +#define GTSAM_CONCEPT_ASSERT4(concept) BOOST_CONCEPT_ASSERT((concept)) +#define GTSAM_CONCEPT_REQUIRES(concept, return_type) BOOST_CONCEPT_REQUIRES(((concept)), (return_type)) +#else +// These do something sensible: +#define BOOST_CONCEPT_USAGE(concept) void check##concept() +// TODO(dellaert): would be nice if it was a single macro... +#define GTSAM_CONCEPT_ASSERT(concept) concept checkConcept [[maybe_unused]]; +#define GTSAM_CONCEPT_ASSERT1(concept) concept checkConcept1 [[maybe_unused]]; +#define GTSAM_CONCEPT_ASSERT2(concept) concept checkConcept2 [[maybe_unused]]; +#define GTSAM_CONCEPT_ASSERT3(concept) concept checkConcept3 [[maybe_unused]]; +#define GTSAM_CONCEPT_ASSERT4(concept) concept checkConcept4 [[maybe_unused]]; +// This one just ignores concept for now: +#define GTSAM_CONCEPT_REQUIRES(concept, return_type) return_type #endif -namespace gtsam { - -template struct traits; - -} diff --git a/gtsam/base/std_optional_serialization.h b/gtsam/base/std_optional_serialization.h index 07a02c228..ec6eec56e 100644 --- a/gtsam/base/std_optional_serialization.h +++ b/gtsam/base/std_optional_serialization.h @@ -67,7 +67,7 @@ void save(Archive& ar, const std::optional& t, const unsigned int /*version*/ // It is an inherent limitation to the serialization of optional.hpp // that the underlying type must be either a pointer or must have a // default constructor. - BOOST_STATIC_ASSERT(boost::serialization::detail::is_default_constructible::value || boost::is_pointer::value); + static_assert(boost::serialization::detail::is_default_constructible::value || boost::is_pointer::value); const bool tflag = t.has_value(); ar << boost::serialization::make_nvp("initialized", tflag); if (tflag) { diff --git a/gtsam/base/tests/testGroup.cpp b/gtsam/base/tests/testGroup.cpp index 7e8cb7821..3c8b04f39 100644 --- a/gtsam/base/tests/testGroup.cpp +++ b/gtsam/base/tests/testGroup.cpp @@ -80,7 +80,7 @@ using namespace gtsam; typedef Symmetric<2> S2; TEST(Group, S2) { S2 e, s1 = S2::Transposition(0, 1); - BOOST_CONCEPT_ASSERT((IsGroup)); + GTSAM_CONCEPT_ASSERT(IsGroup); EXPECT(check_group_invariants(e, s1)); } @@ -88,7 +88,7 @@ TEST(Group, S2) { typedef Symmetric<3> S3; TEST(Group, S3) { S3 e, s1 = S3::Transposition(0, 1), s2 = S3::Transposition(1, 2); - BOOST_CONCEPT_ASSERT((IsGroup)); + GTSAM_CONCEPT_ASSERT(IsGroup); EXPECT(check_group_invariants(e, s1)); EXPECT(assert_equal(s1, s1 * e)); EXPECT(assert_equal(s1, e * s1)); @@ -127,7 +127,7 @@ struct traits : internal::MultiplicativeGroupTraits { TEST(Group, Dih6) { Dih6 e, g(S2::Transposition(0, 1), S3::Transposition(0, 1) * S3::Transposition(1, 2)); - BOOST_CONCEPT_ASSERT((IsGroup)); + GTSAM_CONCEPT_ASSERT(IsGroup); EXPECT(check_group_invariants(e, g)); EXPECT(assert_equal(e, compose_pow(g, 0))); EXPECT(assert_equal(g, compose_pow(g, 1))); diff --git a/gtsam/base/tests/testKruskal.cpp b/gtsam/base/tests/testKruskal.cpp index 9d808670a..bb8cfcaca 100644 --- a/gtsam/base/tests/testKruskal.cpp +++ b/gtsam/base/tests/testKruskal.cpp @@ -37,12 +37,12 @@ gtsam::GaussianFactorGraph makeTestGaussianFactorGraph() { Matrix I = I_2x2; Vector2 b(0, 0); const SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.5)); - gfg += JacobianFactor(X(1), I, X(2), I, b, model); - gfg += JacobianFactor(X(1), I, X(3), I, b, model); - gfg += JacobianFactor(X(1), I, X(4), I, b, model); - gfg += JacobianFactor(X(2), I, X(3), I, b, model); - gfg += JacobianFactor(X(2), I, X(4), I, b, model); - gfg += JacobianFactor(X(3), I, X(4), I, b, model); + gfg.emplace_shared(X(1), I, X(2), I, b, model); + gfg.emplace_shared(X(1), I, X(3), I, b, model); + gfg.emplace_shared(X(1), I, X(4), I, b, model); + gfg.emplace_shared(X(2), I, X(3), I, b, model); + gfg.emplace_shared(X(2), I, X(4), I, b, model); + gfg.emplace_shared(X(3), I, X(4), I, b, model); return gfg; } @@ -53,12 +53,12 @@ gtsam::NonlinearFactorGraph makeTestNonlinearFactorGraph() { NonlinearFactorGraph nfg; const SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.5)); - nfg += BetweenFactor(X(1), X(2), Rot3(), model); - nfg += BetweenFactor(X(1), X(3), Rot3(), model); - nfg += BetweenFactor(X(1), X(4), Rot3(), model); - nfg += BetweenFactor(X(2), X(3), Rot3(), model); - nfg += BetweenFactor(X(2), X(4), Rot3(), model); - nfg += BetweenFactor(X(3), X(4), Rot3(), model); + nfg.emplace_shared>(X(1), X(2), Rot3(), model); + nfg.emplace_shared>(X(1), X(3), Rot3(), model); + nfg.emplace_shared>(X(1), X(4), Rot3(), model); + nfg.emplace_shared>(X(2), X(3), Rot3(), model); + nfg.emplace_shared>(X(2), X(4), Rot3(), model); + nfg.emplace_shared>(X(3), X(4), Rot3(), model); return nfg; } diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index 925369d5e..a000e4864 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -1146,17 +1146,30 @@ TEST(Matrix, DLT ) } //****************************************************************************** -TEST(Matrix , IsVectorSpace) { - BOOST_CONCEPT_ASSERT((IsVectorSpace)); - typedef Eigen::Matrix RowMajor; - BOOST_CONCEPT_ASSERT((IsVectorSpace)); - BOOST_CONCEPT_ASSERT((IsVectorSpace)); - BOOST_CONCEPT_ASSERT((IsVectorSpace)); - typedef Eigen::Matrix RowVector; - BOOST_CONCEPT_ASSERT((IsVectorSpace)); - BOOST_CONCEPT_ASSERT((IsVectorSpace)); +TEST(Matrix, Matrix24IsVectorSpace) { + GTSAM_CONCEPT_ASSERT(IsVectorSpace); } +TEST(Matrix, RowMajorIsVectorSpace) { + typedef Eigen::Matrix RowMajor; + GTSAM_CONCEPT_ASSERT(IsVectorSpace); +} + +TEST(Matrix, MatrixIsVectorSpace) { + GTSAM_CONCEPT_ASSERT(IsVectorSpace); +} + +TEST(Matrix, VectorIsVectorSpace) { + GTSAM_CONCEPT_ASSERT(IsVectorSpace); +} + +TEST(Matrix, RowVectorIsVectorSpace) { + typedef Eigen::Matrix RowVector; + GTSAM_CONCEPT_ASSERT1(IsVectorSpace); + GTSAM_CONCEPT_ASSERT2(IsVectorSpace); +} + +//****************************************************************************** TEST(Matrix, AbsoluteError) { double a = 2000, b = 1997, tol = 1e-1; bool isEqual; diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index 4d479b3c0..c749a5191 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/gtsam/base/tests/testVector.cpp @@ -266,11 +266,14 @@ TEST(Vector, linear_dependent3 ) } //****************************************************************************** -TEST(Vector, IsVectorSpace) { - BOOST_CONCEPT_ASSERT((IsVectorSpace)); - BOOST_CONCEPT_ASSERT((IsVectorSpace)); +TEST(Vector, VectorIsVectorSpace) { + GTSAM_CONCEPT_ASSERT1(IsVectorSpace); + GTSAM_CONCEPT_ASSERT2(IsVectorSpace); +} + +TEST(Vector, RowVectorIsVectorSpace) { typedef Eigen::Matrix RowVector; - BOOST_CONCEPT_ASSERT((IsVectorSpace)); + GTSAM_CONCEPT_ASSERT(IsVectorSpace); } /* ************************************************************************* */ diff --git a/gtsam/base/timing.cpp b/gtsam/base/timing.cpp index 9ca5e24c9..5567ce35d 100644 --- a/gtsam/base/timing.cpp +++ b/gtsam/base/timing.cpp @@ -30,6 +30,9 @@ namespace gtsam { namespace internal { + +// a static shared_ptr to TimingOutline with nullptr as the pointer +const static std::shared_ptr nullTimingOutline; GTSAM_EXPORT std::shared_ptr gTimingRoot( new TimingOutline("Total", getTicTocID("Total"))); @@ -53,13 +56,16 @@ void TimingOutline::add(size_t usecs, size_t usecsWall) { TimingOutline::TimingOutline(const std::string& label, size_t id) : id_(id), t_(0), tWall_(0), t2_(0.0), tIt_(0), tMax_(0), tMin_(0), n_(0), myOrder_( 0), lastChildOrder_(0), label_(label) { +#ifdef GTSAM_USE_BOOST_FEATURES #ifdef GTSAM_USING_NEW_BOOST_TIMERS timer_.stop(); #endif +#endif } /* ************************************************************************* */ size_t TimingOutline::time() const { +#ifdef GTSAM_USE_BOOST_FEATURES size_t time = 0; bool hasChildren = false; for(const ChildMap::value_type& child: children_) { @@ -70,10 +76,14 @@ size_t TimingOutline::time() const { return time; else return t_; +#else + return 0; +#endif } /* ************************************************************************* */ void TimingOutline::print(const std::string& outline) const { +#ifdef GTSAM_USE_BOOST_FEATURES std::string formattedLabel = label_; std::replace(formattedLabel.begin(), formattedLabel.end(), '_', ' '); std::cout << outline << "-" << formattedLabel << ": " << self() << " CPU (" @@ -92,11 +102,12 @@ void TimingOutline::print(const std::string& outline) const { order_child.second->print(childOutline); } std::cout.flush(); +#endif } void TimingOutline::print2(const std::string& outline, const double parentTotal) const { - +#ifdef GTSAM_USE_BOOST_FEATURES const int w1 = 24, w2 = 2, w3 = 6, w4 = 8, precision = 2; const double selfTotal = self(), selfMean = selfTotal / double(n_); const double childTotal = secs(); @@ -135,11 +146,13 @@ void TimingOutline::print2(const std::string& outline, child.second->print2(childOutline, selfTotal); } } +#endif } /* ************************************************************************* */ const std::shared_ptr& TimingOutline::child(size_t child, const std::string& label, const std::weak_ptr& thisPtr) { +#ifdef GTSAM_USE_BOOST_FEATURES assert(thisPtr.lock().get() == this); std::shared_ptr& result = children_[child]; if (!result) { @@ -150,10 +163,15 @@ const std::shared_ptr& TimingOutline::child(size_t child, result->parent_ = thisPtr; } return result; +#else + return nullTimingOutline; +#endif } /* ************************************************************************* */ void TimingOutline::tic() { +// Disable this entire function if we are not using boost +#ifdef GTSAM_USE_BOOST_FEATURES #ifdef GTSAM_USING_NEW_BOOST_TIMERS assert(timer_.is_stopped()); timer_.start(); @@ -166,10 +184,14 @@ void TimingOutline::tic() { #ifdef GTSAM_USE_TBB tbbTimer_ = tbb::tick_count::now(); #endif +#endif } /* ************************************************************************* */ void TimingOutline::toc() { +// Disable this entire function if we are not using boost +#ifdef GTSAM_USE_BOOST_FEATURES + #ifdef GTSAM_USING_NEW_BOOST_TIMERS assert(!timer_.is_stopped()); @@ -197,10 +219,12 @@ void TimingOutline::toc() { #endif add(cpuTime, wallTime); +#endif } /* ************************************************************************* */ void TimingOutline::finishedIteration() { +#ifdef GTSAM_USE_BOOST_FEATURES if (tIt_ > tMax_) tMax_ = tIt_; if (tMin_ == 0 || tIt_ < tMin_) @@ -209,10 +233,13 @@ void TimingOutline::finishedIteration() { for(ChildMap::value_type& child: children_) { child.second->finishedIteration(); } +#endif } /* ************************************************************************* */ size_t getTicTocID(const char *descriptionC) { +// disable anything which refers to TimingOutline as well, for good measure +#ifdef GTSAM_USE_BOOST_FEATURES const std::string description(descriptionC); // Global (static) map from strings to ID numbers and current next ID number static size_t nextId = 0; @@ -227,19 +254,27 @@ size_t getTicTocID(const char *descriptionC) { // Return ID return it->second; +#else + return 0; +#endif } /* ************************************************************************* */ void tic(size_t id, const char *labelC) { +// disable anything which refers to TimingOutline as well, for good measure +#ifdef GTSAM_USE_BOOST_FEATURES const std::string label(labelC); std::shared_ptr node = // gCurrentTimer.lock()->child(id, label, gCurrentTimer); gCurrentTimer = node; node->tic(); +#endif } /* ************************************************************************* */ void toc(size_t id, const char *label) { +// disable anything which refers to TimingOutline as well, for good measure +#ifdef GTSAM_USE_BOOST_FEATURES std::shared_ptr current(gCurrentTimer.lock()); if (id != current->id_) { gTimingRoot->print(); @@ -255,6 +290,7 @@ void toc(size_t id, const char *label) { } current->toc(); gCurrentTimer = current->parent_; +#endif } } // namespace internal diff --git a/gtsam/base/timing.h b/gtsam/base/timing.h index 52e6adff7..99c55a3d7 100644 --- a/gtsam/base/timing.h +++ b/gtsam/base/timing.h @@ -21,7 +21,9 @@ #include #include // for GTSAM_USE_TBB +#ifdef GTSAM_USE_BOOST_FEATURES #include +#endif #include #include @@ -105,6 +107,7 @@ // have matching gttic/gttoc statments. You may want to consider reorganizing your timing // outline to match the scope of your code. +#ifdef GTSAM_USE_BOOST_FEATURES // Automatically use the new Boost timers if version is recent enough. #if BOOST_VERSION >= 104800 # ifndef GTSAM_DISABLE_NEW_TIMERS @@ -118,6 +121,7 @@ # include # include #endif +#endif #ifdef GTSAM_USE_TBB # include @@ -160,6 +164,8 @@ namespace gtsam { typedef FastMap > ChildMap; ChildMap children_; ///< subtrees +// disable all timers if not using boost +#ifdef GTSAM_USE_BOOST_FEATURES #ifdef GTSAM_USING_NEW_BOOST_TIMERS boost::timer::cpu_timer timer_; #else @@ -168,6 +174,7 @@ namespace gtsam { #endif #ifdef GTSAM_USE_TBB tbb::tick_count tbbTimer_; +#endif #endif void add(size_t usecs, size_t usecsWall); @@ -176,11 +183,20 @@ namespace gtsam { GTSAM_EXPORT TimingOutline(const std::string& label, size_t myId); GTSAM_EXPORT size_t time() const; ///< time taken, including children double secs() const { return double(time()) / 1000000.0;} ///< time taken, in seconds, including children +#ifdef GTSAM_USE_BOOST_FEATURES double self() const { return double(t_) / 1000000.0;} ///< self time only, in seconds double wall() const { return double(tWall_) / 1000000.0;} ///< wall time, in seconds double min() const { return double(tMin_) / 1000000.0;} ///< min time, in seconds double max() const { return double(tMax_) / 1000000.0;} ///< max time, in seconds double mean() const { return self() / double(n_); } ///< mean self time, in seconds +#else + // make them no-ops if not using boost + double self() const { return -1.; } ///< self time only, in seconds + double wall() const { return -1.; } ///< wall time, in seconds + double min() const { return -1.; } ///< min time, in seconds + double max() const { return -1.; } ///< max time, in seconds + double mean() const { return -1.; } ///< mean self time, in seconds +#endif GTSAM_EXPORT void print(const std::string& outline = "") const; GTSAM_EXPORT void print2(const std::string& outline = "", const double parentTotal = -1.0) const; GTSAM_EXPORT const std::shared_ptr& diff --git a/gtsam/base/types.h b/gtsam/base/types.h index 3266431ea..3f1c1cd3d 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -20,8 +20,10 @@ #pragma once #include +#ifdef GTSAM_USE_BOOST_FEATURES #include #include +#endif #include // for GTSAM_USE_TBB #include @@ -153,33 +155,7 @@ namespace gtsam { operator T() const { return value; } }; - /* ************************************************************************* */ - /** A helper class that behaves as a container with one element, and works with - * boost::range */ - template - class ListOfOneContainer { - T element_; - public: - typedef T value_type; - typedef const T* const_iterator; - typedef T* iterator; - ListOfOneContainer(const T& element) : element_(element) {} - const T* begin() const { return &element_; } - const T* end() const { return &element_ + 1; } - T* begin() { return &element_; } - T* end() { return &element_ + 1; } - size_t size() const { return 1; } - }; - - BOOST_CONCEPT_ASSERT((boost::RandomAccessRangeConcept >)); - - /** Factory function for ListOfOneContainer to enable ListOfOne(e) syntax. */ - template - ListOfOneContainer ListOfOne(const T& element) { - return ListOfOneContainer(element); - } - - /* ************************************************************************* */ + /* ************************************************************************* */ #ifdef __clang__ # pragma clang diagnostic push # pragma clang diagnostic ignored "-Wunused-private-field" // Clang complains that previousOpenMPThreads is unused in the #else case below diff --git a/gtsam/base/utilities.h b/gtsam/base/utilities.h index 0a05a704c..03e9636da 100644 --- a/gtsam/base/utilities.h +++ b/gtsam/base/utilities.h @@ -28,16 +28,9 @@ private: } -// boost::index_sequence was introduced in 1.66, so we'll manually define an -// implementation if user has 1.65. boost::index_sequence is used to get array -// indices that align with a parameter pack. -#include -#if BOOST_VERSION >= 106600 -#include -#else -namespace boost { -namespace mp11 { +namespace gtsam { // Adapted from https://stackoverflow.com/a/32223343/9151520 +// An adaptation of boost::mp11::index_sequence template struct index_sequence { using type = index_sequence; @@ -49,20 +42,16 @@ template struct _merge_and_renumber; template -struct _merge_and_renumber, index_sequence > +struct _merge_and_renumber, index_sequence> : index_sequence {}; } // namespace detail template -struct make_index_sequence - : detail::_merge_and_renumber< - typename make_index_sequence::type, - typename make_index_sequence::type> {}; +struct make_index_sequence : detail::_merge_and_renumber::type, + typename make_index_sequence::type> {}; template <> struct make_index_sequence<0> : index_sequence<> {}; template <> struct make_index_sequence<1> : index_sequence<0> {}; template using index_sequence_for = make_index_sequence; -} // namespace mp11 -} // namespace boost -#endif +} // namespace gtsam diff --git a/gtsam/discrete/SignatureParser.cpp b/gtsam/discrete/SignatureParser.cpp index ab259d0ea..8e0221f01 100644 --- a/gtsam/discrete/SignatureParser.cpp +++ b/gtsam/discrete/SignatureParser.cpp @@ -33,6 +33,8 @@ std::optional static ParseConditional(const std::string& token) { // can throw exception row.push_back(std::stod(s)); } + // if we ended with a '/' then return false + if (token.back() == '/') return std::nullopt; } catch (...) { return std::nullopt; } diff --git a/gtsam/discrete/tests/testDiscreteBayesNet.cpp b/gtsam/discrete/tests/testDiscreteBayesNet.cpp index a17a20d41..95f407cae 100644 --- a/gtsam/discrete/tests/testDiscreteBayesNet.cpp +++ b/gtsam/discrete/tests/testDiscreteBayesNet.cpp @@ -94,8 +94,7 @@ TEST(DiscreteBayesNet, Asia) { EXPECT(assert_equal(vs, marginals.marginalProbabilities(Smoking))); // Create solver and eliminate - Ordering ordering; - ordering += Key(0), Key(1), Key(2), Key(3), Key(4), Key(5), Key(6), Key(7); + const Ordering ordering{0, 1, 2, 3, 4, 5, 6, 7}; DiscreteBayesNet::shared_ptr chordal = fg.eliminateSequential(ordering); DiscreteConditional expected2(Bronchitis % "11/9"); EXPECT(assert_equal(expected2, *chordal->back())); diff --git a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp index a87592ce3..bb23b7a83 100644 --- a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp +++ b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp @@ -107,8 +107,7 @@ TEST(DiscreteFactorGraph, test) { graph.add(C & B, "3 1 1 3"); // Test EliminateDiscrete - Ordering frontalKeys; - frontalKeys += Key(0); + const Ordering frontalKeys{0}; const auto [conditional, newFactor] = EliminateDiscrete(graph, frontalKeys); // Check Conditional @@ -123,8 +122,7 @@ TEST(DiscreteFactorGraph, test) { EXPECT(assert_equal(expectedFactor, *newFactor)); // Test using elimination tree - Ordering ordering; - ordering += Key(0), Key(1), Key(2); + const Ordering ordering{0, 1, 2}; DiscreteEliminationTree etree(graph, ordering); const auto [actual, remainingGraph] = etree.eliminate(&EliminateDiscrete); @@ -231,8 +229,7 @@ TEST(DiscreteFactorGraph, testMPE_Darwiche09book_p244) { EXPECT(assert_equal(mpe, actualMPE)); // Check Bayes Net - Ordering ordering; - ordering += Key(0), Key(1), Key(2), Key(3), Key(4); + const Ordering ordering{0, 1, 2, 3, 4}; auto chordal = graph.eliminateSequential(ordering); EXPECT_LONGS_EQUAL(5, chordal->size()); diff --git a/gtsam/discrete/tests/testSignatureParser.cpp b/gtsam/discrete/tests/testSignatureParser.cpp index 5ae71442e..e0db84697 100644 --- a/gtsam/discrete/tests/testSignatureParser.cpp +++ b/gtsam/discrete/tests/testSignatureParser.cpp @@ -85,11 +85,16 @@ TEST(SimpleParser, Gibberish) { // If Gibberish is in the middle, it should not parse. TEST(SimpleParser, GibberishInMiddle) { - SignatureParser::Table expectedTable{{1, 1}, {2, 3}}; const auto table = SignatureParser::Parse("1/1 2/3 sdf 1/4"); EXPECT(!table); } +// A test with slash in the end +TEST(SimpleParser, SlashInEnd) { + const auto table = SignatureParser::Parse("1/1 2/"); + EXPECT(!table); +} + /* ************************************************************************* */ int main() { diff --git a/gtsam/geometry/BearingRange.h b/gtsam/geometry/BearingRange.h index 7e6210914..19194bb7a 100644 --- a/gtsam/geometry/BearingRange.h +++ b/gtsam/geometry/BearingRange.h @@ -21,7 +21,6 @@ #include #include #include -#include #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 153b9690e..a936bd02a 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -21,7 +21,6 @@ #include #include -#include #include #include diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 07cc7302a..45e04dab6 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -24,7 +24,6 @@ #include #include -#include #include using namespace std; diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 523255d87..3e50551d5 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -20,7 +20,6 @@ #ifdef GTSAM_USE_QUATERNIONS #include -#include #include using namespace std; diff --git a/gtsam/geometry/tests/testBearingRange.cpp b/gtsam/geometry/tests/testBearingRange.cpp index 1216a6396..ff2a9a6a4 100644 --- a/gtsam/geometry/tests/testBearingRange.cpp +++ b/gtsam/geometry/tests/testBearingRange.cpp @@ -34,7 +34,7 @@ BearingRange3D br3D(Pose3().bearing(Point3(1, 0, 0)), 1); //****************************************************************************** TEST(BearingRange2D, Concept) { - BOOST_CONCEPT_ASSERT((IsManifold)); + GTSAM_CONCEPT_ASSERT(IsManifold); } /* ************************************************************************* */ @@ -46,7 +46,7 @@ TEST(BearingRange, 2D) { //****************************************************************************** TEST(BearingRange3D, Concept) { - BOOST_CONCEPT_ASSERT((IsManifold)); + GTSAM_CONCEPT_ASSERT(IsManifold); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testCyclic.cpp b/gtsam/geometry/tests/testCyclic.cpp index abcef9e5b..e2b250f50 100644 --- a/gtsam/geometry/tests/testCyclic.cpp +++ b/gtsam/geometry/tests/testCyclic.cpp @@ -27,7 +27,7 @@ typedef Cyclic<2> Z2; //****************************************************************************** TEST(Cyclic, Concept) { - BOOST_CONCEPT_ASSERT((IsGroup)); + GTSAM_CONCEPT_ASSERT(IsGroup); EXPECT_LONGS_EQUAL(0, traits::Identity()); } @@ -107,8 +107,8 @@ struct traits : internal::AdditiveGroupTraits { TEST(Cyclic , DirectSum) { // The Direct sum of Z2 and Z2 is *not* Cyclic<4>, but the // smallest non-cyclic group called the Klein four-group: - BOOST_CONCEPT_ASSERT((IsGroup)); - BOOST_CONCEPT_ASSERT((IsTestable)); + GTSAM_CONCEPT_ASSERT1(IsGroup); + GTSAM_CONCEPT_ASSERT2(IsTestable); // Refer to http://en.wikipedia.org/wiki/Klein_four-group K4 e(0,0), a(0, 1), b(1, 0), c(1, 1); diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index 6e4d408c7..4083970b6 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -34,9 +34,9 @@ TEST(Point2 , Constructor) { //****************************************************************************** TEST(Double , Concept) { - BOOST_CONCEPT_ASSERT((IsGroup)); - BOOST_CONCEPT_ASSERT((IsManifold)); - BOOST_CONCEPT_ASSERT((IsVectorSpace)); + GTSAM_CONCEPT_ASSERT1(IsGroup); + GTSAM_CONCEPT_ASSERT2(IsManifold); + GTSAM_CONCEPT_ASSERT3(IsVectorSpace); } //****************************************************************************** @@ -48,9 +48,9 @@ TEST(Double , Invariants) { //****************************************************************************** TEST(Point2 , Concept) { - BOOST_CONCEPT_ASSERT((IsGroup)); - BOOST_CONCEPT_ASSERT((IsManifold)); - BOOST_CONCEPT_ASSERT((IsVectorSpace)); + GTSAM_CONCEPT_ASSERT1(IsGroup); + GTSAM_CONCEPT_ASSERT2(IsManifold); + GTSAM_CONCEPT_ASSERT3(IsVectorSpace); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index b19aa0add..e4263ffd7 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -34,9 +34,9 @@ TEST(Point3 , Constructor) { //****************************************************************************** TEST(Point3 , Concept) { - BOOST_CONCEPT_ASSERT((IsGroup)); - BOOST_CONCEPT_ASSERT((IsManifold)); - BOOST_CONCEPT_ASSERT((IsVectorSpace)); + GTSAM_CONCEPT_ASSERT1(IsGroup); + GTSAM_CONCEPT_ASSERT2(IsManifold); + GTSAM_CONCEPT_ASSERT3(IsVectorSpace); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index e38bfbd75..3985f6ba5 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -35,9 +35,9 @@ GTSAM_CONCEPT_LIE_INST(Pose2) //****************************************************************************** TEST(Pose2 , Concept) { - BOOST_CONCEPT_ASSERT((IsGroup)); - BOOST_CONCEPT_ASSERT((IsManifold)); - BOOST_CONCEPT_ASSERT((IsLieGroup)); + GTSAM_CONCEPT_ASSERT1(IsGroup); + GTSAM_CONCEPT_ASSERT2(IsManifold); + GTSAM_CONCEPT_ASSERT3(IsLieGroup); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp index 281c71f7c..546a1f5b5 100644 --- a/gtsam/geometry/tests/testQuaternion.cpp +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -29,9 +29,9 @@ typedef traits::ChartJacobian QuaternionJacobian; //****************************************************************************** TEST(Quaternion , Concept) { - BOOST_CONCEPT_ASSERT((IsGroup)); - BOOST_CONCEPT_ASSERT((IsManifold)); - BOOST_CONCEPT_ASSERT((IsLieGroup)); + GTSAM_CONCEPT_ASSERT1(IsGroup); + GTSAM_CONCEPT_ASSERT2(IsManifold); + GTSAM_CONCEPT_ASSERT3(IsLieGroup); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 8a1bc1e5c..39cc05fde 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -38,9 +38,9 @@ static double error = 1e-9, epsilon = 0.001; //****************************************************************************** TEST(Rot3 , Concept) { - BOOST_CONCEPT_ASSERT((IsGroup)); - BOOST_CONCEPT_ASSERT((IsManifold)); - BOOST_CONCEPT_ASSERT((IsLieGroup)); + GTSAM_CONCEPT_ASSERT1(IsGroup); + GTSAM_CONCEPT_ASSERT2(IsManifold); + GTSAM_CONCEPT_ASSERT3(IsLieGroup); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 1afe9ff21..2086318cb 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -36,9 +36,9 @@ TEST(SO3, Identity) { //****************************************************************************** TEST(SO3, Concept) { - BOOST_CONCEPT_ASSERT((IsGroup)); - BOOST_CONCEPT_ASSERT((IsManifold)); - BOOST_CONCEPT_ASSERT((IsLieGroup)); + GTSAM_CONCEPT_ASSERT1(IsGroup); + GTSAM_CONCEPT_ASSERT2(IsManifold); + GTSAM_CONCEPT_ASSERT3(IsLieGroup); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testSO4.cpp b/gtsam/geometry/tests/testSO4.cpp index fa550723a..17d9694be 100644 --- a/gtsam/geometry/tests/testSO4.cpp +++ b/gtsam/geometry/tests/testSO4.cpp @@ -42,9 +42,9 @@ TEST(SO4, Identity) { //****************************************************************************** TEST(SO4, Concept) { - BOOST_CONCEPT_ASSERT((IsGroup)); - BOOST_CONCEPT_ASSERT((IsManifold)); - BOOST_CONCEPT_ASSERT((IsLieGroup)); + GTSAM_CONCEPT_ASSERT1(IsGroup); + GTSAM_CONCEPT_ASSERT2(IsManifold); + GTSAM_CONCEPT_ASSERT3(IsLieGroup); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testSOn.cpp b/gtsam/geometry/tests/testSOn.cpp index d9d4da34c..eb453d8c6 100644 --- a/gtsam/geometry/tests/testSOn.cpp +++ b/gtsam/geometry/tests/testSOn.cpp @@ -84,9 +84,9 @@ TEST(SOn, SO5) { //****************************************************************************** TEST(SOn, Concept) { - BOOST_CONCEPT_ASSERT((IsGroup)); - BOOST_CONCEPT_ASSERT((IsManifold)); - BOOST_CONCEPT_ASSERT((IsLieGroup)); + GTSAM_CONCEPT_ASSERT1(IsGroup); + GTSAM_CONCEPT_ASSERT2(IsManifold); + GTSAM_CONCEPT_ASSERT3(IsLieGroup); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testSimilarity2.cpp b/gtsam/geometry/tests/testSimilarity2.cpp index ca041fc7b..8d537fd72 100644 --- a/gtsam/geometry/tests/testSimilarity2.cpp +++ b/gtsam/geometry/tests/testSimilarity2.cpp @@ -35,9 +35,9 @@ static const double s = 4; //****************************************************************************** TEST(Similarity2, Concepts) { - BOOST_CONCEPT_ASSERT((IsGroup)); - BOOST_CONCEPT_ASSERT((IsManifold)); - BOOST_CONCEPT_ASSERT((IsLieGroup)); + GTSAM_CONCEPT_ASSERT1(IsGroup); + GTSAM_CONCEPT_ASSERT2(IsManifold); + GTSAM_CONCEPT_ASSERT3(IsLieGroup); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testSimilarity3.cpp b/gtsam/geometry/tests/testSimilarity3.cpp index fad24f743..d0f661787 100644 --- a/gtsam/geometry/tests/testSimilarity3.cpp +++ b/gtsam/geometry/tests/testSimilarity3.cpp @@ -54,9 +54,9 @@ const double degree = M_PI / 180; //****************************************************************************** TEST(Similarity3, Concepts) { - BOOST_CONCEPT_ASSERT((IsGroup)); - BOOST_CONCEPT_ASSERT((IsManifold)); - BOOST_CONCEPT_ASSERT((IsLieGroup)); + GTSAM_CONCEPT_ASSERT1(IsGroup); + GTSAM_CONCEPT_ASSERT2(IsManifold); + GTSAM_CONCEPT_ASSERT3(IsLieGroup); } //****************************************************************************** diff --git a/gtsam/geometry/tests/testStereoPoint2.cpp b/gtsam/geometry/tests/testStereoPoint2.cpp index ddcc9238a..236ea8b01 100644 --- a/gtsam/geometry/tests/testStereoPoint2.cpp +++ b/gtsam/geometry/tests/testStereoPoint2.cpp @@ -31,9 +31,9 @@ GTSAM_CONCEPT_TESTABLE_INST(StereoPoint2) //****************************************************************************** TEST(StereoPoint2 , Concept) { - BOOST_CONCEPT_ASSERT((IsGroup)); - BOOST_CONCEPT_ASSERT((IsManifold)); - BOOST_CONCEPT_ASSERT((IsVectorSpace)); + GTSAM_CONCEPT_ASSERT1(IsGroup); + GTSAM_CONCEPT_ASSERT2(IsManifold); + GTSAM_CONCEPT_ASSERT3(IsVectorSpace); } /* ************************************************************************* */ diff --git a/gtsam/hybrid/HybridGaussianISAM.cpp b/gtsam/hybrid/HybridGaussianISAM.cpp index 3e5a4f1d1..0dd5fa38b 100644 --- a/gtsam/hybrid/HybridGaussianISAM.cpp +++ b/gtsam/hybrid/HybridGaussianISAM.cpp @@ -84,12 +84,12 @@ void HybridGaussianISAM::updateInternal( // Add the removed top and the new factors HybridGaussianFactorGraph factors; - factors += bn; - factors += newFactors; + factors.push_back(bn); + factors.push_back(newFactors); // Add the orphaned subtrees for (const sharedClique& orphan : *orphans) { - factors += std::make_shared>(orphan); + factors.emplace_shared>(orphan); } const VariableIndex index(factors); diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp index 953025220..6f2898bf1 100644 --- a/gtsam/hybrid/HybridJunctionTree.cpp +++ b/gtsam/hybrid/HybridJunctionTree.cpp @@ -94,9 +94,9 @@ struct HybridConstructorTraversalData { symbolicFactors.reserve(node->factors.size() + data.childSymbolicFactors.size()); // Add ETree node factors - symbolicFactors += node->factors; + symbolicFactors.push_back(node->factors); // Add symbolic factors passed up from children - symbolicFactors += data.childSymbolicFactors; + symbolicFactors.push_back(data.childSymbolicFactors); Ordering keyAsOrdering; keyAsOrdering.push_back(node->key); diff --git a/gtsam/hybrid/tests/testHybridBayesTree.cpp b/gtsam/hybrid/tests/testHybridBayesTree.cpp index f283f7178..578f5d605 100644 --- a/gtsam/hybrid/tests/testHybridBayesTree.cpp +++ b/gtsam/hybrid/tests/testHybridBayesTree.cpp @@ -97,7 +97,7 @@ TEST(HybridBayesTree, OptimizeAssignment) { // Create ordering. Ordering ordering; - for (size_t k = 0; k < s.K; k++) ordering += X(k); + for (size_t k = 0; k < s.K; k++) ordering.push_back(X(k)); const auto [hybridBayesNet, remainingFactorGraph] = s.linearizedFactorGraph.eliminatePartialSequential(ordering); @@ -139,7 +139,7 @@ TEST(HybridBayesTree, Optimize) { // Create ordering. Ordering ordering; - for (size_t k = 0; k < s.K; k++) ordering += X(k); + for (size_t k = 0; k < s.K; k++) ordering.push_back(X(k)); const auto [hybridBayesNet, remainingFactorGraph] = s.linearizedFactorGraph.eliminatePartialSequential(ordering); diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index 07a45bf1b..28f47232b 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -37,6 +37,8 @@ #include "Switching.h" +#include + using namespace std; using namespace gtsam; @@ -44,7 +46,7 @@ using symbol_shorthand::X; Ordering getOrdering(HybridGaussianFactorGraph& factors, const HybridGaussianFactorGraph& newFactors) { - factors += newFactors; + factors.push_back(newFactors); // Get all the discrete keys from the factors KeySet allDiscrete = factors.discreteKeySet(); @@ -83,10 +85,10 @@ TEST(HybridEstimation, Full) { Ordering hybridOrdering; for (size_t k = 0; k < K; k++) { - hybridOrdering += X(k); + hybridOrdering.push_back(X(k)); } for (size_t k = 0; k < K - 1; k++) { - hybridOrdering += M(k); + hybridOrdering.push_back(M(k)); } HybridBayesNet::shared_ptr bayesNet = @@ -442,8 +444,7 @@ TEST(HybridEstimation, eliminateSequentialRegression) { DiscreteConditional expected(m % "0.51341712/1"); // regression // Eliminate into BN using one ordering - Ordering ordering1; - ordering1 += X(0), X(1), M(0); + const Ordering ordering1{X(0), X(1), M(0)}; HybridBayesNet::shared_ptr bn1 = fg->eliminateSequential(ordering1); // Check that the discrete conditional matches the expected. @@ -451,8 +452,7 @@ TEST(HybridEstimation, eliminateSequentialRegression) { EXPECT(assert_equal(expected, *dc1, 1e-9)); // Eliminate into BN using a different ordering - Ordering ordering2; - ordering2 += X(0), X(1), M(0); + const Ordering ordering2{X(0), X(1), M(0)}; HybridBayesNet::shared_ptr bn2 = fg->eliminateSequential(ordering2); // Check that the discrete conditional matches the expected. diff --git a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp index 69a83b7b6..249cbf9c3 100644 --- a/gtsam/hybrid/tests/testHybridGaussianISAM.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianISAM.cpp @@ -126,10 +126,7 @@ TEST(HybridGaussianElimination, IncrementalInference) { /********************************************************/ // Run batch elimination so we can compare results. - Ordering ordering; - ordering += X(0); - ordering += X(1); - ordering += X(2); + const Ordering ordering {X(0), X(1), X(2)}; // Now we calculate the expected factors using full elimination const auto [expectedHybridBayesTree, expectedRemainingGraph] = @@ -158,12 +155,10 @@ TEST(HybridGaussianElimination, IncrementalInference) { // We only perform manual continuous elimination for 0,0. // The other discrete probabilities on M(2) are calculated the same way - Ordering discrete_ordering; - discrete_ordering += M(0); - discrete_ordering += M(1); + const Ordering discreteOrdering{M(0), M(1)}; HybridBayesTree::shared_ptr discreteBayesTree = expectedRemainingGraph->BaseEliminateable::eliminateMultifrontal( - discrete_ordering); + discreteOrdering); DiscreteValues m00; m00[M(0)] = 0, m00[M(1)] = 0; @@ -225,7 +220,7 @@ TEST(HybridGaussianElimination, Approx_inference) { // Create ordering. Ordering ordering; for (size_t j = 0; j < 4; j++) { - ordering += X(j); + ordering.push_back(X(j)); } // Now we calculate the actual factors using full elimination diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index 93a28e53a..af3a23b94 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -265,7 +265,7 @@ TEST(HybridFactorGraph, EliminationTree) { // Create ordering. Ordering ordering; - for (size_t k = 0; k < self.K; k++) ordering += X(k); + for (size_t k = 0; k < self.K; k++) ordering.push_back(X(k)); // Create elimination tree. HybridEliminationTree etree(self.linearizedFactorGraph, ordering); @@ -286,8 +286,7 @@ TEST(GaussianElimination, Eliminate_x0) { factors.push_back(self.linearizedFactorGraph[1]); // Eliminate x0 - Ordering ordering; - ordering += X(0); + const Ordering ordering{X(0)}; auto result = EliminateHybrid(factors, ordering); CHECK(result.first); @@ -310,8 +309,7 @@ TEST(HybridsGaussianElimination, Eliminate_x1) { factors.push_back(self.linearizedFactorGraph[2]); // involves m1 // Eliminate x1 - Ordering ordering; - ordering += X(1); + const Ordering ordering{X(1)}; auto result = EliminateHybrid(factors, ordering); CHECK(result.first); @@ -346,9 +344,7 @@ TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) { auto factors = self.linearizedFactorGraph; // Eliminate x0 - Ordering ordering; - ordering += X(0); - ordering += X(1); + const Ordering ordering{X(0), X(1)}; const auto [hybridConditionalMixture, factorOnModes] = EliminateHybrid(factors, ordering); @@ -379,7 +375,7 @@ TEST(HybridFactorGraph, Partial_Elimination) { // Create ordering of only continuous variables. Ordering ordering; - for (size_t k = 0; k < self.K; k++) ordering += X(k); + for (size_t k = 0; k < self.K; k++) ordering.push_back(X(k)); // Eliminate partially i.e. only continuous part. const auto [hybridBayesNet, remainingFactorGraph] = @@ -415,7 +411,7 @@ TEST(HybridFactorGraph, Full_Elimination) { { // Create ordering. Ordering ordering; - for (size_t k = 0; k < self.K; k++) ordering += X(k); + for (size_t k = 0; k < self.K; k++) ordering.push_back(X(k)); // Eliminate partially. const auto [hybridBayesNet_partial, remainingFactorGraph_partial] = @@ -430,15 +426,15 @@ TEST(HybridFactorGraph, Full_Elimination) { } ordering.clear(); - for (size_t k = 0; k < self.K - 1; k++) ordering += M(k); + for (size_t k = 0; k < self.K - 1; k++) ordering.push_back(M(k)); discreteBayesNet = *discrete_fg.eliminateSequential(ordering, EliminateDiscrete); } // Create ordering. Ordering ordering; - for (size_t k = 0; k < self.K; k++) ordering += X(k); - for (size_t k = 0; k < self.K - 1; k++) ordering += M(k); + for (size_t k = 0; k < self.K; k++) ordering.push_back(X(k)); + for (size_t k = 0; k < self.K - 1; k++) ordering.push_back(M(k)); // Eliminate partially. HybridBayesNet::shared_ptr hybridBayesNet = @@ -479,7 +475,7 @@ TEST(HybridFactorGraph, Printing) { // Create ordering. Ordering ordering; - for (size_t k = 0; k < self.K; k++) ordering += X(k); + for (size_t k = 0; k < self.K; k++) ordering.push_back(X(k)); // Eliminate partially. const auto [hybridBayesNet, remainingFactorGraph] = @@ -705,11 +701,7 @@ TEST(HybridFactorGraph, DefaultDecisionTree) { initialEstimate.insert(L(1), Point2(4.1, 1.8)); // We want to eliminate variables not connected to DCFactors first. - Ordering ordering; - ordering += L(0); - ordering += L(1); - ordering += X(0); - ordering += X(1); + const Ordering ordering {L(0), L(1), X(0), X(1)}; HybridGaussianFactorGraph linearized = *fg.linearize(initialEstimate); diff --git a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp index f4f24afbf..2fb6fd8ba 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearISAM.cpp @@ -143,10 +143,7 @@ TEST(HybridNonlinearISAM, IncrementalInference) { /********************************************************/ // Run batch elimination so we can compare results. - Ordering ordering; - ordering += X(0); - ordering += X(1); - ordering += X(2); + const Ordering ordering {X(0), X(1), X(2)}; // Now we calculate the actual factors using full elimination const auto [expectedHybridBayesTree, expectedRemainingGraph] = @@ -176,12 +173,10 @@ TEST(HybridNonlinearISAM, IncrementalInference) { // We only perform manual continuous elimination for 0,0. // The other discrete probabilities on M(1) are calculated the same way - Ordering discrete_ordering; - discrete_ordering += M(0); - discrete_ordering += M(1); + const Ordering discreteOrdering{M(0), M(1)}; HybridBayesTree::shared_ptr discreteBayesTree = expectedRemainingGraph->BaseEliminateable::eliminateMultifrontal( - discrete_ordering); + discreteOrdering); DiscreteValues m00; m00[M(0)] = 0, m00[M(1)] = 0; @@ -244,7 +239,7 @@ TEST(HybridNonlinearISAM, Approx_inference) { // Create ordering. Ordering ordering; for (size_t j = 0; j < 4; j++) { - ordering += X(j); + ordering.push_back(X(j)); } // Now we calculate the actual factors using full elimination diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index 64cca5546..2d8f917dc 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -381,13 +381,13 @@ namespace gtsam { gttoc(Full_root_factoring); gttic(Variable_joint); - p_BC1C2 += p_B; - p_BC1C2 += *p_C1_B; - p_BC1C2 += *p_C2_B; + p_BC1C2.push_back(p_B); + p_BC1C2.push_back(*p_C1_B); + p_BC1C2.push_back(*p_C2_B); if(C1 != B) - p_BC1C2 += C1->conditional(); + p_BC1C2.push_back(C1->conditional()); if(C2 != B) - p_BC1C2 += C2->conditional(); + p_BC1C2.push_back(C2->conditional()); gttoc(Variable_joint); } else @@ -395,8 +395,8 @@ namespace gtsam { // The nodes have no common ancestor, they're in different trees, so they're joint is just the // product of their marginals. gttic(Disjoint_marginals); - p_BC1C2 += C1->marginal2(function); - p_BC1C2 += C2->marginal2(function); + p_BC1C2.push_back(C1->marginal2(function)); + p_BC1C2.push_back(C2->marginal2(function)); gttoc(Disjoint_marginals); } diff --git a/gtsam/inference/BayesTreeCliqueBase-inst.h b/gtsam/inference/BayesTreeCliqueBase-inst.h index 62ed0b90f..a91fa4f78 100644 --- a/gtsam/inference/BayesTreeCliqueBase-inst.h +++ b/gtsam/inference/BayesTreeCliqueBase-inst.h @@ -123,7 +123,7 @@ namespace gtsam { gttoc(BayesTreeCliqueBase_shortcut); FactorGraphType p_Cp_B(parent->shortcut(B, function)); // P(Sp||B) gttic(BayesTreeCliqueBase_shortcut); - p_Cp_B += parent->conditional_; // P(Fp|Sp) + p_Cp_B.push_back(parent->conditional_); // P(Fp|Sp) // Determine the variables we want to keepSet, S union B KeyVector keep = shortcut_indices(B, p_Cp_B); @@ -171,7 +171,7 @@ namespace gtsam { gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss); // now add the parent conditional - p_Cp += parent->conditional_; // P(Fp|Sp) + p_Cp.push_back(parent->conditional_); // P(Fp|Sp) // The variables we want to keepSet are exactly the ones in S KeyVector indicesS(this->conditional()->beginParents(), @@ -198,7 +198,7 @@ namespace gtsam { // initialize with separator marginal P(S) FactorGraphType p_C = this->separatorMarginal(function); // add the conditional P(F|S) - p_C += std::shared_ptr(this->conditional_); + p_C.push_back(std::shared_ptr(this->conditional_)); return p_C; } diff --git a/gtsam/inference/ClusterTree-inst.h b/gtsam/inference/ClusterTree-inst.h index fe5c53e36..a6175f76b 100644 --- a/gtsam/inference/ClusterTree-inst.h +++ b/gtsam/inference/ClusterTree-inst.h @@ -185,8 +185,8 @@ struct EliminationData { // Gather factors FactorGraphType gatheredFactors; gatheredFactors.reserve(node->factors.size() + node->nrChildren()); - gatheredFactors += node->factors; - gatheredFactors += myData.childFactors; + gatheredFactors.push_back(node->factors); + gatheredFactors.push_back(myData.childFactors); // Check for Bayes tree orphan subtrees, and add them to our children // TODO(frank): should this really happen here? diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h index a45e3df23..0ab8b49a4 100644 --- a/gtsam/inference/Conditional.h +++ b/gtsam/inference/Conditional.h @@ -18,8 +18,6 @@ // \callgraph #pragma once -#include - #include namespace gtsam { diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index caaa13cae..f1dc37c37 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -29,7 +29,10 @@ #include // for Eigen::aligned_allocator +#ifdef GTSAM_USE_BOOST_FEATURES #include +#endif + #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION #include #include @@ -212,6 +215,7 @@ class FactorGraph { push_back(factor); } +#ifdef GTSAM_USE_BOOST_FEATURES /// `+=` works well with boost::assign list inserter. template typename std::enable_if< @@ -221,6 +225,7 @@ class FactorGraph { return boost::assign::make_list_inserter(RefCallPushBack(*this))( factor); } +#endif /// @} /// @name Adding via iterators @@ -271,6 +276,7 @@ class FactorGraph { push_back(factorOrContainer); } +#ifdef GTSAM_USE_BOOST_FEATURES /** * Add a factor or container of factors, including STL collections, * BayesTrees, etc. @@ -281,6 +287,7 @@ class FactorGraph { return boost::assign::make_list_inserter(CRefCallPushBack(*this))( factorOrContainer); } +#endif /// @} /// @name Specialized versions diff --git a/gtsam/inference/ISAM-inst.h b/gtsam/inference/ISAM-inst.h index b93c58c48..dc0750b72 100644 --- a/gtsam/inference/ISAM-inst.h +++ b/gtsam/inference/ISAM-inst.h @@ -36,12 +36,12 @@ void ISAM::updateInternal(const FactorGraphType& newFactors, // Add the removed top and the new factors FactorGraphType factors; - factors += bn; - factors += newFactors; + factors.push_back(bn); + factors.push_back(newFactors); // Add the orphaned subtrees for (const sharedClique& orphan : *orphans) - factors += std::make_shared >(orphan); + factors.template emplace_shared >(orphan); // Get an ordering where the new keys are eliminated last const VariableIndex index(factors); diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h index 0a4c88948..0f79c2a9a 100644 --- a/gtsam/inference/JunctionTree-inst.h +++ b/gtsam/inference/JunctionTree-inst.h @@ -77,9 +77,9 @@ struct ConstructorTraversalData { symbolicFactors.reserve( ETreeNode->factors.size() + myData.childSymbolicFactors.size()); // Add ETree node factors - symbolicFactors += ETreeNode->factors; + symbolicFactors.push_back(ETreeNode->factors); // Add symbolic factors passed up from children - symbolicFactors += myData.childSymbolicFactors; + symbolicFactors.push_back(myData.childSymbolicFactors); Ordering keyAsOrdering; keyAsOrdering.push_back(ETreeNode->key); diff --git a/gtsam/inference/MetisIndex-inl.h b/gtsam/inference/MetisIndex-inl.h index 646523372..672036373 100644 --- a/gtsam/inference/MetisIndex-inl.h +++ b/gtsam/inference/MetisIndex-inl.h @@ -44,7 +44,7 @@ void MetisIndex::augment(const FACTORGRAPH& factors) { for(const Key& key: *factors[i]) { keySet.insert(keySet.end(), key); // Keep a track of all unique keys if (intKeyBMap_.left.find(key) == intKeyBMap_.left.end()) { - intKeyBMap_.insert(bm_type::value_type(key, keyCounter)); + intKeyBMap_.insert(key, keyCounter); keyCounter++; } } diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h index 4b1f3dfce..abdf11c5f 100644 --- a/gtsam/inference/MetisIndex.h +++ b/gtsam/inference/MetisIndex.h @@ -22,17 +22,9 @@ #include #include -// Boost bimap generates many ugly warnings in CLANG -#ifdef __clang__ -# pragma clang diagnostic push -# pragma clang diagnostic ignored "-Wredeclared-class-member" -#endif -#include -#ifdef __clang__ -# pragma clang diagnostic pop -#endif - #include +#include +#include namespace gtsam { /** @@ -45,12 +37,21 @@ namespace gtsam { class GTSAM_EXPORT MetisIndex { public: typedef std::shared_ptr shared_ptr; - typedef boost::bimap bm_type; private: + // Stores Key <-> integer value relationship + struct BiMap { + std::map left; + std::unordered_map right; + void insert(const Key& left_value, const int32_t& right_value) { + left[left_value] = right_value; + right[right_value] = left_value; + } + }; + std::vector xadj_; // Index of node's adjacency list in adj std::vector adj_; // Stores ajacency lists of all nodes, appended into a single vector - boost::bimap intKeyBMap_; // Stores Key <-> integer value relationship + BiMap intKeyBMap_; // Stores Key <-> integer value relationship size_t nKeys_; public: diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index ad41aa5bb..884a93f0d 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -25,7 +25,10 @@ #include #include +#ifdef GTSAM_USE_BOOST_FEATURES #include +#endif + #include #include @@ -58,6 +61,7 @@ public: Base(keys.begin(), keys.end()) { } +#ifdef GTSAM_USE_BOOST_FEATURES /// Add new variables to the ordering as ordering += key1, key2, ... Equivalent to calling /// push_back. boost::assign::list_inserter > operator+=( @@ -65,6 +69,7 @@ public: return boost::assign::make_list_inserter( boost::assign_detail::call_push_back(*this))(key); } +#endif /** * @brief Append new keys to the ordering as `ordering += keys`. diff --git a/gtsam/inference/inferenceExceptions.h b/gtsam/inference/inferenceExceptions.h index bcd986983..fa3e3cb25 100644 --- a/gtsam/inference/inferenceExceptions.h +++ b/gtsam/inference/inferenceExceptions.h @@ -18,7 +18,6 @@ #pragma once #include -#include #include namespace gtsam { diff --git a/gtsam/inference/tests/testSymbol.cpp b/gtsam/inference/tests/testSymbol.cpp index bedd69044..af49a1e2e 100644 --- a/gtsam/inference/tests/testSymbol.cpp +++ b/gtsam/inference/tests/testSymbol.cpp @@ -18,6 +18,8 @@ #include +#include + using namespace std; using namespace gtsam; diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index f99006831..43d349b67 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -19,8 +19,6 @@ #pragma once -#include - #include #include #include diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp index 52c6a296f..ded6bc5e3 100644 --- a/gtsam/linear/KalmanFilter.cpp +++ b/gtsam/linear/KalmanFilter.cpp @@ -54,7 +54,8 @@ KalmanFilter::fuse(const State& p, GaussianFactor::shared_ptr newFactor) const { // Create a factor graph GaussianFactorGraph factorGraph; - factorGraph += p, newFactor; + factorGraph.push_back(p); + factorGraph.push_back(newFactor); // Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1) return solve(factorGraph); @@ -66,7 +67,7 @@ KalmanFilter::State KalmanFilter::init(const Vector& x0, // Create a factor graph f(x0), eliminate it into P(x0) GaussianFactorGraph factorGraph; - factorGraph += JacobianFactor(0, I_, x0, P0); // |x-x0|^2_diagSigma + factorGraph.emplace_shared(0, I_, x0, P0); // |x-x0|^2_diagSigma return solve(factorGraph); } @@ -75,7 +76,7 @@ KalmanFilter::State KalmanFilter::init(const Vector& x, const Matrix& P0) const // Create a factor graph f(x0), eliminate it into P(x0) GaussianFactorGraph factorGraph; - factorGraph += HessianFactor(0, x, P0); // 0.5*(x-x0)'*inv(Sigma)*(x-x0) + factorGraph.emplace_shared(0, x, P0); // 0.5*(x-x0)'*inv(Sigma)*(x-x0) return solve(factorGraph); } diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp index e98cbf69d..cc461e63c 100644 --- a/gtsam/linear/PCGSolver.cpp +++ b/gtsam/linear/PCGSolver.cpp @@ -22,8 +22,6 @@ #include #include -#include - #include #include #include diff --git a/gtsam/linear/SubgraphBuilder.cpp b/gtsam/linear/SubgraphBuilder.cpp index af31d760b..834fc8d12 100644 --- a/gtsam/linear/SubgraphBuilder.cpp +++ b/gtsam/linear/SubgraphBuilder.cpp @@ -25,9 +25,9 @@ #include #include +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION #include #include -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index ece8d13d4..96f4847b5 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -24,8 +24,6 @@ #include #include -#include - #include using std::cout; diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 6ec7fb764..2c9ca7f06 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -18,9 +18,6 @@ #include -#include -#include - using namespace std; namespace gtsam { diff --git a/gtsam/linear/linearExceptions.cpp b/gtsam/linear/linearExceptions.cpp index 7302380d9..75f284348 100644 --- a/gtsam/linear/linearExceptions.cpp +++ b/gtsam/linear/linearExceptions.cpp @@ -18,7 +18,6 @@ #include #include -#include namespace gtsam { diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index 8ed5d8308..966b70915 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -24,7 +24,6 @@ #include #include -#include // STL/C++ #include @@ -211,8 +210,7 @@ TEST(GaussianBayesNet, MonteCarloIntegration) { /* ************************************************************************* */ TEST(GaussianBayesNet, ordering) { - Ordering expected; - expected += _x_, _y_; + const Ordering expected{_x_, _y_}; const auto actual = noisyBayesNet.ordering(); EXPECT(assert_equal(expected, actual)); } @@ -279,15 +277,15 @@ TEST( GaussianBayesNet, backSubstituteTransposeNoisy ) TEST( GaussianBayesNet, DeterminantTest ) { GaussianBayesNet cbn; - cbn += GaussianConditional( + cbn.emplace_shared( 0, Vector2(3.0, 4.0), (Matrix2() << 1.0, 3.0, 0.0, 4.0).finished(), 1, (Matrix2() << 2.0, 1.0, 2.0, 3.0).finished(), noiseModel::Isotropic::Sigma(2, 2.0)); - cbn += GaussianConditional( + cbn.emplace_shared( 1, Vector2(5.0, 6.0), (Matrix2() << 1.0, 1.0, 0.0, 3.0).finished(), 2, (Matrix2() << 1.0, 0.0, 5.0, 2.0).finished(), noiseModel::Isotropic::Sigma(2, 2.0)); - cbn += GaussianConditional( + cbn.emplace_shared( 3, Vector2(7.0, 8.0), (Matrix2() << 1.0, 1.0, 0.0, 5.0).finished(), noiseModel::Isotropic::Sigma(2, 2.0)); double expectedDeterminant = 60.0 / 64.0; @@ -310,22 +308,22 @@ TEST(GaussianBayesNet, ComputeSteepestDescentPoint) { // Create an arbitrary Bayes Net GaussianBayesNet gbn; - gbn += GaussianConditional::shared_ptr(new GaussianConditional( + gbn.emplace_shared( 0, Vector2(1.0,2.0), (Matrix2() << 3.0,4.0,0.0,6.0).finished(), 3, (Matrix2() << 7.0,8.0,9.0,10.0).finished(), - 4, (Matrix2() << 11.0,12.0,13.0,14.0).finished())); - gbn += GaussianConditional::shared_ptr(new GaussianConditional( + 4, (Matrix2() << 11.0,12.0,13.0,14.0).finished()); + gbn.emplace_shared( 1, Vector2(15.0,16.0), (Matrix2() << 17.0,18.0,0.0,20.0).finished(), 2, (Matrix2() << 21.0,22.0,23.0,24.0).finished(), - 4, (Matrix2() << 25.0,26.0,27.0,28.0).finished())); - gbn += GaussianConditional::shared_ptr(new GaussianConditional( + 4, (Matrix2() << 25.0,26.0,27.0,28.0).finished()); + gbn.emplace_shared( 2, Vector2(29.0,30.0), (Matrix2() << 31.0,32.0,0.0,34.0).finished(), - 3, (Matrix2() << 35.0,36.0,37.0,38.0).finished())); - gbn += GaussianConditional::shared_ptr(new GaussianConditional( + 3, (Matrix2() << 35.0,36.0,37.0,38.0).finished()); + gbn.emplace_shared( 3, Vector2(39.0,40.0), (Matrix2() << 41.0,42.0,0.0,44.0).finished(), - 4, (Matrix2() << 45.0,46.0,47.0,48.0).finished())); - gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 4, Vector2(49.0,50.0), (Matrix2() << 51.0,52.0,0.0,54.0).finished())); + 4, (Matrix2() << 45.0,46.0,47.0,48.0).finished()); + gbn.emplace_shared( + 4, Vector2(49.0,50.0), (Matrix2() << 51.0,52.0,0.0,54.0).finished()); // Compute the Hessian numerically Matrix hessian = numericalHessian( diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp index 52a3123bf..899d69814 100644 --- a/gtsam/linear/tests/testGaussianBayesTree.cpp +++ b/gtsam/linear/tests/testGaussianBayesTree.cpp @@ -299,10 +299,10 @@ TEST(GaussianBayesTree, determinant_and_smallestEigenvalue) { GaussianFactorGraph fg; Key x1 = 2, x2 = 0, l1 = 1; SharedDiagonal unit2 = noiseModel::Unit::Create(2); - fg += JacobianFactor(x1, 10 * I_2x2, -1.0 * Vector::Ones(2), unit2); - fg += JacobianFactor(x2, 10 * I_2x2, x1, -10 * I_2x2, Vector2(2.0, -1.0), unit2); - fg += JacobianFactor(l1, 5 * I_2x2, x1, -5 * I_2x2, Vector2(0.0, 1.0), unit2); - fg += JacobianFactor(x2, -5 * I_2x2, l1, 5 * I_2x2, Vector2(-1.0, 1.5), unit2); + fg.emplace_shared(x1, 10 * I_2x2, -1.0 * Vector::Ones(2), unit2); + fg.emplace_shared(x2, 10 * I_2x2, x1, -10 * I_2x2, Vector2(2.0, -1.0), unit2); + fg.emplace_shared(l1, 5 * I_2x2, x1, -5 * I_2x2, Vector2(0.0, 1.0), unit2); + fg.emplace_shared(x2, -5 * I_2x2, l1, 5 * I_2x2, Vector2(-1.0, 1.5), unit2); // create corresponding Bayes tree: std::shared_ptr bt = fg.eliminateMultifrontal(); @@ -327,15 +327,14 @@ TEST(GaussianBayesTree, LogDeterminant) { GaussianFactorGraph fg; Key x1 = X(1), x2 = X(2), l1 = L(1); SharedDiagonal unit2 = noiseModel::Unit::Create(2); - fg += JacobianFactor(x1, 10 * I_2x2, -1.0 * Vector2::Ones(), unit2); - fg += JacobianFactor(x2, 10 * I_2x2, x1, -10 * I_2x2, Vector2(2.0, -1.0), - unit2); - fg += JacobianFactor(l1, 5 * I_2x2, x1, -5 * I_2x2, Vector2(0.0, 1.0), unit2); - fg += - JacobianFactor(x2, -5 * I_2x2, l1, 5 * I_2x2, Vector2(-1.0, 1.5), unit2); - fg += JacobianFactor(x3, 10 * I_2x2, x2, -10 * I_2x2, Vector2(2.0, -1.0), - unit2); - fg += JacobianFactor(x3, 10 * I_2x2, -1.0 * Vector2::Ones(), unit2); + fg.emplace_shared(x1, 10 * I_2x2, -1.0 * Vector2::Ones(), unit2); + fg.emplace_shared(x2, 10 * I_2x2, x1, -10 * I_2x2, + Vector2(2.0, -1.0), unit2); + fg.emplace_shared(l1, 5 * I_2x2, x1, -5 * I_2x2, Vector2(0.0, 1.0), unit2); + fg.emplace_shared(x2, -5 * I_2x2, l1, 5 * I_2x2, Vector2(-1.0, 1.5), unit2); + fg.emplace_shared(x3, 10 * I_2x2, x2, -10 * I_2x2, + Vector2(2.0, -1.0), unit2); + fg.emplace_shared(x3, 10 * I_2x2, -1.0 * Vector2::Ones(), unit2); // create corresponding Bayes net and Bayes tree: std::shared_ptr bn = fg.eliminateSequential(); diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index bf8127541..e9e626296 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -51,11 +51,10 @@ TEST(GaussianFactorGraph, initialization) { GaussianFactorGraph fg; SharedDiagonal unit2 = noiseModel::Unit::Create(2); - fg += - JacobianFactor(0, 10*I_2x2, -1.0*Vector::Ones(2), unit2), - JacobianFactor(0, -10*I_2x2,1, 10*I_2x2, Vector2(2.0, -1.0), unit2), - JacobianFactor(0, -5*I_2x2, 2, 5*I_2x2, Vector2(0.0, 1.0), unit2), - JacobianFactor(1, -5*I_2x2, 2, 5*I_2x2, Vector2(-1.0, 1.5), unit2); + fg.emplace_shared(0, 10*I_2x2, -1.0*Vector::Ones(2), unit2); + fg.emplace_shared(0, -10*I_2x2,1, 10*I_2x2, Vector2(2.0, -1.0), unit2); + fg.emplace_shared(0, -5*I_2x2, 2, 5*I_2x2, Vector2(0.0, 1.0), unit2); + fg.emplace_shared(1, -5*I_2x2, 2, 5*I_2x2, Vector2(-1.0, 1.5), unit2); EXPECT_LONGS_EQUAL(4, (long)fg.size()); @@ -186,13 +185,13 @@ static GaussianFactorGraph createSimpleGaussianFactorGraph() { Key x1 = 2, x2 = 0, l1 = 1; SharedDiagonal unit2 = noiseModel::Unit::Create(2); // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] - fg += JacobianFactor(x1, 10 * I_2x2, -1.0 * Vector::Ones(2), unit2); + fg.emplace_shared(x1, 10 * I_2x2, -1.0 * Vector::Ones(2), unit2); // odometry between x1 and x2: x2-x1=[0.2;-0.1] - fg += JacobianFactor(x2, 10 * I_2x2, x1, -10 * I_2x2, Vector2(2.0, -1.0), unit2); + fg.emplace_shared(x2, 10 * I_2x2, x1, -10 * I_2x2, Vector2(2.0, -1.0), unit2); // measurement between x1 and l1: l1-x1=[0.0;0.2] - fg += JacobianFactor(l1, 5 * I_2x2, x1, -5 * I_2x2, Vector2(0.0, 1.0), unit2); + fg.emplace_shared(l1, 5 * I_2x2, x1, -5 * I_2x2, Vector2(0.0, 1.0), unit2); // measurement between x2 and l1: l1-x2=[-0.2;0.3] - fg += JacobianFactor(x2, -5 * I_2x2, l1, 5 * I_2x2, Vector2(-1.0, 1.5), unit2); + fg.emplace_shared(x2, -5 * I_2x2, l1, 5 * I_2x2, Vector2(-1.0, 1.5), unit2); return fg; } diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp index 332bc4dbd..0ad77b366 100644 --- a/gtsam/linear/tests/testJacobianFactor.cpp +++ b/gtsam/linear/tests/testJacobianFactor.cpp @@ -25,8 +25,6 @@ #include #include -#include - using namespace std; using namespace gtsam; diff --git a/gtsam/linear/tests/testRegularHessianFactor.cpp b/gtsam/linear/tests/testRegularHessianFactor.cpp index 2e4d2249e..b86451600 100644 --- a/gtsam/linear/tests/testRegularHessianFactor.cpp +++ b/gtsam/linear/tests/testRegularHessianFactor.cpp @@ -66,11 +66,11 @@ TEST(RegularHessianFactor, Constructors) // Test constructor from Gaussian Factor Graph GaussianFactorGraph gfg; - gfg += jf; + gfg.push_back(jf); RegularHessianFactor<2> factor4(gfg); EXPECT(assert_equal(factor, factor4)); GaussianFactorGraph gfg2; - gfg2 += factor; + gfg2.push_back(factor); RegularHessianFactor<2> factor5(gfg); EXPECT(assert_equal(factor, factor5)); diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 94a37c2d2..dc0866045 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -27,7 +27,6 @@ #include #include #include -#include namespace gtsam { @@ -336,6 +335,7 @@ public: OptionalMatrixType H5, OptionalMatrixType H6) const override; private: +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -345,6 +345,7 @@ public: "NoiseModelFactor6", boost::serialization::base_object(*this)); ar& BOOST_SERIALIZATION_NVP(_PIM_); } +#endif public: GTSAM_MAKE_ALIGNED_OPERATOR_NEW diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 59ff688af..40bc15e11 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -320,6 +320,7 @@ public: private: +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template @@ -329,6 +330,7 @@ private: boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(_PIM_); } +#endif }; // class ImuFactor2 diff --git a/gtsam/navigation/tests/testAttitudeFactor.cpp b/gtsam/navigation/tests/testAttitudeFactor.cpp index 0973fe8ac..f1fde1dca 100644 --- a/gtsam/navigation/tests/testAttitudeFactor.cpp +++ b/gtsam/navigation/tests/testAttitudeFactor.cpp @@ -20,7 +20,6 @@ #include #include -#include #include #include "gtsam/base/Matrix.h" #include diff --git a/gtsam/navigation/tests/testBarometricFactor.cpp b/gtsam/navigation/tests/testBarometricFactor.cpp index 85ca25075..c7e9032fe 100644 --- a/gtsam/navigation/tests/testBarometricFactor.cpp +++ b/gtsam/navigation/tests/testBarometricFactor.cpp @@ -21,8 +21,6 @@ #include #include -#include - using namespace std::placeholders; using namespace std; using namespace gtsam; diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index aa2559575..3c8f426cb 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -30,7 +30,6 @@ #include #include -#include #include #include "imuFactorTesting.h" diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index f15e00456..2b5dee4ff 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -20,8 +20,6 @@ #include #include -#include - #include #include diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index e658639b0..b8f081518 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -20,7 +20,6 @@ #include #include -#include #include using namespace std::placeholders; diff --git a/gtsam/nonlinear/AdaptAutoDiff.h b/gtsam/nonlinear/AdaptAutoDiff.h index b616a0e63..8f67eb379 100644 --- a/gtsam/nonlinear/AdaptAutoDiff.h +++ b/gtsam/nonlinear/AdaptAutoDiff.h @@ -22,9 +22,6 @@ #include #include -#include -#include - namespace gtsam { /** diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index 6e3c634cc..fb8087133 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -218,7 +218,7 @@ protected: template class ScalarMultiplyExpression : public Expression { // Check that T is a vector space - BOOST_CONCEPT_ASSERT((gtsam::IsVectorSpace)); + GTSAM_CONCEPT_ASSERT(gtsam::IsVectorSpace); public: explicit ScalarMultiplyExpression(double s, const Expression& e); @@ -231,7 +231,7 @@ class ScalarMultiplyExpression : public Expression { template class BinarySumExpression : public Expression { // Check that T is a vector space - BOOST_CONCEPT_ASSERT((gtsam::IsVectorSpace)); + GTSAM_CONCEPT_ASSERT(gtsam::IsVectorSpace); public: explicit BinarySumExpression(const Expression& e1, const Expression& e2); diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index ada4609b9..269bdf924 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -42,7 +42,7 @@ namespace gtsam { */ template class ExpressionFactor : public NoiseModelFactor { - BOOST_CONCEPT_ASSERT((IsTestable)); + GTSAM_CONCEPT_ASSERT(IsTestable); protected: @@ -288,6 +288,7 @@ private: return expression(keys); } +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION friend class boost::serialization::access; template void serialize(ARCHIVE &ar, const unsigned int /*version*/) { @@ -295,6 +296,7 @@ private: "ExpressionFactorN", boost::serialization::base_object>(*this)); } +#endif }; /// traits template diff --git a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h index f252454ef..5bf643c12 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h @@ -35,8 +35,7 @@ namespace gtsam { // Compute the marginal on the last key // Solve the linear factor graph, converting it into a linear Bayes Network // P(x0,x1) = P(x0|x1)*P(x1) - Ordering lastKeyAsOrdering; - lastKeyAsOrdering += lastKey; + const Ordering lastKeyAsOrdering{lastKey}; const GaussianConditional::shared_ptr marginal = linearFactorGraph.marginalMultifrontalBayesNet(lastKeyAsOrdering)->front(); diff --git a/gtsam/nonlinear/ExtendedKalmanFilter.h b/gtsam/nonlinear/ExtendedKalmanFilter.h index e931fc561..a270bc4b1 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter.h @@ -44,8 +44,8 @@ namespace gtsam { template class ExtendedKalmanFilter { // Check that VALUE type is a testable Manifold - BOOST_CONCEPT_ASSERT((IsTestable)); - BOOST_CONCEPT_ASSERT((IsManifold)); + GTSAM_CONCEPT_ASSERT1(IsTestable); + GTSAM_CONCEPT_ASSERT2(IsManifold); public: typedef std::shared_ptr > shared_ptr; diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 727a8befd..f4c61e659 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -294,8 +294,7 @@ void ISAM2::recalculateIncremental(const ISAM2UpdateParams& updateParams, gttic(orphans); // Add the orphaned subtrees for (const auto& orphan : *orphans) - factors += - std::make_shared >(orphan); + factors.emplace_shared >(orphan); gttoc(orphans); // 3. Re-order and eliminate the factor graph into a Bayes net (Algorithm diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 66f4c6c70..922ba9129 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -26,7 +26,9 @@ #include #include #include +#ifdef GTSAM_USE_BOOST_FEATURES #include +#endif #include #include @@ -121,6 +123,7 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear, auto currentState = static_cast(state_.get()); bool verbose = (params_.verbosityLM >= LevenbergMarquardtParams::TRYLAMBDA); +#ifdef GTSAM_USE_BOOST_FEATURES #ifdef GTSAM_USING_NEW_BOOST_TIMERS boost::timer::cpu_timer lamda_iteration_timer; lamda_iteration_timer.start(); @@ -128,6 +131,9 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear, boost::timer lamda_iteration_timer; lamda_iteration_timer.restart(); #endif +#else + auto start = std::chrono::high_resolution_clock::now(); +#endif if (verbose) cout << "trying lambda = " << currentState->lambda << endl; @@ -215,11 +221,16 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear, } // if (systemSolvedSuccessfully) if (params_.verbosityLM == LevenbergMarquardtParams::SUMMARY) { +#ifdef GTSAM_USE_BOOST_FEATURES // do timing #ifdef GTSAM_USING_NEW_BOOST_TIMERS double iterationTime = 1e-9 * lamda_iteration_timer.elapsed().wall; #else double iterationTime = lamda_iteration_timer.elapsed(); +#endif +#else + auto end = std::chrono::high_resolution_clock::now(); + double iterationTime = std::chrono::duration_cast(end - start).count() / 1e6; #endif if (currentState->iterations == 0) { cout << "iter cost cost_change lambda success iter_time" << endl; @@ -228,7 +239,6 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear, << costChange << " " << setw(3) << setprecision(2) << currentState->lambda << " " << setw(4) << systemSolvedSuccessfully << " " << setw(3) << setprecision(2) << iterationTime << endl; } - if (step_is_successful) { // we have successfully decreased the cost and we have good modelFidelity // NOTE(frank): As we return immediately after this, we move the newValues diff --git a/gtsam/nonlinear/LinearContainerFactor.cpp b/gtsam/nonlinear/LinearContainerFactor.cpp index 57136214d..160423a64 100644 --- a/gtsam/nonlinear/LinearContainerFactor.cpp +++ b/gtsam/nonlinear/LinearContainerFactor.cpp @@ -213,7 +213,7 @@ NonlinearFactorGraph LinearContainerFactor::ConvertLinearGraph( result.reserve(linear_graph.size()); for (const auto& f : linear_graph) if (f) - result += std::make_shared(f, linearizationPoint); + result.emplace_shared(f, linearizationPoint); return result; } diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index c8ab0c2af..f81486b30 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -21,8 +21,6 @@ #include #include -#include - #include #include #include @@ -346,6 +344,7 @@ class NonlinearEquality2 : public NoiseModelFactorN { GTSAM_MAKE_ALIGNED_OPERATOR_NEW private: +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// Serialization function friend class boost::serialization::access; template @@ -354,6 +353,7 @@ class NonlinearEquality2 : public NoiseModelFactorN { ar& boost::serialization::make_nvp( "NoiseModelFactor2", boost::serialization::base_object(*this)); } +#endif }; // \NonlinearEquality2 diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index d22409875..e8ae9fd47 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -26,7 +26,7 @@ #include #include #include -#include // boost::index_sequence +#include #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION #include @@ -609,7 +609,7 @@ protected: Vector unwhitenedError( const Values& x, OptionalMatrixVecType H = nullptr) const override { - return unwhitenedError(boost::mp11::index_sequence_for{}, x, + return unwhitenedError(gtsam::index_sequence_for{}, x, H); } @@ -702,7 +702,7 @@ protected: */ template inline Vector unwhitenedError( - boost::mp11::index_sequence, // + gtsam::index_sequence, // const Values& x, OptionalMatrixVecType H = nullptr) const { if (this->active(x)) { diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index f8aaf4f39..6e57a2cf5 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -199,9 +199,9 @@ SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic() const for (const sharedFactor& factor: factors_) { if(factor) - *symbolic += SymbolicFactor(*factor); + symbolic->push_back(SymbolicFactor(*factor)); else - *symbolic += SymbolicFactorGraph::sharedFactor(); + symbolic->push_back(SymbolicFactorGraph::sharedFactor()); } return symbolic; @@ -265,11 +265,11 @@ GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& li linearFG->reserve(size()); // linearize all factors - for(const sharedFactor& factor: factors_) { - if(factor) { - (*linearFG) += factor->linearize(linearizationPoint); + for (const sharedFactor& factor : factors_) { + if (factor) { + linearFG->push_back(factor->linearize(linearizationPoint)); } else - (*linearFG) += GaussianFactor::shared_ptr(); + linearFG->push_back(GaussianFactor::shared_ptr()); } #endif diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 92bfdaf20..cf2565725 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -189,7 +189,7 @@ namespace gtsam { template void addExpressionFactor(const SharedNoiseModel& R, const T& z, const Expression& h) { - push_back(std::make_shared >(R, z, h)); + this->emplace_shared>(R, z, h); } /** diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 8aa611976..7ca5ead95 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -27,8 +27,6 @@ #include -#include - #include #include #include diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index f509fe534..61ea6684a 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -25,11 +25,8 @@ #pragma once #include -#include #include -#include - #include // Only so Eclipse finds class definition namespace gtsam { diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h index cf30b1f65..878b2b9d8 100644 --- a/gtsam/nonlinear/internal/ExpressionNode.h +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -38,7 +38,7 @@ T & upAlign(T & value, unsigned requiredAlignment = TraceAlignment) { // right now only word sized types are supported. // Easy to extend if needed, // by somehow inferring the unsigned integer of same size - BOOST_STATIC_ASSERT(sizeof(T) == sizeof(size_t)); + static_assert(sizeof(T) == sizeof(size_t)); size_t & uiValue = reinterpret_cast(value); size_t misAlignment = uiValue % requiredAlignment; if (misAlignment) { @@ -559,7 +559,7 @@ public: template class ScalarMultiplyNode : public ExpressionNode { // Check that T is a vector space - BOOST_CONCEPT_ASSERT((gtsam::IsVectorSpace)); + GTSAM_CONCEPT_ASSERT(gtsam::IsVectorSpace); double scalar_; std::shared_ptr > expression_; diff --git a/gtsam/nonlinear/internal/LevenbergMarquardtState.h b/gtsam/nonlinear/internal/LevenbergMarquardtState.h index aa0a3daa3..be1afc972 100644 --- a/gtsam/nonlinear/internal/LevenbergMarquardtState.h +++ b/gtsam/nonlinear/internal/LevenbergMarquardtState.h @@ -131,7 +131,7 @@ struct LevenbergMarquardtState : public NonlinearOptimizerState { const Key& key = key_dim.first; const size_t& dim = key_dim.second; const CachedModel* item = getCachedModel(dim); - damped += std::make_shared(key, item->A, item->b, item->model); + damped.emplace_shared(key, item->A, item->b, item->model); } return damped; } @@ -147,7 +147,7 @@ struct LevenbergMarquardtState : public NonlinearOptimizerState { const size_t dim = key_vector.second.size(); CachedModel* item = getCachedModel(dim); item->A.diagonal() = sqrtHessianDiagonal.at(key); // use diag(hessian) - damped += std::make_shared(key, item->A, item->b, item->model); + damped.emplace_shared(key, item->A, item->b, item->model); } catch (const std::out_of_range&) { continue; // Don't attempt any damping if no key found in diagonal } diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index d481ebb6a..e7a05cf62 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -358,6 +358,7 @@ virtual class DoglegOptimizer : gtsam::NonlinearOptimizer { double getDelta() const; }; +// TODO(dellaert): This will only work when GTSAM_USE_BOOST_FEATURES is true. #include template virtual class GncOptimizer { diff --git a/gtsam/nonlinear/nonlinearExceptions.h b/gtsam/nonlinear/nonlinearExceptions.h index 7b704ac39..e36b72fb7 100644 --- a/gtsam/nonlinear/nonlinearExceptions.h +++ b/gtsam/nonlinear/nonlinearExceptions.h @@ -17,7 +17,6 @@ */ #pragma once -#include #include #include diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 1ad7277dc..1af18ef95 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -25,7 +25,6 @@ #include #include -#include #include #include #include diff --git a/gtsam/sam/BearingRangeFactor.h b/gtsam/sam/BearingRangeFactor.h index a1e6251b5..d5467be47 100644 --- a/gtsam/sam/BearingRangeFactor.h +++ b/gtsam/sam/BearingRangeFactor.h @@ -21,7 +21,6 @@ #include #include -#include namespace gtsam { diff --git a/gtsam/sam/tests/testRangeFactor.cpp b/gtsam/sam/tests/testRangeFactor.cpp index 6a71cbd2c..f45349c6b 100644 --- a/gtsam/sam/tests/testRangeFactor.cpp +++ b/gtsam/sam/tests/testRangeFactor.cpp @@ -25,7 +25,6 @@ #include #include -#include using namespace std::placeholders; using namespace std; diff --git a/gtsam/sfm/BinaryMeasurement.h b/gtsam/sfm/BinaryMeasurement.h index 1f991a05a..debb27bc1 100644 --- a/gtsam/sfm/BinaryMeasurement.h +++ b/gtsam/sfm/BinaryMeasurement.h @@ -35,7 +35,7 @@ namespace gtsam { template class BinaryMeasurement : public Factor { // Check that T type is testable - BOOST_CONCEPT_ASSERT((IsTestable)); + GTSAM_CONCEPT_ASSERT(IsTestable); public: // shorthand for a smart pointer to a measurement diff --git a/gtsam/sfm/DsfTrackGenerator.cpp b/gtsam/sfm/DsfTrackGenerator.cpp index e82880193..6880138d9 100644 --- a/gtsam/sfm/DsfTrackGenerator.cpp +++ b/gtsam/sfm/DsfTrackGenerator.cpp @@ -20,6 +20,7 @@ #include #include +#include namespace gtsam { diff --git a/gtsam/sfm/SfmTrack.h b/gtsam/sfm/SfmTrack.h index 6ef1f9512..2b494ed7c 100644 --- a/gtsam/sfm/SfmTrack.h +++ b/gtsam/sfm/SfmTrack.h @@ -18,7 +18,6 @@ #pragma once -#include #include #include diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 3e5b154e3..ee533c215 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -40,8 +40,8 @@ namespace gtsam { class BetweenFactor: public NoiseModelFactorN { // Check that VALUE type is a testable Lie group - BOOST_CONCEPT_ASSERT((IsTestable)); - BOOST_CONCEPT_ASSERT((IsLieGroup)); + GTSAM_CONCEPT_ASSERT1(IsTestable); + GTSAM_CONCEPT_ASSERT2(IsLieGroup); public: diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 5605ad58e..763f3666d 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -36,7 +36,6 @@ #include #include -#include #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION #include #endif diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index 4bec99665..ba9e0dd6b 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -27,8 +27,6 @@ #include #include -#include - #include using namespace std; diff --git a/gtsam/slam/InitializePose3.h b/gtsam/slam/InitializePose3.h index bf86ab6f2..3f69217dc 100644 --- a/gtsam/slam/InitializePose3.h +++ b/gtsam/slam/InitializePose3.h @@ -21,7 +21,6 @@ #pragma once #include -#include #include #include #include diff --git a/gtsam/slam/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h index 6feaa7041..32aa590a6 100644 --- a/gtsam/slam/TriangulationFactor.h +++ b/gtsam/slam/TriangulationFactor.h @@ -19,7 +19,6 @@ #include #include -#include namespace gtsam { @@ -86,7 +85,7 @@ public: if (model && model->dim() != traits::dimension) throw std::invalid_argument( "TriangulationFactor must be created with " - + boost::lexical_cast((int) traits::dimension) + + std::to_string((int) traits::dimension) + "-dimensional noise model."); } diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index ad1bb40ef..1ecaade54 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -30,7 +30,6 @@ #include #include #include -#include #include #include diff --git a/gtsam/slam/lago.cpp b/gtsam/slam/lago.cpp index 5407e60a2..645c5ea91 100644 --- a/gtsam/slam/lago.cpp +++ b/gtsam/slam/lago.cpp @@ -19,17 +19,16 @@ #include #include -#include #include -#include +#include #include +#include #include #include -#include - #include #include +#include using namespace std; @@ -188,7 +187,7 @@ GaussianFactorGraph buildLinearOrientationGraph( ///cout << "REG: key1= " << DefaultKeyFormatter(key1) << " key2= " << DefaultKeyFormatter(key2) << endl; double k2pi_noise = key1_DeltaTheta_key2 + orientationsToRoot.at(key1) - orientationsToRoot.at(key2); // this coincides to summing up measurements along the cycle induced by the chord - double k = boost::math::round(k2pi_noise / (2 * M_PI)); + double k = std::round(k2pi_noise / (2 * M_PI)); //if (k2pi_noise - 2*k*M_PI > 1e-5) cout << k2pi_noise - 2*k*M_PI << endl; // for debug Vector deltaThetaRegularized = (Vector(1) << key1_DeltaTheta_key2 - 2 * k * M_PI).finished(); diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 553791385..ec37b4105 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -50,7 +50,7 @@ class Graph: public NonlinearFactorGraph { public: void addMeasurement(int i, int j, const Point2& z, const SharedNoiseModel& model) { - push_back(std::make_shared(z, model, X(i), L(j))); + emplace_shared(z, model, X(i), L(j)); } void addCameraConstraint(int j, const GeneralCamera& p) { @@ -483,10 +483,7 @@ TEST(GeneralSFMFactor, BinaryJacobianFactor) { actualJacobian.augmentedInformation(), 1e-9)); // Construct from GaussianFactorGraph - GaussianFactorGraph gfg1; - gfg1 += expected; - GaussianFactorGraph gfg2; - gfg2 += actual; + GaussianFactorGraph gfg1 {expected}, gfg2 {actual}; HessianFactor hessian1(gfg1), hessian2(gfg2); EXPECT(assert_equal(hessian1, hessian2, 1e-9)); } diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index f8385352f..3be35f793 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -51,7 +51,7 @@ class Graph: public NonlinearFactorGraph { public: void addMeasurement(const int& i, const int& j, const Point2& z, const SharedNoiseModel& model) { - push_back(std::make_shared(z, model, X(i), L(j))); + emplace_shared(z, model, X(i), L(j)); } void addCameraConstraint(int j, const GeneralCamera& p) { diff --git a/gtsam/slam/tests/testLago.cpp b/gtsam/slam/tests/testLago.cpp index ed4126a89..777686284 100644 --- a/gtsam/slam/tests/testLago.cpp +++ b/gtsam/slam/tests/testLago.cpp @@ -71,17 +71,17 @@ TEST(Lago, findMinimumSpanningTree) { auto gPlus = initialize::buildPoseGraph(g); lago::PredecessorMap tree = lago::findMinimumSpanningTree(gPlus); - // We should recover the following spanning tree: - // - // x2 - // / \ - // / \ - // x3 x1 - // / - // / - // x0 - // | - // a + /* We should recover the following spanning tree: + x2 + / \ + / \ + x3 x1 + / + / + x0 + | + a + */ using initialize::kAnchorKey; EXPECT_LONGS_EQUAL(kAnchorKey, tree[kAnchorKey]); EXPECT_LONGS_EQUAL(kAnchorKey, tree[x0]); diff --git a/gtsam/slam/tests/testTriangulationFactor.cpp b/gtsam/slam/tests/testTriangulationFactor.cpp index 4173a54f8..29b0330c6 100644 --- a/gtsam/slam/tests/testTriangulationFactor.cpp +++ b/gtsam/slam/tests/testTriangulationFactor.cpp @@ -25,8 +25,6 @@ #include #include -#include - using namespace std; using namespace gtsam; using namespace std::placeholders; diff --git a/gtsam/symbolic/SymbolicBayesNet.h b/gtsam/symbolic/SymbolicBayesNet.h index f2c142762..bbfc968e1 100644 --- a/gtsam/symbolic/SymbolicBayesNet.h +++ b/gtsam/symbolic/SymbolicBayesNet.h @@ -69,7 +69,7 @@ namespace gtsam { /// Construct from a single conditional SymbolicBayesNet(SymbolicConditional&& c) { - push_back(std::make_shared(c)); + emplace_shared(c); } /** @@ -79,7 +79,7 @@ namespace gtsam { * SymbolicBayesNet(SymbolicConditional(...))(SymbolicConditional(...)); */ SymbolicBayesNet& operator()(SymbolicConditional&& c) { - push_back(std::make_shared(c)); + emplace_shared(c); return *this; } diff --git a/gtsam/symbolic/SymbolicFactor.cpp b/gtsam/symbolic/SymbolicFactor.cpp index e51b3cf29..1f26389e9 100644 --- a/gtsam/symbolic/SymbolicFactor.cpp +++ b/gtsam/symbolic/SymbolicFactor.cpp @@ -49,7 +49,7 @@ namespace gtsam { SymbolicFactor::eliminate(const Ordering& keys) const { SymbolicFactorGraph graph; - graph += *this; // TODO: Is there a way to avoid copying this factor? + graph.push_back(*this); // TODO: Is there a way to avoid copying this factor? return EliminateSymbolic(graph, keys); } diff --git a/gtsam/symbolic/SymbolicFactorGraph.cpp b/gtsam/symbolic/SymbolicFactorGraph.cpp index ddcb3b2f3..1f17b8af2 100644 --- a/gtsam/symbolic/SymbolicFactorGraph.cpp +++ b/gtsam/symbolic/SymbolicFactorGraph.cpp @@ -40,22 +40,22 @@ namespace gtsam { /* ************************************************************************* */ void SymbolicFactorGraph::push_factor(Key key) { - push_back(std::make_shared(key)); + emplace_shared(key); } /* ************************************************************************* */ void SymbolicFactorGraph::push_factor(Key key1, Key key2) { - push_back(std::make_shared(key1,key2)); + emplace_shared(key1,key2); } /* ************************************************************************* */ void SymbolicFactorGraph::push_factor(Key key1, Key key2, Key key3) { - push_back(std::make_shared(key1,key2,key3)); + emplace_shared(key1,key2,key3); } /* ************************************************************************* */ void SymbolicFactorGraph::push_factor(Key key1, Key key2, Key key3, Key key4) { - push_back(std::make_shared(key1,key2,key3,key4)); + emplace_shared(key1,key2,key3,key4); } /* ************************************************************************* */ diff --git a/gtsam/symbolic/SymbolicFactorGraph.h b/gtsam/symbolic/SymbolicFactorGraph.h index 01b532a5c..fdd5bc27c 100644 --- a/gtsam/symbolic/SymbolicFactorGraph.h +++ b/gtsam/symbolic/SymbolicFactorGraph.h @@ -97,7 +97,7 @@ namespace gtsam { /// Construct from a single factor SymbolicFactorGraph(SymbolicFactor&& c) { - push_back(std::make_shared(c)); + emplace_shared(c); } /** @@ -107,7 +107,7 @@ namespace gtsam { * SymbolicFactorGraph(SymbolicFactor(...))(SymbolicFactor(...)); */ SymbolicFactorGraph& operator()(SymbolicFactor&& c) { - push_back(std::make_shared(c)); + emplace_shared(c); return *this; } diff --git a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp index 64caf399d..bf2b6a214 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesNet.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesNet.cpp @@ -83,9 +83,9 @@ TEST(SymbolicBayesNet, Dot) { using symbol_shorthand::A; using symbol_shorthand::X; SymbolicBayesNet bn; - bn += SymbolicConditional(X(3), X(2), A(2)); - bn += SymbolicConditional(X(2), X(1), A(1)); - bn += SymbolicConditional(X(1)); + bn.emplace_shared(X(3), X(2), A(2)); + bn.emplace_shared(X(2), X(1), A(1)); + bn.emplace_shared(X(1)); DotWriter writer; writer.positionHints.emplace('a', 2); diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp index 6051fa95f..e4a47bdfb 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -257,8 +257,8 @@ TEST(BayesTree, removeTop) { // Check expected outcome SymbolicBayesNet expected; - expected += SymbolicConditional::FromKeys(Keys(_E_)(_L_)(_B_), 3); - expected += SymbolicConditional::FromKeys(Keys(_S_)(_B_)(_L_), 1); + expected.add(SymbolicConditional::FromKeys(Keys(_E_)(_L_)(_B_), 3)); + expected.add(SymbolicConditional::FromKeys(Keys(_S_)(_B_)(_L_), 1)); CHECK(assert_equal(expected, bn)); SymbolicBayesTree::Cliques expectedOrphans{bayesTree[_T_], bayesTree[_X_]}; diff --git a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp index c93eb8593..47ef754a6 100644 --- a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp @@ -110,19 +110,20 @@ TEST(EliminationTree, Create2) { // \ | // l3 SymbolicFactorGraph graph; + graph.emplace_shared(X(1), L(1)); + graph.emplace_shared(X(1), X(2)); + graph.emplace_shared(X(2), L(1)); + graph.emplace_shared(X(2), X(3)); + graph.emplace_shared(X(3), X(4)); + graph.emplace_shared(X(4), L(2)); + graph.emplace_shared(X(4), X(5)); + graph.emplace_shared(L(2), X(5)); + graph.emplace_shared(X(4), L(3)); + graph.emplace_shared(X(5), L(3)); + auto binary = [](Key j1, Key j2) -> SymbolicFactor { return SymbolicFactor(j1, j2); }; - graph += binary(X(1), L(1)); - graph += binary(X(1), X(2)); - graph += binary(X(2), L(1)); - graph += binary(X(2), X(3)); - graph += binary(X(3), X(4)); - graph += binary(X(4), L(2)); - graph += binary(X(4), X(5)); - graph += binary(L(2), X(5)); - graph += binary(X(4), L(3)); - graph += binary(X(5), L(3)); SymbolicEliminationTree expected = EliminationTreeTester::MakeTree( // ChildNodes( // diff --git a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp index 2363a0fad..8afb506ab 100644 --- a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp +++ b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp @@ -80,8 +80,7 @@ TEST(SymbolicFactorGraph, eliminatePartialSequential) { /* ************************************************************************* */ TEST(SymbolicFactorGraph, eliminateFullMultifrontal) { - Ordering ordering; - ordering += 0, 1, 2, 3; + Ordering ordering{0, 1, 2, 3}; SymbolicBayesTree actual1 = *simpleChain.eliminateMultifrontal(ordering); EXPECT(assert_equal(simpleChainBayesTree, actual1)); @@ -223,8 +222,7 @@ TEST(SymbolicFactorGraph, eliminate_disconnected_graph) { expected.emplace_shared(3, 4); expected.emplace_shared(4); - Ordering order; - order += 0, 1, 2, 3, 4; + const Ordering order{0, 1, 2, 3, 4}; SymbolicBayesNet actual = *fg.eliminateSequential(order); EXPECT(assert_equal(expected, actual)); @@ -294,9 +292,9 @@ TEST(SymbolicFactorGraph, constructFromBayesNet) { // create Bayes Net SymbolicBayesNet bayesNet; - bayesNet += SymbolicConditional(0, 1, 2); - bayesNet += SymbolicConditional(1, 2); - bayesNet += SymbolicConditional(1); + bayesNet.emplace_shared(0, 1, 2); + bayesNet.emplace_shared(1, 2); + bayesNet.emplace_shared(1); // create actual factor graph from a Bayes Net SymbolicFactorGraph actual(bayesNet); @@ -351,7 +349,7 @@ TEST(SymbolicFactorGraph, push_back) { TEST(SymbolicFactorGraph, add_factors) { SymbolicFactorGraph fg1; fg1.push_factor(10); - fg1 += SymbolicFactor::shared_ptr(); // empty slot! + fg1.push_back(SymbolicFactor::shared_ptr()); // empty slot! fg1.push_factor(11); SymbolicFactorGraph fg2; diff --git a/gtsam/symbolic/tests/testSymbolicISAM.cpp b/gtsam/symbolic/tests/testSymbolicISAM.cpp index e84af28a3..c09f6f67e 100644 --- a/gtsam/symbolic/tests/testSymbolicISAM.cpp +++ b/gtsam/symbolic/tests/testSymbolicISAM.cpp @@ -79,8 +79,8 @@ TEST( SymbolicISAM, iSAM ) // Now we modify the Bayes tree by inserting a new factor over B and S SymbolicFactorGraph fullGraph; - fullGraph += asiaGraph; - fullGraph += SymbolicFactor(_B_, _S_); + fullGraph.push_back(asiaGraph); + fullGraph.emplace_shared(_B_, _S_); // This ordering is chosen to match the one chosen by COLAMD during the ISAM update Ordering ordering {_X_, _B_, _S_, _E_, _L_, _T_}; diff --git a/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp b/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp index 796bdc49e..100814fe6 100644 --- a/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp @@ -35,7 +35,7 @@ using namespace std; ****************************************************************************/ TEST( JunctionTree, constructor ) { - Ordering order; order += 0, 1, 2, 3; + const Ordering order{0, 1, 2, 3}; SymbolicJunctionTree actual(SymbolicEliminationTree(simpleChain, order)); diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt index 98a1b4ef9..de59f547e 100644 --- a/gtsam_unstable/CMakeLists.txt +++ b/gtsam_unstable/CMakeLists.txt @@ -29,6 +29,22 @@ set (excluded_headers # "") "${CMAKE_CURRENT_SOURCE_DIR}/slam/serialization.h" ) +# if GTSAM_USE_BOOST_FEATURES is not set, then we need to exclude the following: +if(NOT GTSAM_USE_BOOST_FEATURES) + list (APPEND excluded_sources + "${CMAKE_CURRENT_SOURCE_DIR}/linear/QPSParser.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/linear/QPSSolver.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/discrete/Scheduler.cpp" + ) + list (APPEND excluded_headers + "${CMAKE_CURRENT_SOURCE_DIR}/linear/QPSParser.h" + "${CMAKE_CURRENT_SOURCE_DIR}/linear/QPSSolver.h" + "${CMAKE_CURRENT_SOURCE_DIR}/discrete/Scheduler.h" + "${CMAKE_CURRENT_SOURCE_DIR}/parition/FindSeparator.h" + "${CMAKE_CURRENT_SOURCE_DIR}/parition/FindSeparator-inl.h" + ) +endif() + # assemble core libraries foreach(subdir ${gtsam_unstable_subdirs}) # Build convenience libraries diff --git a/gtsam_unstable/discrete/Scheduler.cpp b/gtsam_unstable/discrete/Scheduler.cpp index 9543ab7cf..0de4d32c4 100644 --- a/gtsam_unstable/discrete/Scheduler.cpp +++ b/gtsam_unstable/discrete/Scheduler.cpp @@ -248,7 +248,7 @@ DiscreteBayesNet::shared_ptr Scheduler::eliminate() const { // TODO: fix this!! size_t maxKey = keys().size(); Ordering defaultKeyOrdering; - for (size_t i = 0; i < maxKey; ++i) defaultKeyOrdering += Key(i); + for (size_t i = 0; i < maxKey; ++i) defaultKeyOrdering.push_back(i); DiscreteBayesNet::shared_ptr chordal = this->eliminateSequential(defaultKeyOrdering); gttoc(my_eliminate); diff --git a/gtsam_unstable/discrete/examples/CMakeLists.txt b/gtsam_unstable/discrete/examples/CMakeLists.txt index da06b7dfc..ba4e278c9 100644 --- a/gtsam_unstable/discrete/examples/CMakeLists.txt +++ b/gtsam_unstable/discrete/examples/CMakeLists.txt @@ -1,5 +1,15 @@ -set (excluded_examples - # fileToExclude.cpp -) +# disable tests if GTSAM_USE_BOOST_FEATURES is OFF +if (NOT GTSAM_USE_BOOST_FEATURES) + set (excluded_examples + # fileToExclude.cpp + "schedulingExample.cpp" + "schedulingQuals12.cpp" + "schedulingQuals13.cpp" + ) +else() + set (excluded_examples "") +endif() + + gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam_unstable") diff --git a/gtsam_unstable/discrete/tests/CMakeLists.txt b/gtsam_unstable/discrete/tests/CMakeLists.txt index 2687a760c..1b66e5c85 100644 --- a/gtsam_unstable/discrete/tests/CMakeLists.txt +++ b/gtsam_unstable/discrete/tests/CMakeLists.txt @@ -1 +1,6 @@ -gtsamAddTestsGlob(discrete_unstable "test*.cpp" "" "gtsam_unstable") +set(excluded_sources "") +if (NOT GTSAM_USE_BOOST_FEATURES) + list(APPEND excluded_sources "testScheduler.cpp") +endif() + +gtsamAddTestsGlob(discrete_unstable "test*.cpp" "${excluded_sources}" "gtsam_unstable") diff --git a/gtsam_unstable/discrete/tests/testCSP.cpp b/gtsam_unstable/discrete/tests/testCSP.cpp index 5756cb99c..2b9a20ca6 100644 --- a/gtsam_unstable/discrete/tests/testCSP.cpp +++ b/gtsam_unstable/discrete/tests/testCSP.cpp @@ -173,9 +173,7 @@ TEST(CSP, WesternUS) { {6, 3}, {7, 2}, {8, 0}, {9, 1}, {10, 0}}; // Create ordering according to example in ND-CSP.lyx - Ordering ordering; - ordering += Key(0), Key(1), Key(2), Key(3), Key(4), Key(5), Key(6), Key(7), - Key(8), Key(9), Key(10); + const Ordering ordering{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10}; // Solve using that ordering: auto actualMPE = csp.optimize(ordering); diff --git a/gtsam_unstable/dynamics/FullIMUFactor.h b/gtsam_unstable/dynamics/FullIMUFactor.h index 325321d13..02da899b7 100644 --- a/gtsam_unstable/dynamics/FullIMUFactor.h +++ b/gtsam_unstable/dynamics/FullIMUFactor.h @@ -10,8 +10,6 @@ #include #include -#include - namespace gtsam { /** diff --git a/gtsam_unstable/dynamics/IMUFactor.h b/gtsam_unstable/dynamics/IMUFactor.h index 102c9311e..ee09600e5 100644 --- a/gtsam_unstable/dynamics/IMUFactor.h +++ b/gtsam_unstable/dynamics/IMUFactor.h @@ -10,8 +10,6 @@ #include #include -#include - namespace gtsam { /** diff --git a/gtsam_unstable/dynamics/VelocityConstraint.h b/gtsam_unstable/dynamics/VelocityConstraint.h index bd6a40008..2bb70e1b5 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint.h +++ b/gtsam_unstable/dynamics/VelocityConstraint.h @@ -10,8 +10,6 @@ #include #include -#include - namespace gtsam { namespace dynamics { diff --git a/gtsam_unstable/dynamics/tests/CMakeLists.txt b/gtsam_unstable/dynamics/tests/CMakeLists.txt index 493cef4fa..882d5f4e3 100644 --- a/gtsam_unstable/dynamics/tests/CMakeLists.txt +++ b/gtsam_unstable/dynamics/tests/CMakeLists.txt @@ -1 +1,7 @@ -gtsamAddTestsGlob(dynamics_unstable "test*.cpp" "" "gtsam_unstable") +set (excluded_tests "") + +# TODO(dellaert): these segfault, and are rather obsolete, so we stop compiling them: +list(APPEND excluded_tests "testIMUSystem.cpp" "testPoseRTV.cpp") + +# Build tests +gtsamAddTestsGlob(dynamics_unstable "test*.cpp" "${excluded_tests}" "gtsam_unstable") diff --git a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp index c7e46e400..b7206264a 100644 --- a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp +++ b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp @@ -33,20 +33,20 @@ TEST(testIMUSystem, instantiations) { // just checking for compilation PoseRTV x1_v; - gtsam::SharedNoiseModel model1 = gtsam::noiseModel::Unit::Create(1); - gtsam::SharedNoiseModel model3 = gtsam::noiseModel::Unit::Create(3); - gtsam::SharedNoiseModel model6 = gtsam::noiseModel::Unit::Create(6); - gtsam::SharedNoiseModel model9 = gtsam::noiseModel::Unit::Create(9); + SharedNoiseModel model1 = noiseModel::Unit::Create(1); + SharedNoiseModel model3 = noiseModel::Unit::Create(3); + SharedNoiseModel model6 = noiseModel::Unit::Create(6); + SharedNoiseModel model9 = noiseModel::Unit::Create(9); Vector accel = Vector::Ones(3), gyro = Vector::Ones(3); IMUFactor imu(accel, gyro, 0.01, x1, x2, model6); FullIMUFactor full_imu(accel, gyro, 0.01, x1, x2, model9); - NonlinearEquality poseHardPrior(x1, x1_v); - BetweenFactor odom(x1, x2, x1_v, model9); - RangeFactor range(x1, x2, 1.0, model1); + NonlinearEquality poseHardPrior(x1, x1_v); + BetweenFactor odom(x1, x2, x1_v, model9); + RangeFactor range(x1, x2, 1.0, model1); VelocityConstraint constraint(x1, x2, 0.1, 10000); - PriorFactor posePrior(x1, x1_v, model9); + PriorFactor posePrior(x1, x1_v, model9); DHeightPrior heightPrior(x1, 0.1, model1); VelocityPrior velPrior(x1, Vector::Ones(3), model3); } @@ -68,13 +68,13 @@ TEST( testIMUSystem, optimize_chain ) { // assemble simple graph with IMU measurements and velocity constraints NonlinearFactorGraph graph; - graph += NonlinearEquality(x1, pose1); - graph += IMUFactor(imu12, dt, x1, x2, model); - graph += IMUFactor(imu23, dt, x2, x3, model); - graph += IMUFactor(imu34, dt, x3, x4, model); - graph += VelocityConstraint(x1, x2, dt); - graph += VelocityConstraint(x2, x3, dt); - graph += VelocityConstraint(x3, x4, dt); + graph.emplace_shared>(x1, pose1); + graph.emplace_shared>(imu12, dt, x1, x2, model); + graph.emplace_shared>(imu23, dt, x2, x3, model); + graph.emplace_shared>(imu34, dt, x3, x4, model); + graph.emplace_shared(x1, x2, dt); + graph.emplace_shared(x2, x3, dt); + graph.emplace_shared(x3, x4, dt); // ground truth values Values true_values; @@ -114,10 +114,10 @@ TEST( testIMUSystem, optimize_chain_fullfactor ) { // assemble simple graph with IMU measurements and velocity constraints NonlinearFactorGraph graph; - graph += NonlinearEquality(x1, pose1); - graph += FullIMUFactor(imu12, dt, x1, x2, model); - graph += FullIMUFactor(imu23, dt, x2, x3, model); - graph += FullIMUFactor(imu34, dt, x3, x4, model); + graph.emplace_shared>(x1, pose1); + graph.emplace_shared>(imu12, dt, x1, x2, model); + graph.emplace_shared>(imu23, dt, x2, x3, model); + graph.emplace_shared>(imu34, dt, x3, x4, model); // ground truth values Values true_values; @@ -156,7 +156,7 @@ TEST( testIMUSystem, linear_trajectory) { Values true_traj, init_traj; NonlinearFactorGraph graph; - graph += NonlinearEquality(x0, start); + graph.emplace_shared>(x0, start); true_traj.insert(x0, start); init_traj.insert(x0, start); @@ -165,7 +165,7 @@ TEST( testIMUSystem, linear_trajectory) { for (size_t i=1; i(accel - g, gyro, dt, xA, xB, model); + graph.emplace_shared>(accel - g, gyro, dt, xA, xB, model); true_traj.insert(xB, cur_pose); init_traj.insert(xB, PoseRTV()); } diff --git a/gtsam_unstable/examples/CMakeLists.txt b/gtsam_unstable/examples/CMakeLists.txt index da06b7dfc..967937b22 100644 --- a/gtsam_unstable/examples/CMakeLists.txt +++ b/gtsam_unstable/examples/CMakeLists.txt @@ -1,5 +1,12 @@ -set (excluded_examples - # fileToExclude.cpp -) +set (excluded_examples "") + +# Add examples to exclude if GTSAM_USE_BOOST_FEATURES is not set +if (NOT GTSAM_USE_BOOST_FEATURES) + # add to excluded examples + list (APPEND excluded_examples + "GncPoseAveragingExample.cpp" + ) +endif() + gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam_unstable") diff --git a/gtsam_unstable/examples/ConcurrentCalibration.cpp b/gtsam_unstable/examples/ConcurrentCalibration.cpp index 75dc49eee..cccde076a 100644 --- a/gtsam_unstable/examples/ConcurrentCalibration.cpp +++ b/gtsam_unstable/examples/ConcurrentCalibration.cpp @@ -32,7 +32,6 @@ #include #include #include -#include using namespace std; using namespace gtsam; diff --git a/gtsam_unstable/examples/GncPoseAveragingExample.cpp b/gtsam_unstable/examples/GncPoseAveragingExample.cpp index 730d342f0..183de81fc 100644 --- a/gtsam_unstable/examples/GncPoseAveragingExample.cpp +++ b/gtsam_unstable/examples/GncPoseAveragingExample.cpp @@ -27,7 +27,6 @@ #include #include #include -#include using namespace std; using namespace gtsam; diff --git a/gtsam_unstable/geometry/InvDepthCamera3.h b/gtsam_unstable/geometry/InvDepthCamera3.h index eb9045dff..45b27efe3 100644 --- a/gtsam_unstable/geometry/InvDepthCamera3.h +++ b/gtsam_unstable/geometry/InvDepthCamera3.h @@ -12,7 +12,6 @@ #pragma once #include -#include #include #include #include @@ -20,6 +19,10 @@ #include #include +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#include +#endif + namespace gtsam { /** diff --git a/gtsam_unstable/geometry/Pose3Upright.cpp b/gtsam_unstable/geometry/Pose3Upright.cpp index 9bc8efd54..9cd9f78f5 100644 --- a/gtsam_unstable/geometry/Pose3Upright.cpp +++ b/gtsam_unstable/geometry/Pose3Upright.cpp @@ -5,7 +5,6 @@ * @author Alex Cunningham */ -#include #include #include "gtsam/base/OptionalJacobian.h" #include "gtsam/base/Vector.h" diff --git a/gtsam_unstable/linear/tests/CMakeLists.txt b/gtsam_unstable/linear/tests/CMakeLists.txt index 43df23daa..b2cda1811 100644 --- a/gtsam_unstable/linear/tests/CMakeLists.txt +++ b/gtsam_unstable/linear/tests/CMakeLists.txt @@ -1 +1,10 @@ -gtsamAddTestsGlob(linear_unstable "test*.cpp" "" "gtsam_unstable") +# if GTSAM_USE_BOOST_FEATURES is OFF then exclude some tests +if (NOT GTSAM_USE_BOOST_FEATURES) + # create a semicolon seperated list of files to exclude + set(EXCLUDE_TESTS "testQPSolver.cpp") + message(STATUS "Excluding ${EXCLUDE_TESTS}") +else() + set(EXCLUDE_TESTS "${EXCLUDE_TESTS}") +endif() + +gtsamAddTestsGlob(linear_unstable "test*.cpp" "${EXCLUDE_TESTS}" "gtsam_unstable") diff --git a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp index 9dc5b3fad..7bbcdd897 100644 --- a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp @@ -78,7 +78,7 @@ NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph, NonlinearFactorGraph marginalFactors; marginalFactors.reserve(marginalLinearFactors.size()); for(const GaussianFactor::shared_ptr& gaussianFactor: marginalLinearFactors) { - marginalFactors += std::make_shared(gaussianFactor, theta); + marginalFactors.emplace_shared(gaussianFactor, theta); } return marginalFactors; diff --git a/gtsam_unstable/partition/GenericGraph.cpp b/gtsam_unstable/partition/GenericGraph.cpp index 5f32ab557..4d9bf4f63 100644 --- a/gtsam_unstable/partition/GenericGraph.cpp +++ b/gtsam_unstable/partition/GenericGraph.cpp @@ -6,7 +6,7 @@ * Description: generic graph types used in partitioning */ #include -#include +#include #include @@ -464,9 +464,9 @@ namespace gtsam { namespace partition { } if (minFoundConstraintsPerCamera < minNrConstraintsPerCamera) - throw runtime_error("checkSingularity:minConstraintsPerCamera < " + boost::lexical_cast(minFoundConstraintsPerCamera)); + throw runtime_error("checkSingularity:minConstraintsPerCamera < " + std::to_string(minFoundConstraintsPerCamera)); if (minFoundConstraintsPerLandmark < minNrConstraintsPerLandmark) - throw runtime_error("checkSingularity:minConstraintsPerLandmark < " + boost::lexical_cast(minFoundConstraintsPerLandmark)); + throw runtime_error("checkSingularity:minConstraintsPerLandmark < " + std::to_string(minFoundConstraintsPerLandmark)); } }} // namespace diff --git a/gtsam_unstable/partition/tests/CMakeLists.txt b/gtsam_unstable/partition/tests/CMakeLists.txt index d89a1fe98..0b918e497 100644 --- a/gtsam_unstable/partition/tests/CMakeLists.txt +++ b/gtsam_unstable/partition/tests/CMakeLists.txt @@ -1,2 +1,7 @@ set(ignore_test "testNestedDissection.cpp") + +if (NOT GTSAM_USE_BOOST_FEATURES) + list(APPEND ignore_test "testFindSeparator.cpp") +endif() + gtsamAddTestsGlob(partition "test*.cpp" "${ignore_test}" "gtsam_unstable;gtsam;metis-gtsam-if") diff --git a/gtsam_unstable/partition/tests/testNestedDissection.cpp b/gtsam_unstable/partition/tests/testNestedDissection.cpp index da896e33d..be2d7af7f 100644 --- a/gtsam_unstable/partition/tests/testNestedDissection.cpp +++ b/gtsam_unstable/partition/tests/testNestedDissection.cpp @@ -34,7 +34,7 @@ TEST ( NestedDissection, oneIsland ) fg.addBearingRange(2, 1, Rot2(), 0., bearingRangeNoise); fg.addPoseConstraint(1, Pose2()); - Ordering ordering; ordering += x1, x2, l1; + const Ordering ordering{x1, x2, l1}; int numNodeStopPartition = 1e3; int minNodesPerMap = 1e3; @@ -61,7 +61,7 @@ TEST ( NestedDissection, TwoIslands ) fg.addOdometry(3, 5, Pose2(), odoNoise); fg.addPoseConstraint(1, Pose2()); fg.addPoseConstraint(4, Pose2()); - Ordering ordering; ordering += x1, x2, x3, x4, x5; + const Ordering ordering{x1, x2, x3, x4, x5}; int numNodeStopPartition = 2; int minNodesPerMap = 1; @@ -96,7 +96,7 @@ TEST ( NestedDissection, FourIslands ) fg.addOdometry(3, 5, Pose2(), odoNoise); fg.addPoseConstraint(1, Pose2()); fg.addPoseConstraint(4, Pose2()); - Ordering ordering; ordering += x1, x2, x3, x4, x5; + const Ordering ordering{x1, x2, x3, x4, x5}; int numNodeStopPartition = 2; int minNodesPerMap = 1; @@ -144,7 +144,7 @@ TEST ( NestedDissection, weekLinks ) fg.addPoseConstraint(1, Pose2()); fg.addPoseConstraint(4, Pose2()); fg.addPoseConstraint(5, Pose2()); - Ordering ordering; ordering += x1, x2, x3, x4, x5, l6; + const Ordering ordering{x1, x2, x3, x4, x5, l6}; int numNodeStopPartition = 2; int minNodesPerMap = 1; @@ -206,7 +206,7 @@ TEST ( NestedDissection, manual_cuts ) fg.addPrior(x0, Pose2(0.1, 0, 0), priorNoise); // generate ordering - Ordering ordering; ordering += x0, x1, x2, l1, l2, l3, l4, l5, l6; + const Ordering ordering{x0, x1, x2, l1, l2, l3, l4, l5, l6}; // define cuts std::shared_ptr cuts(new Cuts()); @@ -301,9 +301,9 @@ TEST( NestedDissection, Graph3D) { graph.addMeasurement(3, j, cameras[3].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise); // make an easy ordering - Ordering ordering; ordering += x0, x1, x2, x3; + const Ordering ordering{x0, x1, x2, x3}; for (int j=1; j<=24; j++) - ordering += Symbol('l', j); + ordering.push_back(Symbol('l', j)); // nested dissection const int numNodeStopPartition = 10; diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index f62157845..0accb8f42 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -27,8 +27,6 @@ // Using numerical derivative to calculate d(Pose3::Expmap)/dw #include -#include - #include namespace gtsam { diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index 91dba26e6..8fda5456b 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -26,8 +26,6 @@ // Using numerical derivative to calculate d(Pose3::Expmap)/dw #include -#include - #include namespace gtsam { diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h index 23f8bd3e0..a49b2090c 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant1.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h @@ -17,8 +17,6 @@ #include #include -#include - namespace gtsam { /** diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index 63fcb4d8c..7abbbe89a 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -18,8 +18,6 @@ #include #include -#include - namespace gtsam { /** diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h index 22a725d47..8664002e8 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -17,8 +17,6 @@ #include #include -#include - namespace gtsam { /** diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 0893af75e..52d6eda05 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -30,7 +30,10 @@ #include #include +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION #include +#endif + #include #include @@ -501,7 +504,7 @@ public: private: -#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// Serialization function friend class boost::serialization::access; template diff --git a/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp b/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp index 644283512..33a48cf7a 100644 --- a/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp +++ b/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp @@ -22,7 +22,6 @@ #include #include -#include #include #include diff --git a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp index afd51e1f6..64af687d4 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp @@ -62,10 +62,9 @@ TEST( InvDepthFactor, optimize) { gtsam::NonlinearFactorGraph graph; Values initial; - InverseDepthFactor::shared_ptr factor(new InverseDepthFactor(expected_uv, sigma, - Symbol('x',1), Symbol('l',1), Symbol('d',1), K)); - graph.push_back(factor); - graph += PoseConstraint(Symbol('x',1),level_pose); + graph.emplace_shared(expected_uv, sigma, + Symbol('x',1), Symbol('l',1), Symbol('d',1), K); + graph.emplace_shared(Symbol('x', 1), level_pose); initial.insert(Symbol('x',1), level_pose); initial.insert(Symbol('l',1), inv_landmark); initial.insert(Symbol('d',1), inv_depth); @@ -91,7 +90,7 @@ TEST( InvDepthFactor, optimize) { Symbol('x',2), Symbol('l',1),Symbol('d',1),K)); graph.push_back(factor1); - graph += PoseConstraint(Symbol('x',2),right_pose); + graph.emplace_shared(Symbol('x',2),right_pose); initial.insert(Symbol('x',2), right_pose); diff --git a/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp b/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp index 141a50465..1d438b457 100644 --- a/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp +++ b/gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp @@ -73,10 +73,8 @@ TEST( MultiProjectionFactor, create ){ views.insert(x2); views.insert(x3); - MultiProjectionFactor mpFactor(n_measPixel, noiseProjection, views, l1, K); - graph += mpFactor; - - + graph.emplace_shared>( + n_measPixel, noiseProjection, views, l1, K); } diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 5eaad45dc..d7b68c4ec 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -1,14 +1,23 @@ # exclude certain files # note the source dir on each -set (tests_exclude "") +set (excluded_tests "") if (${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang") # might not be best test - Richard & Jason & Frank # clang linker segfaults on large testSerializationSlam - list (APPEND tests_exclude "testSerializationSlam.cpp") + list (APPEND excluded_tests "testSerializationSlam.cpp") +endif() + +if (NOT GTSAM_USE_BOOST_FEATURES) + list(APPEND excluded_tests "testGncOptimizer.cpp") + list(APPEND excluded_tests "testGraph.cpp") +endif() + +if (NOT GTSAM_ENABLE_BOOST_SERIALIZATION) + list(APPEND excluded_tests "testSerializationSLAM.cpp") endif() # Build tests -gtsamAddTestsGlob(tests "test*.cpp" "${tests_exclude}" "gtsam") +gtsamAddTestsGlob(tests "test*.cpp" "${excluded_tests}" "gtsam") if(MSVC) set_property(SOURCE "${CMAKE_CURRENT_SOURCE_DIR}/testSerializationSlam.cpp" diff --git a/tests/smallExample.h b/tests/smallExample.h index 5ffb5d47b..c5ab10594 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -274,16 +274,16 @@ inline GaussianFactorGraph createGaussianFactorGraph() { GaussianFactorGraph fg; // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] - fg += JacobianFactor(X(1), 10*I_2x2, -1.0*Vector::Ones(2)); + fg.emplace_shared(X(1), 10*I_2x2, -1.0*Vector::Ones(2)); // odometry between x1 and x2: x2-x1=[0.2;-0.1] - fg += JacobianFactor(X(1), -10*I_2x2, X(2), 10*I_2x2, Vector2(2.0, -1.0)); + fg.emplace_shared(X(1), -10*I_2x2, X(2), 10*I_2x2, Vector2(2.0, -1.0)); // measurement between x1 and l1: l1-x1=[0.0;0.2] - fg += JacobianFactor(X(1), -5*I_2x2, L(1), 5*I_2x2, Vector2(0.0, 1.0)); + fg.emplace_shared(X(1), -5*I_2x2, L(1), 5*I_2x2, Vector2(0.0, 1.0)); // measurement between x2 and l1: l1-x2=[-0.2;0.3] - fg += JacobianFactor(X(2), -5*I_2x2, L(1), 5*I_2x2, Vector2(-1.0, 1.5)); + fg.emplace_shared(X(2), -5*I_2x2, L(1), 5*I_2x2, Vector2(-1.0, 1.5)); return fg; } diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp index c67914c8b..4aa357ba2 100644 --- a/tests/testBoundingConstraint.cpp +++ b/tests/testBoundingConstraint.cpp @@ -144,9 +144,9 @@ TEST( testBoundingConstraint, unary_simple_optimization1) { NonlinearFactorGraph graph; Symbol x1('x',1); - graph += iq2D::PoseXInequality(x1, 1.0, true); - graph += iq2D::PoseYInequality(x1, 2.0, true); - graph += simulated2D::Prior(start_pt, soft_model2, x1); + graph.emplace_shared(x1, 1.0, true); + graph.emplace_shared(x1, 2.0, true); + graph.emplace_shared(start_pt, soft_model2, x1); Values initValues; initValues.insert(x1, start_pt); @@ -165,9 +165,9 @@ TEST( testBoundingConstraint, unary_simple_optimization2) { Point2 start_pt(2.0, 3.0); NonlinearFactorGraph graph; - graph += iq2D::PoseXInequality(key, 1.0, false); - graph += iq2D::PoseYInequality(key, 2.0, false); - graph += simulated2D::Prior(start_pt, soft_model2, key); + graph.emplace_shared(key, 1.0, false); + graph.emplace_shared(key, 2.0, false); + graph.emplace_shared(start_pt, soft_model2, key); Values initValues; initValues.insert(key, start_pt); @@ -224,9 +224,9 @@ TEST( testBoundingConstraint, MaxDistance_simple_optimization) { Symbol x1('x',1), x2('x',2); NonlinearFactorGraph graph; - graph += simulated2D::equality_constraints::UnaryEqualityConstraint(pt1, x1); - graph += simulated2D::Prior(pt2_init, soft_model2_alt, x2); - graph += iq2D::PoseMaxDistConstraint(x1, x2, 2.0); + graph.emplace_shared(pt1, x1); + graph.emplace_shared(pt2_init, soft_model2_alt, x2); + graph.emplace_shared(x1, x2, 2.0); Values initial_state; initial_state.insert(x1, pt1); @@ -250,12 +250,12 @@ TEST( testBoundingConstraint, avoid_demo) { Point2 odo(2.0, 0.0); NonlinearFactorGraph graph; - graph += simulated2D::equality_constraints::UnaryEqualityConstraint(x1_pt, x1); - graph += simulated2D::Odometry(odo, soft_model2_alt, x1, x2); - graph += iq2D::LandmarkAvoid(x2, l1, radius); - graph += simulated2D::equality_constraints::UnaryEqualityPointConstraint(l1_pt, l1); - graph += simulated2D::Odometry(odo, soft_model2_alt, x2, x3); - graph += simulated2D::equality_constraints::UnaryEqualityConstraint(x3_pt, x3); + graph.emplace_shared(x1_pt, x1); + graph.emplace_shared(odo, soft_model2_alt, x1, x2); + graph.emplace_shared(x2, l1, radius); + graph.emplace_shared(l1_pt, l1); + graph.emplace_shared(odo, soft_model2_alt, x2, x3); + graph.emplace_shared(x3_pt, x3); Values init, expected; init.insert(x1, x1_pt); diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index 61276e89b..e64e5ab7a 100644 --- a/tests/testDoglegOptimizer.cpp +++ b/tests/testDoglegOptimizer.cpp @@ -30,7 +30,6 @@ #include #include -#include using namespace std; using namespace gtsam; @@ -43,22 +42,22 @@ using symbol_shorthand::L; TEST(DoglegOptimizer, ComputeBlend) { // Create an arbitrary Bayes Net GaussianBayesNet gbn; - gbn += GaussianConditional::shared_ptr(new GaussianConditional( + gbn.emplace_shared( 0, Vector2(1.0,2.0), (Matrix(2, 2) << 3.0,4.0,0.0,6.0).finished(), 3, (Matrix(2, 2) << 7.0,8.0,9.0,10.0).finished(), - 4, (Matrix(2, 2) << 11.0,12.0,13.0,14.0).finished())); - gbn += GaussianConditional::shared_ptr(new GaussianConditional( + 4, (Matrix(2, 2) << 11.0,12.0,13.0,14.0).finished()); + gbn.emplace_shared( 1, Vector2(15.0,16.0), (Matrix(2, 2) << 17.0,18.0,0.0,20.0).finished(), 2, (Matrix(2, 2) << 21.0,22.0,23.0,24.0).finished(), - 4, (Matrix(2, 2) << 25.0,26.0,27.0,28.0).finished())); - gbn += GaussianConditional::shared_ptr(new GaussianConditional( + 4, (Matrix(2, 2) << 25.0,26.0,27.0,28.0).finished()); + gbn.emplace_shared( 2, Vector2(29.0,30.0), (Matrix(2, 2) << 31.0,32.0,0.0,34.0).finished(), - 3, (Matrix(2, 2) << 35.0,36.0,37.0,38.0).finished())); - gbn += GaussianConditional::shared_ptr(new GaussianConditional( + 3, (Matrix(2, 2) << 35.0,36.0,37.0,38.0).finished()); + gbn.emplace_shared( 3, Vector2(39.0,40.0), (Matrix(2, 2) << 41.0,42.0,0.0,44.0).finished(), - 4, (Matrix(2, 2) << 45.0,46.0,47.0,48.0).finished())); - gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 4, Vector2(49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0).finished())); + 4, (Matrix(2, 2) << 45.0,46.0,47.0,48.0).finished()); + gbn.emplace_shared( + 4, Vector2(49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0).finished()); // Compute steepest descent point VectorValues xu = gbn.optimizeGradientSearch(); @@ -79,22 +78,22 @@ TEST(DoglegOptimizer, ComputeBlend) { TEST(DoglegOptimizer, ComputeDoglegPoint) { // Create an arbitrary Bayes Net GaussianBayesNet gbn; - gbn += GaussianConditional::shared_ptr(new GaussianConditional( + gbn.emplace_shared( 0, Vector2(1.0,2.0), (Matrix(2, 2) << 3.0,4.0,0.0,6.0).finished(), 3, (Matrix(2, 2) << 7.0,8.0,9.0,10.0).finished(), - 4, (Matrix(2, 2) << 11.0,12.0,13.0,14.0).finished())); - gbn += GaussianConditional::shared_ptr(new GaussianConditional( + 4, (Matrix(2, 2) << 11.0,12.0,13.0,14.0).finished()); + gbn.emplace_shared( 1, Vector2(15.0,16.0), (Matrix(2, 2) << 17.0,18.0,0.0,20.0).finished(), 2, (Matrix(2, 2) << 21.0,22.0,23.0,24.0).finished(), - 4, (Matrix(2, 2) << 25.0,26.0,27.0,28.0).finished())); - gbn += GaussianConditional::shared_ptr(new GaussianConditional( + 4, (Matrix(2, 2) << 25.0,26.0,27.0,28.0).finished()); + gbn.emplace_shared( 2, Vector2(29.0,30.0), (Matrix(2, 2) << 31.0,32.0,0.0,34.0).finished(), - 3, (Matrix(2, 2) << 35.0,36.0,37.0,38.0).finished())); - gbn += GaussianConditional::shared_ptr(new GaussianConditional( + 3, (Matrix(2, 2) << 35.0,36.0,37.0,38.0).finished()); + gbn.emplace_shared( 3, Vector2(39.0,40.0), (Matrix(2, 2) << 41.0,42.0,0.0,44.0).finished(), - 4, (Matrix(2, 2) << 45.0,46.0,47.0,48.0).finished())); - gbn += GaussianConditional::shared_ptr(new GaussianConditional( - 4, Vector2(49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0).finished())); + 4, (Matrix(2, 2) << 45.0,46.0,47.0,48.0).finished()); + gbn.emplace_shared( + 4, Vector2(49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0).finished()); // Compute dogleg point for different deltas diff --git a/tests/testGaussianBayesTreeB.cpp b/tests/testGaussianBayesTreeB.cpp index fedc75945..bb1180903 100644 --- a/tests/testGaussianBayesTreeB.cpp +++ b/tests/testGaussianBayesTreeB.cpp @@ -75,7 +75,7 @@ TEST( GaussianBayesTree, linear_smoother_shortcuts ) double sigma3 = 0.61808; Matrix A56 = (Matrix(2,2) << -0.382022,0.,0.,-0.382022).finished(); GaussianBayesNet expected3; - expected3 += GaussianConditional(X(5), Z_2x1, I_2x2/sigma3, X(6), A56/sigma3); + expected3.emplace_shared(X(5), Z_2x1, I_2x2/sigma3, X(6), A56/sigma3); GaussianBayesTree::sharedClique C3 = bayesTree[X(4)]; GaussianBayesNet actual3 = C3->shortcut(R); EXPECT(assert_equal(expected3,actual3,tol)); @@ -84,7 +84,7 @@ TEST( GaussianBayesTree, linear_smoother_shortcuts ) double sigma4 = 0.661968; Matrix A46 = (Matrix(2,2) << -0.146067,0.,0.,-0.146067).finished(); GaussianBayesNet expected4; - expected4 += GaussianConditional(X(4), Z_2x1, I_2x2/sigma4, X(6), A46/sigma4); + expected4.emplace_shared(X(4), Z_2x1, I_2x2/sigma4, X(6), A46/sigma4); GaussianBayesTree::sharedClique C4 = bayesTree[X(3)]; GaussianBayesNet actual4 = C4->shortcut(R); EXPECT(assert_equal(expected4,actual4,tol)); @@ -114,8 +114,7 @@ TEST(GaussianBayesTree, balanced_smoother_marginals) { GaussianFactorGraph smoother = createSmoother(7); // Create the Bayes tree - Ordering ordering; - ordering += X(1), X(3), X(5), X(7), X(2), X(6), X(4); + const Ordering ordering{X(1), X(3), X(5), X(7), X(2), X(6), X(4)}; GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering); VectorValues actualSolution = bayesTree.optimize(); @@ -162,8 +161,7 @@ TEST( GaussianBayesTree, balanced_smoother_shortcuts ) GaussianFactorGraph smoother = createSmoother(7); // Create the Bayes tree - Ordering ordering; - ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4); + const Ordering ordering{X(1), X(3), X(5), X(7), X(2), X(6), X(4)}; GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering); // Check the conditional P(Root|Root) @@ -194,8 +192,7 @@ TEST( GaussianBayesTree, balanced_smoother_shortcuts ) //TEST( BayesTree, balanced_smoother_clique_marginals ) //{ // // Create smoother with 7 nodes -// Ordering ordering; -// ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4); +// const Ordering ordering{X(1),X(3),X(5),X(7),X(2),X(6),X(4)}; // GaussianFactorGraph smoother = createSmoother(7, ordering).first; // // // Create the Bayes tree @@ -223,8 +220,7 @@ TEST( GaussianBayesTree, balanced_smoother_shortcuts ) TEST( GaussianBayesTree, balanced_smoother_joint ) { // Create smoother with 7 nodes - Ordering ordering; - ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4); + const Ordering ordering{X(1), X(3), X(5), X(7), X(2), X(6), X(4)}; GaussianFactorGraph smoother = createSmoother(7); // Create the Bayes tree, expected to look like: diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index aa41ed350..36f618c59 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -78,8 +78,7 @@ TEST(GaussianFactorGraph, eliminateOne_x1) { /* ************************************************************************* */ TEST(GaussianFactorGraph, eliminateOne_x2) { - Ordering ordering; - ordering += X(2), L(1), X(1); + const Ordering ordering{X(2), L(1), X(1)}; GaussianFactorGraph fg = createGaussianFactorGraph(); auto actual = EliminateQR(fg, Ordering{X(2)}).first; @@ -94,8 +93,7 @@ TEST(GaussianFactorGraph, eliminateOne_x2) { /* ************************************************************************* */ TEST(GaussianFactorGraph, eliminateOne_l1) { - Ordering ordering; - ordering += L(1), X(1), X(2); + const Ordering ordering{L(1), X(1), X(2)}; GaussianFactorGraph fg = createGaussianFactorGraph(); auto actual = EliminateQR(fg, Ordering{L(1)}).first; @@ -282,8 +280,7 @@ TEST(GaussianFactorGraph, elimination) { fg.emplace_shared(X(2), Ap, b, sigma); // Eliminate - Ordering ordering; - ordering += X(1), X(2); + const Ordering ordering{X(1), X(2)}; GaussianBayesNet bayesNet = *fg.eliminateSequential(); // Check matrix @@ -348,7 +345,6 @@ static SharedDiagonal model = noiseModel::Isotropic::Sigma(2,1); /* ************************************************************************* */ TEST(GaussianFactorGraph, replace) { - Ordering ord; ord += X(1),X(2),X(3),X(4),X(5),X(6); SharedDiagonal noise(noiseModel::Isotropic::Sigma(3, 1.0)); GaussianFactorGraph::sharedFactor f1(new JacobianFactor( @@ -431,10 +427,10 @@ TEST( GaussianFactorGraph, conditional_sigma_failure) { 0.0, 1., 0.0, -1.2246468e-16, 0.0, -1), Point3(0.511832102, 8.42819594, 5.76841725)), priorModel); - factors += ProjectionFactor(Point2(333.648615, 98.61535), measModel, xC1, l32, K); - factors += ProjectionFactor(Point2(218.508, 83.8022039), measModel, xC1, l41, K); - factors += RangeFactor(xC1, l32, relElevation, elevationModel); - factors += RangeFactor(xC1, l41, relElevation, elevationModel); + factors.emplace_shared(Point2(333.648615, 98.61535), measModel, xC1, l32, K); + factors.emplace_shared(Point2(218.508, 83.8022039), measModel, xC1, l41, K); + factors.emplace_shared>(xC1, l32, relElevation, elevationModel); + factors.emplace_shared>(xC1, l41, relElevation, elevationModel); // Check that sigmas are correct (i.e., unit) GaussianFactorGraph lfg = *factors.linearize(initValues); diff --git a/tests/testGaussianISAM.cpp b/tests/testGaussianISAM.cpp index 053dedb93..ee333f4c4 100644 --- a/tests/testGaussianISAM.cpp +++ b/tests/testGaussianISAM.cpp @@ -33,7 +33,7 @@ using symbol_shorthand::L; TEST( ISAM, iSAM_smoother ) { Ordering ordering; - for (int t = 1; t <= 7; t++) ordering += X(t); + for (int t = 1; t <= 7; t++) ordering.push_back(X(t)); // Create smoother with 7 nodes GaussianFactorGraph smoother = createSmoother(7); diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 23b3609f3..5331b7dc4 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -74,7 +74,7 @@ ISAM2 createSlamlikeISAM2( // Add odometry from time 0 to time 5 for( ; i<5; ++i) { NonlinearFactorGraph newfactors; - newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.emplace_shared>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -93,9 +93,9 @@ ISAM2 createSlamlikeISAM2( // Add odometry from time 5 to 6 and landmark measurement at time 5 { NonlinearFactorGraph newfactors; - newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors += BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); - newfactors += BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); + newfactors.emplace_shared>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.emplace_shared>(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); + newfactors.emplace_shared>(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); fullgraph.push_back(newfactors); Values init; @@ -116,7 +116,7 @@ ISAM2 createSlamlikeISAM2( // Add odometry from time 6 to time 10 for( ; i<10; ++i) { NonlinearFactorGraph newfactors; - newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.emplace_shared>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -135,9 +135,9 @@ ISAM2 createSlamlikeISAM2( // Add odometry from time 10 to 11 and landmark measurement at time 10 { NonlinearFactorGraph newfactors; - newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors += BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - newfactors += BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + newfactors.emplace_shared>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.emplace_shared>(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + newfactors.emplace_shared>(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); fullgraph.push_back(newfactors); Values init; @@ -230,7 +230,7 @@ bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, c // Check information GaussianFactorGraph isamGraph(isam); - isamGraph += isam.roots().front()->cachedFactor_; + isamGraph.push_back(isam.roots().front()->cachedFactor_); Matrix expectedHessian = fullgraph.linearize(isam.getLinearizationPoint())->augmentedHessian(); Matrix actualHessian = isamGraph.augmentedHessian(); expectedHessian.bottomRightCorner(1,1) = actualHessian.bottomRightCorner(1,1); @@ -249,7 +249,7 @@ bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, c for (const auto& [key, clique] : isam.nodes()) { // Compute expected gradient GaussianFactorGraph jfg; - jfg += clique->conditional(); + jfg.push_back(clique->conditional()); VectorValues expectedGradient = jfg.gradientAtZero(); // Compare with actual gradients DenseIndex variablePosition = 0; @@ -353,7 +353,7 @@ TEST(ISAM2, clone) { // Modify original isam NonlinearFactorGraph factors; - factors += BetweenFactor(0, 10, + factors.emplace_shared>(0, 10, isam.calculateEstimate(0).between(isam.calculateEstimate(10)), noiseModel::Unit::Create(3)); isam.update(factors); @@ -439,7 +439,7 @@ TEST(ISAM2, swapFactors) NonlinearFactorGraph swapfactors; // swapfactors += BearingRange(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise; // original factor - swapfactors += BearingRangeFactor(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 5.0, brNoise); + swapfactors.emplace_shared>(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 5.0, brNoise); fullgraph.push_back(swapfactors); isam.update(swapfactors, Values(), toRemove); } @@ -452,7 +452,7 @@ TEST(ISAM2, swapFactors) for (const auto& [key, clique]: isam.nodes()) { // Compute expected gradient GaussianFactorGraph jfg; - jfg += clique->conditional(); + jfg.push_back(clique->conditional()); VectorValues expectedGradient = jfg.gradientAtZero(); // Compare with actual gradients DenseIndex variablePosition = 0; @@ -507,7 +507,7 @@ TEST(ISAM2, constrained_ordering) // Add odometry from time 0 to time 5 for( ; i<5; ++i) { NonlinearFactorGraph newfactors; - newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.emplace_shared>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -523,9 +523,9 @@ TEST(ISAM2, constrained_ordering) // Add odometry from time 5 to 6 and landmark measurement at time 5 { NonlinearFactorGraph newfactors; - newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors += BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); - newfactors += BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); + newfactors.emplace_shared>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.emplace_shared>(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); + newfactors.emplace_shared>(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); fullgraph.push_back(newfactors); Values init; @@ -543,7 +543,7 @@ TEST(ISAM2, constrained_ordering) // Add odometry from time 6 to time 10 for( ; i<10; ++i) { NonlinearFactorGraph newfactors; - newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.emplace_shared>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -556,9 +556,9 @@ TEST(ISAM2, constrained_ordering) // Add odometry from time 10 to 11 and landmark measurement at time 10 { NonlinearFactorGraph newfactors; - newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); - newfactors += BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); - newfactors += BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + newfactors.emplace_shared>(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors.emplace_shared>(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + newfactors.emplace_shared>(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); fullgraph.push_back(newfactors); Values init; @@ -576,7 +576,7 @@ TEST(ISAM2, constrained_ordering) for (const auto& [key, clique]: isam.nodes()) { // Compute expected gradient GaussianFactorGraph jfg; - jfg += clique->conditional(); + jfg.push_back(clique->conditional()); VectorValues expectedGradient = jfg.gradientAtZero(); // Compare with actual gradients DenseIndex variablePosition = 0; @@ -665,9 +665,9 @@ TEST(ISAM2, marginalizeLeaves1) { NonlinearFactorGraph factors; factors.addPrior(0, 0.0, model); - factors += BetweenFactor(0, 1, 0.0, model); - factors += BetweenFactor(1, 2, 0.0, model); - factors += BetweenFactor(0, 2, 0.0, model); + factors.emplace_shared>(0, 1, 0.0, model); + factors.emplace_shared>(1, 2, 0.0, model); + factors.emplace_shared>(0, 2, 0.0, model); Values values; values.insert(0, 0.0); @@ -692,10 +692,10 @@ TEST(ISAM2, marginalizeLeaves2) { NonlinearFactorGraph factors; factors.addPrior(0, 0.0, model); - factors += BetweenFactor(0, 1, 0.0, model); - factors += BetweenFactor(1, 2, 0.0, model); - factors += BetweenFactor(0, 2, 0.0, model); - factors += BetweenFactor(2, 3, 0.0, model); + factors.emplace_shared>(0, 1, 0.0, model); + factors.emplace_shared>(1, 2, 0.0, model); + factors.emplace_shared>(0, 2, 0.0, model); + factors.emplace_shared>(2, 3, 0.0, model); Values values; values.insert(0, 0.0); @@ -722,15 +722,15 @@ TEST(ISAM2, marginalizeLeaves3) { NonlinearFactorGraph factors; factors.addPrior(0, 0.0, model); - factors += BetweenFactor(0, 1, 0.0, model); - factors += BetweenFactor(1, 2, 0.0, model); - factors += BetweenFactor(0, 2, 0.0, model); + factors.emplace_shared>(0, 1, 0.0, model); + factors.emplace_shared>(1, 2, 0.0, model); + factors.emplace_shared>(0, 2, 0.0, model); - factors += BetweenFactor(2, 3, 0.0, model); + factors.emplace_shared>(2, 3, 0.0, model); - factors += BetweenFactor(3, 4, 0.0, model); - factors += BetweenFactor(4, 5, 0.0, model); - factors += BetweenFactor(3, 5, 0.0, model); + factors.emplace_shared>(3, 4, 0.0, model); + factors.emplace_shared>(4, 5, 0.0, model); + factors.emplace_shared>(3, 5, 0.0, model); Values values; values.insert(0, 0.0); @@ -760,8 +760,8 @@ TEST(ISAM2, marginalizeLeaves4) { NonlinearFactorGraph factors; factors.addPrior(0, 0.0, model); - factors += BetweenFactor(0, 2, 0.0, model); - factors += BetweenFactor(1, 2, 0.0, model); + factors.emplace_shared>(0, 2, 0.0, model); + factors.emplace_shared>(1, 2, 0.0, model); Values values; values.insert(0, 0.0); diff --git a/tests/testGaussianJunctionTreeB.cpp b/tests/testGaussianJunctionTreeB.cpp index 7db457879..2c5dc493b 100644 --- a/tests/testGaussianJunctionTreeB.cpp +++ b/tests/testGaussianJunctionTreeB.cpp @@ -78,20 +78,7 @@ TEST( GaussianJunctionTreeB, constructor2 ) { SymbolicEliminationTree stree(*symbolic, ordering); GaussianJunctionTree actual(etree); - Ordering o324; - o324 += X(3), X(2), X(4); - Ordering o56; - o56 += X(5), X(6); - Ordering o7; - o7 += X(7); - Ordering o1; - o1 += X(1); - - // Can no longer test these: -// Ordering sep1; -// Ordering sep2; sep2 += X(4); -// Ordering sep3; sep3 += X(6); -// Ordering sep4; sep4 += X(2); + const Ordering o324{X(3), X(2), X(4)}, o56{X(5), X(6)}, o7{X(7)}, o1{X(1)}; GaussianJunctionTree::sharedNode x324 = actual.roots().front(); LONGS_EQUAL(2, x324->children.size()); @@ -228,8 +215,7 @@ TEST(GaussianJunctionTreeB, optimizeMultiFrontal2) { // init.insert(X(0), Pose2()); // init.insert(X(1), Pose2(1.0, 0.0, 0.0)); // -// Ordering ordering; -// ordering += X(1), X(0); +// const Ordering ordering{X(1), X(0)}; // // GaussianFactorGraph gfg = *fg.linearize(init, ordering); // diff --git a/tests/testGradientDescentOptimizer.cpp b/tests/testGradientDescentOptimizer.cpp index 68508e6df..d7ca70459 100644 --- a/tests/testGradientDescentOptimizer.cpp +++ b/tests/testGradientDescentOptimizer.cpp @@ -39,15 +39,15 @@ std::tuple generateProblem() { // 2b. Add odometry factors SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas( Vector3(0.2, 0.2, 0.1)); - graph += BetweenFactor(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise); - graph += BetweenFactor(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise); - graph += BetweenFactor(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); - graph += BetweenFactor(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + graph.emplace_shared>(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise); + graph.emplace_shared>(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + graph.emplace_shared>(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise); + graph.emplace_shared>(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise); // 2c. Add pose constraint SharedDiagonal constraintUncertainty = noiseModel::Diagonal::Sigmas( Vector3(0.2, 0.2, 0.1)); - graph += BetweenFactor(5, 2, Pose2(2.0, 0.0, M_PI_2), + graph.emplace_shared>(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty); // 3. Create the data structure to hold the initialEstimate estimate to the solution diff --git a/tests/testIterative.cpp b/tests/testIterative.cpp index 012b34f08..6d9918b76 100644 --- a/tests/testIterative.cpp +++ b/tests/testIterative.cpp @@ -87,8 +87,8 @@ TEST( Iterative, conjugateGradientDescent_hard_constraint ) config.insert(X(2), Pose2(1.5,0.,0.)); NonlinearFactorGraph graph; - graph += NonlinearEquality(X(1), pose1); - graph += BetweenFactor(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); + graph.emplace_shared>(X(1), pose1); + graph.emplace_shared>(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); std::shared_ptr fg = graph.linearize(config); @@ -115,7 +115,7 @@ TEST( Iterative, conjugateGradientDescent_soft_constraint ) NonlinearFactorGraph graph; graph.addPrior(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)); - graph += BetweenFactor(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); + graph.emplace_shared>(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); std::shared_ptr fg = graph.linearize(config); diff --git a/tests/testLie.cpp b/tests/testLie.cpp index fe1173f22..f411bd6ef 100644 --- a/tests/testLie.cpp +++ b/tests/testLie.cpp @@ -47,9 +47,9 @@ template<> struct traits : internal::LieGroupTraits { //****************************************************************************** TEST(Lie, ProductLieGroup) { - BOOST_CONCEPT_ASSERT((IsGroup)); - BOOST_CONCEPT_ASSERT((IsManifold)); - BOOST_CONCEPT_ASSERT((IsLieGroup)); + GTSAM_CONCEPT_ASSERT1(IsGroup); + GTSAM_CONCEPT_ASSERT2(IsManifold); + GTSAM_CONCEPT_ASSERT3(IsLieGroup); Product pair1; Vector5 d; d << 1, 2, 0.1, 0.2, 0.3; diff --git a/tests/testManifold.cpp b/tests/testManifold.cpp index 7d788d109..6b853061b 100644 --- a/tests/testManifold.cpp +++ b/tests/testManifold.cpp @@ -37,27 +37,27 @@ typedef PinholeCamera Camera; //****************************************************************************** TEST(Manifold, SomeManifoldsGTSAM) { - //BOOST_CONCEPT_ASSERT((IsManifold)); // integer is not a manifold - BOOST_CONCEPT_ASSERT((IsManifold)); - BOOST_CONCEPT_ASSERT((IsManifold)); - BOOST_CONCEPT_ASSERT((IsManifold)); - BOOST_CONCEPT_ASSERT((IsManifold)); + //GTSAM_CONCEPT_ASSERT(IsManifold); // integer is not a manifold + GTSAM_CONCEPT_ASSERT1(IsManifold); + GTSAM_CONCEPT_ASSERT2(IsManifold); + GTSAM_CONCEPT_ASSERT3(IsManifold); + GTSAM_CONCEPT_ASSERT4(IsManifold); } //****************************************************************************** TEST(Manifold, SomeLieGroupsGTSAM) { - BOOST_CONCEPT_ASSERT((IsLieGroup)); - BOOST_CONCEPT_ASSERT((IsLieGroup)); - BOOST_CONCEPT_ASSERT((IsLieGroup)); - BOOST_CONCEPT_ASSERT((IsLieGroup)); + GTSAM_CONCEPT_ASSERT1(IsLieGroup); + GTSAM_CONCEPT_ASSERT2(IsLieGroup); + GTSAM_CONCEPT_ASSERT3(IsLieGroup); + GTSAM_CONCEPT_ASSERT4(IsLieGroup); } //****************************************************************************** TEST(Manifold, SomeVectorSpacesGTSAM) { - BOOST_CONCEPT_ASSERT((IsVectorSpace)); - BOOST_CONCEPT_ASSERT((IsVectorSpace)); - BOOST_CONCEPT_ASSERT((IsVectorSpace)); - BOOST_CONCEPT_ASSERT((IsVectorSpace)); + GTSAM_CONCEPT_ASSERT1(IsVectorSpace); + GTSAM_CONCEPT_ASSERT2(IsVectorSpace); + GTSAM_CONCEPT_ASSERT3(IsVectorSpace); + GTSAM_CONCEPT_ASSERT4(IsVectorSpace); } //****************************************************************************** diff --git a/tests/testMarginals.cpp b/tests/testMarginals.cpp index d7746e08d..344a1f56c 100644 --- a/tests/testMarginals.cpp +++ b/tests/testMarginals.cpp @@ -59,8 +59,8 @@ TEST(Marginals, planarSLAMmarginals) { SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) // create between factors to represent odometry - graph += BetweenFactor(x1, x2, odometry, odometryNoise); - graph += BetweenFactor(x2, x3, odometry, odometryNoise); + graph.emplace_shared>(x1, x2, odometry, odometryNoise); + graph.emplace_shared>(x2, x3, odometry, odometryNoise); /* add measurements */ // general noisemodel for measurements @@ -75,9 +75,9 @@ TEST(Marginals, planarSLAMmarginals) { range32 = 2.0; // create bearing/range factors - graph += BearingRangeFactor(x1, l1, bearing11, range11, measurementNoise); - graph += BearingRangeFactor(x2, l1, bearing21, range21, measurementNoise); - graph += BearingRangeFactor(x3, l2, bearing32, range32, measurementNoise); + graph.emplace_shared>(x1, l1, bearing11, range11, measurementNoise); + graph.emplace_shared>(x2, l1, bearing21, range21, measurementNoise); + graph.emplace_shared>(x3, l2, bearing32, range32, measurementNoise); // linearization point for marginals Values soln; @@ -195,9 +195,9 @@ TEST(Marginals, planarSLAMmarginals) { TEST(Marginals, order) { NonlinearFactorGraph fg; fg.addPrior(0, Pose2(), noiseModel::Unit::Create(3)); - fg += BetweenFactor(0, 1, Pose2(1,0,0), noiseModel::Unit::Create(3)); - fg += BetweenFactor(1, 2, Pose2(1,0,0), noiseModel::Unit::Create(3)); - fg += BetweenFactor(2, 3, Pose2(1,0,0), noiseModel::Unit::Create(3)); + fg.emplace_shared>(0, 1, Pose2(1,0,0), noiseModel::Unit::Create(3)); + fg.emplace_shared>(1, 2, Pose2(1,0,0), noiseModel::Unit::Create(3)); + fg.emplace_shared>(2, 3, Pose2(1,0,0), noiseModel::Unit::Create(3)); Values vals; vals.insert(0, Pose2()); @@ -208,31 +208,31 @@ TEST(Marginals, order) { vals.insert(100, Point2(0,1)); vals.insert(101, Point2(1,1)); - fg += BearingRangeFactor(0, 100, + fg.emplace_shared>(0, 100, vals.at(0).bearing(vals.at(100)), vals.at(0).range(vals.at(100)), noiseModel::Unit::Create(2)); - fg += BearingRangeFactor(0, 101, + fg.emplace_shared>(0, 101, vals.at(0).bearing(vals.at(101)), vals.at(0).range(vals.at(101)), noiseModel::Unit::Create(2)); - fg += BearingRangeFactor(1, 100, + fg.emplace_shared>(1, 100, vals.at(1).bearing(vals.at(100)), vals.at(1).range(vals.at(100)), noiseModel::Unit::Create(2)); - fg += BearingRangeFactor(1, 101, + fg.emplace_shared>(1, 101, vals.at(1).bearing(vals.at(101)), vals.at(1).range(vals.at(101)), noiseModel::Unit::Create(2)); - fg += BearingRangeFactor(2, 100, + fg.emplace_shared>(2, 100, vals.at(2).bearing(vals.at(100)), vals.at(2).range(vals.at(100)), noiseModel::Unit::Create(2)); - fg += BearingRangeFactor(2, 101, + fg.emplace_shared>(2, 101, vals.at(2).bearing(vals.at(101)), vals.at(2).range(vals.at(101)), noiseModel::Unit::Create(2)); - fg += BearingRangeFactor(3, 100, + fg.emplace_shared>(3, 100, vals.at(3).bearing(vals.at(100)), vals.at(3).range(vals.at(100)), noiseModel::Unit::Create(2)); - fg += BearingRangeFactor(3, 101, + fg.emplace_shared>(3, 101, vals.at(3).bearing(vals.at(101)), vals.at(3).range(vals.at(101)), noiseModel::Unit::Create(2)); diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index efc0020a5..73eb2a2d9 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -193,11 +193,10 @@ TEST ( NonlinearEquality, allow_error_optimize ) { Symbol key1('x', 1); Pose2 feasible1(1.0, 2.0, 3.0); double error_gain = 500.0; - PoseNLE nle(key1, feasible1, error_gain); // add to a graph NonlinearFactorGraph graph; - graph += nle; + graph.emplace_shared(key1, feasible1, error_gain); // initialize away from the ideal Pose2 initPose(0.0, 2.0, 3.0); @@ -227,16 +226,13 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) { Pose2 initPose(0.0, 2.0, 3.0); init.insert(key1, initPose); + // add nle to a graph double error_gain = 500.0; - PoseNLE nle(key1, feasible1, error_gain); - - // create a soft prior that conflicts - PosePrior prior(key1, initPose, noiseModel::Isotropic::Sigma(3, 0.1)); - - // add to a graph NonlinearFactorGraph graph; - graph += nle; - graph += prior; + graph.emplace_shared(key1, feasible1, error_gain); + + // add a soft prior that conflicts + graph.emplace_shared(key1, initPose, noiseModel::Isotropic::Sigma(3, 0.1)); // optimize Ordering ordering; @@ -440,17 +436,17 @@ TEST (testNonlinearEqualityConstraint, two_pose ) { Symbol x1('x', 1), x2('x', 2); Symbol l1('l', 1), l2('l', 2); Point2 pt_x1(1.0, 1.0), pt_x2(5.0, 6.0); - graph += eq2D::UnaryEqualityConstraint(pt_x1, x1); - graph += eq2D::UnaryEqualityConstraint(pt_x2, x2); + graph.emplace_shared(pt_x1, x1); + graph.emplace_shared(pt_x2, x2); Point2 z1(0.0, 5.0); SharedNoiseModel sigma(noiseModel::Isotropic::Sigma(2, 0.1)); - graph += simulated2D::Measurement(z1, sigma, x1, l1); + graph.emplace_shared(z1, sigma, x1, l1); Point2 z2(-4.0, 0.0); - graph += simulated2D::Measurement(z2, sigma, x2, l2); + graph.emplace_shared(z2, sigma, x2, l2); - graph += eq2D::PointEqualityConstraint(l1, l2); + graph.emplace_shared(l1, l2); Values initialEstimate; initialEstimate.insert(x1, pt_x1); @@ -480,20 +476,20 @@ TEST (testNonlinearEqualityConstraint, map_warp ) { // constant constraint on x1 Point2 pose1(1.0, 1.0); - graph += eq2D::UnaryEqualityConstraint(pose1, x1); + graph.emplace_shared(pose1, x1); SharedDiagonal sigma = noiseModel::Isotropic::Sigma(2, 0.1); // measurement from x1 to l1 Point2 z1(0.0, 5.0); - graph += simulated2D::Measurement(z1, sigma, x1, l1); + graph.emplace_shared(z1, sigma, x1, l1); // measurement from x2 to l2 Point2 z2(-4.0, 0.0); - graph += simulated2D::Measurement(z2, sigma, x2, l2); + graph.emplace_shared(z2, sigma, x2, l2); // equality constraint between l1 and l2 - graph += eq2D::PointEqualityConstraint(l1, l2); + graph.emplace_shared(l1, l2); // create an initial estimate Values initialEstimate; @@ -542,18 +538,18 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) { NonlinearFactorGraph graph; // create equality constraints for poses - graph += NonlinearEquality(key_x1, leftCamera.pose()); - graph += NonlinearEquality(key_x2, rightCamera.pose()); + graph.emplace_shared>(key_x1, leftCamera.pose()); + graph.emplace_shared>(key_x2, rightCamera.pose()); // create factors SharedDiagonal vmodel = noiseModel::Unit::Create(2); - graph += GenericProjectionFactor( + graph.emplace_shared>( leftCamera.project(landmark), vmodel, key_x1, key_l1, shK); - graph += GenericProjectionFactor( + graph.emplace_shared>( rightCamera.project(landmark), vmodel, key_x2, key_l2, shK); // add equality constraint saying there is only one point - graph += NonlinearEquality2(key_l1, key_l2); + graph.emplace_shared>(key_l1, key_l2); // create initial data Point3 landmark1(0.5, 5, 0); diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index 136cd064f..305cd963a 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -78,13 +78,13 @@ TEST( NonlinearFactorGraph, keys ) /* ************************************************************************* */ TEST( NonlinearFactorGraph, GET_ORDERING) { - Ordering expected; expected += L(1), X(2), X(1); // For starting with l1,x1,x2 + const Ordering expected{L(1), X(2), X(1)}; // For starting with l1,x1,x2 NonlinearFactorGraph nlfg = createNonlinearFactorGraph(); Ordering actual = Ordering::Colamd(nlfg); EXPECT(assert_equal(expected,actual)); // Constrained ordering - put x2 at the end - Ordering expectedConstrained; expectedConstrained += L(1), X(1), X(2); + const Ordering expectedConstrained{L(1), X(1), X(2)}; FastMap constraints; constraints[X(2)] = 1; Ordering actualConstrained = Ordering::ColamdConstrained(nlfg, constraints); @@ -162,8 +162,8 @@ TEST( NonlinearFactorGraph, rekey ) // updated measurements Point2 z3(0, -1), z4(-1.5, -1.); SharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2); - expRekey += simulated2D::Measurement(z3, sigma0_2, X(1), L(4)); - expRekey += simulated2D::Measurement(z4, sigma0_2, X(2), L(4)); + expRekey.emplace_shared(z3, sigma0_2, X(1), L(4)); + expRekey.emplace_shared(z4, sigma0_2, X(2), L(4)); EXPECT(assert_equal(expRekey, actRekey)); } @@ -198,8 +198,7 @@ TEST(NonlinearFactorGraph, UpdateCholesky) { EXPECT(assert_equal(expected, fg.updateCholesky(initial))); // solve with Ordering - Ordering ordering; - ordering += L(1), X(2), X(1); + const Ordering ordering{L(1), X(2), X(1)}; EXPECT(assert_equal(expected, fg.updateCholesky(initial, ordering))); // solve with new method, heavily damped @@ -251,8 +250,7 @@ TEST(testNonlinearFactorGraph, eliminate) { auto linearized = graph.linearize(values); // Eliminate - Ordering ordering; - ordering += 11, 21, 12, 22; + const Ordering ordering{11, 21, 12, 22}; auto bn = linearized->eliminateSequential(ordering); EXPECT_LONGS_EQUAL(4, bn->size()); } diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index 4249f5bc4..4ffdbdafe 100644 --- a/tests/testNonlinearISAM.cpp +++ b/tests/testNonlinearISAM.cpp @@ -36,7 +36,7 @@ TEST(testNonlinearISAM, markov_chain ) { // create initial graph Pose2 cur_pose; // start at origin NonlinearFactorGraph start_factors; - start_factors += NonlinearEquality(0, cur_pose); + start_factors.emplace_shared>(0, cur_pose); Values init; Values expected; @@ -50,7 +50,7 @@ TEST(testNonlinearISAM, markov_chain ) { Pose2 z(1.0, 2.0, 0.1); for (size_t i=1; i<=nrPoses; ++i) { NonlinearFactorGraph new_factors; - new_factors += BetweenFactor(i-1, i, z, model); + new_factors.emplace_shared>(i-1, i, z, model); Values new_init; cur_pose = cur_pose.compose(z); @@ -84,7 +84,7 @@ TEST(testNonlinearISAM, markov_chain_with_disconnects ) { // create initial graph Pose2 cur_pose; // start at origin NonlinearFactorGraph start_factors; - start_factors += NonlinearEquality(0, cur_pose); + start_factors.emplace_shared>(0, cur_pose); Values init; Values expected; @@ -106,7 +106,7 @@ TEST(testNonlinearISAM, markov_chain_with_disconnects ) { Pose2 z(1.0, 2.0, 0.1); for (size_t i=1; i<=nrPoses; ++i) { NonlinearFactorGraph new_factors; - new_factors += BetweenFactor(i-1, i, z, model3); + new_factors.emplace_shared>(i-1, i, z, model3); Values new_init; cur_pose = cur_pose.compose(z); @@ -161,7 +161,7 @@ TEST(testNonlinearISAM, markov_chain_with_reconnect ) { // create initial graph Pose2 cur_pose; // start at origin NonlinearFactorGraph start_factors; - start_factors += NonlinearEquality(0, cur_pose); + start_factors.emplace_shared>(0, cur_pose); Values init; Values expected; @@ -183,7 +183,7 @@ TEST(testNonlinearISAM, markov_chain_with_reconnect ) { Pose2 z(1.0, 2.0, 0.1); for (size_t i=1; i<=nrPoses; ++i) { NonlinearFactorGraph new_factors; - new_factors += BetweenFactor(i-1, i, z, model3); + new_factors.emplace_shared>(i-1, i, z, model3); Values new_init; cur_pose = cur_pose.compose(z); @@ -204,7 +204,7 @@ TEST(testNonlinearISAM, markov_chain_with_reconnect ) { // Reconnect with observation later if (i == 15) { - new_factors += BearingRangeFactor( + new_factors.emplace_shared>( i, lm1, cur_pose.bearing(landmark1), cur_pose.range(landmark1), model2); } diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index b73ac7376..cac25c246 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -190,7 +190,7 @@ TEST( NonlinearOptimizer, Factorization ) NonlinearFactorGraph graph; graph.addPrior(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)); - graph += BetweenFactor(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); + graph.emplace_shared>(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)); Ordering ordering; ordering.push_back(X(1)); @@ -249,9 +249,9 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) { NonlinearFactorGraph fg; fg.addPrior(0, Pose2(0, 0, 0), noiseModel::Isotropic::Sigma(3, 1)); - fg += BetweenFactor(0, 1, Pose2(1, 0, M_PI / 2), + fg.emplace_shared>(0, 1, Pose2(1, 0, M_PI / 2), noiseModel::Isotropic::Sigma(3, 1)); - fg += BetweenFactor(1, 2, Pose2(1, 0, M_PI / 2), + fg.emplace_shared>(1, 2, Pose2(1, 0, M_PI / 2), noiseModel::Isotropic::Sigma(3, 1)); Values init; @@ -352,10 +352,10 @@ TEST(NonlinearOptimizer, Pose2OptimizationWithHuberNoOutlier) { NonlinearFactorGraph fg; fg.addPrior(0, Pose2(0,0,0), noiseModel::Isotropic::Sigma(3,1)); - fg += BetweenFactor(0, 1, Pose2(1,1.1,M_PI/4), + fg.emplace_shared>(0, 1, Pose2(1,1.1,M_PI/4), noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(2.0), noiseModel::Isotropic::Sigma(3,1))); - fg += BetweenFactor(0, 1, Pose2(1,0.9,M_PI/2), + fg.emplace_shared>(0, 1, Pose2(1,0.9,M_PI/2), noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(3.0), noiseModel::Isotropic::Sigma(3,1))); @@ -383,13 +383,13 @@ TEST(NonlinearOptimizer, Point2LinearOptimizationWithHuber) { NonlinearFactorGraph fg; fg.addPrior(0, Point2(0,0), noiseModel::Isotropic::Sigma(2,0.01)); - fg += BetweenFactor(0, 1, Point2(1,1.8), + fg.emplace_shared>(0, 1, Point2(1,1.8), noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0), noiseModel::Isotropic::Sigma(2,1))); - fg += BetweenFactor(0, 1, Point2(1,0.9), + fg.emplace_shared>(0, 1, Point2(1,0.9), noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0), noiseModel::Isotropic::Sigma(2,1))); - fg += BetweenFactor(0, 1, Point2(1,90), + fg.emplace_shared>(0, 1, Point2(1,90), noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0), noiseModel::Isotropic::Sigma(2,1))); @@ -417,16 +417,16 @@ TEST(NonlinearOptimizer, Pose2OptimizationWithHuber) { NonlinearFactorGraph fg; fg.addPrior(0, Pose2(0,0, 0), noiseModel::Isotropic::Sigma(3,0.1)); - fg += BetweenFactor(0, 1, Pose2(0,9, M_PI/2), + fg.emplace_shared>(0, 1, Pose2(0,9, M_PI/2), noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2), noiseModel::Isotropic::Sigma(3,1))); - fg += BetweenFactor(0, 1, Pose2(0, 11, M_PI/2), + fg.emplace_shared>(0, 1, Pose2(0, 11, M_PI/2), noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2), noiseModel::Isotropic::Sigma(3,1))); - fg += BetweenFactor(0, 1, Pose2(0, 10, M_PI/2), + fg.emplace_shared>(0, 1, Pose2(0, 10, M_PI/2), noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2), noiseModel::Isotropic::Sigma(3,1))); - fg += BetweenFactor(0, 1, Pose2(0,9, 0), + fg.emplace_shared>(0, 1, Pose2(0,9, 0), noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2), noiseModel::Isotropic::Sigma(3,1))); @@ -495,7 +495,7 @@ TEST(NonlinearOptimizer, disconnected_graph) { NonlinearFactorGraph graph; graph.addPrior(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3,1)); - graph += BetweenFactor(X(1),X(2), Pose2(1.5,0.,0.), noiseModel::Isotropic::Sigma(3,1)); + graph.emplace_shared>(X(1),X(2), Pose2(1.5,0.,0.), noiseModel::Isotropic::Sigma(3,1)); graph.addPrior(X(3), Pose2(3.,0.,0.), noiseModel::Isotropic::Sigma(3,1)); EXPECT(assert_equal(expected, LevenbergMarquardtOptimizer(graph, init).optimize())); @@ -541,7 +541,7 @@ TEST(NonlinearOptimizer, subclass_solver) { NonlinearFactorGraph graph; graph.addPrior(X(1), Pose2(0., 0., 0.), noiseModel::Isotropic::Sigma(3, 1)); - graph += BetweenFactor(X(1), X(2), Pose2(1.5, 0., 0.), + graph.emplace_shared>(X(1), X(2), Pose2(1.5, 0., 0.), noiseModel::Isotropic::Sigma(3, 1)); graph.addPrior(X(3), Pose2(3., 0., 0.), noiseModel::Isotropic::Sigma(3, 1)); diff --git a/tests/testPCGSolver.cpp b/tests/testPCGSolver.cpp index 9497c00d7..bc9f9e608 100644 --- a/tests/testPCGSolver.cpp +++ b/tests/testPCGSolver.cpp @@ -84,13 +84,13 @@ TEST( GaussianFactorGraphSystem, multiply_getb) // Create a Gaussian Factor Graph GaussianFactorGraph simpleGFG; SharedDiagonal unit2 = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.3)); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << -1, -1).finished(), unit2); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -10, 0, 0, -10).finished(), 0, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << 2, -1).finished(), unit2); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << 0, 1).finished(), unit2); - simpleGFG += JacobianFactor(0, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << -1, 1.5).finished(), unit2); - simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2); - simpleGFG += JacobianFactor(1, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2); + simpleGFG.emplace_shared(2, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << -1, -1).finished(), unit2); + simpleGFG.emplace_shared(2, (Matrix(2,2)<< -10, 0, 0, -10).finished(), 0, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << 2, -1).finished(), unit2); + simpleGFG.emplace_shared(2, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << 0, 1).finished(), unit2); + simpleGFG.emplace_shared(0, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << -1, 1.5).finished(), unit2); + simpleGFG.emplace_shared(0, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2); + simpleGFG.emplace_shared(1, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2); + simpleGFG.emplace_shared(2, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2); // Create a dummy-preconditioner and a GaussianFactorGraphSystem DummyPreconditioner dummyPreconditioner; diff --git a/tests/testPreconditioner.cpp b/tests/testPreconditioner.cpp index 8e3c6dcc5..ecdf36322 100644 --- a/tests/testPreconditioner.cpp +++ b/tests/testPreconditioner.cpp @@ -39,7 +39,7 @@ TEST( PCGsolver, verySimpleLinearSystem) { // Create a Gaussian Factor Graph GaussianFactorGraph simpleGFG; - simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 4, 1, 1, 3).finished(), (Vector(2) << 1,2 ).finished(), noiseModel::Unit::Create(2)); + simpleGFG.emplace_shared(0, (Matrix(2,2)<< 4, 1, 1, 3).finished(), (Vector(2) << 1,2 ).finished(), noiseModel::Unit::Create(2)); // Exact solution already known VectorValues exactSolution; @@ -81,13 +81,13 @@ TEST(PCGSolver, simpleLinearSystem) { GaussianFactorGraph simpleGFG; //SharedDiagonal unit2 = noiseModel::Unit::Create(2); SharedDiagonal unit2 = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.3)); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << -1, -1).finished(), unit2); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -10, 0, 0, -10).finished(), 0, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << 2, -1).finished(), unit2); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << 0, 1).finished(), unit2); - simpleGFG += JacobianFactor(0, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << -1, 1.5).finished(), unit2); - simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2); - simpleGFG += JacobianFactor(1, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2); - simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2); + simpleGFG.emplace_shared(2, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << -1, -1).finished(), unit2); + simpleGFG.emplace_shared(2, (Matrix(2,2)<< -10, 0, 0, -10).finished(), 0, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << 2, -1).finished(), unit2); + simpleGFG.emplace_shared(2, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << 0, 1).finished(), unit2); + simpleGFG.emplace_shared(0, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << -1, 1.5).finished(), unit2); + simpleGFG.emplace_shared(0, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2); + simpleGFG.emplace_shared(1, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2); + simpleGFG.emplace_shared(2, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2); //simpleGFG.print("system"); // Expected solution diff --git a/tests/testRot3Optimization.cpp b/tests/testRot3Optimization.cpp index 5746022a3..5cb31ef0a 100644 --- a/tests/testRot3Optimization.cpp +++ b/tests/testRot3Optimization.cpp @@ -17,7 +17,6 @@ #include #include -#include #include #include #include @@ -32,6 +31,8 @@ using namespace gtsam; typedef BetweenFactor Between; typedef NonlinearFactorGraph Graph; +using symbol_shorthand::R; + /* ************************************************************************* */ TEST(Rot3, optimize) { @@ -39,11 +40,11 @@ TEST(Rot3, optimize) { Values truth; Values initial; Graph fg; - fg.addPrior(Symbol('r',0), Rot3(), noiseModel::Isotropic::Sigma(3, 0.01)); + fg.addPrior(R(0), Rot3(), noiseModel::Isotropic::Sigma(3, 0.01)); for(int j=0; j<6; ++j) { - truth.insert(Symbol('r',j), Rot3::Rz(M_PI/3.0 * double(j))); - initial.insert(Symbol('r',j), Rot3::Rz(M_PI/3.0 * double(j) + 0.1 * double(j%2))); - fg += Between(Symbol('r',j), Symbol('r',(j+1)%6), Rot3::Rz(M_PI/3.0), noiseModel::Isotropic::Sigma(3, 0.01)); + truth.insert(R(j), Rot3::Rz(M_PI/3.0 * double(j))); + initial.insert(R(j), Rot3::Rz(M_PI/3.0 * double(j) + 0.1 * double(j%2))); + fg.emplace_shared(R(j), R((j+1)%6), Rot3::Rz(M_PI/3.0), noiseModel::Isotropic::Sigma(3, 0.01)); } Values final = GaussNewtonOptimizer(fg, initial).optimize(); diff --git a/tests/testSerializationSlam.cpp b/tests/testSerializationSlam.cpp index e03ef015b..c4170b108 100644 --- a/tests/testSerializationSlam.cpp +++ b/tests/testSerializationSlam.cpp @@ -217,7 +217,7 @@ BOOST_CLASS_EXPORT_GUID(GenericStereoFactor3D, "gtsam::GenericStereoFactor3D") TEST (testSerializationSLAM, smallExample_linear) { using namespace example; - Ordering ordering; ordering += X(1),X(2),L(1); + const Ordering ordering{X(1), X(2), L(1)}; EXPECT(equalsObj(ordering)); EXPECT(equalsXML(ordering)); EXPECT(equalsBinary(ordering)); diff --git a/tests/testSimulated3D.cpp b/tests/testSimulated3D.cpp index cfe00856a..9bc67c115 100644 --- a/tests/testSimulated3D.cpp +++ b/tests/testSimulated3D.cpp @@ -21,7 +21,6 @@ #include #include -#include #include #include diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp index def877cd6..994fe5112 100644 --- a/tests/testSubgraphPreconditioner.cpp +++ b/tests/testSubgraphPreconditioner.cpp @@ -42,11 +42,9 @@ Symbol key(int x, int y) { return symbol_shorthand::X(1000 * x + y); } /* ************************************************************************* */ TEST(SubgraphPreconditioner, planarOrdering) { // Check canonical ordering - Ordering expected, ordering = planarOrdering(3); - expected += - key(3, 3), key(2, 3), key(1, 3), - key(3, 2), key(2, 2), key(1, 2), - key(3, 1), key(2, 1), key(1, 1); + Ordering ordering = planarOrdering(3), + expected{key(3, 3), key(2, 3), key(1, 3), key(3, 2), key(2, 2), + key(1, 2), key(3, 1), key(2, 1), key(1, 1)}; EXPECT(assert_equal(expected, ordering)); } diff --git a/timing/CMakeLists.txt b/timing/CMakeLists.txt index fc16d3ac8..2ebfeb9d8 100644 --- a/timing/CMakeLists.txt +++ b/timing/CMakeLists.txt @@ -1,3 +1,22 @@ -gtsamAddTimingGlob("*.cpp" "" "gtsam") +set (excluded_scripts + elaboratePoint2KalmanFilter.cpp +) +# Add scripts to exclude if GTSAM_ENABLE_BOOST_SERIALIZATION is not set +if (NOT GTSAM_ENABLE_BOOST_SERIALIZATION) + # add to excluded scripts + list (APPEND excluded_scripts + "timeIncremental.cpp" + ) +endif() + +# Add scripts to exclude if GTSAM_USE_BOOST_FEATURES is not set +if (NOT GTSAM_USE_BOOST_FEATURES) + # add to excluded scripts + list (APPEND excluded_scripts + "timeISAM2Chain.cpp" + ) +endif() + +gtsamAddTimingGlob("*.cpp" "${excluded_scripts}" "gtsam") target_link_libraries(timeGaussianFactorGraph CppUnitLite) diff --git a/timing/timeGaussianFactor.cpp b/timing/timeGaussianFactor.cpp index 045b5ef67..04bd090e1 100644 --- a/timing/timeGaussianFactor.cpp +++ b/timing/timeGaussianFactor.cpp @@ -115,8 +115,7 @@ int main() cout << ((double)n/seconds) << " calls/second" << endl; // time matrix_augmented -// Ordering ordering; -// ordering += _x2_, _l1_, _x1_; +// const Ordering ordering{_x2_, _l1_, _x1_}; // size_t n1 = 10000000; // timeLog = clock(); // diff --git a/timing/timeVirtual.cpp b/timing/timeVirtual.cpp index d4f9634d5..62133761a 100644 --- a/timing/timeVirtual.cpp +++ b/timing/timeVirtual.cpp @@ -19,12 +19,10 @@ #include #include -#include #include using namespace std; -using namespace boost; using namespace gtsam; struct Plain { @@ -48,14 +46,6 @@ struct VirtualCounted { virtual ~VirtualCounted() {} }; -void intrusive_ptr_add_ref(VirtualCounted* obj) { ++ obj->refCount; } -void intrusive_ptr_release(VirtualCounted* obj) { - assert(obj->refCount > 0); - -- obj->refCount; - if(obj->refCount == 0) - delete obj; -} - int main(int argc, char *argv[]) { size_t trials = 10000000; @@ -145,7 +135,7 @@ int main(int argc, char *argv[]) { gttic_(intrusive_virtual_alloc_dealloc_call); for(size_t i=0; i obj(new VirtualCounted(i)); + std::shared_ptr obj(new VirtualCounted(i)); obj->setData(i+1); } gttoc_(intrusive_virtual_alloc_dealloc_call); diff --git a/timing/timeVirtual2.cpp b/timing/timeVirtual2.cpp index 3d6e02288..dd6c8e268 100644 --- a/timing/timeVirtual2.cpp +++ b/timing/timeVirtual2.cpp @@ -21,7 +21,6 @@ #include using namespace std; -using namespace boost; using namespace gtsam; struct DtorTestBase { diff --git a/wrap/matlab.h b/wrap/matlab.h index 645ba8edf..5c38ac808 100644 --- a/wrap/matlab.h +++ b/wrap/matlab.h @@ -490,7 +490,7 @@ Class* unwrap_ptr(const mxArray* obj, const string& propertyName) { //template <> //Vector unwrap_shared_ptr(const mxArray* obj, const string& propertyName) { // bool unwrap_shared_ptr_Vector_attempted = false; -// BOOST_STATIC_ASSERT(unwrap_shared_ptr_Vector_attempted, "Vector cannot be unwrapped as a shared pointer"); +// static_assert(unwrap_shared_ptr_Vector_attempted, "Vector cannot be unwrapped as a shared pointer"); // return Vector(); //} @@ -498,7 +498,7 @@ Class* unwrap_ptr(const mxArray* obj, const string& propertyName) { //template <> //Matrix unwrap_shared_ptr(const mxArray* obj, const string& propertyName) { // bool unwrap_shared_ptr_Matrix_attempted = false; -// BOOST_STATIC_ASSERT(unwrap_shared_ptr_Matrix_attempted, "Matrix cannot be unwrapped as a shared pointer"); +// static_assert(unwrap_shared_ptr_Matrix_attempted, "Matrix cannot be unwrapped as a shared pointer"); // return Matrix(); //}