diff --git a/CMakeLists.txt b/CMakeLists.txt index 63c7f9e547..16848a7219 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -36,13 +36,6 @@ set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${CMAKE_CURRENT_SOURCE_DIR}/cmake" include(GtsamMakeConfigFile) include(GNUInstallDirs) -# Load build type flags and default to Debug mode -include(GtsamBuildTypes) - -# Use macros for creating tests/timing scripts -include(GtsamTesting) -include(GtsamPrinting) - # guard against in-source builds if(${GTSAM_SOURCE_DIR} STREQUAL ${GTSAM_BINARY_DIR}) message(FATAL_ERROR "In-source builds not allowed. Please make a new directory (called a build directory) and run CMake from there. You may need to remove CMakeCache.txt. ") @@ -50,6 +43,13 @@ endif() include(cmake/HandleGeneralOptions.cmake) # CMake build options +# Load build type flags and default to Debug mode +include(GtsamBuildTypes) + +# Use macros for creating tests/timing scripts +include(GtsamTesting) +include(GtsamPrinting) + ############### Decide on BOOST ###################################### # Enable or disable serialization with GTSAM_ENABLE_BOOST_SERIALIZATION option(GTSAM_ENABLE_BOOST_SERIALIZATION "Enable Boost serialization" ON) diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index b24be5f08c..2aad58abb8 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -55,9 +55,6 @@ if(NOT CMAKE_BUILD_TYPE AND NOT MSVC AND NOT XCODE_VERSION) "Choose the type of build, options are: None Debug Release Timing Profiling RelWithDebInfo." FORCE) endif() -# Add option for using build type postfixes to allow installing multiple build modes -option(GTSAM_BUILD_TYPE_POSTFIXES "Enable/Disable appending the build type to the name of compiled libraries" ON) - # Define all cache variables, to be populated below depending on the OS/compiler: set(GTSAM_COMPILE_OPTIONS_PRIVATE "" CACHE INTERNAL "(Do not edit) Private compiler flags for all build configurations." FORCE) set(GTSAM_COMPILE_OPTIONS_PUBLIC "" CACHE INTERNAL "(Do not edit) Public compiler flags (exported to user projects) for all build configurations." FORCE) @@ -82,6 +79,13 @@ set(GTSAM_COMPILE_DEFINITIONS_PRIVATE_RELWITHDEBINFO "NDEBUG" CACHE STRING "(Us set(GTSAM_COMPILE_DEFINITIONS_PRIVATE_RELEASE "NDEBUG" CACHE STRING "(User editable) Private preprocessor macros for Release configuration.") set(GTSAM_COMPILE_DEFINITIONS_PRIVATE_PROFILING "NDEBUG" CACHE STRING "(User editable) Private preprocessor macros for Profiling configuration.") set(GTSAM_COMPILE_DEFINITIONS_PRIVATE_TIMING "NDEBUG;ENABLE_TIMING" CACHE STRING "(User editable) Private preprocessor macros for Timing configuration.") + +mark_as_advanced(GTSAM_COMPILE_DEFINITIONS_PRIVATE_DEBUG) +mark_as_advanced(GTSAM_COMPILE_DEFINITIONS_PRIVATE_RELWITHDEBINFO) +mark_as_advanced(GTSAM_COMPILE_DEFINITIONS_PRIVATE_RELEASE) +mark_as_advanced(GTSAM_COMPILE_DEFINITIONS_PRIVATE_PROFILING) +mark_as_advanced(GTSAM_COMPILE_DEFINITIONS_PRIVATE_TIMING) + if(MSVC) # Common to all configurations: list_append_cache(GTSAM_COMPILE_DEFINITIONS_PRIVATE @@ -143,6 +147,13 @@ else() set(GTSAM_COMPILE_OPTIONS_PRIVATE_TIMING -g -O3 CACHE STRING "(User editable) Private compiler flags for Timing configuration.") endif() +mark_as_advanced(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON) +mark_as_advanced(GTSAM_COMPILE_OPTIONS_PRIVATE_DEBUG) +mark_as_advanced(GTSAM_COMPILE_OPTIONS_PRIVATE_RELWITHDEBINFO) +mark_as_advanced(GTSAM_COMPILE_OPTIONS_PRIVATE_RELEASE) +mark_as_advanced(GTSAM_COMPILE_OPTIONS_PRIVATE_PROFILING) +mark_as_advanced(GTSAM_COMPILE_OPTIONS_PRIVATE_TIMING) + # Enable C++17: if (NOT CMAKE_VERSION VERSION_LESS 3.8) set(GTSAM_COMPILE_FEATURES_PUBLIC "cxx_std_17" CACHE STRING "CMake compile features property for all gtsam targets.") @@ -198,7 +209,6 @@ if(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang") endif() if (NOT MSVC) - option(GTSAM_BUILD_WITH_MARCH_NATIVE "Enable/Disable building with all instructions supported by native architecture (binary may not be portable!)" OFF) if(GTSAM_BUILD_WITH_MARCH_NATIVE) # Check if Apple OS and compiler is [Apple]Clang if(APPLE AND (${CMAKE_CXX_COMPILER_ID} MATCHES "^(Apple)?Clang$")) diff --git a/cmake/GtsamTesting.cmake b/cmake/GtsamTesting.cmake index 573fb696ac..47b059213e 100644 --- a/cmake/GtsamTesting.cmake +++ b/cmake/GtsamTesting.cmake @@ -42,7 +42,7 @@ endmacro() # GTSAM_BUILD_EXAMPLES_ALWAYS is enabled. They may also be built with 'make examples'. # # Usage example: -# gtsamAddExamplesGlob("*.cpp" "BrokenExample.cpp" "gtsam;GeographicLib") +# gtsamAddExamplesGlob("*.cpp" "BrokenExample.cpp" "gtsam;GeographicLib" ON) # # Arguments: # globPatterns: The list of files or glob patterns from which to create examples, with @@ -51,8 +51,9 @@ endmacro() # excludedFiles: A list of files or globs to exclude, e.g. "C*.cpp;BrokenExample.cpp". Pass # an empty string "" if nothing needs to be excluded. # linkLibraries: The list of libraries to link to. -macro(gtsamAddExamplesGlob globPatterns excludedFiles linkLibraries) - gtsamAddExesGlob_impl("${globPatterns}" "${excludedFiles}" "${linkLibraries}" "examples" ${GTSAM_BUILD_EXAMPLES_ALWAYS}) +# buildWithAll: Build examples with `make` and/or `make all` +macro(gtsamAddExamplesGlob globPatterns excludedFiles linkLibraries buildWithAll) + gtsamAddExesGlob_impl("${globPatterns}" "${excludedFiles}" "${linkLibraries}" "examples" ${buildWithAll}) endmacro() @@ -76,8 +77,9 @@ endmacro() # excludedFiles: A list of files or globs to exclude, e.g. "C*.cpp;BrokenExample.cpp". Pass # an empty string "" if nothing needs to be excluded. # linkLibraries: The list of libraries to link to. -macro(gtsamAddTimingGlob globPatterns excludedFiles linkLibraries) - gtsamAddExesGlob_impl("${globPatterns}" "${excludedFiles}" "${linkLibraries}" "timing" ${GTSAM_BUILD_TIMING_ALWAYS}) +# buildWithAll: Build examples with `make` and/or `make all` +macro(gtsamAddTimingGlob globPatterns excludedFiles linkLibraries buildWithAll) + gtsamAddExesGlob_impl("${globPatterns}" "${excludedFiles}" "${linkLibraries}" "timing" ${buildWithAll}) endmacro() @@ -86,9 +88,8 @@ endmacro() # Build macros for using tests enable_testing() -option(GTSAM_BUILD_TESTS "Enable/Disable building of tests" ON) -option(GTSAM_BUILD_EXAMPLES_ALWAYS "Build examples with 'make all' (build with 'make examples' if not)" ON) -option(GTSAM_BUILD_TIMING_ALWAYS "Build timing scripts with 'make all' (build with 'make timing' if not" OFF) +#TODO(Varun) Move to HandlePrintConfiguration.cmake. This will require additional changes. +option(GTSAM_BUILD_TESTS "Enable/Disable building of tests" ON) # Add option for combining unit tests if(MSVC OR XCODE_VERSION) @@ -123,6 +124,7 @@ add_custom_target(timing) # Implementations of this file's macros: macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries) + #TODO(Varun) Building of tests should not depend on global gtsam flag if(GTSAM_BUILD_TESTS) # Add group target if it doesn't already exist if(NOT TARGET check.${groupName}) diff --git a/cmake/HandleGeneralOptions.cmake b/cmake/HandleGeneralOptions.cmake index 13000a5b07..302bdf8603 100644 --- a/cmake/HandleGeneralOptions.cmake +++ b/cmake/HandleGeneralOptions.cmake @@ -8,6 +8,18 @@ else() set(GTSAM_UNSTABLE_AVAILABLE 0) endif() +### GtsamTesting related options +option(GTSAM_BUILD_EXAMPLES_ALWAYS "Build examples with 'make all' (build with 'make examples' if not)" ON) +option(GTSAM_BUILD_TIMING_ALWAYS "Build timing scripts with 'make all' (build with 'make timing' if not" OFF) +### + +# Add option for using build type postfixes to allow installing multiple build modes +option(GTSAM_BUILD_TYPE_POSTFIXES "Enable/Disable appending the build type to the name of compiled libraries" ON) + +if (NOT MSVC) + option(GTSAM_BUILD_WITH_MARCH_NATIVE "Enable/Disable building with all instructions supported by native architecture (binary may not be portable!)" OFF) +endif() + # Configurable Options if(GTSAM_UNSTABLE_AVAILABLE) option(GTSAM_BUILD_UNSTABLE "Enable/Disable libgtsam_unstable" ON) diff --git a/cmake/README.html b/cmake/README.html index 8170cd4897..9cdb5c7584 100644 --- a/cmake/README.html +++ b/cmake/README.html @@ -47,9 +47,9 @@

GtsamTesting

  • -

    gtsamAddExamplesGlob(globPatterns excludedFiles linkLibraries) Add scripts that will serve as examples of how to use the library. A list of files or glob patterns is specified, and one executable will be created for each matching .cpp file. These executables will not be installed. They are build with 'make all' if GTSAM_BUILD_EXAMPLES_ALWAYS is enabled. They may also be built with 'make examples'.

    +

    gtsamAddExamplesGlob(globPatterns excludedFiles linkLibraries buildWithAll) Add scripts that will serve as examples of how to use the library. A list of files or glob patterns is specified, and one executable will be created for each matching .cpp file. These executables will not be installed. They are build with 'make all' if GTSAM_BUILD_EXAMPLES_ALWAYS is enabled. They may also be built with 'make examples'.

    Usage example:

    -
    gtsamAddExamplesGlob("*.cpp" "BrokenExample.cpp" "gtsam;GeographicLib")
+
    gtsamAddExamplesGlob("*.cpp" "BrokenExample.cpp" "gtsam;GeographicLib" ON)
 
    
 
    Arguments:
    
 
    globPatterns:  The list of files or glob patterns from which to create unit tests, with
@@ -58,6 +58,7 @@ 
    GtsamTesting
    
 excludedFiles: A list of files or globs to exclude, e.g. "C*.cpp;BrokenExample.cpp".  Pass
                an empty string "" if nothing needs to be excluded.
 linkLibraries: The list of libraries to link to.
+buildWithAll: Build examples with `make` and/or `make all`
    • diff --git a/cmake/README.md b/cmake/README.md index 569a401b17..4203b8d3c0 100644 --- a/cmake/README.md +++ b/cmake/README.md @@ -52,11 +52,11 @@ Defines two useful functions for creating CTest unit tests. Also immediately cr Pass an empty string "" if nothing needs to be excluded. linkLibraries: The list of libraries to link to in addition to CppUnitLite. -* `gtsamAddExamplesGlob(globPatterns excludedFiles linkLibraries)` Add scripts that will serve as examples of how to use the library. A list of files or glob patterns is specified, and one executable will be created for each matching .cpp file. These executables will not be installed. They are build with 'make all' if GTSAM_BUILD_EXAMPLES_ALWAYS is enabled. They may also be built with 'make examples'. +* `gtsamAddExamplesGlob(globPatterns excludedFiles linkLibraries buildWithAll)` Add scripts that will serve as examples of how to use the library. A list of files or glob patterns is specified, and one executable will be created for each matching .cpp file. These executables will not be installed. They are build with 'make all' if GTSAM_BUILD_EXAMPLES_ALWAYS is enabled. They may also be built with 'make examples'. Usage example: - gtsamAddExamplesGlob("*.cpp" "BrokenExample.cpp" "gtsam;GeographicLib") + gtsamAddExamplesGlob("*.cpp" "BrokenExample.cpp" "gtsam;GeographicLib" ON) Arguments: @@ -66,6 +66,7 @@ Defines two useful functions for creating CTest unit tests. Also immediately cr excludedFiles: A list of files or globs to exclude, e.g. "C*.cpp;BrokenExample.cpp". Pass an empty string "" if nothing needs to be excluded. linkLibraries: The list of libraries to link to. + buildWithAll: Build examples with `make` and/or `make all` ## GtsamMakeConfigFile diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 52d90deb96..280b82265e 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -20,4 +20,4 @@ if (NOT GTSAM_USE_BOOST_FEATURES) ) endif() -gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam;${Boost_PROGRAM_OPTIONS_LIBRARY}") +gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam;${Boost_PROGRAM_OPTIONS_LIBRARY}" ${GTSAM_BUILD_EXAMPLES_ALWAYS}) diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index df38c171e4..b240e16265 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -42,8 +42,8 @@ #include #include #include -#include #include +#include #include "Switching.h" #include "TinyHybridExample.h" @@ -613,11 +613,11 @@ TEST(HybridGaussianFactorGraph, assembleGraphTree) { // Create expected decision tree with two factor graphs: // Get mixture factor: - auto mixture = std::dynamic_pointer_cast(fg.at(0)); + auto mixture = fg.at(0); CHECK(mixture); // Get prior factor: - const auto gf = std::dynamic_pointer_cast(fg.at(1)); + const auto gf = fg.at(1); CHECK(gf); using GF = GaussianFactor::shared_ptr; const GF prior = gf->asGaussian(); diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 327fca49a6..b6046d0bb4 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -310,6 +310,21 @@ class FactorGraph { */ sharedFactor& at(size_t i) { return factors_.at(i); } + /** Get a specific factor by index and typecast to factor type F + * (this checks array bounds and may throw + * an exception, as opposed to operator[] which does not). + */ + template + std::shared_ptr at(size_t i) { + return std::dynamic_pointer_cast(factors_.at(i)); + } + + /// Const version of templated `at` method. + template + const std::shared_ptr at(size_t i) const { + return std::dynamic_pointer_cast(factors_.at(i)); + } + /** Get a specific factor by index (this does not check array bounds, as * opposed to at() which does). */ diff --git a/gtsam/inference/Key.cpp b/gtsam/inference/Key.cpp index 8fcea0d05a..15d633eeb7 100644 --- a/gtsam/inference/Key.cpp +++ b/gtsam/inference/Key.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file Key.h + * @file Key.cpp * @brief * @author Richard Roberts * @author Alex Cunningham @@ -26,6 +26,9 @@ using namespace std; namespace gtsam { +/// Assign default key formatter +KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter; + /* ************************************************************************* */ string _defaultKeyFormatter(Key key) { const Symbol asSymbol(key); diff --git a/gtsam/inference/Key.h b/gtsam/inference/Key.h index 31428a50e7..a02d018f5b 100644 --- a/gtsam/inference/Key.h +++ b/gtsam/inference/Key.h @@ -37,10 +37,16 @@ using KeyFormatter = std::function; // Helper function for DefaultKeyFormatter GTSAM_EXPORT std::string _defaultKeyFormatter(Key key); -/// The default KeyFormatter, which is used if no KeyFormatter is passed to -/// a nonlinear 'print' function. Automatically detects plain integer keys -/// and Symbol keys. -static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter; +/** + * The default KeyFormatter, which is used if no KeyFormatter is passed + * to a 'print' function. + * + * Automatically detects plain integer keys and Symbol keys. + * + * Marked as `extern` so that it can be updated by external libraries. + * + */ +extern GTSAM_EXPORT KeyFormatter DefaultKeyFormatter; // Helper function for Multi-robot Key Formatter GTSAM_EXPORT std::string _multirobotKeyFormatter(gtsam::Key key); @@ -124,7 +130,3 @@ struct traits { }; } // namespace gtsam - - - - diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 375e826980..407ed1e277 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -312,6 +312,12 @@ namespace gtsam { /** Get a view of the A matrix */ ABlock getA() { return Ab_.range(0, size()); } + /** + * Get a view of the A matrix for the variable + * pointed to by the given key. + */ + ABlock getA(const Key& key) { return Ab_(find(key) - begin()); } + /** Update an information matrix by adding the information corresponding to this factor * (used internally during elimination). * @param scatter A mapping from variable index to slot index in this HessianFactor diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index 2ba00abc1d..03222bb3f4 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -391,8 +391,7 @@ TEST(GaussianFactorGraph, clone) { EXPECT(assert_equal(init_graph, actCloned)); // Same as the original version // Apply an in-place change to init_graph and compare - JacobianFactor::shared_ptr jacFactor0 = - std::dynamic_pointer_cast(init_graph.at(0)); + JacobianFactor::shared_ptr jacFactor0 = init_graph.at(0); CHECK(jacFactor0); jacFactor0->getA(jacFactor0->begin()) *= 7.; EXPECT(assert_inequal(init_graph, exp_graph)); diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp index 0ad77b3660..234466620b 100644 --- a/gtsam/linear/tests/testJacobianFactor.cpp +++ b/gtsam/linear/tests/testJacobianFactor.cpp @@ -65,7 +65,10 @@ TEST(JacobianFactor, constructors_and_accessors) JacobianFactor actual(terms[0].first, terms[0].second, b, noise); EXPECT(assert_equal(expected, actual)); LONGS_EQUAL((long)terms[0].first, (long)actual.keys().back()); + // Key iterator EXPECT(assert_equal(terms[0].second, actual.getA(actual.end() - 1))); + // Key + EXPECT(assert_equal(terms[0].second, actual.getA(terms[0].first))); EXPECT(assert_equal(b, expected.getb())); EXPECT(assert_equal(b, actual.getb())); EXPECT(noise == expected.get_model()); @@ -78,7 +81,10 @@ TEST(JacobianFactor, constructors_and_accessors) terms[1].first, terms[1].second, b, noise); EXPECT(assert_equal(expected, actual)); LONGS_EQUAL((long)terms[1].first, (long)actual.keys().back()); + // Key iterator EXPECT(assert_equal(terms[1].second, actual.getA(actual.end() - 1))); + // Key + EXPECT(assert_equal(terms[1].second, actual.getA(terms[1].first))); EXPECT(assert_equal(b, expected.getb())); EXPECT(assert_equal(b, actual.getb())); EXPECT(noise == expected.get_model()); @@ -91,7 +97,10 @@ TEST(JacobianFactor, constructors_and_accessors) terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise); EXPECT(assert_equal(expected, actual)); LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back()); + // Key iterator EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1))); + // Key + EXPECT(assert_equal(terms[2].second, actual.getA(terms[2].first))); EXPECT(assert_equal(b, expected.getb())); EXPECT(assert_equal(b, actual.getb())); EXPECT(noise == expected.get_model()); diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index d59eb43718..b646d009ee 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -65,10 +65,9 @@ class GncOptimizer { nfg_.resize(graph.size()); for (size_t i = 0; i < graph.size(); i++) { if (graph[i]) { - NoiseModelFactor::shared_ptr factor = std::dynamic_pointer_cast< - NoiseModelFactor>(graph[i]); - auto robust = std::dynamic_pointer_cast< - noiseModel::Robust>(factor->noiseModel()); + NoiseModelFactor::shared_ptr factor = graph.at(i); + auto robust = + std::dynamic_pointer_cast(factor->noiseModel()); // if the factor has a robust loss, we remove the robust loss nfg_[i] = robust ? factor-> cloneWithNewNoiseModel(robust->noise()) : factor; } @@ -401,11 +400,9 @@ class GncOptimizer { newGraph.resize(nfg_.size()); for (size_t i = 0; i < nfg_.size(); i++) { if (nfg_[i]) { - auto factor = std::dynamic_pointer_cast< - NoiseModelFactor>(nfg_[i]); - auto noiseModel = - std::dynamic_pointer_cast( - factor->noiseModel()); + auto factor = nfg_.at(i); + auto noiseModel = std::dynamic_pointer_cast( + factor->noiseModel()); if (noiseModel) { Matrix newInfo = weights[i] * noiseModel->information(); auto newNoiseModel = noiseModel::Gaussian::Information(newInfo); diff --git a/gtsam/nonlinear/values.i b/gtsam/nonlinear/values.i index e36b1cf1c5..1b0322699d 100644 --- a/gtsam/nonlinear/values.i +++ b/gtsam/nonlinear/values.i @@ -59,10 +59,12 @@ class Values { // enabling serialization functionality void serialize() const; - // New in 4.0, we have to specialize every insert/update/at to generate - // wrappers Instead of the old: void insert(size_t j, const gtsam::Value& - // value); void update(size_t j, const gtsam::Value& val); gtsam::Value - // at(size_t j) const; + // New in 4.0, we have to specialize every insert/update/at + // to generate wrappers. + // Instead of the old: + // void insert(size_t j, const gtsam::Value& value); + // void update(size_t j, const gtsam::Value& val); + // gtsam::Value at(size_t j) const; // The order is important: Vector has to precede Point2/Point3 so `atVector` // can work for those fixed-size vectors. @@ -99,6 +101,10 @@ class Values { template void insert(size_t j, const T& val); + // The order is important: Vector has to precede Point2/Point3 so `atVector` + // can work for those fixed-size vectors. + void update(size_t j, Vector vector); + void update(size_t j, Matrix matrix); void update(size_t j, const gtsam::Point2& point2); void update(size_t j, const gtsam::Point3& point3); void update(size_t j, const gtsam::Rot2& rot2); @@ -125,10 +131,12 @@ class Values { void update(size_t j, const gtsam::PinholePose& camera); void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void update(size_t j, const gtsam::NavState& nav_state); - void update(size_t j, Vector vector); - void update(size_t j, Matrix matrix); void update(size_t j, double c); + // The order is important: Vector has to precede Point2/Point3 so `atVector` + // can work for those fixed-size vectors. + void insert_or_assign(size_t j, Vector vector); + void insert_or_assign(size_t j, Matrix matrix); void insert_or_assign(size_t j, const gtsam::Point2& point2); void insert_or_assign(size_t j, const gtsam::Point3& point3); void insert_or_assign(size_t j, const gtsam::Rot2& rot2); @@ -165,8 +173,6 @@ class Values { void insert_or_assign(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void insert_or_assign(size_t j, const gtsam::NavState& nav_state); - void insert_or_assign(size_t j, Vector vector); - void insert_or_assign(size_t j, Matrix matrix); void insert_or_assign(size_t j, double c); template ( - std::dynamic_pointer_cast(graph[i])->noiseModel()); - EXPECT(robust); // we expect the factors to be use a robust noise model (in particular, Huber) + const auto &robust = std::dynamic_pointer_cast( + graph.at(i)->noiseModel()); + // we expect the factors to be use a robust noise model + // (in particular, Huber) + EXPECT(robust); } // test result diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index d35a87eeeb..7135137bba 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -156,6 +156,9 @@ virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor { // enabling serialization functionality void serialize() const; + + gtsam::TriangulationResult point() const; + gtsam::TriangulationResult point(const gtsam::Values& values) const; }; typedef gtsam::SmartProjectionPoseFactor diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 8591a39323..13104174f8 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -97,8 +97,7 @@ TEST(dataSet, load2D) { auto model = noiseModel::Unit::Create(3); BetweenFactor expected(1, 0, Pose2(-0.99879, 0.0417574, -0.00818381), model); - BetweenFactor::shared_ptr actual = - std::dynamic_pointer_cast>(graph->at(0)); + BetweenFactor::shared_ptr actual = graph->at>(0); EXPECT(assert_equal(expected, *actual)); // Check binary measurements, Pose2 @@ -113,9 +112,8 @@ TEST(dataSet, load2D) { // // Check factor parsing const auto actualFactors = parseFactors(filename); for (size_t i : {0, 1, 2, 3, 4, 5}) { - EXPECT(assert_equal( - *std::dynamic_pointer_cast>(graph->at(i)), - *actualFactors[i], 1e-5)); + EXPECT(assert_equal(*graph->at>(i), *actualFactors[i], + 1e-5)); } // Check pose parsing @@ -194,9 +192,8 @@ TEST(dataSet, readG2o3D) { // Check factor parsing const auto actualFactors = parseFactors(g2oFile); for (size_t i : {0, 1, 2, 3, 4, 5}) { - EXPECT(assert_equal( - *std::dynamic_pointer_cast>(expectedGraph[i]), - *actualFactors[i], 1e-5)); + EXPECT(assert_equal(*expectedGraph.at>(i), + *actualFactors[i], 1e-5)); } // Check pose parsing diff --git a/gtsam_unstable/discrete/examples/CMakeLists.txt b/gtsam_unstable/discrete/examples/CMakeLists.txt index ba4e278c9e..16e057cfed 100644 --- a/gtsam_unstable/discrete/examples/CMakeLists.txt +++ b/gtsam_unstable/discrete/examples/CMakeLists.txt @@ -12,4 +12,4 @@ endif() -gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam_unstable") +gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam_unstable" ${GTSAM_BUILD_EXAMPLES_ALWAYS}) diff --git a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp index 7c769fd784..39d9d743be 100644 --- a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp +++ b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp @@ -183,13 +183,10 @@ class LoopyBelief { // accumulate unary factors if (graph.at(factorIndex)->size() == 1) { if (!prodOfUnaries) - prodOfUnaries = std::dynamic_pointer_cast( - graph.at(factorIndex)); + prodOfUnaries = graph.at(factorIndex); else prodOfUnaries = std::make_shared( - *prodOfUnaries * - (*std::dynamic_pointer_cast( - graph.at(factorIndex)))); + *prodOfUnaries * (*graph.at(factorIndex))); } } diff --git a/gtsam_unstable/examples/CMakeLists.txt b/gtsam_unstable/examples/CMakeLists.txt index 967937b220..120f293e71 100644 --- a/gtsam_unstable/examples/CMakeLists.txt +++ b/gtsam_unstable/examples/CMakeLists.txt @@ -9,4 +9,4 @@ if (NOT GTSAM_USE_BOOST_FEATURES) endif() -gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam_unstable") +gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam_unstable" ${GTSAM_BUILD_EXAMPLES_ALWAYS}) diff --git a/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp b/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp index 38cfd73489..bbb461abb7 100644 --- a/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp +++ b/gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp @@ -87,7 +87,7 @@ TEST(NonlinearClusterTree, Clusters) { Ordering ordering; ordering.push_back(x1); const auto [bn, fg] = gfg->eliminatePartialSequential(ordering); - auto expectedFactor = std::dynamic_pointer_cast(fg->at(0)); + auto expectedFactor = fg->at(0); if (!expectedFactor) throw std::runtime_error("Expected HessianFactor"); diff --git a/gtsam_unstable/timing/CMakeLists.txt b/gtsam_unstable/timing/CMakeLists.txt index b5130ae927..44ea2f5af9 100644 --- a/gtsam_unstable/timing/CMakeLists.txt +++ b/gtsam_unstable/timing/CMakeLists.txt @@ -1 +1 @@ -gtsamAddTimingGlob("*.cpp" "" "gtsam_unstable") +gtsamAddTimingGlob("*.cpp" "" "gtsam_unstable" ${GTSAM_BUILD_TIMING_ALWAYS}) diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 06ae3366c1..cca78b80ea 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -323,7 +323,7 @@ TEST( NonlinearFactor, cloneWithNewNoiseModel ) // create actual NonlinearFactorGraph actual; SharedNoiseModel noise2 = noiseModel::Isotropic::Sigma(2,sigma2); - actual.push_back( std::dynamic_pointer_cast(nfg[0])->cloneWithNewNoiseModel(noise2) ); + actual.push_back(nfg.at(0)->cloneWithNewNoiseModel(noise2)); // check it's all good CHECK(assert_equal(expected, actual)); diff --git a/timing/CMakeLists.txt b/timing/CMakeLists.txt index e3f97ee0ca..f4fbae9408 100644 --- a/timing/CMakeLists.txt +++ b/timing/CMakeLists.txt @@ -14,6 +14,6 @@ if (NOT GTSAM_USE_BOOST_FEATURES) ) endif() -gtsamAddTimingGlob("*.cpp" "${excluded_scripts}" "gtsam") +gtsamAddTimingGlob("*.cpp" "${excluded_scripts}" "gtsam" ${GTSAM_BUILD_TIMING_ALWAYS}) target_link_libraries(timeGaussianFactorGraph CppUnitLite)