12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #1444 from borglab/feature/remove_boost_completely 

Optionally remove boost completely
release/4.3a0
Frank Dellaert 2023-02-06 12:56:55 -08:00 committed by GitHub
parent 749098f239 b644aa3fd2
commit 2856282932
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
195 changed files with 871 additions and 804 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
.github/workflows/build-special.yml vendored
View File

@ -15,7 +15,7 @@ jobs:
BOOST_VERSION: 1.67.0 BOOST_VERSION: 1.67.0
strategy: strategy:
fail-fast: true fail-fast: false
matrix: matrix:
# Github Actions requires a single row to be added to the build matrix. # Github Actions requires a single row to be added to the build matrix.
@ -27,6 +27,7 @@ jobs:
ubuntu-clang-tbb, ubuntu-clang-tbb,
ubuntu-clang-cayleymap, ubuntu-clang-cayleymap,
ubuntu-clang-system-libs, ubuntu-clang-system-libs,
ubuntu-no-boost,
] ]
build_type: [Debug, Release] build_type: [Debug, Release]
@ -62,6 +63,12 @@ jobs:
version: "14" version: "14"
flag: system flag: system
- name: ubuntu-no-boost
os: ubuntu-22.04
compiler: clang
version: "14"
flag: no_boost
steps: steps:
- name: Checkout - name: Checkout
uses: actions/checkout@v3 uses: actions/checkout@v3
@ -137,6 +144,13 @@ jobs:
# sudo apt-get install metis # sudo apt-get install metis
# echo "GTSAM_USE_SYSTEM_METIS=ON" >> $GITHUB_ENV # 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 - name: Build & Test
run: | run: |
bash .github/scripts/unix.sh -t bash .github/scripts/unix.sh -t

20
CMakeLists.txt
View File

@ -50,8 +50,24 @@ endif()
include(cmake/HandleGeneralOptions.cmake) # CMake build options include(cmake/HandleGeneralOptions.cmake) # CMake build options
# Libraries: ############### Decide on BOOST ######################################
include(cmake/HandleBoost.cmake) # 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/HandleCCache.cmake) # ccache
include(cmake/HandleCPack.cmake) # CPack include(cmake/HandleCPack.cmake) # CPack
include(cmake/HandleEigen.cmake) # Eigen3 include(cmake/HandleEigen.cmake) # Eigen3

1
CppUnitLite/CMakeLists.txt
View File

@ -6,7 +6,6 @@ file(GLOB cppunitlite_src "*.cpp")
add_library(CppUnitLite STATIC ${cppunitlite_src} ${cppunitlite_headers}) add_library(CppUnitLite STATIC ${cppunitlite_src} ${cppunitlite_headers})
list(APPEND GTSAM_EXPORTED_TARGETS CppUnitLite) list(APPEND GTSAM_EXPORTED_TARGETS CppUnitLite)
set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE) 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 gtsam_assign_source_folders("${cppunitlite_headers};${cppunitlite_src}") # MSVC project structure

10
CppUnitLite/Test.cpp
View File

@ -15,8 +15,6 @@
#include "TestResult.h" #include "TestResult.h"
#include "Failure.h" #include "Failure.h"
#include <boost/lexical_cast.hpp>
Test::Test (const std::string& testName) Test::Test (const std::string& testName)
: name_ (testName), next_(0), lineNumber_(-1), safeCheck_(true) : 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 ( result.addFailure (
Failure ( Failure (
name_, name_,
boost::lexical_cast<std::string> (__FILE__), std::string(__FILE__),
__LINE__, __LINE__,
boost::lexical_cast<std::string> (expected), std::to_string(expected),
boost::lexical_cast<std::string> (actual))); std::to_string(actual)));
return false; return false;
@ -64,7 +62,7 @@ bool Test::check(const std::string& expected, const std::string& actual, TestRes
result.addFailure ( result.addFailure (
Failure ( Failure (
name_, name_,
boost::lexical_cast<std::string> (__FILE__), std::string(__FILE__),
__LINE__, __LINE__,
expected, expected,
actual)); actual));

22
CppUnitLite/Test.h
View File

@ -23,7 +23,7 @@
#include <cmath> #include <cmath>
#include <boost/lexical_cast.hpp> #include <string>
class TestResult; class TestResult;
@ -112,17 +112,17 @@ protected:
#define THROWS_EXCEPTION(condition)\ #define THROWS_EXCEPTION(condition)\
{ try { condition; \ { try { condition; \
result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Didn't throw: ") + boost::lexical_cast<std::string>(#condition))); \ result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Didn't throw: ") + std::string(#condition))); \
return; } \ return; } \
catch (...) {} } catch (...) {} }
#define CHECK_EXCEPTION(condition, exception_name)\ #define CHECK_EXCEPTION(condition, exception_name)\
{ try { condition; \ { try { condition; \
result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Didn't throw: ") + boost::lexical_cast<std::string>(#condition))); \ result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Didn't throw: ") + std::string(#condition))); \
return; } \ return; } \
catch (exception_name&) {} \ catch (exception_name&) {} \
catch (...) { \ catch (...) { \
result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Wrong exception: ") + boost::lexical_cast<std::string>(#condition) + boost::lexical_cast<std::string>(", expected: ") + boost::lexical_cast<std::string>(#exception_name))); \ result_.addFailure (Failure (name_, __FILE__,__LINE__, std::string("Wrong exception: ") + std::string(#condition) + std::string(", expected: ") + std::string(#exception_name))); \
return; } } return; } }
#define EQUALITY(expected,actual)\ #define EQUALITY(expected,actual)\
@ -130,21 +130,21 @@ protected:
result_.addFailure(Failure(name_, __FILE__, __LINE__, #expected, #actual)); } result_.addFailure(Failure(name_, __FILE__, __LINE__, #expected, #actual)); }
#define CHECK_EQUAL(expected,actual)\ #define CHECK_EQUAL(expected,actual)\
{ if ((expected) == (actual)) return; result_.addFailure(Failure(name_, __FILE__, __LINE__, boost::lexical_cast<std::string>(expected), boost::lexical_cast<std::string>(actual))); } { if ((expected) == (actual)) return; result_.addFailure(Failure(name_, __FILE__, __LINE__, std::to_string(expected), std::to_string(actual))); }
#define LONGS_EQUAL(expected,actual)\ #define LONGS_EQUAL(expected,actual)\
{ long actualTemp = actual; \ { long actualTemp = actual; \
long expectedTemp = expected; \ long expectedTemp = expected; \
if ((expectedTemp) != (actualTemp)) \ if ((expectedTemp) != (actualTemp)) \
{ result_.addFailure (Failure (name_, __FILE__, __LINE__, boost::lexical_cast<std::string>(expectedTemp), \ { result_.addFailure (Failure (name_, __FILE__, __LINE__, std::to_string(expectedTemp), \
boost::lexical_cast<std::string>(actualTemp))); return; } } std::to_string(actualTemp))); return; } }
#define DOUBLES_EQUAL(expected,actual,threshold)\ #define DOUBLES_EQUAL(expected,actual,threshold)\
{ double actualTemp = actual; \ { double actualTemp = actual; \
double expectedTemp = expected; \ double expectedTemp = expected; \
if (!std::isfinite(actualTemp) || !std::isfinite(expectedTemp) || fabs ((expectedTemp)-(actualTemp)) > threshold) \ if (!std::isfinite(actualTemp) || !std::isfinite(expectedTemp) || fabs ((expectedTemp)-(actualTemp)) > threshold) \
{ result_.addFailure (Failure (name_, __FILE__, __LINE__, \ { result_.addFailure (Failure (name_, __FILE__, __LINE__, \
boost::lexical_cast<std::string>((double)expectedTemp), boost::lexical_cast<std::string>((double)actualTemp))); return; } } std::to_string((double)expectedTemp), std::to_string((double)actualTemp))); return; } }
/* EXPECTs: tests will continue running after a failure */ /* EXPECTs: tests will continue running after a failure */
@ -156,15 +156,15 @@ boost::lexical_cast<std::string>((double)expectedTemp), boost::lexical_cast<std:
{ long actualTemp = actual; \ { long actualTemp = actual; \
long expectedTemp = expected; \ long expectedTemp = expected; \
if ((expectedTemp) != (actualTemp)) \ if ((expectedTemp) != (actualTemp)) \
{ result_.addFailure (Failure (name_, __FILE__, __LINE__, boost::lexical_cast<std::string>(expectedTemp), \ { result_.addFailure (Failure (name_, __FILE__, __LINE__, std::to_string(expectedTemp), \
boost::lexical_cast<std::string>(actualTemp))); } } std::to_string(actualTemp))); } }
#define EXPECT_DOUBLES_EQUAL(expected,actual,threshold)\ #define EXPECT_DOUBLES_EQUAL(expected,actual,threshold)\
{ double actualTemp = actual; \ { double actualTemp = actual; \
double expectedTemp = expected; \ double expectedTemp = expected; \
if (!std::isfinite(actualTemp) || !std::isfinite(expectedTemp) || fabs ((expectedTemp)-(actualTemp)) > threshold) \ if (!std::isfinite(actualTemp) || !std::isfinite(expectedTemp) || fabs ((expectedTemp)-(actualTemp)) > threshold) \
{ result_.addFailure (Failure (name_, __FILE__, __LINE__, \ { result_.addFailure (Failure (name_, __FILE__, __LINE__, \
boost::lexical_cast<std::string>((double)expectedTemp), boost::lexical_cast<std::string>((double)actualTemp))); } } std::to_string((double)expectedTemp), std::to_string((double)actualTemp))); } }
#define FAIL(text) \ #define FAIL(text) \

2
GTSAM-Concepts.md
View File

@ -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 provides a nice way to check whether a given type satisfies a concept. For example, the following
BOOST_CONCEPT_ASSERT(IsVectorSpace<Point2>) GTSAM_CONCEPT_ASSERT(IsVectorSpace<Point2>)
asserts that Point2 indeed is a model for the VectorSpace concept. asserts that Point2 indeed is a model for the VectorSpace concept.

2
cmake/example_cmake_find_gtsam/CMakeLists.txt
View File

@ -12,6 +12,6 @@ add_executable(example
) )
# By using CMake exported targets, a simple "link" dependency introduces the # 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.) # required build flags (e.g. C++11, etc.)
target_link_libraries(example PRIVATE gtsam) target_link_libraries(example PRIVATE gtsam)

10
cmake/obsolete/GtsamTestingObsolete.cmake
View File

@ -2,7 +2,7 @@
# Macro for adding categorized tests in a "tests" folder, with # Macro for adding categorized tests in a "tests" folder, with
# optional exclusion of tests and convenience library linking options # 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: # Arguments:
# - subdir The name of the category for this test # - 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) # - 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 # Macro for adding categorized timing scripts in a "tests" folder, with
# optional exclusion of tests and convenience library linking options # optional exclusion of tests and convenience library linking options
# #
# By default, all tests are linked with boost
# Arguments: # Arguments:
# - subdir The name of the category for this timing script # - 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) # - 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() endmacro()
# Macro for adding executables matching a pattern - builds one executable for # Macro for adding executables matching a pattern - builds one executable for
# each file matching the pattern. These exectuables are automatically linked # each file matching the pattern.
# with boost.
# Arguments: # Arguments:
# - pattern The glob pattern to match source files # - pattern The glob pattern to match source files
# - local_libs A list of convenience libraries to use (if GTSAM_BUILD_CONVENIENCE_LIBRARIES is true) # - 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 # Linking and dependendencies
if (GTSAM_BUILD_CONVENIENCE_LIBRARIES) if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
target_link_libraries(${script_bin} ${local_libs} ${GTSAM_BOOST_LIBRARIES}) target_link_libraries(${script_bin} ${local_libs})
else() else()
target_link_libraries(${script_bin} ${full_libs} ${GTSAM_BOOST_LIBRARIES}) target_link_libraries(${script_bin} ${full_libs})
endif() endif()
# Add .run target # Add .run target

18
examples/CMakeLists.txt
View File

@ -1,13 +1,23 @@
set (excluded_examples set (excluded_examples
elaboratePoint2KalmanFilter.cpp "elaboratePoint2KalmanFilter.cpp"
) )
# if GTSAM_ENABLE_BOOST_SERIALIZATION is not set then SolverComparer.cpp will not compile # if GTSAM_ENABLE_BOOST_SERIALIZATION is not set then SolverComparer.cpp will not compile
if (NOT GTSAM_ENABLE_BOOST_SERIALIZATION) if (NOT GTSAM_ENABLE_BOOST_SERIALIZATION)
set (excluded_examples list (APPEND excluded_examples
${excluded_examples} "SolverComparer.cpp"
SolverComparer.cpp
) )
endif() 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}") gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam;gtsam_unstable;${Boost_PROGRAM_OPTIONS_LIBRARY}")

3
examples/DiscreteBayesNetExample.cpp
View File

@ -51,8 +51,7 @@ int main(int argc, char **argv) {
DiscreteFactorGraph fg(asia); DiscreteFactorGraph fg(asia);
// Create solver and eliminate // Create solver and eliminate
Ordering ordering; const Ordering ordering{0, 1, 2, 3, 4, 5, 6, 7};
ordering += Key(0), Key(1), Key(2), Key(3), Key(4), Key(5), Key(6), Key(7);
// solve // solve
auto mpe = fg.optimize(); auto mpe = fg.optimize();

17
gtsam/CMakeLists.txt
View File

@ -48,6 +48,23 @@ else()
set(excluded_sources ${excluded_sources} "${CMAKE_CURRENT_SOURCE_DIR}/geometry/Rot3Q.cpp") set(excluded_sources ${excluded_sources} "${CMAKE_CURRENT_SOURCE_DIR}/geometry/Rot3Q.cpp")
endif() 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 # Common headers
file(GLOB gtsam_core_headers "*.h") file(GLOB gtsam_core_headers "*.h")
install(FILES ${gtsam_core_headers} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam) install(FILES ${gtsam_core_headers} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam)

1
gtsam/base/ConcurrentMap.h
View File

@ -52,7 +52,6 @@ using ConcurrentMapBase = gtsam::FastMap<KEY, VALUE>;
#include <boost/serialization/nvp.hpp> #include <boost/serialization/nvp.hpp>
#include <boost/serialization/split_member.hpp> #include <boost/serialization/split_member.hpp>
#endif #endif
#include <boost/static_assert.hpp>
#include <gtsam/base/FastVector.h> #include <gtsam/base/FastVector.h>

1
gtsam/base/FastList.h
View File

@ -20,7 +20,6 @@
#include <gtsam/base/FastDefaultAllocator.h> #include <gtsam/base/FastDefaultAllocator.h>
#include <list> #include <list>
#include <boost/utility/enable_if.hpp>
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/nvp.hpp> #include <boost/serialization/nvp.hpp>
#include <boost/serialization/version.hpp> #include <boost/serialization/version.hpp>

4
gtsam/base/FastSet.h
View File

@ -18,8 +18,8 @@
#pragma once #pragma once
#include <boost/version.hpp>
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/version.hpp>
#if BOOST_VERSION >= 107400 #if BOOST_VERSION >= 107400
#include <boost/serialization/library_version_type.hpp> #include <boost/serialization/library_version_type.hpp>
#endif #endif
@ -51,7 +51,7 @@ template<typename VALUE>
class FastSet: public std::set<VALUE, std::less<VALUE>, class FastSet: public std::set<VALUE, std::less<VALUE>,
typename internal::FastDefaultAllocator<VALUE>::type> { typename internal::FastDefaultAllocator<VALUE>::type> {
BOOST_CONCEPT_ASSERT ((IsTestable<VALUE> )); GTSAM_CONCEPT_ASSERT(IsTestable<VALUE>);
public: public:

2
gtsam/base/GenericValue.h
View File

@ -23,8 +23,6 @@
#include <gtsam/base/types.h> #include <gtsam/base/types.h>
#include <gtsam/base/Value.h> #include <gtsam/base/Value.h>
#include <boost/pool/pool_alloc.hpp>
#include <cmath> #include <cmath>
#include <iostream> #include <iostream>
#include <typeinfo> // operator typeid #include <typeinfo> // operator typeid

17
gtsam/base/Group.h
View File

@ -22,10 +22,13 @@
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#ifdef GTSAM_USE_BOOST_FEATURES
#include <boost/concept_check.hpp> #include <boost/concept_check.hpp>
#include <boost/concept/requires.hpp> #include <boost/concept/requires.hpp>
#include <boost/type_traits/is_base_of.hpp> #include <boost/type_traits/is_base_of.hpp>
#include <boost/static_assert.hpp> #include <boost/static_assert.hpp>
#endif
#include <utility> #include <utility>
namespace gtsam { namespace gtsam {
@ -50,7 +53,7 @@ public:
//typedef typename traits<G>::identity::value_type identity_value_type; //typedef typename traits<G>::identity::value_type identity_value_type;
BOOST_CONCEPT_USAGE(IsGroup) { BOOST_CONCEPT_USAGE(IsGroup) {
BOOST_STATIC_ASSERT_MSG( static_assert(
(std::is_base_of<group_tag, structure_category_tag>::value), (std::is_base_of<group_tag, structure_category_tag>::value),
"This type's structure_category trait does not assert it as a group (or derived)"); "This type's structure_category trait does not assert it as a group (or derived)");
e = traits<G>::Identity(); e = traits<G>::Identity();
@ -79,7 +82,7 @@ private:
/// Check invariants /// Check invariants
template<typename G> template<typename G>
BOOST_CONCEPT_REQUIRES(((IsGroup<G>)),(bool)) // GTSAM_CONCEPT_REQUIRES(IsGroup<G>,bool) //
check_group_invariants(const G& a, const G& b, double tol = 1e-9) { check_group_invariants(const G& a, const G& b, double tol = 1e-9) {
G e = traits<G>::Identity(); G e = traits<G>::Identity();
return traits<G>::Equals(traits<G>::Compose(a, traits<G>::Inverse(a)), e, tol) return traits<G>::Equals(traits<G>::Compose(a, traits<G>::Inverse(a)), e, tol)
@ -125,7 +128,7 @@ struct AdditiveGroup : AdditiveGroupTraits<Class>, Testable<Class> {};
/// compose multiple times /// compose multiple times
template<typename G> template<typename G>
BOOST_CONCEPT_REQUIRES(((IsGroup<G>)),(G)) // GTSAM_CONCEPT_REQUIRES(IsGroup<G>,G) //
compose_pow(const G& g, size_t n) { compose_pow(const G& g, size_t n) {
if (n == 0) return traits<G>::Identity(); if (n == 0) return traits<G>::Identity();
else if (n == 1) return g; 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 /// Assumes nothing except group structure and Testable from G and H
template<typename G, typename H> template<typename G, typename H>
class DirectProduct: public std::pair<G, H> { class DirectProduct: public std::pair<G, H> {
BOOST_CONCEPT_ASSERT((IsGroup<G>)); GTSAM_CONCEPT_ASSERT1(IsGroup<G>);
BOOST_CONCEPT_ASSERT((IsGroup<H>)); GTSAM_CONCEPT_ASSERT2(IsGroup<H>);
public: public:
/// Default constructor yields identity /// Default constructor yields identity
@ -167,8 +170,8 @@ struct traits<DirectProduct<G, H> > :
/// Assumes existence of three additive operators for both groups /// Assumes existence of three additive operators for both groups
template<typename G, typename H> template<typename G, typename H>
class DirectSum: public std::pair<G, H> { class DirectSum: public std::pair<G, H> {
BOOST_CONCEPT_ASSERT((IsGroup<G>)); // TODO(frank): check additive GTSAM_CONCEPT_ASSERT1(IsGroup<G>); // TODO(frank): check additive
BOOST_CONCEPT_ASSERT((IsGroup<H>)); // TODO(frank): check additive GTSAM_CONCEPT_ASSERT2(IsGroup<H>); // TODO(frank): check additive
const G& g() const { return this->first; } const G& g() const { return this->first; }
const H& h() const { return this->second;} const H& h() const { return this->second;}

2
gtsam/base/Lie.h
View File

@ -265,7 +265,7 @@ public:
typedef typename traits<T>::ChartJacobian ChartJacobian; typedef typename traits<T>::ChartJacobian ChartJacobian;
BOOST_CONCEPT_USAGE(IsLieGroup) { BOOST_CONCEPT_USAGE(IsLieGroup) {
BOOST_STATIC_ASSERT_MSG( static_assert(
(std::is_base_of<lie_group_tag, structure_category_tag>::value), (std::is_base_of<lie_group_tag, structure_category_tag>::value),
"This type's trait does not assert it is a Lie group (or derived)"); "This type's trait does not assert it is a Lie group (or derived)");

15
gtsam/base/Manifold.h
View File

@ -22,10 +22,7 @@
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/OptionalJacobian.h> #include <gtsam/base/OptionalJacobian.h>
#include <gtsam/base/concepts.h>
#include <boost/concept_check.hpp>
#include <boost/concept/requires.hpp>
#include <boost/type_traits/is_base_of.hpp>
namespace gtsam { namespace gtsam {
@ -95,7 +92,7 @@ template<class Class>
struct ManifoldTraits: GetDimensionImpl<Class, Class::dimension> { struct ManifoldTraits: GetDimensionImpl<Class, Class::dimension> {
// Check that Class has the necessary machinery // Check that Class has the necessary machinery
BOOST_CONCEPT_ASSERT((HasManifoldPrereqs<Class>)); GTSAM_CONCEPT_ASSERT(HasManifoldPrereqs<Class>);
// Dimension of the manifold // Dimension of the manifold
enum { dimension = Class::dimension }; enum { dimension = Class::dimension };
@ -123,7 +120,7 @@ template<class Class> struct Manifold: ManifoldTraits<Class>, Testable<Class> {}
/// Check invariants for Manifold type /// Check invariants for Manifold type
template<typename T> template<typename T>
BOOST_CONCEPT_REQUIRES(((IsTestable<T>)),(bool)) // GTSAM_CONCEPT_REQUIRES(IsTestable<T>, bool) //
check_manifold_invariants(const T& a, const T& b, double tol=1e-9) { check_manifold_invariants(const T& a, const T& b, double tol=1e-9) {
typename traits<T>::TangentVector v0 = traits<T>::Local(a,a); typename traits<T>::TangentVector v0 = traits<T>::Local(a,a);
typename traits<T>::TangentVector v = traits<T>::Local(a,b); typename traits<T>::TangentVector v = traits<T>::Local(a,b);
@ -143,10 +140,10 @@ public:
typedef typename traits<T>::TangentVector TangentVector; typedef typename traits<T>::TangentVector TangentVector;
BOOST_CONCEPT_USAGE(IsManifold) { BOOST_CONCEPT_USAGE(IsManifold) {
BOOST_STATIC_ASSERT_MSG( static_assert(
(std::is_base_of<manifold_tag, structure_category_tag>::value), (std::is_base_of<manifold_tag, structure_category_tag>::value),
"This type's structure_category trait does not assert it as a manifold (or derived)"); "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 // make sure Chart methods are defined
v = traits<T>::Local(p, q); v = traits<T>::Local(p, q);
@ -164,7 +161,7 @@ template<typename T>
struct FixedDimension { struct FixedDimension {
typedef const int value_type; typedef const int value_type;
static const int value = traits<T>::dimension; static const int value = traits<T>::dimension;
BOOST_STATIC_ASSERT_MSG(value != Eigen::Dynamic, static_assert(value != Eigen::Dynamic,
"FixedDimension instantiated for dymanically-sized type."); "FixedDimension instantiated for dymanically-sized type.");
}; };
} // \ namespace gtsam } // \ namespace gtsam

2
gtsam/base/Matrix.h
View File

@ -279,7 +279,7 @@ struct Reshape<N, M, InOptions, M, N, InOptions> {
template <int OutM, int OutN, int OutOptions, int InM, int InN, int InOptions> template <int OutM, int OutN, int OutOptions, int InM, int InN, int InOptions>
inline typename Reshape<OutM, OutN, OutOptions, InM, InN, InOptions>::ReshapedType reshape(const Eigen::Matrix<double, InM, InN, InOptions> & m){ inline typename Reshape<OutM, OutN, OutOptions, InM, InN, InOptions>::ReshapedType reshape(const Eigen::Matrix<double, InM, InN, InOptions> & m){
BOOST_STATIC_ASSERT(InM * InN == OutM * OutN); static_assert(InM * InN == OutM * OutN);
return Reshape<OutM, OutN, OutOptions, InM, InN, InOptions>::reshape(m); return Reshape<OutM, OutN, OutOptions, InM, InN, InOptions>::reshape(m);
} }

4
gtsam/base/ProductLieGroup.h
View File

@ -27,8 +27,8 @@ namespace gtsam {
/// Assumes Lie group structure for G and H /// Assumes Lie group structure for G and H
template<typename G, typename H> template<typename G, typename H>
class ProductLieGroup: public std::pair<G, H> { class ProductLieGroup: public std::pair<G, H> {
BOOST_CONCEPT_ASSERT((IsLieGroup<G>)); GTSAM_CONCEPT_ASSERT1(IsLieGroup<G>);
BOOST_CONCEPT_ASSERT((IsLieGroup<H>)); GTSAM_CONCEPT_ASSERT2(IsLieGroup<H>);
typedef std::pair<G, H> Base; typedef std::pair<G, H> Base;
protected: protected:

7
gtsam/base/Testable.h
View File

@ -33,7 +33,8 @@
#pragma once #pragma once
#include <boost/concept_check.hpp> #include <gtsam/base/concepts.h>
#include <functional> #include <functional>
#include <iostream> #include <iostream>
#include <memory> #include <memory>
@ -151,7 +152,7 @@ namespace gtsam {
struct Testable { struct Testable {
// Check that T has the necessary methods // Check that T has the necessary methods
BOOST_CONCEPT_ASSERT((HasTestablePrereqs<T>)); GTSAM_CONCEPT_ASSERT(HasTestablePrereqs<T>);
static void Print(const T& m, const std::string& str = "") { static void Print(const T& m, const std::string& str = "") {
m.print(str); m.print(str);
@ -170,7 +171,7 @@ namespace gtsam {
* *
* NOTE: intentionally not in the gtsam namespace to allow for classes not in * NOTE: intentionally not in the gtsam namespace to allow for classes not in
* the gtsam namespace to be more easily enforced as testable * 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<T>; #define GTSAM_CONCEPT_TESTABLE_INST(T) template class gtsam::IsTestable<T>;
#define GTSAM_CONCEPT_TESTABLE_TYPE(T) using _gtsam_Testable_##T = gtsam::IsTestable<T>; #define GTSAM_CONCEPT_TESTABLE_TYPE(T) using _gtsam_Testable_##T = gtsam::IsTestable<T>;

4
gtsam/base/VectorSpace.h
View File

@ -185,7 +185,7 @@ template<class Class>
struct VectorSpaceTraits: VectorSpaceImpl<Class, Class::dimension> { struct VectorSpaceTraits: VectorSpaceImpl<Class, Class::dimension> {
// Check that Class has the necessary machinery // Check that Class has the necessary machinery
BOOST_CONCEPT_ASSERT((HasVectorSpacePrereqs<Class>)); GTSAM_CONCEPT_ASSERT(HasVectorSpacePrereqs<Class>);
typedef vector_space_tag structure_category; typedef vector_space_tag structure_category;
@ -473,7 +473,7 @@ public:
typedef typename traits<T>::structure_category structure_category_tag; typedef typename traits<T>::structure_category structure_category_tag;
BOOST_CONCEPT_USAGE(IsVectorSpace) { BOOST_CONCEPT_USAGE(IsVectorSpace) {
BOOST_STATIC_ASSERT_MSG( static_assert(
(std::is_base_of<vector_space_tag, structure_category_tag>::value), (std::is_base_of<vector_space_tag, structure_category_tag>::value),
"This type's trait does not assert it as a vector space (or derived)"); "This type's trait does not assert it as a vector space (or derived)");
r = p + q; r = p + q;

2
gtsam/base/chartTesting.h
View File

@ -39,7 +39,7 @@ void testDefaultChart(TestResult& result_,
// First, check the basic chart concept. This checks that the interface is satisfied. // 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. // The rest of the function is even more detailed, checking the correctness of the chart.
BOOST_CONCEPT_ASSERT((ChartConcept<Chart>)); GTSAM_CONCEPT_ASSERT(ChartConcept<Chart>);
T other = value; T other = value;

42
gtsam/base/concepts.h
View File

@ -8,25 +8,27 @@
#pragma once #pragma once
// This is a helper to ease the transition to the new traits defined in this file. #ifdef GTSAM_USE_BOOST_FEATURES
// Uncomment this if you want all methods that are tagged as not implemented #include <boost/concept_check.hpp>
// to cause compilation errors. #include <boost/concept/assert.hpp>
#ifdef COMPILE_ERROR_NOT_IMPLEMENTED #include <boost/concept/requires.hpp>
#include <boost/concept_check.hpp>
#include <boost/static_assert.hpp> #define GTSAM_CONCEPT_ASSERT(concept) BOOST_CONCEPT_ASSERT((concept))
#define CONCEPT_NOT_IMPLEMENTED BOOST_STATIC_ASSERT_MSG(boost::false_type, \ #define GTSAM_CONCEPT_ASSERT1(concept) BOOST_CONCEPT_ASSERT((concept))
"This method is required by the new concepts framework but has not been implemented yet."); #define GTSAM_CONCEPT_ASSERT2(concept) BOOST_CONCEPT_ASSERT((concept))
#define GTSAM_CONCEPT_ASSERT3(concept) BOOST_CONCEPT_ASSERT((concept))
#else #define GTSAM_CONCEPT_ASSERT4(concept) BOOST_CONCEPT_ASSERT((concept))
#define GTSAM_CONCEPT_REQUIRES(concept, return_type) BOOST_CONCEPT_REQUIRES(((concept)), (return_type))
#include <exception> #else
#define CONCEPT_NOT_IMPLEMENTED \ // These do something sensible:
throw std::runtime_error("This method is required by the new concepts framework but has not been implemented yet."); #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 #endif
namespace gtsam {
template <typename T> struct traits;
}

2
gtsam/base/std_optional_serialization.h
View File

@ -67,7 +67,7 @@ void save(Archive& ar, const std::optional<T>& t, const unsigned int /*version*/
// It is an inherent limitation to the serialization of optional.hpp // It is an inherent limitation to the serialization of optional.hpp
// that the underlying type must be either a pointer or must have a // that the underlying type must be either a pointer or must have a
// default constructor. // default constructor.
BOOST_STATIC_ASSERT(boost::serialization::detail::is_default_constructible<T>::value || boost::is_pointer<T>::value); static_assert(boost::serialization::detail::is_default_constructible<T>::value || boost::is_pointer<T>::value);
const bool tflag = t.has_value(); const bool tflag = t.has_value();
ar << boost::serialization::make_nvp("initialized", tflag); ar << boost::serialization::make_nvp("initialized", tflag);
if (tflag) { if (tflag) {

6
gtsam/base/tests/testGroup.cpp
View File

@ -80,7 +80,7 @@ using namespace gtsam;
typedef Symmetric<2> S2; typedef Symmetric<2> S2;
TEST(Group, S2) { TEST(Group, S2) {
S2 e, s1 = S2::Transposition(0, 1); S2 e, s1 = S2::Transposition(0, 1);
BOOST_CONCEPT_ASSERT((IsGroup<S2>)); GTSAM_CONCEPT_ASSERT(IsGroup<S2>);
EXPECT(check_group_invariants(e, s1)); EXPECT(check_group_invariants(e, s1));
} }
@ -88,7 +88,7 @@ TEST(Group, S2) {
typedef Symmetric<3> S3; typedef Symmetric<3> S3;
TEST(Group, S3) { TEST(Group, S3) {
S3 e, s1 = S3::Transposition(0, 1), s2 = S3::Transposition(1, 2); S3 e, s1 = S3::Transposition(0, 1), s2 = S3::Transposition(1, 2);
BOOST_CONCEPT_ASSERT((IsGroup<S3>)); GTSAM_CONCEPT_ASSERT(IsGroup<S3>);
EXPECT(check_group_invariants(e, s1)); EXPECT(check_group_invariants(e, s1));
EXPECT(assert_equal(s1, s1 * e)); EXPECT(assert_equal(s1, s1 * e));
EXPECT(assert_equal(s1, e * s1)); EXPECT(assert_equal(s1, e * s1));
@ -127,7 +127,7 @@ struct traits<Dih6> : internal::MultiplicativeGroupTraits<Dih6> {
TEST(Group, Dih6) { TEST(Group, Dih6) {
Dih6 e, g(S2::Transposition(0, 1), Dih6 e, g(S2::Transposition(0, 1),
S3::Transposition(0, 1) * S3::Transposition(1, 2)); S3::Transposition(0, 1) * S3::Transposition(1, 2));
BOOST_CONCEPT_ASSERT((IsGroup<Dih6>)); GTSAM_CONCEPT_ASSERT(IsGroup<Dih6>);
EXPECT(check_group_invariants(e, g)); EXPECT(check_group_invariants(e, g));
EXPECT(assert_equal(e, compose_pow(g, 0))); EXPECT(assert_equal(e, compose_pow(g, 0)));
EXPECT(assert_equal(g, compose_pow(g, 1))); EXPECT(assert_equal(g, compose_pow(g, 1)));

24
gtsam/base/tests/testKruskal.cpp
View File

@ -37,12 +37,12 @@ gtsam::GaussianFactorGraph makeTestGaussianFactorGraph() {
Matrix I = I_2x2; Matrix I = I_2x2;
Vector2 b(0, 0); Vector2 b(0, 0);
const SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.5)); const SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.5));
gfg += JacobianFactor(X(1), I, X(2), I, b, model); gfg.emplace_shared<JacobianFactor>(X(1), I, X(2), I, b, model);
gfg += JacobianFactor(X(1), I, X(3), I, b, model); gfg.emplace_shared<JacobianFactor>(X(1), I, X(3), I, b, model);
gfg += JacobianFactor(X(1), I, X(4), I, b, model); gfg.emplace_shared<JacobianFactor>(X(1), I, X(4), I, b, model);
gfg += JacobianFactor(X(2), I, X(3), I, b, model); gfg.emplace_shared<JacobianFactor>(X(2), I, X(3), I, b, model);
gfg += JacobianFactor(X(2), I, X(4), I, b, model); gfg.emplace_shared<JacobianFactor>(X(2), I, X(4), I, b, model);
gfg += JacobianFactor(X(3), I, X(4), I, b, model); gfg.emplace_shared<JacobianFactor>(X(3), I, X(4), I, b, model);
return gfg; return gfg;
} }
@ -53,12 +53,12 @@ gtsam::NonlinearFactorGraph makeTestNonlinearFactorGraph() {
NonlinearFactorGraph nfg; NonlinearFactorGraph nfg;
const SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.5)); const SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.5));
nfg += BetweenFactor(X(1), X(2), Rot3(), model); nfg.emplace_shared<BetweenFactor<Rot3>>(X(1), X(2), Rot3(), model);
nfg += BetweenFactor(X(1), X(3), Rot3(), model); nfg.emplace_shared<BetweenFactor<Rot3>>(X(1), X(3), Rot3(), model);
nfg += BetweenFactor(X(1), X(4), Rot3(), model); nfg.emplace_shared<BetweenFactor<Rot3>>(X(1), X(4), Rot3(), model);
nfg += BetweenFactor(X(2), X(3), Rot3(), model); nfg.emplace_shared<BetweenFactor<Rot3>>(X(2), X(3), Rot3(), model);
nfg += BetweenFactor(X(2), X(4), Rot3(), model); nfg.emplace_shared<BetweenFactor<Rot3>>(X(2), X(4), Rot3(), model);
nfg += BetweenFactor(X(3), X(4), Rot3(), model); nfg.emplace_shared<BetweenFactor<Rot3>>(X(3), X(4), Rot3(), model);
return nfg; return nfg;
} }

31
gtsam/base/tests/testMatrix.cpp
View File

@ -1146,17 +1146,30 @@ TEST(Matrix, DLT )
} }
//****************************************************************************** //******************************************************************************
TEST(Matrix , IsVectorSpace) { TEST(Matrix, Matrix24IsVectorSpace) {
BOOST_CONCEPT_ASSERT((IsVectorSpace<Matrix24>)); GTSAM_CONCEPT_ASSERT(IsVectorSpace<Matrix24>);
typedef Eigen::Matrix<double,2,3,Eigen::RowMajor> RowMajor;
BOOST_CONCEPT_ASSERT((IsVectorSpace<RowMajor>));
BOOST_CONCEPT_ASSERT((IsVectorSpace<Matrix>));
BOOST_CONCEPT_ASSERT((IsVectorSpace<Vector>));
typedef Eigen::Matrix<double,1,-1> RowVector;
BOOST_CONCEPT_ASSERT((IsVectorSpace<RowVector>));
BOOST_CONCEPT_ASSERT((IsVectorSpace<Vector5>));
} }
TEST(Matrix, RowMajorIsVectorSpace) {
typedef Eigen::Matrix<double, 2, 3, Eigen::RowMajor> RowMajor;
GTSAM_CONCEPT_ASSERT(IsVectorSpace<RowMajor>);
}
TEST(Matrix, MatrixIsVectorSpace) {
GTSAM_CONCEPT_ASSERT(IsVectorSpace<Matrix>);
}
TEST(Matrix, VectorIsVectorSpace) {
GTSAM_CONCEPT_ASSERT(IsVectorSpace<Vector>);
}
TEST(Matrix, RowVectorIsVectorSpace) {
typedef Eigen::Matrix<double, 1, -1> RowVector;
GTSAM_CONCEPT_ASSERT1(IsVectorSpace<RowVector>);
GTSAM_CONCEPT_ASSERT2(IsVectorSpace<Vector5>);
}
//******************************************************************************
TEST(Matrix, AbsoluteError) { TEST(Matrix, AbsoluteError) {
double a = 2000, b = 1997, tol = 1e-1; double a = 2000, b = 1997, tol = 1e-1;
bool isEqual; bool isEqual;

11
gtsam/base/tests/testVector.cpp
View File

@ -266,11 +266,14 @@ TEST(Vector, linear_dependent3 )
} }
//****************************************************************************** //******************************************************************************
TEST(Vector, IsVectorSpace) { TEST(Vector, VectorIsVectorSpace) {
BOOST_CONCEPT_ASSERT((IsVectorSpace<Vector5>)); GTSAM_CONCEPT_ASSERT1(IsVectorSpace<Vector5>);
BOOST_CONCEPT_ASSERT((IsVectorSpace<Vector>)); GTSAM_CONCEPT_ASSERT2(IsVectorSpace<Vector>);
}
TEST(Vector, RowVectorIsVectorSpace) {
typedef Eigen::Matrix<double,1,-1> RowVector; typedef Eigen::Matrix<double,1,-1> RowVector;
BOOST_CONCEPT_ASSERT((IsVectorSpace<RowVector>)); GTSAM_CONCEPT_ASSERT(IsVectorSpace<RowVector>);
} }
/* ************************************************************************* */ /* ************************************************************************* */

38
gtsam/base/timing.cpp
View File

@ -30,6 +30,9 @@
namespace gtsam { namespace gtsam {
namespace internal { namespace internal {
// a static shared_ptr to TimingOutline with nullptr as the pointer
const static std::shared_ptr<TimingOutline> nullTimingOutline;
GTSAM_EXPORT std::shared_ptr<TimingOutline> gTimingRoot( GTSAM_EXPORT std::shared_ptr<TimingOutline> gTimingRoot(
new TimingOutline("Total", getTicTocID("Total"))); 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) : 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_( id_(id), t_(0), tWall_(0), t2_(0.0), tIt_(0), tMax_(0), tMin_(0), n_(0), myOrder_(
0), lastChildOrder_(0), label_(label) { 0), lastChildOrder_(0), label_(label) {
#ifdef GTSAM_USE_BOOST_FEATURES
#ifdef GTSAM_USING_NEW_BOOST_TIMERS #ifdef GTSAM_USING_NEW_BOOST_TIMERS
timer_.stop(); timer_.stop();
#endif #endif
#endif
} }
/* ************************************************************************* */ /* ************************************************************************* */
size_t TimingOutline::time() const { size_t TimingOutline::time() const {
#ifdef GTSAM_USE_BOOST_FEATURES
size_t time = 0; size_t time = 0;
bool hasChildren = false; bool hasChildren = false;
for(const ChildMap::value_type& child: children_) { for(const ChildMap::value_type& child: children_) {
@ -70,10 +76,14 @@ size_t TimingOutline::time() const {
return time; return time;
else else
return t_; return t_;
#else
return 0;
#endif
} }
/* ************************************************************************* */ /* ************************************************************************* */
void TimingOutline::print(const std::string& outline) const { void TimingOutline::print(const std::string& outline) const {
#ifdef GTSAM_USE_BOOST_FEATURES
std::string formattedLabel = label_; std::string formattedLabel = label_;
std::replace(formattedLabel.begin(), formattedLabel.end(), '_', ' '); std::replace(formattedLabel.begin(), formattedLabel.end(), '_', ' ');
std::cout << outline << "-" << formattedLabel << ": " << self() << " CPU (" std::cout << outline << "-" << formattedLabel << ": " << self() << " CPU ("
@ -92,11 +102,12 @@ void TimingOutline::print(const std::string& outline) const {
order_child.second->print(childOutline); order_child.second->print(childOutline);
} }
std::cout.flush(); std::cout.flush();
#endif
} }
void TimingOutline::print2(const std::string& outline, void TimingOutline::print2(const std::string& outline,
const double parentTotal) const { const double parentTotal) const {
#ifdef GTSAM_USE_BOOST_FEATURES
const int w1 = 24, w2 = 2, w3 = 6, w4 = 8, precision = 2; const int w1 = 24, w2 = 2, w3 = 6, w4 = 8, precision = 2;
const double selfTotal = self(), selfMean = selfTotal / double(n_); const double selfTotal = self(), selfMean = selfTotal / double(n_);
const double childTotal = secs(); const double childTotal = secs();
@ -135,11 +146,13 @@ void TimingOutline::print2(const std::string& outline,
child.second->print2(childOutline, selfTotal); child.second->print2(childOutline, selfTotal);
} }
} }
#endif
} }
/* ************************************************************************* */ /* ************************************************************************* */
const std::shared_ptr<TimingOutline>& TimingOutline::child(size_t child, const std::shared_ptr<TimingOutline>& TimingOutline::child(size_t child,
const std::string& label, const std::weak_ptr<TimingOutline>& thisPtr) { const std::string& label, const std::weak_ptr<TimingOutline>& thisPtr) {
#ifdef GTSAM_USE_BOOST_FEATURES
assert(thisPtr.lock().get() == this); assert(thisPtr.lock().get() == this);
std::shared_ptr<TimingOutline>& result = children_[child]; std::shared_ptr<TimingOutline>& result = children_[child];
if (!result) { if (!result) {
@ -150,10 +163,15 @@ const std::shared_ptr<TimingOutline>& TimingOutline::child(size_t child,
result->parent_ = thisPtr; result->parent_ = thisPtr;
} }
return result; return result;
#else
return nullTimingOutline;
#endif
} }
/* ************************************************************************* */ /* ************************************************************************* */
void TimingOutline::tic() { void TimingOutline::tic() {
// Disable this entire function if we are not using boost
#ifdef GTSAM_USE_BOOST_FEATURES
#ifdef GTSAM_USING_NEW_BOOST_TIMERS #ifdef GTSAM_USING_NEW_BOOST_TIMERS
assert(timer_.is_stopped()); assert(timer_.is_stopped());
timer_.start(); timer_.start();
@ -166,10 +184,14 @@ void TimingOutline::tic() {
#ifdef GTSAM_USE_TBB #ifdef GTSAM_USE_TBB
tbbTimer_ = tbb::tick_count::now(); tbbTimer_ = tbb::tick_count::now();
#endif #endif
#endif
} }
/* ************************************************************************* */ /* ************************************************************************* */
void TimingOutline::toc() { void TimingOutline::toc() {
// Disable this entire function if we are not using boost
#ifdef GTSAM_USE_BOOST_FEATURES
#ifdef GTSAM_USING_NEW_BOOST_TIMERS #ifdef GTSAM_USING_NEW_BOOST_TIMERS
assert(!timer_.is_stopped()); assert(!timer_.is_stopped());
@ -197,10 +219,12 @@ void TimingOutline::toc() {
#endif #endif
add(cpuTime, wallTime); add(cpuTime, wallTime);
#endif
} }
/* ************************************************************************* */ /* ************************************************************************* */
void TimingOutline::finishedIteration() { void TimingOutline::finishedIteration() {
#ifdef GTSAM_USE_BOOST_FEATURES
if (tIt_ > tMax_) if (tIt_ > tMax_)
tMax_ = tIt_; tMax_ = tIt_;
if (tMin_ == 0 || tIt_ < tMin_) if (tMin_ == 0 || tIt_ < tMin_)
@ -209,10 +233,13 @@ void TimingOutline::finishedIteration() {
for(ChildMap::value_type& child: children_) { for(ChildMap::value_type& child: children_) {
child.second->finishedIteration(); child.second->finishedIteration();
} }
#endif
} }
/* ************************************************************************* */ /* ************************************************************************* */
size_t getTicTocID(const char *descriptionC) { 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); const std::string description(descriptionC);
// Global (static) map from strings to ID numbers and current next ID number // Global (static) map from strings to ID numbers and current next ID number
static size_t nextId = 0; static size_t nextId = 0;
@ -227,19 +254,27 @@ size_t getTicTocID(const char *descriptionC) {
// Return ID // Return ID
return it->second; return it->second;
#else
return 0;
#endif
} }
/* ************************************************************************* */ /* ************************************************************************* */
void tic(size_t id, const char *labelC) { 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); const std::string label(labelC);
std::shared_ptr<TimingOutline> node = // std::shared_ptr<TimingOutline> node = //
gCurrentTimer.lock()->child(id, label, gCurrentTimer); gCurrentTimer.lock()->child(id, label, gCurrentTimer);
gCurrentTimer = node; gCurrentTimer = node;
node->tic(); node->tic();
#endif
} }
/* ************************************************************************* */ /* ************************************************************************* */
void toc(size_t id, const char *label) { 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<TimingOutline> current(gCurrentTimer.lock()); std::shared_ptr<TimingOutline> current(gCurrentTimer.lock());
if (id != current->id_) { if (id != current->id_) {
gTimingRoot->print(); gTimingRoot->print();
@ -255,6 +290,7 @@ void toc(size_t id, const char *label) {
} }
current->toc(); current->toc();
gCurrentTimer = current->parent_; gCurrentTimer = current->parent_;
#endif
} }
} // namespace internal } // namespace internal

16
gtsam/base/timing.h
View File

@ -21,7 +21,9 @@
#include <gtsam/dllexport.h> #include <gtsam/dllexport.h>
#include <gtsam/config.h> // for GTSAM_USE_TBB #include <gtsam/config.h> // for GTSAM_USE_TBB
#ifdef GTSAM_USE_BOOST_FEATURES
#include <boost/version.hpp> #include <boost/version.hpp>
#endif
#include <memory> #include <memory>
#include <cstddef> #include <cstddef>
@ -105,6 +107,7 @@
// have matching gttic/gttoc statments. You may want to consider reorganizing your timing // have matching gttic/gttoc statments. You may want to consider reorganizing your timing
// outline to match the scope of your code. // outline to match the scope of your code.
#ifdef GTSAM_USE_BOOST_FEATURES
// Automatically use the new Boost timers if version is recent enough. // Automatically use the new Boost timers if version is recent enough.
#if BOOST_VERSION >= 104800 #if BOOST_VERSION >= 104800
# ifndef GTSAM_DISABLE_NEW_TIMERS # ifndef GTSAM_DISABLE_NEW_TIMERS
@ -118,6 +121,7 @@
# include <boost/timer.hpp> # include <boost/timer.hpp>
# include <gtsam/base/types.h> # include <gtsam/base/types.h>
#endif #endif
#endif
#ifdef GTSAM_USE_TBB #ifdef GTSAM_USE_TBB
# include <tbb/tick_count.h> # include <tbb/tick_count.h>
@ -160,6 +164,8 @@ namespace gtsam {
typedef FastMap<size_t, std::shared_ptr<TimingOutline> > ChildMap; typedef FastMap<size_t, std::shared_ptr<TimingOutline> > ChildMap;
ChildMap children_; ///< subtrees ChildMap children_; ///< subtrees
// disable all timers if not using boost
#ifdef GTSAM_USE_BOOST_FEATURES
#ifdef GTSAM_USING_NEW_BOOST_TIMERS #ifdef GTSAM_USING_NEW_BOOST_TIMERS
boost::timer::cpu_timer timer_; boost::timer::cpu_timer timer_;
#else #else
@ -168,6 +174,7 @@ namespace gtsam {
#endif #endif
#ifdef GTSAM_USE_TBB #ifdef GTSAM_USE_TBB
tbb::tick_count tbbTimer_; tbb::tick_count tbbTimer_;
#endif
#endif #endif
void add(size_t usecs, size_t usecsWall); 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 TimingOutline(const std::string& label, size_t myId);
GTSAM_EXPORT size_t time() const; ///< time taken, including children GTSAM_EXPORT size_t time() const; ///< time taken, including children
double secs() const { return double(time()) / 1000000.0;} ///< time taken, in seconds, 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 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 wall() const { return double(tWall_) / 1000000.0;} ///< wall time, in seconds
double min() const { return double(tMin_) / 1000000.0;} ///< min 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 max() const { return double(tMax_) / 1000000.0;} ///< max time, in seconds
double mean() const { return self() / double(n_); } ///< mean self 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 print(const std::string& outline = "") const;
GTSAM_EXPORT void print2(const std::string& outline = "", const double parentTotal = -1.0) const; GTSAM_EXPORT void print2(const std::string& outline = "", const double parentTotal = -1.0) const;
GTSAM_EXPORT const std::shared_ptr<TimingOutline>& GTSAM_EXPORT const std::shared_ptr<TimingOutline>&

30
gtsam/base/types.h
View File

@ -20,8 +20,10 @@
#pragma once #pragma once
#include <gtsam/dllexport.h> #include <gtsam/dllexport.h>
#ifdef GTSAM_USE_BOOST_FEATURES
#include <boost/concept/assert.hpp> #include <boost/concept/assert.hpp>
#include <boost/range/concepts.hpp> #include <boost/range/concepts.hpp>
#endif
#include <gtsam/config.h> // for GTSAM_USE_TBB #include <gtsam/config.h> // for GTSAM_USE_TBB
#include <cstddef> #include <cstddef>
@ -153,33 +155,7 @@ namespace gtsam {
operator T() const { return value; } operator T() const { return value; }
}; };
/* ************************************************************************* */ /* ************************************************************************* */
/** A helper class that behaves as a container with one element, and works with
* boost::range */
template<typename T>
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<ListOfOneContainer<int> >));
/** Factory function for ListOfOneContainer to enable ListOfOne(e) syntax. */
template<typename T>
ListOfOneContainer<T> ListOfOne(const T& element) {
return ListOfOneContainer<T>(element);
}
/* ************************************************************************* */
#ifdef __clang__ #ifdef __clang__
# pragma clang diagnostic push # pragma clang diagnostic push
# pragma clang diagnostic ignored "-Wunused-private-field" // Clang complains that previousOpenMPThreads is unused in the #else case below # pragma clang diagnostic ignored "-Wunused-private-field" // Clang complains that previousOpenMPThreads is unused in the #else case below

23
gtsam/base/utilities.h
View File

@ -28,16 +28,9 @@ private:
} }
// boost::index_sequence was introduced in 1.66, so we'll manually define an namespace gtsam {
// implementation if user has 1.65. boost::index_sequence is used to get array
// indices that align with a parameter pack.
#include <boost/version.hpp>
#if BOOST_VERSION >= 106600
#include <boost/mp11/integer_sequence.hpp>
#else
namespace boost {
namespace mp11 {
// Adapted from https://stackoverflow.com/a/32223343/9151520 // Adapted from https://stackoverflow.com/a/32223343/9151520
// An adaptation of boost::mp11::index_sequence
template <size_t... Ints> template <size_t... Ints>
struct index_sequence { struct index_sequence {
using type = index_sequence; using type = index_sequence;
@ -49,20 +42,16 @@ template <class Sequence1, class Sequence2>
struct _merge_and_renumber; struct _merge_and_renumber;
template <size_t... I1, size_t... I2> template <size_t... I1, size_t... I2>
struct _merge_and_renumber<index_sequence<I1...>, index_sequence<I2...> > struct _merge_and_renumber<index_sequence<I1...>, index_sequence<I2...>>
: index_sequence<I1..., (sizeof...(I1) + I2)...> {}; : index_sequence<I1..., (sizeof...(I1) + I2)...> {};
} // namespace detail } // namespace detail
template <size_t N> template <size_t N>
struct make_index_sequence struct make_index_sequence : detail::_merge_and_renumber<typename make_index_sequence<N / 2>::type,
: detail::_merge_and_renumber< typename make_index_sequence<N - N / 2>::type> {};
typename make_index_sequence<N / 2>::type,
typename make_index_sequence<N - N / 2>::type> {};
template <> template <>
struct make_index_sequence<0> : index_sequence<> {}; struct make_index_sequence<0> : index_sequence<> {};
template <> template <>
struct make_index_sequence<1> : index_sequence<0> {}; struct make_index_sequence<1> : index_sequence<0> {};
template <class... T> template <class... T>
using index_sequence_for = make_index_sequence<sizeof...(T)>; using index_sequence_for = make_index_sequence<sizeof...(T)>;
} // namespace mp11 } // namespace gtsam
} // namespace boost
#endif

2
gtsam/discrete/SignatureParser.cpp
View File

@ -33,6 +33,8 @@ std::optional<Row> static ParseConditional(const std::string& token) {
// can throw exception // can throw exception
row.push_back(std::stod(s)); row.push_back(std::stod(s));
} }
// if we ended with a '/' then return false
if (token.back() == '/') return std::nullopt;
} catch (...) { } catch (...) {
return std::nullopt; return std::nullopt;
} }

3
gtsam/discrete/tests/testDiscreteBayesNet.cpp
View File

@ -94,8 +94,7 @@ TEST(DiscreteBayesNet, Asia) {
EXPECT(assert_equal(vs, marginals.marginalProbabilities(Smoking))); EXPECT(assert_equal(vs, marginals.marginalProbabilities(Smoking)));
// Create solver and eliminate // Create solver and eliminate
Ordering ordering; const Ordering ordering{0, 1, 2, 3, 4, 5, 6, 7};
ordering += Key(0), Key(1), Key(2), Key(3), Key(4), Key(5), Key(6), Key(7);
DiscreteBayesNet::shared_ptr chordal = fg.eliminateSequential(ordering); DiscreteBayesNet::shared_ptr chordal = fg.eliminateSequential(ordering);
DiscreteConditional expected2(Bronchitis % "11/9"); DiscreteConditional expected2(Bronchitis % "11/9");
EXPECT(assert_equal(expected2, *chordal->back())); EXPECT(assert_equal(expected2, *chordal->back()));

9
gtsam/discrete/tests/testDiscreteFactorGraph.cpp
View File

@ -107,8 +107,7 @@ TEST(DiscreteFactorGraph, test) {
graph.add(C & B, "3 1 1 3"); graph.add(C & B, "3 1 1 3");
// Test EliminateDiscrete // Test EliminateDiscrete
Ordering frontalKeys; const Ordering frontalKeys{0};
frontalKeys += Key(0);
const auto [conditional, newFactor] = EliminateDiscrete(graph, frontalKeys); const auto [conditional, newFactor] = EliminateDiscrete(graph, frontalKeys);
// Check Conditional // Check Conditional
@ -123,8 +122,7 @@ TEST(DiscreteFactorGraph, test) {
EXPECT(assert_equal(expectedFactor, *newFactor)); EXPECT(assert_equal(expectedFactor, *newFactor));
// Test using elimination tree // Test using elimination tree
Ordering ordering; const Ordering ordering{0, 1, 2};
ordering += Key(0), Key(1), Key(2);
DiscreteEliminationTree etree(graph, ordering); DiscreteEliminationTree etree(graph, ordering);
const auto [actual, remainingGraph] = etree.eliminate(&EliminateDiscrete); const auto [actual, remainingGraph] = etree.eliminate(&EliminateDiscrete);
@ -231,8 +229,7 @@ TEST(DiscreteFactorGraph, testMPE_Darwiche09book_p244) {
EXPECT(assert_equal(mpe, actualMPE)); EXPECT(assert_equal(mpe, actualMPE));
// Check Bayes Net // Check Bayes Net
Ordering ordering; const Ordering ordering{0, 1, 2, 3, 4};
ordering += Key(0), Key(1), Key(2), Key(3), Key(4);
auto chordal = graph.eliminateSequential(ordering); auto chordal = graph.eliminateSequential(ordering);
EXPECT_LONGS_EQUAL(5, chordal->size()); EXPECT_LONGS_EQUAL(5, chordal->size());

7
gtsam/discrete/tests/testSignatureParser.cpp
View File

@ -85,11 +85,16 @@ TEST(SimpleParser, Gibberish) {
// If Gibberish is in the middle, it should not parse. // If Gibberish is in the middle, it should not parse.
TEST(SimpleParser, GibberishInMiddle) { TEST(SimpleParser, GibberishInMiddle) {
SignatureParser::Table expectedTable{{1, 1}, {2, 3}};
const auto table = SignatureParser::Parse("1/1 2/3 sdf 1/4"); const auto table = SignatureParser::Parse("1/1 2/3 sdf 1/4");
EXPECT(!table); EXPECT(!table);
} }
// A test with slash in the end
TEST(SimpleParser, SlashInEnd) {
const auto table = SignatureParser::Parse("1/1 2/");
EXPECT(!table);
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {

1
gtsam/geometry/BearingRange.h
View File

@ -21,7 +21,6 @@
#include <gtsam/base/Manifold.h> #include <gtsam/base/Manifold.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/OptionalJacobian.h> #include <gtsam/base/OptionalJacobian.h>
#include <boost/concept/assert.hpp>
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/nvp.hpp> #include <boost/serialization/nvp.hpp>
#endif #endif

1
gtsam/geometry/Rot3.cpp
View File

@ -21,7 +21,6 @@
#include <gtsam/geometry/Rot3.h> #include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/SO3.h> #include <gtsam/geometry/SO3.h>
#include <boost/math/constants/constants.hpp>
#include <cmath> #include <cmath>
#include <random> #include <random>

1
gtsam/geometry/Rot3M.cpp
View File

@ -24,7 +24,6 @@
#include <gtsam/geometry/Rot3.h> #include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/SO3.h> #include <gtsam/geometry/SO3.h>
#include <boost/math/constants/constants.hpp>
#include <cmath> #include <cmath>
using namespace std; using namespace std;

1
gtsam/geometry/Rot3Q.cpp
View File

@ -20,7 +20,6 @@
#ifdef GTSAM_USE_QUATERNIONS #ifdef GTSAM_USE_QUATERNIONS
#include <gtsam/geometry/Rot3.h> #include <gtsam/geometry/Rot3.h>
#include <boost/math/constants/constants.hpp>
#include <cmath> #include <cmath>
using namespace std; using namespace std;

4
gtsam/geometry/tests/testBearingRange.cpp
View File

@ -34,7 +34,7 @@ BearingRange3D br3D(Pose3().bearing(Point3(1, 0, 0)), 1);
//****************************************************************************** //******************************************************************************
TEST(BearingRange2D, Concept) { TEST(BearingRange2D, Concept) {
BOOST_CONCEPT_ASSERT((IsManifold<BearingRange2D>)); GTSAM_CONCEPT_ASSERT(IsManifold<BearingRange2D>);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -46,7 +46,7 @@ TEST(BearingRange, 2D) {
//****************************************************************************** //******************************************************************************
TEST(BearingRange3D, Concept) { TEST(BearingRange3D, Concept) {
BOOST_CONCEPT_ASSERT((IsManifold<BearingRange3D>)); GTSAM_CONCEPT_ASSERT(IsManifold<BearingRange3D>);
} }
/* ************************************************************************* */ /* ************************************************************************* */

6
gtsam/geometry/tests/testCyclic.cpp
View File

@ -27,7 +27,7 @@ typedef Cyclic<2> Z2;
//****************************************************************************** //******************************************************************************
TEST(Cyclic, Concept) { TEST(Cyclic, Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<Z3>)); GTSAM_CONCEPT_ASSERT(IsGroup<Z3>);
EXPECT_LONGS_EQUAL(0, traits<Z3>::Identity()); EXPECT_LONGS_EQUAL(0, traits<Z3>::Identity());
} }
@ -107,8 +107,8 @@ struct traits<K4> : internal::AdditiveGroupTraits<K4> {
TEST(Cyclic , DirectSum) { TEST(Cyclic , DirectSum) {
// The Direct sum of Z2 and Z2 is *not* Cyclic<4>, but the // The Direct sum of Z2 and Z2 is *not* Cyclic<4>, but the
// smallest non-cyclic group called the Klein four-group: // smallest non-cyclic group called the Klein four-group:
BOOST_CONCEPT_ASSERT((IsGroup<K4>)); GTSAM_CONCEPT_ASSERT1(IsGroup<K4>);
BOOST_CONCEPT_ASSERT((IsTestable<K4>)); GTSAM_CONCEPT_ASSERT2(IsTestable<K4>);
// Refer to http://en.wikipedia.org/wiki/Klein_four-group // Refer to http://en.wikipedia.org/wiki/Klein_four-group
K4 e(0,0), a(0, 1), b(1, 0), c(1, 1); K4 e(0,0), a(0, 1), b(1, 0), c(1, 1);

12
gtsam/geometry/tests/testPoint2.cpp
View File

@ -34,9 +34,9 @@ TEST(Point2 , Constructor) {
//****************************************************************************** //******************************************************************************
TEST(Double , Concept) { TEST(Double , Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<double>)); GTSAM_CONCEPT_ASSERT1(IsGroup<double>);
BOOST_CONCEPT_ASSERT((IsManifold<double>)); GTSAM_CONCEPT_ASSERT2(IsManifold<double>);
BOOST_CONCEPT_ASSERT((IsVectorSpace<double>)); GTSAM_CONCEPT_ASSERT3(IsVectorSpace<double>);
} }
//****************************************************************************** //******************************************************************************
@ -48,9 +48,9 @@ TEST(Double , Invariants) {
//****************************************************************************** //******************************************************************************
TEST(Point2 , Concept) { TEST(Point2 , Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<Point2>)); GTSAM_CONCEPT_ASSERT1(IsGroup<Point2>);
BOOST_CONCEPT_ASSERT((IsManifold<Point2>)); GTSAM_CONCEPT_ASSERT2(IsManifold<Point2>);
BOOST_CONCEPT_ASSERT((IsVectorSpace<Point2>)); GTSAM_CONCEPT_ASSERT3(IsVectorSpace<Point2>);
} }
//****************************************************************************** //******************************************************************************

6
gtsam/geometry/tests/testPoint3.cpp
View File

@ -34,9 +34,9 @@ TEST(Point3 , Constructor) {
//****************************************************************************** //******************************************************************************
TEST(Point3 , Concept) { TEST(Point3 , Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<Point3>)); GTSAM_CONCEPT_ASSERT1(IsGroup<Point3>);
BOOST_CONCEPT_ASSERT((IsManifold<Point3>)); GTSAM_CONCEPT_ASSERT2(IsManifold<Point3>);
BOOST_CONCEPT_ASSERT((IsVectorSpace<Point3>)); GTSAM_CONCEPT_ASSERT3(IsVectorSpace<Point3>);
} }
//****************************************************************************** //******************************************************************************

6
gtsam/geometry/tests/testPose2.cpp
View File

@ -35,9 +35,9 @@ GTSAM_CONCEPT_LIE_INST(Pose2)
//****************************************************************************** //******************************************************************************
TEST(Pose2 , Concept) { TEST(Pose2 , Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<Pose2 >)); GTSAM_CONCEPT_ASSERT1(IsGroup<Pose2 >);
BOOST_CONCEPT_ASSERT((IsManifold<Pose2 >)); GTSAM_CONCEPT_ASSERT2(IsManifold<Pose2 >);
BOOST_CONCEPT_ASSERT((IsLieGroup<Pose2 >)); GTSAM_CONCEPT_ASSERT3(IsLieGroup<Pose2 >);
} }
/* ************************************************************************* */ /* ************************************************************************* */

6
gtsam/geometry/tests/testQuaternion.cpp
View File

@ -29,9 +29,9 @@ typedef traits<Q>::ChartJacobian QuaternionJacobian;
//****************************************************************************** //******************************************************************************
TEST(Quaternion , Concept) { TEST(Quaternion , Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<Quaternion >)); GTSAM_CONCEPT_ASSERT1(IsGroup<Quaternion >);
BOOST_CONCEPT_ASSERT((IsManifold<Quaternion >)); GTSAM_CONCEPT_ASSERT2(IsManifold<Quaternion >);
BOOST_CONCEPT_ASSERT((IsLieGroup<Quaternion >)); GTSAM_CONCEPT_ASSERT3(IsLieGroup<Quaternion >);
} }
//****************************************************************************** //******************************************************************************

6
gtsam/geometry/tests/testRot3.cpp
View File

@ -38,9 +38,9 @@ static double error = 1e-9, epsilon = 0.001;
//****************************************************************************** //******************************************************************************
TEST(Rot3 , Concept) { TEST(Rot3 , Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<Rot3 >)); GTSAM_CONCEPT_ASSERT1(IsGroup<Rot3 >);
BOOST_CONCEPT_ASSERT((IsManifold<Rot3 >)); GTSAM_CONCEPT_ASSERT2(IsManifold<Rot3 >);
BOOST_CONCEPT_ASSERT((IsLieGroup<Rot3 >)); GTSAM_CONCEPT_ASSERT3(IsLieGroup<Rot3 >);
} }
/* ************************************************************************* */ /* ************************************************************************* */

6
gtsam/geometry/tests/testSO3.cpp
View File

@ -36,9 +36,9 @@ TEST(SO3, Identity) {
//****************************************************************************** //******************************************************************************
TEST(SO3, Concept) { TEST(SO3, Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<SO3>)); GTSAM_CONCEPT_ASSERT1(IsGroup<SO3>);
BOOST_CONCEPT_ASSERT((IsManifold<SO3>)); GTSAM_CONCEPT_ASSERT2(IsManifold<SO3>);
BOOST_CONCEPT_ASSERT((IsLieGroup<SO3>)); GTSAM_CONCEPT_ASSERT3(IsLieGroup<SO3>);
} }
//****************************************************************************** //******************************************************************************

6
gtsam/geometry/tests/testSO4.cpp
View File

@ -42,9 +42,9 @@ TEST(SO4, Identity) {
//****************************************************************************** //******************************************************************************
TEST(SO4, Concept) { TEST(SO4, Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<SO4>)); GTSAM_CONCEPT_ASSERT1(IsGroup<SO4>);
BOOST_CONCEPT_ASSERT((IsManifold<SO4>)); GTSAM_CONCEPT_ASSERT2(IsManifold<SO4>);
BOOST_CONCEPT_ASSERT((IsLieGroup<SO4>)); GTSAM_CONCEPT_ASSERT3(IsLieGroup<SO4>);
} }
//****************************************************************************** //******************************************************************************

6
gtsam/geometry/tests/testSOn.cpp
View File

@ -84,9 +84,9 @@ TEST(SOn, SO5) {
//****************************************************************************** //******************************************************************************
TEST(SOn, Concept) { TEST(SOn, Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<SOn>)); GTSAM_CONCEPT_ASSERT1(IsGroup<SOn>);
BOOST_CONCEPT_ASSERT((IsManifold<SOn>)); GTSAM_CONCEPT_ASSERT2(IsManifold<SOn>);
BOOST_CONCEPT_ASSERT((IsLieGroup<SOn>)); GTSAM_CONCEPT_ASSERT3(IsLieGroup<SOn>);
} }
//****************************************************************************** //******************************************************************************

6
gtsam/geometry/tests/testSimilarity2.cpp
View File

@ -35,9 +35,9 @@ static const double s = 4;
//****************************************************************************** //******************************************************************************
TEST(Similarity2, Concepts) { TEST(Similarity2, Concepts) {
BOOST_CONCEPT_ASSERT((IsGroup<Similarity2>)); GTSAM_CONCEPT_ASSERT1(IsGroup<Similarity2>);
BOOST_CONCEPT_ASSERT((IsManifold<Similarity2>)); GTSAM_CONCEPT_ASSERT2(IsManifold<Similarity2>);
BOOST_CONCEPT_ASSERT((IsLieGroup<Similarity2>)); GTSAM_CONCEPT_ASSERT3(IsLieGroup<Similarity2>);
} }
//****************************************************************************** //******************************************************************************

6
gtsam/geometry/tests/testSimilarity3.cpp
View File

@ -54,9 +54,9 @@ const double degree = M_PI / 180;
//****************************************************************************** //******************************************************************************
TEST(Similarity3, Concepts) { TEST(Similarity3, Concepts) {
BOOST_CONCEPT_ASSERT((IsGroup<Similarity3 >)); GTSAM_CONCEPT_ASSERT1(IsGroup<Similarity3 >);
BOOST_CONCEPT_ASSERT((IsManifold<Similarity3 >)); GTSAM_CONCEPT_ASSERT2(IsManifold<Similarity3 >);
BOOST_CONCEPT_ASSERT((IsLieGroup<Similarity3 >)); GTSAM_CONCEPT_ASSERT3(IsLieGroup<Similarity3 >);
} }
//****************************************************************************** //******************************************************************************

6
gtsam/geometry/tests/testStereoPoint2.cpp
View File

@ -31,9 +31,9 @@ GTSAM_CONCEPT_TESTABLE_INST(StereoPoint2)
//****************************************************************************** //******************************************************************************
TEST(StereoPoint2 , Concept) { TEST(StereoPoint2 , Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<StereoPoint2>)); GTSAM_CONCEPT_ASSERT1(IsGroup<StereoPoint2>);
BOOST_CONCEPT_ASSERT((IsManifold<StereoPoint2 >)); GTSAM_CONCEPT_ASSERT2(IsManifold<StereoPoint2 >);
BOOST_CONCEPT_ASSERT((IsVectorSpace<StereoPoint2>)); GTSAM_CONCEPT_ASSERT3(IsVectorSpace<StereoPoint2>);
} }
/* ************************************************************************* */ /* ************************************************************************* */

6
gtsam/hybrid/HybridGaussianISAM.cpp
View File

@ -84,12 +84,12 @@ void HybridGaussianISAM::updateInternal(
// Add the removed top and the new factors // Add the removed top and the new factors
HybridGaussianFactorGraph factors; HybridGaussianFactorGraph factors;
factors += bn; factors.push_back(bn);
factors += newFactors; factors.push_back(newFactors);
// Add the orphaned subtrees // Add the orphaned subtrees
for (const sharedClique& orphan : *orphans) { for (const sharedClique& orphan : *orphans) {
factors += std::make_shared<BayesTreeOrphanWrapper<Node>>(orphan); factors.emplace_shared<BayesTreeOrphanWrapper<Node>>(orphan);
} }
const VariableIndex index(factors); const VariableIndex index(factors);

4
gtsam/hybrid/HybridJunctionTree.cpp
View File

@ -94,9 +94,9 @@ struct HybridConstructorTraversalData {
symbolicFactors.reserve(node->factors.size() + symbolicFactors.reserve(node->factors.size() +
data.childSymbolicFactors.size()); data.childSymbolicFactors.size());
// Add ETree node factors // Add ETree node factors
symbolicFactors += node->factors; symbolicFactors.push_back(node->factors);
// Add symbolic factors passed up from children // Add symbolic factors passed up from children
symbolicFactors += data.childSymbolicFactors; symbolicFactors.push_back(data.childSymbolicFactors);
Ordering keyAsOrdering; Ordering keyAsOrdering;
keyAsOrdering.push_back(node->key); keyAsOrdering.push_back(node->key);

4
gtsam/hybrid/tests/testHybridBayesTree.cpp
View File

@ -97,7 +97,7 @@ TEST(HybridBayesTree, OptimizeAssignment) {
// Create ordering. // Create ordering.
Ordering 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] = const auto [hybridBayesNet, remainingFactorGraph] =
s.linearizedFactorGraph.eliminatePartialSequential(ordering); s.linearizedFactorGraph.eliminatePartialSequential(ordering);
@ -139,7 +139,7 @@ TEST(HybridBayesTree, Optimize) {
// Create ordering. // Create ordering.
Ordering 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] = const auto [hybridBayesNet, remainingFactorGraph] =
s.linearizedFactorGraph.eliminatePartialSequential(ordering); s.linearizedFactorGraph.eliminatePartialSequential(ordering);

14
gtsam/hybrid/tests/testHybridEstimation.cpp
View File

@ -37,6 +37,8 @@
#include "Switching.h" #include "Switching.h"
#include <bitset>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -44,7 +46,7 @@ using symbol_shorthand::X;
Ordering getOrdering(HybridGaussianFactorGraph& factors, Ordering getOrdering(HybridGaussianFactorGraph& factors,
const HybridGaussianFactorGraph& newFactors) { const HybridGaussianFactorGraph& newFactors) {
factors += newFactors; factors.push_back(newFactors);
// Get all the discrete keys from the factors // Get all the discrete keys from the factors
KeySet allDiscrete = factors.discreteKeySet(); KeySet allDiscrete = factors.discreteKeySet();
@ -83,10 +85,10 @@ TEST(HybridEstimation, Full) {
Ordering hybridOrdering; Ordering hybridOrdering;
for (size_t k = 0; k < K; k++) { 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++) { for (size_t k = 0; k < K - 1; k++) {
hybridOrdering += M(k); hybridOrdering.push_back(M(k));
} }
HybridBayesNet::shared_ptr bayesNet = HybridBayesNet::shared_ptr bayesNet =
@ -442,8 +444,7 @@ TEST(HybridEstimation, eliminateSequentialRegression) {
DiscreteConditional expected(m % "0.51341712/1"); // regression DiscreteConditional expected(m % "0.51341712/1"); // regression
// Eliminate into BN using one ordering // Eliminate into BN using one ordering
Ordering ordering1; const Ordering ordering1{X(0), X(1), M(0)};
ordering1 += X(0), X(1), M(0);
HybridBayesNet::shared_ptr bn1 = fg->eliminateSequential(ordering1); HybridBayesNet::shared_ptr bn1 = fg->eliminateSequential(ordering1);
// Check that the discrete conditional matches the expected. // Check that the discrete conditional matches the expected.
@ -451,8 +452,7 @@ TEST(HybridEstimation, eliminateSequentialRegression) {
EXPECT(assert_equal(expected, *dc1, 1e-9)); EXPECT(assert_equal(expected, *dc1, 1e-9));
// Eliminate into BN using a different ordering // Eliminate into BN using a different ordering
Ordering ordering2; const Ordering ordering2{X(0), X(1), M(0)};
ordering2 += X(0), X(1), M(0);
HybridBayesNet::shared_ptr bn2 = fg->eliminateSequential(ordering2); HybridBayesNet::shared_ptr bn2 = fg->eliminateSequential(ordering2);
// Check that the discrete conditional matches the expected. // Check that the discrete conditional matches the expected.

13
gtsam/hybrid/tests/testHybridGaussianISAM.cpp
View File

@ -126,10 +126,7 @@ TEST(HybridGaussianElimination, IncrementalInference) {
/********************************************************/ /********************************************************/
// Run batch elimination so we can compare results. // Run batch elimination so we can compare results.
Ordering ordering; const Ordering ordering {X(0), X(1), X(2)};
ordering += X(0);
ordering += X(1);
ordering += X(2);
// Now we calculate the expected factors using full elimination // Now we calculate the expected factors using full elimination
const auto [expectedHybridBayesTree, expectedRemainingGraph] = const auto [expectedHybridBayesTree, expectedRemainingGraph] =
@ -158,12 +155,10 @@ TEST(HybridGaussianElimination, IncrementalInference) {
// We only perform manual continuous elimination for 0,0. // We only perform manual continuous elimination for 0,0.
// The other discrete probabilities on M(2) are calculated the same way // The other discrete probabilities on M(2) are calculated the same way
Ordering discrete_ordering; const Ordering discreteOrdering{M(0), M(1)};
discrete_ordering += M(0);
discrete_ordering += M(1);
HybridBayesTree::shared_ptr discreteBayesTree = HybridBayesTree::shared_ptr discreteBayesTree =
expectedRemainingGraph->BaseEliminateable::eliminateMultifrontal( expectedRemainingGraph->BaseEliminateable::eliminateMultifrontal(
discrete_ordering); discreteOrdering);
DiscreteValues m00; DiscreteValues m00;
m00[M(0)] = 0, m00[M(1)] = 0; m00[M(0)] = 0, m00[M(1)] = 0;
@ -225,7 +220,7 @@ TEST(HybridGaussianElimination, Approx_inference) {
// Create ordering. // Create ordering.
Ordering ordering; Ordering ordering;
for (size_t j = 0; j < 4; j++) { 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 // Now we calculate the actual factors using full elimination

30
gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp
View File

@ -265,7 +265,7 @@ TEST(HybridFactorGraph, EliminationTree) {
// Create ordering. // Create ordering.
Ordering 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. // Create elimination tree.
HybridEliminationTree etree(self.linearizedFactorGraph, ordering); HybridEliminationTree etree(self.linearizedFactorGraph, ordering);
@ -286,8 +286,7 @@ TEST(GaussianElimination, Eliminate_x0) {
factors.push_back(self.linearizedFactorGraph[1]); factors.push_back(self.linearizedFactorGraph[1]);
// Eliminate x0 // Eliminate x0
Ordering ordering; const Ordering ordering{X(0)};
ordering += X(0);
auto result = EliminateHybrid(factors, ordering); auto result = EliminateHybrid(factors, ordering);
CHECK(result.first); CHECK(result.first);
@ -310,8 +309,7 @@ TEST(HybridsGaussianElimination, Eliminate_x1) {
factors.push_back(self.linearizedFactorGraph[2]); // involves m1 factors.push_back(self.linearizedFactorGraph[2]); // involves m1
// Eliminate x1 // Eliminate x1
Ordering ordering; const Ordering ordering{X(1)};
ordering += X(1);
auto result = EliminateHybrid(factors, ordering); auto result = EliminateHybrid(factors, ordering);
CHECK(result.first); CHECK(result.first);
@ -346,9 +344,7 @@ TEST(HybridGaussianElimination, EliminateHybrid_2_Variable) {
auto factors = self.linearizedFactorGraph; auto factors = self.linearizedFactorGraph;
// Eliminate x0 // Eliminate x0
Ordering ordering; const Ordering ordering{X(0), X(1)};
ordering += X(0);
ordering += X(1);
const auto [hybridConditionalMixture, factorOnModes] = const auto [hybridConditionalMixture, factorOnModes] =
EliminateHybrid(factors, ordering); EliminateHybrid(factors, ordering);
@ -379,7 +375,7 @@ TEST(HybridFactorGraph, Partial_Elimination) {
// Create ordering of only continuous variables. // Create ordering of only continuous variables.
Ordering 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 i.e. only continuous part. // Eliminate partially i.e. only continuous part.
const auto [hybridBayesNet, remainingFactorGraph] = const auto [hybridBayesNet, remainingFactorGraph] =
@ -415,7 +411,7 @@ TEST(HybridFactorGraph, Full_Elimination) {
{ {
// Create ordering. // Create ordering.
Ordering 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. // Eliminate partially.
const auto [hybridBayesNet_partial, remainingFactorGraph_partial] = const auto [hybridBayesNet_partial, remainingFactorGraph_partial] =
@ -430,15 +426,15 @@ TEST(HybridFactorGraph, Full_Elimination) {
} }
ordering.clear(); 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 = discreteBayesNet =
*discrete_fg.eliminateSequential(ordering, EliminateDiscrete); *discrete_fg.eliminateSequential(ordering, EliminateDiscrete);
} }
// Create ordering. // Create ordering.
Ordering 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));
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));
// Eliminate partially. // Eliminate partially.
HybridBayesNet::shared_ptr hybridBayesNet = HybridBayesNet::shared_ptr hybridBayesNet =
@ -479,7 +475,7 @@ TEST(HybridFactorGraph, Printing) {
// Create ordering. // Create ordering.
Ordering 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. // Eliminate partially.
const auto [hybridBayesNet, remainingFactorGraph] = const auto [hybridBayesNet, remainingFactorGraph] =
@ -705,11 +701,7 @@ TEST(HybridFactorGraph, DefaultDecisionTree) {
initialEstimate.insert(L(1), Point2(4.1, 1.8)); initialEstimate.insert(L(1), Point2(4.1, 1.8));
// We want to eliminate variables not connected to DCFactors first. // We want to eliminate variables not connected to DCFactors first.
Ordering ordering; const Ordering ordering {L(0), L(1), X(0), X(1)};
ordering += L(0);
ordering += L(1);
ordering += X(0);
ordering += X(1);
HybridGaussianFactorGraph linearized = *fg.linearize(initialEstimate); HybridGaussianFactorGraph linearized = *fg.linearize(initialEstimate);

13
gtsam/hybrid/tests/testHybridNonlinearISAM.cpp
View File

@ -143,10 +143,7 @@ TEST(HybridNonlinearISAM, IncrementalInference) {
/********************************************************/ /********************************************************/
// Run batch elimination so we can compare results. // Run batch elimination so we can compare results.
Ordering ordering; const Ordering ordering {X(0), X(1), X(2)};
ordering += X(0);
ordering += X(1);
ordering += X(2);
// Now we calculate the actual factors using full elimination // Now we calculate the actual factors using full elimination
const auto [expectedHybridBayesTree, expectedRemainingGraph] = const auto [expectedHybridBayesTree, expectedRemainingGraph] =
@ -176,12 +173,10 @@ TEST(HybridNonlinearISAM, IncrementalInference) {
// We only perform manual continuous elimination for 0,0. // We only perform manual continuous elimination for 0,0.
// The other discrete probabilities on M(1) are calculated the same way // The other discrete probabilities on M(1) are calculated the same way
Ordering discrete_ordering; const Ordering discreteOrdering{M(0), M(1)};
discrete_ordering += M(0);
discrete_ordering += M(1);
HybridBayesTree::shared_ptr discreteBayesTree = HybridBayesTree::shared_ptr discreteBayesTree =
expectedRemainingGraph->BaseEliminateable::eliminateMultifrontal( expectedRemainingGraph->BaseEliminateable::eliminateMultifrontal(
discrete_ordering); discreteOrdering);
DiscreteValues m00; DiscreteValues m00;
m00[M(0)] = 0, m00[M(1)] = 0; m00[M(0)] = 0, m00[M(1)] = 0;
@ -244,7 +239,7 @@ TEST(HybridNonlinearISAM, Approx_inference) {
// Create ordering. // Create ordering.
Ordering ordering; Ordering ordering;
for (size_t j = 0; j < 4; j++) { 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 // Now we calculate the actual factors using full elimination

14
gtsam/inference/BayesTree-inst.h
View File

@ -381,13 +381,13 @@ namespace gtsam {
gttoc(Full_root_factoring); gttoc(Full_root_factoring);
gttic(Variable_joint); gttic(Variable_joint);
p_BC1C2 += p_B; p_BC1C2.push_back(p_B);
p_BC1C2 += *p_C1_B; p_BC1C2.push_back(*p_C1_B);
p_BC1C2 += *p_C2_B; p_BC1C2.push_back(*p_C2_B);
if(C1 != B) if(C1 != B)
p_BC1C2 += C1->conditional(); p_BC1C2.push_back(C1->conditional());
if(C2 != B) if(C2 != B)
p_BC1C2 += C2->conditional(); p_BC1C2.push_back(C2->conditional());
gttoc(Variable_joint); gttoc(Variable_joint);
} }
else 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 // The nodes have no common ancestor, they're in different trees, so they're joint is just the
// product of their marginals. // product of their marginals.
gttic(Disjoint_marginals); gttic(Disjoint_marginals);
p_BC1C2 += C1->marginal2(function); p_BC1C2.push_back(C1->marginal2(function));
p_BC1C2 += C2->marginal2(function); p_BC1C2.push_back(C2->marginal2(function));
gttoc(Disjoint_marginals); gttoc(Disjoint_marginals);
} }

6
gtsam/inference/BayesTreeCliqueBase-inst.h
View File

@ -123,7 +123,7 @@ namespace gtsam {
gttoc(BayesTreeCliqueBase_shortcut); gttoc(BayesTreeCliqueBase_shortcut);
FactorGraphType p_Cp_B(parent->shortcut(B, function)); // P(Sp||B) FactorGraphType p_Cp_B(parent->shortcut(B, function)); // P(Sp||B)
gttic(BayesTreeCliqueBase_shortcut); 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 // Determine the variables we want to keepSet, S union B
KeyVector keep = shortcut_indices(B, p_Cp_B); KeyVector keep = shortcut_indices(B, p_Cp_B);
@ -171,7 +171,7 @@ namespace gtsam {
gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss); gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss);
// now add the parent conditional // 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 // The variables we want to keepSet are exactly the ones in S
KeyVector indicesS(this->conditional()->beginParents(), KeyVector indicesS(this->conditional()->beginParents(),
@ -198,7 +198,7 @@ namespace gtsam {
// initialize with separator marginal P(S) // initialize with separator marginal P(S)
FactorGraphType p_C = this->separatorMarginal(function); FactorGraphType p_C = this->separatorMarginal(function);
// add the conditional P(F|S) // add the conditional P(F|S)
p_C += std::shared_ptr<FactorType>(this->conditional_); p_C.push_back(std::shared_ptr<FactorType>(this->conditional_));
return p_C; return p_C;
} }

4
gtsam/inference/ClusterTree-inst.h
View File

@ -185,8 +185,8 @@ struct EliminationData {
// Gather factors // Gather factors
FactorGraphType gatheredFactors; FactorGraphType gatheredFactors;
gatheredFactors.reserve(node->factors.size() + node->nrChildren()); gatheredFactors.reserve(node->factors.size() + node->nrChildren());
gatheredFactors += node->factors; gatheredFactors.push_back(node->factors);
gatheredFactors += myData.childFactors; gatheredFactors.push_back(myData.childFactors);
// Check for Bayes tree orphan subtrees, and add them to our children // Check for Bayes tree orphan subtrees, and add them to our children
// TODO(frank): should this really happen here? // TODO(frank): should this really happen here?

2
gtsam/inference/Conditional.h
View File

@ -18,8 +18,6 @@
// \callgraph // \callgraph
#pragma once #pragma once
#include <boost/range.hpp>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
namespace gtsam { namespace gtsam {

7
gtsam/inference/FactorGraph.h
View File

@ -29,7 +29,10 @@
#include <Eigen/Core> // for Eigen::aligned_allocator #include <Eigen/Core> // for Eigen::aligned_allocator
#ifdef GTSAM_USE_BOOST_FEATURES
#include <boost/assign/list_inserter.hpp> #include <boost/assign/list_inserter.hpp>
#endif
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/nvp.hpp> #include <boost/serialization/nvp.hpp>
#include <boost/serialization/vector.hpp> #include <boost/serialization/vector.hpp>
@ -212,6 +215,7 @@ class FactorGraph {
push_back(factor); push_back(factor);
} }
#ifdef GTSAM_USE_BOOST_FEATURES
/// `+=` works well with boost::assign list inserter. /// `+=` works well with boost::assign list inserter.
template <class DERIVEDFACTOR> template <class DERIVEDFACTOR>
typename std::enable_if< typename std::enable_if<
@ -221,6 +225,7 @@ class FactorGraph {
return boost::assign::make_list_inserter(RefCallPushBack<This>(*this))( return boost::assign::make_list_inserter(RefCallPushBack<This>(*this))(
factor); factor);
} }
#endif
/// @} /// @}
/// @name Adding via iterators /// @name Adding via iterators
@ -271,6 +276,7 @@ class FactorGraph {
push_back(factorOrContainer); push_back(factorOrContainer);
} }
#ifdef GTSAM_USE_BOOST_FEATURES
/** /**
* Add a factor or container of factors, including STL collections, * Add a factor or container of factors, including STL collections,
* BayesTrees, etc. * BayesTrees, etc.
@ -281,6 +287,7 @@ class FactorGraph {
return boost::assign::make_list_inserter(CRefCallPushBack<This>(*this))( return boost::assign::make_list_inserter(CRefCallPushBack<This>(*this))(
factorOrContainer); factorOrContainer);
} }
#endif
/// @} /// @}
/// @name Specialized versions /// @name Specialized versions

6
gtsam/inference/ISAM-inst.h
View File

@ -36,12 +36,12 @@ void ISAM<BAYESTREE>::updateInternal(const FactorGraphType& newFactors,
// Add the removed top and the new factors // Add the removed top and the new factors
FactorGraphType factors; FactorGraphType factors;
factors += bn; factors.push_back(bn);
factors += newFactors; factors.push_back(newFactors);
// Add the orphaned subtrees // Add the orphaned subtrees
for (const sharedClique& orphan : *orphans) for (const sharedClique& orphan : *orphans)
factors += std::make_shared<BayesTreeOrphanWrapper<Clique> >(orphan); factors.template emplace_shared<BayesTreeOrphanWrapper<Clique> >(orphan);
// Get an ordering where the new keys are eliminated last // Get an ordering where the new keys are eliminated last
const VariableIndex index(factors); const VariableIndex index(factors);

4
gtsam/inference/JunctionTree-inst.h
View File

@ -77,9 +77,9 @@ struct ConstructorTraversalData {
symbolicFactors.reserve( symbolicFactors.reserve(
ETreeNode->factors.size() + myData.childSymbolicFactors.size()); ETreeNode->factors.size() + myData.childSymbolicFactors.size());
// Add ETree node factors // Add ETree node factors
symbolicFactors += ETreeNode->factors; symbolicFactors.push_back(ETreeNode->factors);
// Add symbolic factors passed up from children // Add symbolic factors passed up from children
symbolicFactors += myData.childSymbolicFactors; symbolicFactors.push_back(myData.childSymbolicFactors);
Ordering keyAsOrdering; Ordering keyAsOrdering;
keyAsOrdering.push_back(ETreeNode->key); keyAsOrdering.push_back(ETreeNode->key);

2
gtsam/inference/MetisIndex-inl.h
View File

@ -44,7 +44,7 @@ void MetisIndex::augment(const FACTORGRAPH& factors) {
for(const Key& key: *factors[i]) { for(const Key& key: *factors[i]) {
keySet.insert(keySet.end(), key); // Keep a track of all unique keys keySet.insert(keySet.end(), key); // Keep a track of all unique keys
if (intKeyBMap_.left.find(key) == intKeyBMap_.left.end()) { if (intKeyBMap_.left.find(key) == intKeyBMap_.left.end()) {
intKeyBMap_.insert(bm_type::value_type(key, keyCounter)); intKeyBMap_.insert(key, keyCounter);
keyCounter++; keyCounter++;
} }
} }

25
gtsam/inference/MetisIndex.h
View File

@ -22,17 +22,9 @@
#include <gtsam/base/types.h> #include <gtsam/base/types.h>
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
// Boost bimap generates many ugly warnings in CLANG
#ifdef __clang__
# pragma clang diagnostic push
# pragma clang diagnostic ignored "-Wredeclared-class-member"
#endif
#include <boost/bimap.hpp>
#ifdef __clang__
# pragma clang diagnostic pop
#endif
#include <vector> #include <vector>
#include <map>
#include <unordered_map>
namespace gtsam { namespace gtsam {
/** /**
@ -45,12 +37,21 @@ namespace gtsam {
class GTSAM_EXPORT MetisIndex { class GTSAM_EXPORT MetisIndex {
public: public:
typedef std::shared_ptr<MetisIndex> shared_ptr; typedef std::shared_ptr<MetisIndex> shared_ptr;
typedef boost::bimap<Key, int32_t> bm_type;
private: private:
// Stores Key <-> integer value relationship
struct BiMap {
std::map<Key, int32_t> left;
std::unordered_map<int32_t, Key> right;
void insert(const Key& left_value, const int32_t& right_value) {
left[left_value] = right_value;
right[right_value] = left_value;
}
};
std::vector<int32_t> xadj_; // Index of node's adjacency list in adj std::vector<int32_t> xadj_; // Index of node's adjacency list in adj
std::vector<int32_t> adj_; // Stores ajacency lists of all nodes, appended into a single vector std::vector<int32_t> adj_; // Stores ajacency lists of all nodes, appended into a single vector
boost::bimap<Key, int32_t> intKeyBMap_; // Stores Key <-> integer value relationship BiMap intKeyBMap_; // Stores Key <-> integer value relationship
size_t nKeys_; size_t nKeys_;
public: public:

5
gtsam/inference/Ordering.h
View File

@ -25,7 +25,10 @@
#include <gtsam/inference/MetisIndex.h> #include <gtsam/inference/MetisIndex.h>
#include <gtsam/base/FastSet.h> #include <gtsam/base/FastSet.h>
#ifdef GTSAM_USE_BOOST_FEATURES
#include <boost/assign/list_inserter.hpp> #include <boost/assign/list_inserter.hpp>
#endif
#include <algorithm> #include <algorithm>
#include <vector> #include <vector>
@ -58,6 +61,7 @@ public:
Base(keys.begin(), keys.end()) { Base(keys.begin(), keys.end()) {
} }
#ifdef GTSAM_USE_BOOST_FEATURES
/// Add new variables to the ordering as ordering += key1, key2, ... Equivalent to calling /// Add new variables to the ordering as ordering += key1, key2, ... Equivalent to calling
/// push_back. /// push_back.
boost::assign::list_inserter<boost::assign_detail::call_push_back<This> > operator+=( boost::assign::list_inserter<boost::assign_detail::call_push_back<This> > operator+=(
@ -65,6 +69,7 @@ public:
return boost::assign::make_list_inserter( return boost::assign::make_list_inserter(
boost::assign_detail::call_push_back<This>(*this))(key); boost::assign_detail::call_push_back<This>(*this))(key);
} }
#endif
/** /**
* @brief Append new keys to the ordering as `ordering += keys`. * @brief Append new keys to the ordering as `ordering += keys`.

1
gtsam/inference/inferenceExceptions.h
View File

@ -18,7 +18,6 @@
#pragma once #pragma once
#include <gtsam/global_includes.h> #include <gtsam/global_includes.h>
#include <boost/lexical_cast.hpp>
#include <exception> #include <exception>
namespace gtsam { namespace gtsam {

2
gtsam/inference/tests/testSymbol.cpp
View File

@ -18,6 +18,8 @@
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <sstream>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;

2
gtsam/linear/GaussianConditional.h
View File

@ -19,8 +19,6 @@
#pragma once #pragma once
#include <boost/utility.hpp>
#include <gtsam/global_includes.h> #include <gtsam/global_includes.h>
#include <gtsam/linear/JacobianFactor.h> #include <gtsam/linear/JacobianFactor.h>
#include <gtsam/inference/Conditional.h> #include <gtsam/inference/Conditional.h>

7
gtsam/linear/KalmanFilter.cpp
View File

@ -54,7 +54,8 @@ KalmanFilter::fuse(const State& p, GaussianFactor::shared_ptr newFactor) const {
// Create a factor graph // Create a factor graph
GaussianFactorGraph factorGraph; 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) // Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1)
return solve(factorGraph); 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) // Create a factor graph f(x0), eliminate it into P(x0)
GaussianFactorGraph factorGraph; GaussianFactorGraph factorGraph;
factorGraph += JacobianFactor(0, I_, x0, P0); // |x-x0|^2_diagSigma factorGraph.emplace_shared<JacobianFactor>(0, I_, x0, P0); // |x-x0|^2_diagSigma
return solve(factorGraph); 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) // Create a factor graph f(x0), eliminate it into P(x0)
GaussianFactorGraph factorGraph; GaussianFactorGraph factorGraph;
factorGraph += HessianFactor(0, x, P0); // 0.5*(x-x0)'*inv(Sigma)*(x-x0) factorGraph.emplace_shared<HessianFactor>(0, x, P0); // 0.5*(x-x0)'*inv(Sigma)*(x-x0)
return solve(factorGraph); return solve(factorGraph);
} }

2
gtsam/linear/PCGSolver.cpp
View File

@ -22,8 +22,6 @@
#include <gtsam/linear/Preconditioner.h> #include <gtsam/linear/Preconditioner.h>
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#include <boost/algorithm/string.hpp>
#include <algorithm> #include <algorithm>
#include <iostream> #include <iostream>
#include <stdexcept> #include <stdexcept>

2
gtsam/linear/SubgraphBuilder.cpp
View File

@ -25,9 +25,9 @@
#include <gtsam/linear/SubgraphBuilder.h> #include <gtsam/linear/SubgraphBuilder.h>
#include <gtsam/base/kruskal.h> #include <gtsam/base/kruskal.h>
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/archive/text_iarchive.hpp> #include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp> #include <boost/archive/text_oarchive.hpp>
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/vector.hpp> #include <boost/serialization/vector.hpp>
#endif #endif

2
gtsam/linear/SubgraphPreconditioner.cpp
View File

@ -24,8 +24,6 @@
#include <gtsam/base/types.h> #include <gtsam/base/types.h>
#include <gtsam/base/Vector.h> #include <gtsam/base/Vector.h>
#include <boost/algorithm/string.hpp>
#include <stdexcept> #include <stdexcept>
using std::cout; using std::cout;

3
gtsam/linear/VectorValues.cpp
View File

@ -18,9 +18,6 @@
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#include <boost/bind/bind.hpp>
#include <boost/range/numeric.hpp>
using namespace std; using namespace std;
namespace gtsam { namespace gtsam {

1
gtsam/linear/linearExceptions.cpp
View File

@ -18,7 +18,6 @@
#include <gtsam/linear/linearExceptions.h> #include <gtsam/linear/linearExceptions.h>
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <boost/lexical_cast.hpp>
namespace gtsam { namespace gtsam {

30
gtsam/linear/tests/testGaussianBayesNet.cpp
View File

@ -24,7 +24,6 @@
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <boost/bind/bind.hpp>
// STL/C++ // STL/C++
#include <iostream> #include <iostream>
@ -211,8 +210,7 @@ TEST(GaussianBayesNet, MonteCarloIntegration) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(GaussianBayesNet, ordering) TEST(GaussianBayesNet, ordering)
{ {
Ordering expected; const Ordering expected{_x_, _y_};
expected += _x_, _y_;
const auto actual = noisyBayesNet.ordering(); const auto actual = noisyBayesNet.ordering();
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
} }
@ -279,15 +277,15 @@ TEST( GaussianBayesNet, backSubstituteTransposeNoisy )
TEST( GaussianBayesNet, DeterminantTest ) TEST( GaussianBayesNet, DeterminantTest )
{ {
GaussianBayesNet cbn; GaussianBayesNet cbn;
cbn += GaussianConditional( cbn.emplace_shared<GaussianConditional>(
0, Vector2(3.0, 4.0), (Matrix2() << 1.0, 3.0, 0.0, 4.0).finished(), 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)); 1, (Matrix2() << 2.0, 1.0, 2.0, 3.0).finished(), noiseModel::Isotropic::Sigma(2, 2.0));
cbn += GaussianConditional( cbn.emplace_shared<GaussianConditional>(
1, Vector2(5.0, 6.0), (Matrix2() << 1.0, 1.0, 0.0, 3.0).finished(), 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)); 2, (Matrix2() << 1.0, 0.0, 5.0, 2.0).finished(), noiseModel::Isotropic::Sigma(2, 2.0));
cbn += GaussianConditional( cbn.emplace_shared<GaussianConditional>(
3, Vector2(7.0, 8.0), (Matrix2() << 1.0, 1.0, 0.0, 5.0).finished(), noiseModel::Isotropic::Sigma(2, 2.0)); 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; double expectedDeterminant = 60.0 / 64.0;
@ -310,22 +308,22 @@ TEST(GaussianBayesNet, ComputeSteepestDescentPoint) {
// Create an arbitrary Bayes Net // Create an arbitrary Bayes Net
GaussianBayesNet gbn; GaussianBayesNet gbn;
gbn += GaussianConditional::shared_ptr(new GaussianConditional( gbn.emplace_shared<GaussianConditional>(
0, Vector2(1.0,2.0), (Matrix2() << 3.0,4.0,0.0,6.0).finished(), 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(), 3, (Matrix2() << 7.0,8.0,9.0,10.0).finished(),
4, (Matrix2() << 11.0,12.0,13.0,14.0).finished())); 4, (Matrix2() << 11.0,12.0,13.0,14.0).finished());
gbn += GaussianConditional::shared_ptr(new GaussianConditional( gbn.emplace_shared<GaussianConditional>(
1, Vector2(15.0,16.0), (Matrix2() << 17.0,18.0,0.0,20.0).finished(), 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(), 2, (Matrix2() << 21.0,22.0,23.0,24.0).finished(),
4, (Matrix2() << 25.0,26.0,27.0,28.0).finished())); 4, (Matrix2() << 25.0,26.0,27.0,28.0).finished());
gbn += GaussianConditional::shared_ptr(new GaussianConditional( gbn.emplace_shared<GaussianConditional>(
2, Vector2(29.0,30.0), (Matrix2() << 31.0,32.0,0.0,34.0).finished(), 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())); 3, (Matrix2() << 35.0,36.0,37.0,38.0).finished());
gbn += GaussianConditional::shared_ptr(new GaussianConditional( gbn.emplace_shared<GaussianConditional>(
3, Vector2(39.0,40.0), (Matrix2() << 41.0,42.0,0.0,44.0).finished(), 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())); 4, (Matrix2() << 45.0,46.0,47.0,48.0).finished());
gbn += GaussianConditional::shared_ptr(new GaussianConditional( gbn.emplace_shared<GaussianConditional>(
4, Vector2(49.0,50.0), (Matrix2() << 51.0,52.0,0.0,54.0).finished())); 4, Vector2(49.0,50.0), (Matrix2() << 51.0,52.0,0.0,54.0).finished());
// Compute the Hessian numerically // Compute the Hessian numerically
Matrix hessian = numericalHessian<Vector10>( Matrix hessian = numericalHessian<Vector10>(

25
gtsam/linear/tests/testGaussianBayesTree.cpp
View File

@ -299,10 +299,10 @@ TEST(GaussianBayesTree, determinant_and_smallestEigenvalue) {
GaussianFactorGraph fg; GaussianFactorGraph fg;
Key x1 = 2, x2 = 0, l1 = 1; Key x1 = 2, x2 = 0, l1 = 1;
SharedDiagonal unit2 = noiseModel::Unit::Create(2); SharedDiagonal unit2 = noiseModel::Unit::Create(2);
fg += JacobianFactor(x1, 10 * I_2x2, -1.0 * Vector::Ones(2), unit2); fg.emplace_shared<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.emplace_shared<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.emplace_shared<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<JacobianFactor>(x2, -5 * I_2x2, l1, 5 * I_2x2, Vector2(-1.0, 1.5), unit2);
// create corresponding Bayes tree: // create corresponding Bayes tree:
std::shared_ptr<gtsam::GaussianBayesTree> bt = fg.eliminateMultifrontal(); std::shared_ptr<gtsam::GaussianBayesTree> bt = fg.eliminateMultifrontal();
@ -327,15 +327,14 @@ TEST(GaussianBayesTree, LogDeterminant) {
GaussianFactorGraph fg; GaussianFactorGraph fg;
Key x1 = X(1), x2 = X(2), l1 = L(1); Key x1 = X(1), x2 = X(2), l1 = L(1);
SharedDiagonal unit2 = noiseModel::Unit::Create(2); SharedDiagonal unit2 = noiseModel::Unit::Create(2);
fg += JacobianFactor(x1, 10 * I_2x2, -1.0 * Vector2::Ones(), unit2); fg.emplace_shared<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), fg.emplace_shared<JacobianFactor>(x2, 10 * I_2x2, x1, -10 * I_2x2,
unit2); Vector2(2.0, -1.0), unit2);
fg += JacobianFactor(l1, 5 * I_2x2, x1, -5 * I_2x2, Vector2(0.0, 1.0), unit2); fg.emplace_shared<JacobianFactor>(l1, 5 * I_2x2, x1, -5 * I_2x2, Vector2(0.0, 1.0), unit2);
fg += fg.emplace_shared<JacobianFactor>(x2, -5 * I_2x2, l1, 5 * I_2x2, Vector2(-1.0, 1.5), unit2);
JacobianFactor(x2, -5 * I_2x2, l1, 5 * I_2x2, Vector2(-1.0, 1.5), unit2); fg.emplace_shared<JacobianFactor>(x3, 10 * I_2x2, x2, -10 * I_2x2,
fg += JacobianFactor(x3, 10 * I_2x2, x2, -10 * I_2x2, Vector2(2.0, -1.0), Vector2(2.0, -1.0), unit2);
unit2); fg.emplace_shared<JacobianFactor>(x3, 10 * I_2x2, -1.0 * Vector2::Ones(), unit2);
fg += JacobianFactor(x3, 10 * I_2x2, -1.0 * Vector2::Ones(), unit2);
// create corresponding Bayes net and Bayes tree: // create corresponding Bayes net and Bayes tree:
std::shared_ptr<gtsam::GaussianBayesNet> bn = fg.eliminateSequential(); std::shared_ptr<gtsam::GaussianBayesNet> bn = fg.eliminateSequential();

17
gtsam/linear/tests/testGaussianFactorGraph.cpp
View File

@ -51,11 +51,10 @@ TEST(GaussianFactorGraph, initialization) {
GaussianFactorGraph fg; GaussianFactorGraph fg;
SharedDiagonal unit2 = noiseModel::Unit::Create(2); SharedDiagonal unit2 = noiseModel::Unit::Create(2);
fg += fg.emplace_shared<JacobianFactor>(0, 10*I_2x2, -1.0*Vector::Ones(2), unit2);
JacobianFactor(0, 10*I_2x2, -1.0*Vector::Ones(2), unit2), fg.emplace_shared<JacobianFactor>(0, -10*I_2x2,1, 10*I_2x2, Vector2(2.0, -1.0), unit2);
JacobianFactor(0, -10*I_2x2,1, 10*I_2x2, Vector2(2.0, -1.0), unit2), fg.emplace_shared<JacobianFactor>(0, -5*I_2x2, 2, 5*I_2x2, Vector2(0.0, 1.0), unit2);
JacobianFactor(0, -5*I_2x2, 2, 5*I_2x2, Vector2(0.0, 1.0), unit2), fg.emplace_shared<JacobianFactor>(1, -5*I_2x2, 2, 5*I_2x2, Vector2(-1.0, 1.5), unit2);
JacobianFactor(1, -5*I_2x2, 2, 5*I_2x2, Vector2(-1.0, 1.5), unit2);
EXPECT_LONGS_EQUAL(4, (long)fg.size()); EXPECT_LONGS_EQUAL(4, (long)fg.size());
@ -186,13 +185,13 @@ static GaussianFactorGraph createSimpleGaussianFactorGraph() {
Key x1 = 2, x2 = 0, l1 = 1; Key x1 = 2, x2 = 0, l1 = 1;
SharedDiagonal unit2 = noiseModel::Unit::Create(2); SharedDiagonal unit2 = noiseModel::Unit::Create(2);
// linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] // 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<JacobianFactor>(x1, 10 * I_2x2, -1.0 * Vector::Ones(2), unit2);
// odometry between x1 and x2: x2-x1=[0.2;-0.1] // 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<JacobianFactor>(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] // 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<JacobianFactor>(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] // 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<JacobianFactor>(x2, -5 * I_2x2, l1, 5 * I_2x2, Vector2(-1.0, 1.5), unit2);
return fg; return fg;
} }

2
gtsam/linear/tests/testJacobianFactor.cpp
View File

@ -25,8 +25,6 @@
#include <gtsam/linear/GaussianConditional.h> #include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#include <boost/range/iterator_range.hpp>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;

4
gtsam/linear/tests/testRegularHessianFactor.cpp
View File

@ -66,11 +66,11 @@ TEST(RegularHessianFactor, Constructors)
// Test constructor from Gaussian Factor Graph // Test constructor from Gaussian Factor Graph
GaussianFactorGraph gfg; GaussianFactorGraph gfg;
gfg += jf; gfg.push_back(jf);
RegularHessianFactor<2> factor4(gfg); RegularHessianFactor<2> factor4(gfg);
EXPECT(assert_equal(factor, factor4)); EXPECT(assert_equal(factor, factor4));
GaussianFactorGraph gfg2; GaussianFactorGraph gfg2;
gfg2 += factor; gfg2.push_back(factor);
RegularHessianFactor<2> factor5(gfg); RegularHessianFactor<2> factor5(gfg);
EXPECT(assert_equal(factor, factor5)); EXPECT(assert_equal(factor, factor5));

3
gtsam/navigation/CombinedImuFactor.h
View File

@ -27,7 +27,6 @@
#include <gtsam/navigation/TangentPreintegration.h> #include <gtsam/navigation/TangentPreintegration.h>
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/base/serialization.h>
namespace gtsam { namespace gtsam {
@ -336,6 +335,7 @@ public:
OptionalMatrixType H5, OptionalMatrixType H6) const override; OptionalMatrixType H5, OptionalMatrixType H6) const override;
private: private:
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template <class ARCHIVE> template <class ARCHIVE>
@ -345,6 +345,7 @@ public:
"NoiseModelFactor6", boost::serialization::base_object<Base>(*this)); "NoiseModelFactor6", boost::serialization::base_object<Base>(*this));
ar& BOOST_SERIALIZATION_NVP(_PIM_); ar& BOOST_SERIALIZATION_NVP(_PIM_);
} }
#endif
public: public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW GTSAM_MAKE_ALIGNED_OPERATOR_NEW

2
gtsam/navigation/ImuFactor.h
View File

@ -320,6 +320,7 @@ public:
private: private:
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
@ -329,6 +330,7 @@ private:
boost::serialization::base_object<Base>(*this)); boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(_PIM_); ar & BOOST_SERIALIZATION_NVP(_PIM_);
} }
#endif
}; };
// class ImuFactor2 // class ImuFactor2

1
gtsam/navigation/tests/testAttitudeFactor.cpp
View File

@ -20,7 +20,6 @@
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <boost/bind/bind.hpp>
#include <functional> #include <functional>
#include "gtsam/base/Matrix.h" #include "gtsam/base/Matrix.h"
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>

2
gtsam/navigation/tests/testBarometricFactor.cpp
View File

@ -21,8 +21,6 @@
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/navigation/BarometricFactor.h> #include <gtsam/navigation/BarometricFactor.h>
#include <boost/bind/bind.hpp>
using namespace std::placeholders; using namespace std::placeholders;
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;

1
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -30,7 +30,6 @@
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <boost/bind/bind.hpp>
#include <list> #include <list>
#include "imuFactorTesting.h" #include "imuFactorTesting.h"

2
gtsam/navigation/tests/testMagFactor.cpp
View File

@ -20,8 +20,6 @@
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <GeographicLib/LocalCartesian.hpp> #include <GeographicLib/LocalCartesian.hpp>

1
gtsam/navigation/tests/testNavState.cpp
View File

@ -20,7 +20,6 @@
#include <gtsam/base/TestableAssertions.h> #include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
using namespace std::placeholders; using namespace std::placeholders;

3
gtsam/nonlinear/AdaptAutoDiff.h
View File

@ -22,9 +22,6 @@
#include <gtsam/base/OptionalJacobian.h> #include <gtsam/base/OptionalJacobian.h>
#include <gtsam/3rdparty/ceres/autodiff.h> #include <gtsam/3rdparty/ceres/autodiff.h>
#include <boost/static_assert.hpp>
#include <boost/type_traits/is_base_of.hpp>
namespace gtsam { namespace gtsam {
/** /**

4
gtsam/nonlinear/Expression.h
View File

@ -218,7 +218,7 @@ protected:
template <typename T> template <typename T>
class ScalarMultiplyExpression : public Expression<T> { class ScalarMultiplyExpression : public Expression<T> {
// Check that T is a vector space // Check that T is a vector space
BOOST_CONCEPT_ASSERT((gtsam::IsVectorSpace<T>)); GTSAM_CONCEPT_ASSERT(gtsam::IsVectorSpace<T>);
public: public:
explicit ScalarMultiplyExpression(double s, const Expression<T>& e); explicit ScalarMultiplyExpression(double s, const Expression<T>& e);
@ -231,7 +231,7 @@ class ScalarMultiplyExpression : public Expression<T> {
template <typename T> template <typename T>
class BinarySumExpression : public Expression<T> { class BinarySumExpression : public Expression<T> {
// Check that T is a vector space // Check that T is a vector space
BOOST_CONCEPT_ASSERT((gtsam::IsVectorSpace<T>)); GTSAM_CONCEPT_ASSERT(gtsam::IsVectorSpace<T>);
public: public:
explicit BinarySumExpression(const Expression<T>& e1, const Expression<T>& e2); explicit BinarySumExpression(const Expression<T>& e1, const Expression<T>& e2);

4
gtsam/nonlinear/ExpressionFactor.h
View File

@ -42,7 +42,7 @@ namespace gtsam {
*/ */
template <typename T> template <typename T>
class ExpressionFactor : public NoiseModelFactor { class ExpressionFactor : public NoiseModelFactor {
BOOST_CONCEPT_ASSERT((IsTestable<T>)); GTSAM_CONCEPT_ASSERT(IsTestable<T>);
protected: protected:
@ -288,6 +288,7 @@ private:
return expression(keys); return expression(keys);
} }
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
friend class boost::serialization::access; friend class boost::serialization::access;
template <class ARCHIVE> template <class ARCHIVE>
void serialize(ARCHIVE &ar, const unsigned int /*version*/) { void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
@ -295,6 +296,7 @@ private:
"ExpressionFactorN", "ExpressionFactorN",
boost::serialization::base_object<ExpressionFactor<T>>(*this)); boost::serialization::base_object<ExpressionFactor<T>>(*this));
} }
#endif
}; };
/// traits /// traits
template <typename T, typename... Args> template <typename T, typename... Args>

3
gtsam/nonlinear/ExtendedKalmanFilter-inl.h
View File

@ -35,8 +35,7 @@ namespace gtsam {
// Compute the marginal on the last key // Compute the marginal on the last key
// Solve the linear factor graph, converting it into a linear Bayes Network // Solve the linear factor graph, converting it into a linear Bayes Network
// P(x0,x1) = P(x0|x1)*P(x1) // P(x0,x1) = P(x0|x1)*P(x1)
Ordering lastKeyAsOrdering; const Ordering lastKeyAsOrdering{lastKey};
lastKeyAsOrdering += lastKey;
const GaussianConditional::shared_ptr marginal = const GaussianConditional::shared_ptr marginal =
linearFactorGraph.marginalMultifrontalBayesNet(lastKeyAsOrdering)->front(); linearFactorGraph.marginalMultifrontalBayesNet(lastKeyAsOrdering)->front();

4
gtsam/nonlinear/ExtendedKalmanFilter.h
View File

@ -44,8 +44,8 @@ namespace gtsam {
template <class VALUE> template <class VALUE>
class ExtendedKalmanFilter { class ExtendedKalmanFilter {
// Check that VALUE type is a testable Manifold // Check that VALUE type is a testable Manifold
BOOST_CONCEPT_ASSERT((IsTestable<VALUE>)); GTSAM_CONCEPT_ASSERT1(IsTestable<VALUE>);
BOOST_CONCEPT_ASSERT((IsManifold<VALUE>)); GTSAM_CONCEPT_ASSERT2(IsManifold<VALUE>);
public: public:
typedef std::shared_ptr<ExtendedKalmanFilter<VALUE> > shared_ptr; typedef std::shared_ptr<ExtendedKalmanFilter<VALUE> > shared_ptr;

3
gtsam/nonlinear/ISAM2.cpp
View File

@ -294,8 +294,7 @@ void ISAM2::recalculateIncremental(const ISAM2UpdateParams& updateParams,
gttic(orphans); gttic(orphans);
// Add the orphaned subtrees // Add the orphaned subtrees
for (const auto& orphan : *orphans) for (const auto& orphan : *orphans)
factors += factors.emplace_shared<BayesTreeOrphanWrapper<ISAM2::Clique> >(orphan);
std::make_shared<BayesTreeOrphanWrapper<ISAM2::Clique> >(orphan);
gttoc(orphans); gttoc(orphans);
// 3. Re-order and eliminate the factor graph into a Bayes net (Algorithm // 3. Re-order and eliminate the factor graph into a Bayes net (Algorithm

12
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -26,7 +26,9 @@
#include <gtsam/linear/linearExceptions.h> #include <gtsam/linear/linearExceptions.h>
#include <gtsam/inference/Ordering.h> #include <gtsam/inference/Ordering.h>
#include <gtsam/base/Vector.h> #include <gtsam/base/Vector.h>
#ifdef GTSAM_USE_BOOST_FEATURES
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
#endif
#include <cmath> #include <cmath>
#include <fstream> #include <fstream>
@ -121,6 +123,7 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear,
auto currentState = static_cast<const State*>(state_.get()); auto currentState = static_cast<const State*>(state_.get());
bool verbose = (params_.verbosityLM >= LevenbergMarquardtParams::TRYLAMBDA); bool verbose = (params_.verbosityLM >= LevenbergMarquardtParams::TRYLAMBDA);
#ifdef GTSAM_USE_BOOST_FEATURES
#ifdef GTSAM_USING_NEW_BOOST_TIMERS #ifdef GTSAM_USING_NEW_BOOST_TIMERS
boost::timer::cpu_timer lamda_iteration_timer; boost::timer::cpu_timer lamda_iteration_timer;
lamda_iteration_timer.start(); lamda_iteration_timer.start();
@ -128,6 +131,9 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear,
boost::timer lamda_iteration_timer; boost::timer lamda_iteration_timer;
lamda_iteration_timer.restart(); lamda_iteration_timer.restart();
#endif #endif
#else
auto start = std::chrono::high_resolution_clock::now();
#endif
if (verbose) if (verbose)
cout << "trying lambda = " << currentState->lambda << endl; cout << "trying lambda = " << currentState->lambda << endl;
@ -215,11 +221,16 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear,
} // if (systemSolvedSuccessfully) } // if (systemSolvedSuccessfully)
if (params_.verbosityLM == LevenbergMarquardtParams::SUMMARY) { if (params_.verbosityLM == LevenbergMarquardtParams::SUMMARY) {
#ifdef GTSAM_USE_BOOST_FEATURES
// do timing // do timing
#ifdef GTSAM_USING_NEW_BOOST_TIMERS #ifdef GTSAM_USING_NEW_BOOST_TIMERS
double iterationTime = 1e-9 * lamda_iteration_timer.elapsed().wall; double iterationTime = 1e-9 * lamda_iteration_timer.elapsed().wall;
#else #else
double iterationTime = lamda_iteration_timer.elapsed(); double iterationTime = lamda_iteration_timer.elapsed();
#endif
#else
auto end = std::chrono::high_resolution_clock::now();
double iterationTime = std::chrono::duration_cast<std::chrono::microseconds>(end - start).count() / 1e6;
#endif #endif
if (currentState->iterations == 0) { if (currentState->iterations == 0) {
cout << "iter cost cost_change lambda success iter_time" << endl; 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) << costChange << " " << setw(3) << setprecision(2) << currentState->lambda << " " << setw(4)
<< systemSolvedSuccessfully << " " << setw(3) << setprecision(2) << iterationTime << endl; << systemSolvedSuccessfully << " " << setw(3) << setprecision(2) << iterationTime << endl;
} }
if (step_is_successful) { if (step_is_successful) {
// we have successfully decreased the cost and we have good modelFidelity // we have successfully decreased the cost and we have good modelFidelity
// NOTE(frank): As we return immediately after this, we move the newValues // NOTE(frank): As we return immediately after this, we move the newValues

2
gtsam/nonlinear/LinearContainerFactor.cpp
View File

@ -213,7 +213,7 @@ NonlinearFactorGraph LinearContainerFactor::ConvertLinearGraph(
result.reserve(linear_graph.size()); result.reserve(linear_graph.size());
for (const auto& f : linear_graph) for (const auto& f : linear_graph)
if (f) if (f)
result += std::make_shared<LinearContainerFactor>(f, linearizationPoint); result.emplace_shared<LinearContainerFactor>(f, linearizationPoint);
return result; return result;
} }

Some files were not shown because too many files have changed in this diff Show More