diff --git a/cmake/HandleGeneralOptions.cmake b/cmake/HandleGeneralOptions.cmake index 8c56ae242..3282ea682 100644 --- a/cmake/HandleGeneralOptions.cmake +++ b/cmake/HandleGeneralOptions.cmake @@ -21,12 +21,13 @@ if (NOT MSVC) endif() # Configurable Options +option(BUILD_SHARED_LIBS "Build shared libraries" ON) if(GTSAM_UNSTABLE_AVAILABLE) option(GTSAM_BUILD_UNSTABLE "Enable/Disable libgtsam_unstable" ON) option(GTSAM_UNSTABLE_BUILD_PYTHON "Enable/Disable Python wrapper for libgtsam_unstable" ON) option(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX "Enable/Disable MATLAB wrapper for libgtsam_unstable" OFF) endif() -option(GTSAM_FORCE_SHARED_LIB "Force gtsam to be a shared library, overriding BUILD_SHARED_LIBS" ON) +option(GTSAM_FORCE_SHARED_LIB "Force gtsam to be a shared library, overriding BUILD_SHARED_LIBS" OFF) option(GTSAM_FORCE_STATIC_LIB "Force gtsam to be a static library, overriding BUILD_SHARED_LIBS" OFF) option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF) option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." ON) @@ -46,7 +47,9 @@ option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration option(GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR "Use the slower but correct version of BetweenFactor" OFF) option(GTSAM_SLOW_BUT_CORRECT_EXPMAP "Use slower but correct expmap for Pose2" OFF) -if (GTSAM_FORCE_SHARED_LIB) +if (GTSAM_FORCE_SHARED_LIB AND GTSAM_FORCE_STATIC_LIB) + message(FATAL_ERROR "GTSAM_FORCE_SHARED_LIB and GTSAM_FORCE_STATIC_LIB are both true. Please, to unambiguously select the desired library type to use to build GTSAM, set one of GTSAM_FORCE_SHARED_LIB=ON, GTSAM_FORCE_STATIC_LIB=ON, or BUILD_SHARED_LIBS={ON/OFF}") +elseif (GTSAM_FORCE_SHARED_LIB) message(STATUS "GTSAM is a shared library due to GTSAM_FORCE_SHARED_LIB") set(GTSAM_LIBRARY_TYPE SHARED CACHE STRING "" FORCE) set(GTSAM_SHARED_LIB 1 CACHE BOOL "" FORCE) @@ -55,10 +58,9 @@ elseif (GTSAM_FORCE_STATIC_LIB) set(GTSAM_LIBRARY_TYPE STATIC CACHE STRING "" FORCE) set(GTSAM_SHARED_LIB 0 CACHE BOOL "" FORCE) elseif (BUILD_SHARED_LIBS) - message(STATUS "GTSAM is a shared library due to BUILD_SHARED_LIBS is ON") set(GTSAM_LIBRARY_TYPE SHARED CACHE STRING "" FORCE) set(GTSAM_SHARED_LIB 1 CACHE BOOL "" FORCE) -elseif((DEFINED BUILD_SHARED_LIBS) AND (NOT BUILD_SHARED_LIBS)) +elseif(NOT BUILD_SHARED_LIBS) message(STATUS "GTSAM is a static library due to BUILD_SHARED_LIBS is OFF") set(GTSAM_LIBRARY_TYPE STATIC CACHE STRING "" FORCE) set(GTSAM_SHARED_LIB 0 CACHE BOOL "" FORCE) diff --git a/gtsam/3rdparty/metis/libmetis/CMakeLists.txt b/gtsam/3rdparty/metis/libmetis/CMakeLists.txt index 92f931b98..f3b8694f4 100644 --- a/gtsam/3rdparty/metis/libmetis/CMakeLists.txt +++ b/gtsam/3rdparty/metis/libmetis/CMakeLists.txt @@ -13,7 +13,8 @@ if(WIN32) set_target_properties(metis-gtsam PROPERTIES PREFIX "" COMPILE_FLAGS /w - RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/../../../bin") + RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/../../../bin" + WINDOWS_EXPORT_ALL_SYMBOLS ON) endif() if (APPLE) diff --git a/gtsam/base/ConcurrentMap.h b/gtsam/base/ConcurrentMap.h index 03687f562..8ce44dda3 100644 --- a/gtsam/base/ConcurrentMap.h +++ b/gtsam/base/ConcurrentMap.h @@ -85,6 +85,8 @@ public: /** Copy constructor from the base map class */ ConcurrentMap(const Base& x) : Base(x) {} + ConcurrentMap& operator=(const ConcurrentMap& other) = default; + /** Handy 'exists' function */ bool exists(const KEY& e) const { return this->count(e); } diff --git a/gtsam/base/FastList.h b/gtsam/base/FastList.h index 1a3b9ed3f..821575ca9 100644 --- a/gtsam/base/FastList.h +++ b/gtsam/base/FastList.h @@ -62,6 +62,8 @@ public: /// Construct from c++11 initializer list: FastList(std::initializer_list l) : Base(l) {} + FastList& operator=(const FastList& other) = default; + #ifdef GTSAM_ALLOCATOR_BOOSTPOOL /** Copy constructor from a standard STL container */ FastList(const std::list& x) { diff --git a/gtsam/base/FastMap.h b/gtsam/base/FastMap.h index 75998fd2f..30052908d 100644 --- a/gtsam/base/FastMap.h +++ b/gtsam/base/FastMap.h @@ -54,6 +54,8 @@ public: /** Copy constructor from another FastMap */ FastMap(const FastMap& x) : Base(x) {} + FastMap& operator=(const FastMap& x) = default; + /** Copy constructor from the base map class */ FastMap(const Base& x) : Base(x) {} diff --git a/gtsam/base/FastSet.h b/gtsam/base/FastSet.h index 9a2f62497..1a2627e24 100644 --- a/gtsam/base/FastSet.h +++ b/gtsam/base/FastSet.h @@ -80,6 +80,8 @@ public: Base(x) { } + FastSet& operator=(const FastSet& other) = default; + #ifdef GTSAM_ALLOCATOR_BOOSTPOOL /** Copy constructor from a standard STL container */ FastSet(const std::set& x) { diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index 62ea8ec39..396ca16ba 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -56,9 +56,10 @@ public: GenericValue(){} /// Construct from value - GenericValue(const T& value) : - value_(value) { - } + GenericValue(const T& value) : Value(), + value_(value) {} + + GenericValue(const GenericValue& other) = default; /// Return a constant value const T& value() const { @@ -112,7 +113,7 @@ public: * Clone this value (normal clone on the heap, delete with 'delete' operator) */ std::shared_ptr clone() const override { - return std::allocate_shared(Eigen::aligned_allocator(), *this); + return std::allocate_shared(Eigen::aligned_allocator(), *this); } /// Generic Value interface version of retract diff --git a/gtsam/base/MatrixSerialization.h b/gtsam/base/MatrixSerialization.h index 11b6a417a..43c97097d 100644 --- a/gtsam/base/MatrixSerialization.h +++ b/gtsam/base/MatrixSerialization.h @@ -24,6 +24,7 @@ #include +#include #include #include #include @@ -87,6 +88,45 @@ void serialize(Archive& ar, gtsam::Matrix& m, const unsigned int version) { split_free(ar, m, version); } +/******************************************************************************/ +/// Customized functions for serializing Eigen::SparseVector +template +void save(Archive& ar, const Eigen::SparseVector<_Scalar, _Options, _Index>& m, + const unsigned int /*version*/) { + _Index size = m.size(); + + std::vector> data; + for (typename Eigen::SparseVector<_Scalar, _Options, _Index>::InnerIterator + it(m); + it; ++it) + data.push_back({it.index(), it.value()}); + + ar << BOOST_SERIALIZATION_NVP(size); + ar << BOOST_SERIALIZATION_NVP(data); +} + +template +void load(Archive& ar, Eigen::SparseVector<_Scalar, _Options, _Index>& m, + const unsigned int /*version*/) { + _Index size; + ar >> BOOST_SERIALIZATION_NVP(size); + m.resize(size); + + std::vector> data; + ar >> BOOST_SERIALIZATION_NVP(data); + + for (auto&& d : data) { + m.coeffRef(d.first) = d.second; + } +} + +template +void serialize(Archive& ar, Eigen::SparseVector<_Scalar, _Options, _Index>& m, + const unsigned int version) { + split_free(ar, m, version); +} +/******************************************************************************/ + } // namespace serialization } // namespace boost #endif diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h index 09a3f4878..e80b9d4a4 100644 --- a/gtsam/base/Value.h +++ b/gtsam/base/Value.h @@ -38,6 +38,9 @@ namespace gtsam { */ class GTSAM_EXPORT Value { public: + // todo - not sure if valid + Value() = default; + Value(const Value& other) = default; /** Clone this value in a special memory pool, must be deleted with Value::deallocate_, *not* with the 'delete' operator. */ virtual Value* clone_() const = 0; diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index c9f76ca9f..6ff1a44d4 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -8,7 +8,7 @@ #pragma once -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES #include #include #include diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index 3fbf11746..4c8808722 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -1151,7 +1151,7 @@ TEST(Matrix, Matrix24IsVectorSpace) { } TEST(Matrix, RowMajorIsVectorSpace) { -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES typedef Eigen::Matrix RowMajor; GTSAM_CONCEPT_ASSERT(IsVectorSpace); #endif @@ -1166,7 +1166,7 @@ TEST(Matrix, VectorIsVectorSpace) { } TEST(Matrix, RowVectorIsVectorSpace) { -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES typedef Eigen::Matrix RowVector; GTSAM_CONCEPT_ASSERT(IsVectorSpace); GTSAM_CONCEPT_ASSERT(IsVectorSpace); diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index d95b14292..46d44bb80 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/gtsam/base/tests/testVector.cpp @@ -272,7 +272,7 @@ TEST(Vector, VectorIsVectorSpace) { } TEST(Vector, RowVectorIsVectorSpace) { -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES typedef Eigen::Matrix RowVector; GTSAM_CONCEPT_ASSERT(IsVectorSpace); #endif diff --git a/gtsam/base/timing.cpp b/gtsam/base/timing.cpp index b43595066..07fb7b61b 100644 --- a/gtsam/base/timing.cpp +++ b/gtsam/base/timing.cpp @@ -57,7 +57,7 @@ void TimingOutline::add(size_t usecs, size_t usecsWall) { TimingOutline::TimingOutline(const std::string& label, size_t id) : id_(id), t_(0), tWall_(0), t2_(0.0), tIt_(0), tMax_(0), tMin_(0), n_(0), myOrder_( 0), lastChildOrder_(0), label_(label) { -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES #ifdef GTSAM_USING_NEW_BOOST_TIMERS timer_.stop(); #endif @@ -66,7 +66,7 @@ TimingOutline::TimingOutline(const std::string& label, size_t id) : /* ************************************************************************* */ size_t TimingOutline::time() const { -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES size_t time = 0; bool hasChildren = false; for(const ChildMap::value_type& child: children_) { @@ -84,7 +84,7 @@ size_t TimingOutline::time() const { /* ************************************************************************* */ void TimingOutline::print(const std::string& outline) const { -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES std::string formattedLabel = label_; std::replace(formattedLabel.begin(), formattedLabel.end(), '_', ' '); std::cout << outline << "-" << formattedLabel << ": " << self() << " CPU (" @@ -108,7 +108,7 @@ void TimingOutline::print(const std::string& outline) const { void TimingOutline::print2(const std::string& outline, const double parentTotal) const { -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES const int w1 = 24, w2 = 2, w3 = 6, w4 = 8, precision = 2; const double selfTotal = self(), selfMean = selfTotal / double(n_); const double childTotal = secs(); @@ -153,7 +153,7 @@ void TimingOutline::print2(const std::string& outline, /* ************************************************************************* */ const std::shared_ptr& TimingOutline::child(size_t child, const std::string& label, const std::weak_ptr& thisPtr) { -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES assert(thisPtr.lock().get() == this); std::shared_ptr& result = children_[child]; if (!result) { @@ -172,7 +172,7 @@ const std::shared_ptr& TimingOutline::child(size_t child, /* ************************************************************************* */ void TimingOutline::tic() { // Disable this entire function if we are not using boost -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES #ifdef GTSAM_USING_NEW_BOOST_TIMERS assert(timer_.is_stopped()); timer_.start(); @@ -191,7 +191,7 @@ void TimingOutline::tic() { /* ************************************************************************* */ void TimingOutline::toc() { // Disable this entire function if we are not using boost -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES #ifdef GTSAM_USING_NEW_BOOST_TIMERS @@ -225,7 +225,7 @@ void TimingOutline::toc() { /* ************************************************************************* */ void TimingOutline::finishedIteration() { -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES if (tIt_ > tMax_) tMax_ = tIt_; if (tMin_ == 0 || tIt_ < tMin_) @@ -240,7 +240,7 @@ void TimingOutline::finishedIteration() { /* ************************************************************************* */ size_t getTicTocID(const char *descriptionC) { // disable anything which refers to TimingOutline as well, for good measure -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES const std::string description(descriptionC); // Global (static) map from strings to ID numbers and current next ID number static size_t nextId = 0; @@ -263,7 +263,7 @@ size_t getTicTocID(const char *descriptionC) { /* ************************************************************************* */ void tic(size_t id, const char *labelC) { // disable anything which refers to TimingOutline as well, for good measure -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES const std::string label(labelC); std::shared_ptr node = // gCurrentTimer.lock()->child(id, label, gCurrentTimer); @@ -275,7 +275,7 @@ void tic(size_t id, const char *labelC) { /* ************************************************************************* */ void toc(size_t id, const char *labelC) { // disable anything which refers to TimingOutline as well, for good measure -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES const std::string label(labelC); std::shared_ptr current(gCurrentTimer.lock()); if (id != current->id_) { diff --git a/gtsam/base/timing.h b/gtsam/base/timing.h index 99c55a3d7..cc38f32b8 100644 --- a/gtsam/base/timing.h +++ b/gtsam/base/timing.h @@ -21,7 +21,7 @@ #include #include // for GTSAM_USE_TBB -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES #include #endif @@ -107,7 +107,7 @@ // have matching gttic/gttoc statments. You may want to consider reorganizing your timing // outline to match the scope of your code. -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES // Automatically use the new Boost timers if version is recent enough. #if BOOST_VERSION >= 104800 # ifndef GTSAM_DISABLE_NEW_TIMERS @@ -165,7 +165,7 @@ namespace gtsam { ChildMap children_; ///< subtrees // disable all timers if not using boost -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES #ifdef GTSAM_USING_NEW_BOOST_TIMERS boost::timer::cpu_timer timer_; #else @@ -183,7 +183,7 @@ namespace gtsam { GTSAM_EXPORT TimingOutline(const std::string& label, size_t myId); GTSAM_EXPORT size_t time() const; ///< time taken, including children double secs() const { return double(time()) / 1000000.0;} ///< time taken, in seconds, including children -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES double self() const { return double(t_) / 1000000.0;} ///< self time only, in seconds double wall() const { return double(tWall_) / 1000000.0;} ///< wall time, in seconds double min() const { return double(tMin_) / 1000000.0;} ///< min time, in seconds diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index ba05417aa..8c950050b 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -112,13 +112,12 @@ namespace gtsam { // } /** - * @brief Multiply all the `factors` and normalize the - * product to prevent underflow. + * @brief Multiply all the `factors`. * * @param factors The factors to multiply as a DiscreteFactorGraph. * @return DecisionTreeFactor */ - static DecisionTreeFactor ProductAndNormalize( + static DecisionTreeFactor DiscreteProduct( const DiscreteFactorGraph& factors) { // PRODUCT: multiply all factors gttic(product); @@ -126,14 +125,12 @@ namespace gtsam { gttoc(product); // Max over all the potentials by pretending all keys are frontal: - auto normalization = product.max(product.size()); + auto denominator = product.max(product.size()); // Normalize the product factor to prevent underflow. - auto normalized_product = - product / - (*std::dynamic_pointer_cast(normalization)); + product = product / (*denominator); - return normalized_product; + return product; } /* ************************************************************************ */ @@ -141,7 +138,7 @@ namespace gtsam { std::pair // EliminateForMPE(const DiscreteFactorGraph& factors, const Ordering& frontalKeys) { - DecisionTreeFactor product = ProductAndNormalize(factors); + DecisionTreeFactor product = DiscreteProduct(factors); // max out frontals, this is the factor on the separator gttic(max); @@ -210,8 +207,7 @@ namespace gtsam { return dag.argmax(); } - DiscreteValues DiscreteFactorGraph::optimize( - const Ordering& ordering) const { + DiscreteValues DiscreteFactorGraph::optimize(const Ordering& ordering) const { gttic(DiscreteFactorGraph_optimize); DiscreteLookupDAG dag = maxProduct(ordering); return dag.argmax(); @@ -221,7 +217,7 @@ namespace gtsam { std::pair // EliminateDiscrete(const DiscreteFactorGraph& factors, const Ordering& frontalKeys) { - DecisionTreeFactor product = ProductAndNormalize(factors); + DecisionTreeFactor product = DiscreteProduct(factors); // sum out frontals, this is the factor on the separator gttic(sum); diff --git a/gtsam/discrete/SignatureParser.cpp b/gtsam/discrete/SignatureParser.cpp index 31e425392..8e0221f01 100644 --- a/gtsam/discrete/SignatureParser.cpp +++ b/gtsam/discrete/SignatureParser.cpp @@ -38,7 +38,7 @@ std::optional static ParseConditional(const std::string& token) { } catch (...) { return std::nullopt; } - return std::move(row); + return row; } std::optional static ParseConditionalTable( @@ -62,7 +62,7 @@ std::optional
static ParseConditionalTable( } } } - return std::move(table); + return table; } std::vector static Tokenize(const std::string& str) { diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index 22548da07..a59095d40 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -87,7 +87,15 @@ static Eigen::SparseVector ComputeSparseTable( }); sparseTable.reserve(nrValues); - std::set allKeys(dt.keys().begin(), dt.keys().end()); + KeySet allKeys(dt.keys().begin(), dt.keys().end()); + + // Compute denominators to be used in computing sparse table indices + std::map denominators; + double denom = sparseTable.size(); + for (const DiscreteKey& dkey : dkeys) { + denom /= dkey.second; + denominators.insert(std::pair(dkey.first, denom)); + } /** * @brief Functor which is called by the DecisionTree for each leaf. @@ -102,13 +110,13 @@ static Eigen::SparseVector ComputeSparseTable( auto op = [&](const Assignment& assignment, double p) { if (p > 0) { // Get all the keys involved in this assignment - std::set assignmentKeys; + KeySet assignmentKeys; for (auto&& [k, _] : assignment) { assignmentKeys.insert(k); } // Find the keys missing in the assignment - std::vector diff; + KeyVector diff; std::set_difference(allKeys.begin(), allKeys.end(), assignmentKeys.begin(), assignmentKeys.end(), std::back_inserter(diff)); @@ -127,12 +135,10 @@ static Eigen::SparseVector ComputeSparseTable( // Generate index and add to the sparse vector. Eigen::Index idx = 0; - size_t previousCardinality = 1; // We go in reverse since a DecisionTree has the highest label first for (auto&& it = updatedAssignment.rbegin(); it != updatedAssignment.rend(); it++) { - idx += previousCardinality * it->second; - previousCardinality *= dt.cardinality(it->first); + idx += it->second * denominators.at(it->first); } sparseTable.coeffRef(idx) = p; } @@ -252,41 +258,22 @@ DecisionTreeFactor TableFactor::operator*(const DecisionTreeFactor& f) const { DecisionTreeFactor TableFactor::toDecisionTreeFactor() const { DiscreteKeys dkeys = discreteKeys(); - // Record key assignment and value pairs in pair_table. - // The assignments are stored in descending order of keys so that the order of - // the values matches what is expected by a DecisionTree. - // This is why we reverse the keys and then - // query for the key value/assignment. - DiscreteKeys rdkeys(dkeys.rbegin(), dkeys.rend()); - std::vector> pair_table; - for (auto i = 0; i < sparse_table_.size(); i++) { - std::stringstream ss; - for (auto&& [key, _] : rdkeys) { - ss << keyValueForIndex(key, i); + // If no keys, then return empty DecisionTreeFactor + if (dkeys.size() == 0) { + AlgebraicDecisionTree tree; + // We can have an empty sparse_table_ or one with a single value. + if (sparse_table_.size() != 0) { + tree = AlgebraicDecisionTree(sparse_table_.coeff(0)); } - // k will be in reverse key order already - uint64_t k; - ss >> k; - pair_table.push_back(std::make_pair(k, sparse_table_.coeff(i))); + return DecisionTreeFactor(dkeys, tree); } - // Sort the pair_table (of assignment-value pairs) based on assignment so we - // get values in reverse key order. - std::sort( - pair_table.begin(), pair_table.end(), - [](const std::pair& a, - const std::pair& b) { return a.first < b.first; }); + std::vector table(sparse_table_.size(), 0.0); + for (SparseIt it(sparse_table_); it; ++it) { + table[it.index()] = it.value(); + } - // Create the table vector by extracting the values from pair_table. - // The pair_table has already been sorted in the desired order, - // so the values will be in descending key order. - std::vector table; - std::for_each(pair_table.begin(), pair_table.end(), - [&table](const std::pair& pair) { - table.push_back(pair.second); - }); - - AlgebraicDecisionTree tree(rdkeys, table); + AlgebraicDecisionTree tree(dkeys, table); DecisionTreeFactor f(dkeys, tree); return f; } diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h index dbe4671f9..16354026d 100644 --- a/gtsam/discrete/TableFactor.h +++ b/gtsam/discrete/TableFactor.h @@ -32,6 +32,12 @@ #include #include +#if GTSAM_ENABLE_BOOST_SERIALIZATION +#include + +#include +#endif + namespace gtsam { class DiscreteConditional; @@ -360,6 +366,19 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { double error(const HybridValues& values) const override; /// @} + + private: +#if GTSAM_ENABLE_BOOST_SERIALIZATION + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(sparse_table_); + ar& BOOST_SERIALIZATION_NVP(denominators_); + ar& BOOST_SERIALIZATION_NVP(sorted_dkeys_); + } +#endif }; // traits diff --git a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp index 71c88bb7d..cbcf5234e 100644 --- a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp +++ b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp @@ -117,9 +117,9 @@ TEST(DiscreteFactorGraph, test) { *std::dynamic_pointer_cast(newFactorPtr); // Normalize newFactor by max for comparison with expected - auto normalization = newFactor.max(newFactor.size()); + auto normalizer = newFactor.max(newFactor.size()); - newFactor = newFactor / normalization; + newFactor = newFactor / *normalizer; // Check Conditional CHECK(conditional); @@ -131,9 +131,9 @@ TEST(DiscreteFactorGraph, test) { CHECK(&newFactor); DecisionTreeFactor expectedFactor(B & A, "10 6 6 10"); // Normalize by max. - normalization = expectedFactor.max(expectedFactor.size()); - // Ensure normalization is correct. - expectedFactor = expectedFactor / normalization; + normalizer = expectedFactor.max(expectedFactor.size()); + // Ensure normalizer is correct. + expectedFactor = expectedFactor / *normalizer; EXPECT(assert_equal(expectedFactor, newFactor)); // Test using elimination tree diff --git a/gtsam/discrete/tests/testSerializationDiscrete.cpp b/gtsam/discrete/tests/testSerializationDiscrete.cpp index df7df0b7e..b118a00f6 100644 --- a/gtsam/discrete/tests/testSerializationDiscrete.cpp +++ b/gtsam/discrete/tests/testSerializationDiscrete.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include using namespace std; @@ -32,6 +33,7 @@ BOOST_CLASS_EXPORT_GUID(Tree::Leaf, "gtsam_DecisionTreeStringInt_Leaf") BOOST_CLASS_EXPORT_GUID(Tree::Choice, "gtsam_DecisionTreeStringInt_Choice") BOOST_CLASS_EXPORT_GUID(DecisionTreeFactor, "gtsam_DecisionTreeFactor"); +BOOST_CLASS_EXPORT_GUID(TableFactor, "gtsam_TableFactor"); using ADT = AlgebraicDecisionTree; BOOST_CLASS_EXPORT_GUID(ADT, "gtsam_AlgebraicDecisionTree"); @@ -79,6 +81,19 @@ TEST(DiscreteSerialization, DecisionTreeFactor) { EXPECT(equalsBinary(f)); } +/* ************************************************************************* */ +// Check serialization for TableFactor +TEST(DiscreteSerialization, TableFactor) { + using namespace serializationTestHelpers; + + DiscreteKey A(Symbol('x', 1), 3); + TableFactor tf(A, "1 2 2"); + + EXPECT(equalsObj(tf)); + EXPECT(equalsXML(tf)); + EXPECT(equalsBinary(tf)); +} + /* ************************************************************************* */ // Check serialization for DiscreteConditional & DiscreteDistribution TEST(DiscreteSerialization, DiscreteConditional) { diff --git a/gtsam/discrete/tests/testTableFactor.cpp b/gtsam/discrete/tests/testTableFactor.cpp index 066f4aae3..147c3aea9 100644 --- a/gtsam/discrete/tests/testTableFactor.cpp +++ b/gtsam/discrete/tests/testTableFactor.cpp @@ -173,6 +173,36 @@ TEST(TableFactor, Conversion) { TableFactor tf(dtf.discreteKeys(), dtf); EXPECT(assert_equal(dtf, tf.toDecisionTreeFactor())); + + // Test for correct construction when keys are not in reverse order. + // This is possible in conditionals e.g. P(x1 | x0) + DiscreteKey X(1, 2), Y(0, 2); + DiscreteConditional dtf2( + X, {Y}, std::vector{0.33333333, 0.6, 0.66666667, 0.4}); + + TableFactor tf2(dtf2); + // GTSAM_PRINT(dtf2); + // GTSAM_PRINT(tf2); + // GTSAM_PRINT(tf2.toDecisionTreeFactor()); + + // Check for ADT equality since the order of keys is irrelevant + EXPECT(assert_equal>(dtf2, + tf2.toDecisionTreeFactor())); +} + +/* ************************************************************************* */ +TEST(TableFactor, Empty) { + DiscreteKey X(1, 2); + + TableFactor single = *TableFactor({X}, "1 1").sum(1); + // Should not throw a segfault + EXPECT(assert_equal(*DecisionTreeFactor(X, "1 1").sum(1), + single.toDecisionTreeFactor())); + + TableFactor empty = *TableFactor({X}, "0 0").sum(1); + // Should not throw a segfault + EXPECT(assert_equal(*DecisionTreeFactor(X, "0 0").sum(1), + empty.toDecisionTreeFactor())); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 3f85a7f42..da7bc922e 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -60,7 +60,10 @@ public: } /** copy constructor */ - Pose2(const Pose2& pose) : r_(pose.r_), t_(pose.t_) {} + Pose2(const Pose2& pose) = default; + // : r_(pose.r_), t_(pose.t_) {} + + Pose2& operator=(const Pose2& other) = default; /** * construct from (x,y,theta) diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 5805ce41b..54bdd7f4c 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -55,9 +55,10 @@ public: Pose3() : R_(traits::Identity()), t_(traits::Identity()) {} /** Copy constructor */ - Pose3(const Pose3& pose) : - R_(pose.R_), t_(pose.t_) { - } + Pose3(const Pose3& pose) = default; + // : + // R_(pose.R_), t_(pose.t_) { + // } /** Construct from R,t */ Pose3(const Rot3& R, const Point3& t) : diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 07f328b96..7a6f25c4d 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -52,11 +52,14 @@ namespace gtsam { Rot2() : c_(1.0), s_(0.0) {} /** copy constructor */ - Rot2(const Rot2& r) : Rot2(r.c_, r.s_) {} + Rot2(const Rot2& r) = default; + // : Rot2(r.c_, r.s_) {} /// Constructor from angle in radians == exponential map at identity Rot2(double theta) : c_(cos(theta)), s_(sin(theta)) {} + // Rot2& operator=(const gtsam::Rot2& other) = default; + /// Named constructor from angle in radians static Rot2 fromAngle(double theta) { return Rot2(theta); diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 8832cbb34..b9051554a 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -282,7 +282,7 @@ discreteElimination(const HybridGaussianFactorGraph &factors, } else if (auto hc = dynamic_pointer_cast(f)) { auto dc = hc->asDiscrete(); if (!dc) throwRuntimeError("discreteElimination", dc); - dfg.push_back(hc->asDiscrete()); + dfg.push_back(dc); } else { throwRuntimeError("discreteElimination", f); } diff --git a/gtsam/linear/ConjugateGradientSolver.h b/gtsam/linear/ConjugateGradientSolver.h index 46d49ca4f..aaec039e9 100644 --- a/gtsam/linear/ConjugateGradientSolver.h +++ b/gtsam/linear/ConjugateGradientSolver.h @@ -69,6 +69,8 @@ struct GTSAM_EXPORT ConjugateGradientParameters epsilon_abs(p.epsilon_abs), blas_kernel(GTSAM) {} + ConjugateGradientParameters& operator=(const ConjugateGradientParameters& other) = default; + #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43 inline size_t getMinIterations() const { return minIterations; } inline size_t getMaxIterations() const { return maxIterations; } diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 4e8ef804c..1172dc281 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -379,7 +379,7 @@ GaussianFactor::shared_ptr HessianFactor::negate() const { shared_ptr result = std::make_shared(*this); // Negate the information matrix of the result result->info_.negate(); - return std::move(result); + return result; } /* ************************************************************************* */ diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 4a5b6d5d1..a9933374f 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -117,6 +117,8 @@ namespace gtsam { /** Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix) */ explicit JacobianFactor(const HessianFactor& hf); + JacobianFactor& operator=(const JacobianFactor& jf) = default; + /** default constructor for I/O */ JacobianFactor(); diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 39666b46b..2f693bbc4 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -64,7 +64,7 @@ std::optional checkIfDiagonal(const Matrix& M) { Vector diagonal(n); for (j = 0; j < n; j++) diagonal(j) = M(j, j); - return std::move(diagonal); + return diagonal; } } diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index e60209a83..4728706c5 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -119,6 +119,8 @@ namespace gtsam { /// Constructor from Vector, with Scatter VectorValues(const Vector& c, const Scatter& scatter); + VectorValues& operator=(const VectorValues& other) = default; + /** Create a VectorValues with the same structure as \c other, but filled with zeros. */ static VectorValues Zero(const VectorValues& other); diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index 4f6648849..df89f5b44 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -149,7 +149,7 @@ protected: noiseModel_->WhitenSystem(Ab.matrix(), b); } - return std::move(factor); + return factor; } /// @return a deep copy of this factor diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 99682fb77..f6711f0f0 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -26,7 +26,7 @@ #include #include #include -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES #include #endif @@ -123,7 +123,7 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear, auto currentState = static_cast(state_.get()); bool verbose = (params_.verbosityLM >= LevenbergMarquardtParams::TRYLAMBDA); -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES #ifdef GTSAM_USING_NEW_BOOST_TIMERS boost::timer::cpu_timer lamda_iteration_timer; lamda_iteration_timer.start(); @@ -222,7 +222,7 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear, } // if (systemSolvedSuccessfully) if (params_.verbosityLM == LevenbergMarquardtParams::SUMMARY) { -#ifdef GTSAM_USE_BOOST_FEATURES +#if GTSAM_USE_BOOST_FEATURES // do timing #ifdef GTSAM_USING_NEW_BOOST_TIMERS double iterationTime = 1e-9 * lamda_iteration_timer.elapsed().wall; diff --git a/gtsam/slam/FrobeniusFactor.cpp b/gtsam/slam/FrobeniusFactor.cpp index e70f64d96..982430d81 100644 --- a/gtsam/slam/FrobeniusFactor.cpp +++ b/gtsam/slam/FrobeniusFactor.cpp @@ -61,7 +61,7 @@ ConvertNoiseModel(const SharedNoiseModel &model, size_t d, bool defaultToUnit) { return noiseModel::Robust::Create( noiseModel::mEstimator::Huber::Create(1.345), isoModel); } else { - return std::move(isoModel); + return isoModel; } } diff --git a/gtsam_unstable/geometry/Pose3Upright.h b/gtsam_unstable/geometry/Pose3Upright.h index 20e0fde5d..464f46928 100644 --- a/gtsam_unstable/geometry/Pose3Upright.h +++ b/gtsam_unstable/geometry/Pose3Upright.h @@ -43,6 +43,7 @@ public: Pose3Upright(const Rot2& bearing, const Point3& t); Pose3Upright(double x, double y, double z, double theta); Pose3Upright(const Pose2& pose, double z); + Pose3Upright& operator=(const Pose3Upright& x) = default; /// Down-converts from a full Pose3 Pose3Upright(const Pose3& fullpose); diff --git a/gtsam_unstable/slam/Mechanization_bRn2.h b/gtsam_unstable/slam/Mechanization_bRn2.h index 4714f2b6f..3d91f6b0e 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.h +++ b/gtsam_unstable/slam/Mechanization_bRn2.h @@ -35,9 +35,7 @@ public: } /// Copy constructor - Mechanization_bRn2(const Mechanization_bRn2& other) : - bRn_(other.bRn_), x_g_(other.x_g_), x_a_(other.x_a_) { - } + Mechanization_bRn2(const Mechanization_bRn2& other) = default; /// gravity in the body frame Vector3 b_g(double g_e) const {