diff --git a/cmake/HandleGeneralOptions.cmake b/cmake/HandleGeneralOptions.cmake index 43659718b..e5e2e194a 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) @@ -48,7 +49,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) @@ -57,10 +60,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/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 83556cbd2..9e9d1d113 100644 --- a/gtsam/base/timing.cpp +++ b/gtsam/base/timing.cpp @@ -31,7 +31,9 @@ namespace gtsam { namespace internal { - + +using ChildOrder = FastMap>; + // a static shared_ptr to TimingOutline with nullptr as the pointer const static std::shared_ptr nullTimingOutline; @@ -57,7 +59,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 +68,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,14 +86,13 @@ 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 (" << n_ << " times, " << wall() << " wall, " << secs() << " children, min: " << min() << " max: " << max() << ")\n"; // Order children - typedef FastMap > ChildOrder; ChildOrder childOrder; for(const ChildMap::value_type& child: children_) { childOrder[child.second->myOrder_] = child.second; @@ -107,7 +108,7 @@ void TimingOutline::print(const std::string& outline) const { } /* ************************************************************************* */ -void TimingOutline::print_csv_header(bool addLineBreak) const { +void TimingOutline::printCsvHeader(bool addLineBreak) const { #ifdef GTSAM_USE_BOOST_FEATURES // Order is (CPU time, number of times, wall time, time + children in seconds, // min time, max time) @@ -116,14 +117,13 @@ void TimingOutline::print_csv_header(bool addLineBreak) const { << "," << label_ + " min time (s)" << "," << label_ + "max time(s)" << ","; // Order children - typedef FastMap > ChildOrder; ChildOrder childOrder; for (const ChildMap::value_type& child : children_) { childOrder[child.second->myOrder_] = child.second; } // Print children for (const ChildOrder::value_type& order_child : childOrder) { - order_child.second->print_csv_header(); + order_child.second->printCsvHeader(); } if (addLineBreak) { std::cout << std::endl; @@ -133,21 +133,20 @@ void TimingOutline::print_csv_header(bool addLineBreak) const { } /* ************************************************************************* */ -void TimingOutline::print_csv(bool addLineBreak) const { +void TimingOutline::printCsv(bool addLineBreak) const { #ifdef GTSAM_USE_BOOST_FEATURES // Order is (CPU time, number of times, wall time, time + children in seconds, // min time, max time) std::cout << self() << "," << n_ << "," << wall() << "," << secs() << "," << min() << "," << max() << ","; // Order children - typedef FastMap > ChildOrder; ChildOrder childOrder; for (const ChildMap::value_type& child : children_) { childOrder[child.second->myOrder_] = child.second; } // Print children for (const ChildOrder::value_type& order_child : childOrder) { - order_child.second->print_csv(false); + order_child.second->printCsv(false); } if (addLineBreak) { std::cout << std::endl; @@ -158,7 +157,7 @@ void TimingOutline::print_csv(bool addLineBreak) 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(); @@ -203,7 +202,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) { @@ -222,7 +221,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(); @@ -241,7 +240,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 @@ -275,7 +274,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_) @@ -290,7 +289,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; @@ -313,7 +312,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); @@ -325,7 +324,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 dfc2928f1..5a96633db 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 @@ -209,7 +209,7 @@ namespace gtsam { * @param addLineBreak Flag indicating if a line break should be added at * the end. Only used at the top-leve. */ - GTSAM_EXPORT void print_csv_header(bool addLineBreak = false) const; + GTSAM_EXPORT void printCsvHeader(bool addLineBreak = false) const; /** * @brief Print the times recursively from parent to child in CSV format. @@ -220,7 +220,7 @@ namespace gtsam { * @param addLineBreak Flag indicating if a line break should be added at * the end. Only used at the top-leve. */ - GTSAM_EXPORT void print_csv(bool addLineBreak = false) const; + GTSAM_EXPORT void printCsv(bool addLineBreak = false) const; GTSAM_EXPORT const std::shared_ptr& child(size_t child, const std::string& label, const std::weak_ptr& thisPtr); @@ -292,11 +292,11 @@ inline void tictoc_print_() { ::gtsam::internal::gTimingRoot->print(); } // print timing in CSV format -inline void tictoc_print_csv_(bool displayHeader = false) { +inline void tictoc_printCsv_(bool displayHeader = false) { if (displayHeader) { - ::gtsam::internal::gTimingRoot->print_csv_header(true); + ::gtsam::internal::gTimingRoot->printCsvHeader(true); } - ::gtsam::internal::gTimingRoot->print_csv(true); + ::gtsam::internal::gTimingRoot->printCsv(true); } // print mean and standard deviation diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index 2b63be660..b48e09b03 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -112,22 +112,27 @@ 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 DecisionTreeFactor product = factors.product(); +#if GTSAM_HYBRID_TIMING + gttic_(DiscreteNormalize); +#endif // Max over all the potentials by pretending all keys are frontal: - auto normalizer = product.max(product.size()); + auto denominator = product.max(product.size()); // Normalize the product factor to prevent underflow. - product = product / (*normalizer); + product = product / (*denominator); +#if GTSAM_HYBRID_TIMING + gttoc_(DiscreteNormalize); +#endif return product; } @@ -137,7 +142,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); @@ -215,7 +220,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 DecisionTreeFactor::shared_ptr sum = product.sum(frontalKeys); 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 bf9662e34..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,9 +258,19 @@ DecisionTreeFactor TableFactor::operator*(const DecisionTreeFactor& f) const { DecisionTreeFactor TableFactor::toDecisionTreeFactor() const { DiscreteKeys dkeys = discreteKeys(); - std::vector table; - for (auto i = 0; i < sparse_table_.size(); i++) { - table.push_back(sparse_table_.coeff(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)); + } + return DecisionTreeFactor(dkeys, tree); + } + + std::vector table(sparse_table_.size(), 0.0); + for (SparseIt it(sparse_table_); it; ++it) { + table[it.index()] = it.value(); } AlgebraicDecisionTree tree(dkeys, table); diff --git a/gtsam/discrete/tests/testTableFactor.cpp b/gtsam/discrete/tests/testTableFactor.cpp index a455faaaa..e6c71e15c 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/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 {