From ddd95f4a51549d3c19cdca36d488da1323430676 Mon Sep 17 00:00:00 2001 From: Jade Turner Date: Sat, 7 Dec 2024 00:33:21 +0800 Subject: [PATCH 01/25] [build] Allow disabling tests and examples When you have gtsam as a dependency in CMake these would get built. This doesn't make sense and increases build times, allowing them to be disabled fixes this. Signed-off-by: Jade Turner --- CMakeLists.txt | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e3b462eec..ffedf56e4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -112,10 +112,17 @@ add_subdirectory(CppUnitLite) add_subdirectory(gtsam) # Build Tests -add_subdirectory(tests) +option(BUILD_TESTS "Builds unit tests" ON) +if (BUILD_TESTS) + add_subdirectory(tests) +endif() + # Build examples -add_subdirectory(examples) +option(BUILD_EXAMPLES "Builds examples" ON) +if (BUILD_EXAMPLES) + add_subdirectory(examples) +endif() # Build timing add_subdirectory(timing) From 9b93411d69b1cde30c720e80e644f096b6d7d6cb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 8 Dec 2024 11:45:10 -0500 Subject: [PATCH 02/25] put Ring struct in a separate file --- gtsam/discrete/AlgebraicDecisionTree.h | 24 ++------------- gtsam/discrete/DiscreteConditional.cpp | 3 +- gtsam/discrete/Ring.h | 37 +++++++++++++++++++++++ gtsam/discrete/TableFactor.h | 16 +--------- gtsam/discrete/tests/testDecisionTree.cpp | 9 +----- 5 files changed, 43 insertions(+), 46 deletions(-) create mode 100644 gtsam/discrete/Ring.h diff --git a/gtsam/discrete/AlgebraicDecisionTree.h b/gtsam/discrete/AlgebraicDecisionTree.h index 9948b0be6..383346ab1 100644 --- a/gtsam/discrete/AlgebraicDecisionTree.h +++ b/gtsam/discrete/AlgebraicDecisionTree.h @@ -20,12 +20,12 @@ #include #include +#include -#include +#include #include #include #include -#include #include namespace gtsam { @@ -55,26 +55,6 @@ namespace gtsam { public: using Base = DecisionTree; - /** The Real ring with addition and multiplication */ - struct Ring { - static inline double zero() { return 0.0; } - static inline double one() { return 1.0; } - static inline double add(const double& a, const double& b) { - return a + b; - } - static inline double max(const double& a, const double& b) { - return std::max(a, b); - } - static inline double mul(const double& a, const double& b) { - return a * b; - } - static inline double div(const double& a, const double& b) { - return a / b; - } - static inline double id(const double& x) { return x; } - static inline double negate(const double& x) { return -x; } - }; - AlgebraicDecisionTree(double leaf = 1.0) : Base(leaf) {} // Explicitly non-explicit constructor diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index 5ab0c59ec..c44905d79 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -104,7 +105,7 @@ DiscreteConditional DiscreteConditional::operator*( // Finally, add parents to keys, in order for (auto&& dk : parents) discreteKeys.push_back(dk); - ADT product = ADT::apply(other, ADT::Ring::mul); + ADT product = ADT::apply(other, Ring::mul); return DiscreteConditional(newFrontals.size(), discreteKeys, product); } diff --git a/gtsam/discrete/Ring.h b/gtsam/discrete/Ring.h new file mode 100644 index 000000000..cf7c6424a --- /dev/null +++ b/gtsam/discrete/Ring.h @@ -0,0 +1,37 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Ring.h + * @brief Real Ring definition + * @author Varun Agrawal + * @date Dec 8, 2024 + */ + +#pragma once + +#include + +/** The Real ring with addition and multiplication */ +struct Ring { + static inline double zero() { return 0.0; } + static inline double one() { return 1.0; } + static inline double add(const double& a, const double& b) { return a + b; } + static inline double max(const double& a, const double& b) { + return std::max(a, b); + } + static inline double mul(const double& a, const double& b) { return a * b; } + static inline double div(const double& a, const double& b) { + return (a == 0 || b == 0) ? 0 : (a / b); + } + static inline double id(const double& x) { return x; } + static inline double negate(const double& x) { return -x; } +}; diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h index f0ecd66a3..7015353e1 100644 --- a/gtsam/discrete/TableFactor.h +++ b/gtsam/discrete/TableFactor.h @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -99,21 +100,6 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { using Binary = std::function; public: - /** The Real ring with addition and multiplication */ - struct Ring { - static inline double zero() { return 0.0; } - static inline double one() { return 1.0; } - static inline double add(const double& a, const double& b) { return a + b; } - static inline double max(const double& a, const double& b) { - return std::max(a, b); - } - static inline double mul(const double& a, const double& b) { return a * b; } - static inline double div(const double& a, const double& b) { - return (a == 0 || b == 0) ? 0 : (a / b); - } - static inline double id(const double& x) { return x; } - }; - /// @name Standard Constructors /// @{ diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp index 526001b51..c664fe6b5 100644 --- a/gtsam/discrete/tests/testDecisionTree.cpp +++ b/gtsam/discrete/tests/testDecisionTree.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -124,14 +125,6 @@ struct traits
: public Testable
{}; GTSAM_CONCEPT_TESTABLE_INST(DT) -struct Ring { - static inline int zero() { return 0; } - static inline int one() { return 1; } - static inline int id(const int& a) { return a; } - static inline int add(const int& a, const int& b) { return a + b; } - static inline int mul(const int& a, const int& b) { return a * b; } -}; - /* ************************************************************************** */ // Check that creating decision trees respects key order. TEST(DecisionTree, ConstructorOrder) { From 7bf8ee167d0405d5822c021ef5b3d2ccc4942ffa Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 8 Dec 2024 11:51:55 -0500 Subject: [PATCH 03/25] update DecisionTreeFactor.h --- gtsam/discrete/DecisionTreeFactor.h | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index a8ab2644f..9a3cde96d 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -21,11 +21,12 @@ #include #include #include +#include #include #include -#include #include +#include #include #include #include @@ -145,7 +146,7 @@ namespace gtsam { /// multiply two factors DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override { - return apply(f, ADT::Ring::mul); + return apply(f, Ring::mul); } static double safe_div(const double& a, const double& b); @@ -160,22 +161,22 @@ namespace gtsam { /// Create new factor by summing all values with the same separator values shared_ptr sum(size_t nrFrontals) const { - return combine(nrFrontals, ADT::Ring::add); + return combine(nrFrontals, Ring::add); } /// Create new factor by summing all values with the same separator values shared_ptr sum(const Ordering& keys) const { - return combine(keys, ADT::Ring::add); + return combine(keys, Ring::add); } /// Create new factor by maximizing over all values with the same separator. shared_ptr max(size_t nrFrontals) const { - return combine(nrFrontals, ADT::Ring::max); + return combine(nrFrontals, Ring::max); } /// Create new factor by maximizing over all values with the same separator. shared_ptr max(const Ordering& keys) const { - return combine(keys, ADT::Ring::max); + return combine(keys, Ring::max); } /// @} From 699757d381758cc32c5da96e4b021b42c4e283c0 Mon Sep 17 00:00:00 2001 From: Jade Turner Date: Mon, 9 Dec 2024 15:17:16 +0800 Subject: [PATCH 04/25] Revert "[build] Allow disabling tests and examples" This reverts commit ddd95f4a51549d3c19cdca36d488da1323430676. --- CMakeLists.txt | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ffedf56e4..e3b462eec 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -112,17 +112,10 @@ add_subdirectory(CppUnitLite) add_subdirectory(gtsam) # Build Tests -option(BUILD_TESTS "Builds unit tests" ON) -if (BUILD_TESTS) - add_subdirectory(tests) -endif() - +add_subdirectory(tests) # Build examples -option(BUILD_EXAMPLES "Builds examples" ON) -if (BUILD_EXAMPLES) - add_subdirectory(examples) -endif() +add_subdirectory(examples) # Build timing add_subdirectory(timing) From f8d9da1b8edeebdba2acdfc2100da85d6b8ed2e0 Mon Sep 17 00:00:00 2001 From: Joel Truher Date: Mon, 9 Dec 2024 06:57:35 -0800 Subject: [PATCH 05/25] actually fix CHECK_EQUAL --- CppUnitLite/Test.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CppUnitLite/Test.h b/CppUnitLite/Test.h index 9ee0d10eb..c3fa83234 100644 --- a/CppUnitLite/Test.h +++ b/CppUnitLite/Test.h @@ -129,7 +129,7 @@ protected: result_.addFailure(Failure(name_, __FILE__, __LINE__, #expected, #actual)); } #define CHECK_EQUAL(expected,actual)\ -{ if (!((expected) == (actual))) { result_.addFailure(Failure(name_, __FILE__, __LINE__, std::to_string(expected), std::to_string(actual))); } } +{ if (!((expected) == (actual))) { result_.addFailure(Failure(name_, __FILE__, __LINE__, std::to_string(expected), std::to_string(actual))); return; } } #define LONGS_EQUAL(expected,actual)\ { long actualTemp = actual; \ From 88b36da602c9697ca86db5d42921fa6d058cc1af Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 9 Dec 2024 16:55:15 -0500 Subject: [PATCH 06/25] make evaluate a common base method --- gtsam/discrete/DecisionTreeFactor.h | 9 ++------- gtsam/discrete/DiscreteFactor.h | 15 ++++++++++++++- gtsam/discrete/TableFactor.cpp | 3 ++- gtsam/discrete/TableFactor.h | 10 ++-------- gtsam_unstable/discrete/AllDiff.cpp | 2 +- gtsam_unstable/discrete/AllDiff.h | 2 +- gtsam_unstable/discrete/BinaryAllDiff.h | 2 +- gtsam_unstable/discrete/Domain.cpp | 2 +- gtsam_unstable/discrete/Domain.h | 2 +- gtsam_unstable/discrete/SingleValue.cpp | 2 +- gtsam_unstable/discrete/SingleValue.h | 2 +- 11 files changed, 27 insertions(+), 24 deletions(-) diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index 9a3cde96d..8f3dd85a6 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -130,14 +130,9 @@ namespace gtsam { /// @name Standard Interface /// @{ - /// Calculate probability for given values `x`, + /// Calculate probability for given values, /// is just look up in AlgebraicDecisionTree. - double evaluate(const Assignment& values) const { - return ADT::operator()(values); - } - - /// Evaluate probability distribution, sugar. - double operator()(const DiscreteValues& values) const override { + double operator()(const Assignment& values) const override { return ADT::operator()(values); } diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index 19af5bd13..5bc3c463f 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -92,8 +92,21 @@ class GTSAM_EXPORT DiscreteFactor : public Factor { size_t cardinality(Key j) const { return cardinalities_.at(j); } + /** + * @brief Calculate probability for given values. + * Calls specialized evaluation under the hood. + * + * Note: Uses Assignment as it is the base class of DiscreteValues. + * + * @param values Discrete assignment. + * @return double + */ + double evaluate(const Assignment& values) const { + return operator()(values); + } + /// Find value for given assignment of values to variables - virtual double operator()(const DiscreteValues&) const = 0; + virtual double operator()(const Assignment& values) const = 0; /// Error is just -log(value) virtual double error(const DiscreteValues& values) const; diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index f4e023a4d..32cba84ed 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -133,7 +133,7 @@ bool TableFactor::equals(const DiscreteFactor& other, double tol) const { } /* ************************************************************************ */ -double TableFactor::operator()(const DiscreteValues& values) const { +double TableFactor::operator()(const Assignment& values) const { // a b c d => D * (C * (B * (a) + b) + c) + d uint64_t idx = 0, card = 1; for (auto it = sorted_dkeys_.rbegin(); it != sorted_dkeys_.rend(); ++it) { @@ -180,6 +180,7 @@ DecisionTreeFactor TableFactor::toDecisionTreeFactor() const { for (auto i = 0; i < sparse_table_.size(); i++) { table.push_back(sparse_table_.coeff(i)); } + // NOTE(Varun): This constructor is really expensive!! DecisionTreeFactor f(dkeys, table); return f; } diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h index 7015353e1..53495d078 100644 --- a/gtsam/discrete/TableFactor.h +++ b/gtsam/discrete/TableFactor.h @@ -155,14 +155,8 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { // /// @name Standard Interface // /// @{ - /// Calculate probability for given values `x`, - /// is just look up in TableFactor. - double evaluate(const DiscreteValues& values) const { - return operator()(values); - } - - /// Evaluate probability distribution, sugar. - double operator()(const DiscreteValues& values) const override; + /// Evaluate probability distribution, is just look up in TableFactor. + double operator()(const Assignment& values) const override; /// Calculate error for DiscreteValues `x`, is -log(probability). double error(const DiscreteValues& values) const override; diff --git a/gtsam_unstable/discrete/AllDiff.cpp b/gtsam_unstable/discrete/AllDiff.cpp index 2bd9e6dfd..a450605b3 100644 --- a/gtsam_unstable/discrete/AllDiff.cpp +++ b/gtsam_unstable/discrete/AllDiff.cpp @@ -26,7 +26,7 @@ void AllDiff::print(const std::string& s, const KeyFormatter& formatter) const { } /* ************************************************************************* */ -double AllDiff::operator()(const DiscreteValues& values) const { +double AllDiff::operator()(const Assignment& values) const { std::set taken; // record values taken by keys for (Key dkey : keys_) { size_t value = values.at(dkey); // get the value for that key diff --git a/gtsam_unstable/discrete/AllDiff.h b/gtsam_unstable/discrete/AllDiff.h index d7a63eae0..6fe43568e 100644 --- a/gtsam_unstable/discrete/AllDiff.h +++ b/gtsam_unstable/discrete/AllDiff.h @@ -45,7 +45,7 @@ class GTSAM_UNSTABLE_EXPORT AllDiff : public Constraint { } /// Calculate value = expensive ! - double operator()(const DiscreteValues& values) const override; + double operator()(const Assignment& values) const override; /// Convert into a decisiontree, can be *very* expensive ! DecisionTreeFactor toDecisionTreeFactor() const override; diff --git a/gtsam_unstable/discrete/BinaryAllDiff.h b/gtsam_unstable/discrete/BinaryAllDiff.h index 18b335092..a55865c77 100644 --- a/gtsam_unstable/discrete/BinaryAllDiff.h +++ b/gtsam_unstable/discrete/BinaryAllDiff.h @@ -47,7 +47,7 @@ class BinaryAllDiff : public Constraint { } /// Calculate value - double operator()(const DiscreteValues& values) const override { + double operator()(const Assignment& values) const override { return (double)(values.at(keys_[0]) != values.at(keys_[1])); } diff --git a/gtsam_unstable/discrete/Domain.cpp b/gtsam_unstable/discrete/Domain.cpp index bbbc87667..752228c18 100644 --- a/gtsam_unstable/discrete/Domain.cpp +++ b/gtsam_unstable/discrete/Domain.cpp @@ -30,7 +30,7 @@ string Domain::base1Str() const { } /* ************************************************************************* */ -double Domain::operator()(const DiscreteValues& values) const { +double Domain::operator()(const Assignment& values) const { return contains(values.at(key())); } diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h index 7f7b717c2..13249d733 100644 --- a/gtsam_unstable/discrete/Domain.h +++ b/gtsam_unstable/discrete/Domain.h @@ -82,7 +82,7 @@ class GTSAM_UNSTABLE_EXPORT Domain : public Constraint { bool contains(size_t value) const { return values_.count(value) > 0; } /// Calculate value - double operator()(const DiscreteValues& values) const override; + double operator()(const Assignment& values) const override; /// Convert into a decisiontree DecisionTreeFactor toDecisionTreeFactor() const override; diff --git a/gtsam_unstable/discrete/SingleValue.cpp b/gtsam_unstable/discrete/SingleValue.cpp index 6b78f38f5..9762aec0f 100644 --- a/gtsam_unstable/discrete/SingleValue.cpp +++ b/gtsam_unstable/discrete/SingleValue.cpp @@ -22,7 +22,7 @@ void SingleValue::print(const string& s, const KeyFormatter& formatter) const { } /* ************************************************************************* */ -double SingleValue::operator()(const DiscreteValues& values) const { +double SingleValue::operator()(const Assignment& values) const { return (double)(values.at(keys_[0]) == value_); } diff --git a/gtsam_unstable/discrete/SingleValue.h b/gtsam_unstable/discrete/SingleValue.h index 3f7f22d6a..93fe38aaa 100644 --- a/gtsam_unstable/discrete/SingleValue.h +++ b/gtsam_unstable/discrete/SingleValue.h @@ -55,7 +55,7 @@ class GTSAM_UNSTABLE_EXPORT SingleValue : public Constraint { } /// Calculate value - double operator()(const DiscreteValues& values) const override; + double operator()(const Assignment& values) const override; /// Convert into a decisiontree DecisionTreeFactor toDecisionTreeFactor() const override; From e0fedda712808b557ef229b2ec4cda50f25fb743 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 9 Dec 2024 17:22:51 -0500 Subject: [PATCH 07/25] make the Unary and Binary ops common --- gtsam/discrete/DecisionTreeFactor.cpp | 12 ++++++------ gtsam/discrete/DecisionTreeFactor.h | 15 ++++++++++----- gtsam/discrete/DiscreteFactor.h | 5 +++++ gtsam/discrete/TableFactor.h | 4 ---- 4 files changed, 21 insertions(+), 15 deletions(-) diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index 9ec3b0ac5..776d4bd90 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -83,7 +83,7 @@ namespace gtsam { } /* ************************************************************************ */ - DecisionTreeFactor DecisionTreeFactor::apply(ADT::Unary op) const { + DecisionTreeFactor DecisionTreeFactor::apply(Unary op) const { // apply operand ADT result = ADT::apply(op); // Make a new factor @@ -91,7 +91,7 @@ namespace gtsam { } /* ************************************************************************ */ - DecisionTreeFactor DecisionTreeFactor::apply(ADT::UnaryAssignment op) const { + DecisionTreeFactor DecisionTreeFactor::apply(UnaryAssignment op) const { // apply operand ADT result = ADT::apply(op); // Make a new factor @@ -100,7 +100,7 @@ namespace gtsam { /* ************************************************************************ */ DecisionTreeFactor DecisionTreeFactor::apply(const DecisionTreeFactor& f, - ADT::Binary op) const { + Binary op) const { map cs; // new cardinalities // make unique key-cardinality map for (Key j : keys()) cs[j] = cardinality(j); @@ -118,8 +118,8 @@ namespace gtsam { } /* ************************************************************************ */ - DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine( - size_t nrFrontals, ADT::Binary op) const { + DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine(size_t nrFrontals, + Binary op) const { if (nrFrontals > size()) { throw invalid_argument( "DecisionTreeFactor::combine: invalid number of frontal " @@ -146,7 +146,7 @@ namespace gtsam { /* ************************************************************************ */ DecisionTreeFactor::shared_ptr DecisionTreeFactor::combine( - const Ordering& frontalKeys, ADT::Binary op) const { + const Ordering& frontalKeys, Binary op) const { if (frontalKeys.size() > size()) { throw invalid_argument( "DecisionTreeFactor::combine: invalid number of frontal " diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index 8f3dd85a6..9172180b1 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -51,6 +51,11 @@ namespace gtsam { typedef std::shared_ptr shared_ptr; typedef AlgebraicDecisionTree ADT; + // Needed since we have definitions in both DiscreteFactor and DecisionTree + using Base::Binary; + using Base::Unary; + using Base::UnaryAssignment; + /// @name Standard Constructors /// @{ @@ -182,21 +187,21 @@ namespace gtsam { * Apply unary operator (*this) "op" f * @param op a unary operator that operates on AlgebraicDecisionTree */ - DecisionTreeFactor apply(ADT::Unary op) const; + DecisionTreeFactor apply(Unary op) const; /** * Apply unary operator (*this) "op" f * @param op a unary operator that operates on AlgebraicDecisionTree. Takes * both the assignment and the value. */ - DecisionTreeFactor apply(ADT::UnaryAssignment op) const; + DecisionTreeFactor apply(UnaryAssignment op) const; /** * Apply binary operator (*this) "op" f * @param f the second argument for op * @param op a binary operator that operates on AlgebraicDecisionTree */ - DecisionTreeFactor apply(const DecisionTreeFactor& f, ADT::Binary op) const; + DecisionTreeFactor apply(const DecisionTreeFactor& f, Binary op) const; /** * Combine frontal variables using binary operator "op" @@ -204,7 +209,7 @@ namespace gtsam { * @param op a binary operator that operates on AlgebraicDecisionTree * @return shared pointer to newly created DecisionTreeFactor */ - shared_ptr combine(size_t nrFrontals, ADT::Binary op) const; + shared_ptr combine(size_t nrFrontals, Binary op) const; /** * Combine frontal variables in an Ordering using binary operator "op" @@ -212,7 +217,7 @@ namespace gtsam { * @param op a binary operator that operates on AlgebraicDecisionTree * @return shared pointer to newly created DecisionTreeFactor */ - shared_ptr combine(const Ordering& keys, ADT::Binary op) const; + shared_ptr combine(const Ordering& keys, Binary op) const; /// Enumerate all values into a map from values to double. std::vector> enumerate() const; diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index 5bc3c463f..7175f7608 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -46,6 +46,11 @@ class GTSAM_EXPORT DiscreteFactor : public Factor { using Values = DiscreteValues; ///< backwards compatibility + using Unary = std::function; + using UnaryAssignment = + std::function&, const double&)>; + using Binary = std::function; + protected: /// Map of Keys and their cardinalities. std::map cardinalities_; diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h index 53495d078..e7ed2294f 100644 --- a/gtsam/discrete/TableFactor.h +++ b/gtsam/discrete/TableFactor.h @@ -94,10 +94,6 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { typedef std::shared_ptr shared_ptr; typedef Eigen::SparseVector::InnerIterator SparseIt; typedef std::vector> AssignValList; - using Unary = std::function; - using UnaryAssignment = - std::function&, const double&)>; - using Binary = std::function; public: /// @name Standard Constructors From 1152470800d2f7845343b20ce592e32837564b6a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 9 Dec 2024 17:37:11 -0500 Subject: [PATCH 08/25] fix wrapper --- gtsam/discrete/discrete.i | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i index 55c8f9e22..d82ba9794 100644 --- a/gtsam/discrete/discrete.i +++ b/gtsam/discrete/discrete.i @@ -41,7 +41,7 @@ virtual class DiscreteFactor : gtsam::Factor { const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::DiscreteFactor& other, double tol = 1e-9) const; - double operator()(const gtsam::DiscreteValues& values) const; + double operator()(const gtsam::Assignment& values) const; }; #include @@ -69,7 +69,7 @@ virtual class DecisionTreeFactor : gtsam::DiscreteFactor { size_t cardinality(gtsam::Key j) const; - double operator()(const gtsam::DiscreteValues& values) const; + double operator()(const gtsam::Assignment& values) const; gtsam::DecisionTreeFactor operator*(const gtsam::DecisionTreeFactor& f) const; size_t cardinality(gtsam::Key j) const; gtsam::DecisionTreeFactor operator/(const gtsam::DecisionTreeFactor& f) const; @@ -248,7 +248,6 @@ class DiscreteBayesTree { void saveGraph(string s, const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; - double operator()(const gtsam::DiscreteValues& values) const; string markdown(const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; From ab943b539ec3f7c186aa40be2579407fa320278d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 9 Dec 2024 18:21:26 -0500 Subject: [PATCH 09/25] Revert "fix wrapper" This reverts commit 1152470800d2f7845343b20ce592e32837564b6a. --- gtsam/discrete/discrete.i | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i index d82ba9794..55c8f9e22 100644 --- a/gtsam/discrete/discrete.i +++ b/gtsam/discrete/discrete.i @@ -41,7 +41,7 @@ virtual class DiscreteFactor : gtsam::Factor { const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::DiscreteFactor& other, double tol = 1e-9) const; - double operator()(const gtsam::Assignment& values) const; + double operator()(const gtsam::DiscreteValues& values) const; }; #include @@ -69,7 +69,7 @@ virtual class DecisionTreeFactor : gtsam::DiscreteFactor { size_t cardinality(gtsam::Key j) const; - double operator()(const gtsam::Assignment& values) const; + double operator()(const gtsam::DiscreteValues& values) const; gtsam::DecisionTreeFactor operator*(const gtsam::DecisionTreeFactor& f) const; size_t cardinality(gtsam::Key j) const; gtsam::DecisionTreeFactor operator/(const gtsam::DecisionTreeFactor& f) const; @@ -248,6 +248,7 @@ class DiscreteBayesTree { void saveGraph(string s, const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; + double operator()(const gtsam::DiscreteValues& values) const; string markdown(const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; From a98ac0fdb232e8bf50b48df7008565010bfd0a4f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 9 Dec 2024 21:09:00 -0500 Subject: [PATCH 10/25] make evaluate the overriden method --- gtsam/discrete/DecisionTreeFactor.cpp | 2 +- gtsam/discrete/DecisionTreeFactor.h | 5 ++++- gtsam/discrete/DiscreteConditional.h | 4 ++-- gtsam/discrete/DiscreteFactor.h | 8 ++++---- gtsam/discrete/TableFactor.cpp | 2 +- gtsam/discrete/TableFactor.h | 2 +- gtsam_unstable/discrete/AllDiff.cpp | 2 +- gtsam_unstable/discrete/AllDiff.h | 2 +- gtsam_unstable/discrete/BinaryAllDiff.h | 2 +- gtsam_unstable/discrete/Domain.cpp | 2 +- gtsam_unstable/discrete/Domain.h | 2 +- gtsam_unstable/discrete/SingleValue.cpp | 2 +- gtsam_unstable/discrete/SingleValue.h | 2 +- 13 files changed, 20 insertions(+), 17 deletions(-) diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index 9ec3b0ac5..a57915e45 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -195,7 +195,7 @@ namespace gtsam { // Construct unordered_map with values std::vector> result; for (const auto& assignment : assignments) { - result.emplace_back(assignment, operator()(assignment)); + result.emplace_back(assignment, evaluate(assignment)); } return result; } diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index 8f3dd85a6..741715e43 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -132,10 +132,13 @@ namespace gtsam { /// Calculate probability for given values, /// is just look up in AlgebraicDecisionTree. - double operator()(const Assignment& values) const override { + virtual double evaluate(const Assignment& values) const override { return ADT::operator()(values); } + /// Disambiguate to use DiscreteFactor version. Mainly for wrapper + using DiscreteFactor::operator(); + /// Calculate error for DiscreteValues `x`, is -log(probability). double error(const DiscreteValues& values) const override; diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index f59e29285..858623301 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -169,12 +169,12 @@ class GTSAM_EXPORT DiscreteConditional } /// Evaluate, just look up in AlgebraicDecisionTree - double evaluate(const DiscreteValues& values) const { + virtual double evaluate(const Assignment& values) const override { return ADT::operator()(values); } using DecisionTreeFactor::error; ///< DiscreteValues version - using DecisionTreeFactor::operator(); ///< DiscreteValues version + using DiscreteFactor::operator(); ///< DiscreteValues version /** * @brief restrict to given *parent* values. diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index 5bc3c463f..2ba670004 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -101,12 +101,12 @@ class GTSAM_EXPORT DiscreteFactor : public Factor { * @param values Discrete assignment. * @return double */ - double evaluate(const Assignment& values) const { - return operator()(values); - } + virtual double evaluate(const Assignment& values) const = 0; /// Find value for given assignment of values to variables - virtual double operator()(const Assignment& values) const = 0; + double operator()(const DiscreteValues& values) const { + return evaluate(values); + } /// Error is just -log(value) virtual double error(const DiscreteValues& values) const; diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index 32cba84ed..ea51a996c 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -133,7 +133,7 @@ bool TableFactor::equals(const DiscreteFactor& other, double tol) const { } /* ************************************************************************ */ -double TableFactor::operator()(const Assignment& values) const { +double TableFactor::evaluate(const Assignment& values) const { // a b c d => D * (C * (B * (a) + b) + c) + d uint64_t idx = 0, card = 1; for (auto it = sorted_dkeys_.rbegin(); it != sorted_dkeys_.rend(); ++it) { diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h index 53495d078..1aecc1669 100644 --- a/gtsam/discrete/TableFactor.h +++ b/gtsam/discrete/TableFactor.h @@ -156,7 +156,7 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { // /// @{ /// Evaluate probability distribution, is just look up in TableFactor. - double operator()(const Assignment& values) const override; + double evaluate(const Assignment& values) const override; /// Calculate error for DiscreteValues `x`, is -log(probability). double error(const DiscreteValues& values) const override; diff --git a/gtsam_unstable/discrete/AllDiff.cpp b/gtsam_unstable/discrete/AllDiff.cpp index a450605b3..585ca8103 100644 --- a/gtsam_unstable/discrete/AllDiff.cpp +++ b/gtsam_unstable/discrete/AllDiff.cpp @@ -26,7 +26,7 @@ void AllDiff::print(const std::string& s, const KeyFormatter& formatter) const { } /* ************************************************************************* */ -double AllDiff::operator()(const Assignment& values) const { +double AllDiff::evaluate(const Assignment& values) const { std::set taken; // record values taken by keys for (Key dkey : keys_) { size_t value = values.at(dkey); // get the value for that key diff --git a/gtsam_unstable/discrete/AllDiff.h b/gtsam_unstable/discrete/AllDiff.h index 6fe43568e..1180abad4 100644 --- a/gtsam_unstable/discrete/AllDiff.h +++ b/gtsam_unstable/discrete/AllDiff.h @@ -45,7 +45,7 @@ class GTSAM_UNSTABLE_EXPORT AllDiff : public Constraint { } /// Calculate value = expensive ! - double operator()(const Assignment& values) const override; + double evaluate(const Assignment& values) const override; /// Convert into a decisiontree, can be *very* expensive ! DecisionTreeFactor toDecisionTreeFactor() const override; diff --git a/gtsam_unstable/discrete/BinaryAllDiff.h b/gtsam_unstable/discrete/BinaryAllDiff.h index a55865c77..e96bfdfde 100644 --- a/gtsam_unstable/discrete/BinaryAllDiff.h +++ b/gtsam_unstable/discrete/BinaryAllDiff.h @@ -47,7 +47,7 @@ class BinaryAllDiff : public Constraint { } /// Calculate value - double operator()(const Assignment& values) const override { + double evaluate(const Assignment& values) const override { return (double)(values.at(keys_[0]) != values.at(keys_[1])); } diff --git a/gtsam_unstable/discrete/Domain.cpp b/gtsam_unstable/discrete/Domain.cpp index 752228c18..74f621dc7 100644 --- a/gtsam_unstable/discrete/Domain.cpp +++ b/gtsam_unstable/discrete/Domain.cpp @@ -30,7 +30,7 @@ string Domain::base1Str() const { } /* ************************************************************************* */ -double Domain::operator()(const Assignment& values) const { +double Domain::evaluate(const Assignment& values) const { return contains(values.at(key())); } diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h index 13249d733..23a566d24 100644 --- a/gtsam_unstable/discrete/Domain.h +++ b/gtsam_unstable/discrete/Domain.h @@ -82,7 +82,7 @@ class GTSAM_UNSTABLE_EXPORT Domain : public Constraint { bool contains(size_t value) const { return values_.count(value) > 0; } /// Calculate value - double operator()(const Assignment& values) const override; + double evaluate(const Assignment& values) const override; /// Convert into a decisiontree DecisionTreeFactor toDecisionTreeFactor() const override; diff --git a/gtsam_unstable/discrete/SingleValue.cpp b/gtsam_unstable/discrete/SingleValue.cpp index 9762aec0f..220bc9c06 100644 --- a/gtsam_unstable/discrete/SingleValue.cpp +++ b/gtsam_unstable/discrete/SingleValue.cpp @@ -22,7 +22,7 @@ void SingleValue::print(const string& s, const KeyFormatter& formatter) const { } /* ************************************************************************* */ -double SingleValue::operator()(const Assignment& values) const { +double SingleValue::evaluate(const Assignment& values) const { return (double)(values.at(keys_[0]) == value_); } diff --git a/gtsam_unstable/discrete/SingleValue.h b/gtsam_unstable/discrete/SingleValue.h index 93fe38aaa..3df1209b8 100644 --- a/gtsam_unstable/discrete/SingleValue.h +++ b/gtsam_unstable/discrete/SingleValue.h @@ -55,7 +55,7 @@ class GTSAM_UNSTABLE_EXPORT SingleValue : public Constraint { } /// Calculate value - double operator()(const Assignment& values) const override; + double evaluate(const Assignment& values) const override; /// Convert into a decisiontree DecisionTreeFactor toDecisionTreeFactor() const override; From 8145086e5b57315bc94491def0bcdc67aa48148d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 9 Dec 2024 21:09:37 -0500 Subject: [PATCH 11/25] formatting --- gtsam/discrete/discrete.i | 4 ++-- python/gtsam/tests/test_DecisionTreeFactor.py | 3 ++- python/gtsam/tests/test_DiscreteBayesTree.py | 4 ++-- python/gtsam/tests/test_DiscreteConditional.py | 3 ++- python/gtsam/tests/test_DiscreteFactorGraph.py | 5 ++++- 5 files changed, 12 insertions(+), 7 deletions(-) diff --git a/gtsam/discrete/discrete.i b/gtsam/discrete/discrete.i index 55c8f9e22..b2e2524f8 100644 --- a/gtsam/discrete/discrete.i +++ b/gtsam/discrete/discrete.i @@ -61,14 +61,14 @@ virtual class DecisionTreeFactor : gtsam::DiscreteFactor { DecisionTreeFactor(const std::vector& keys, string table); DecisionTreeFactor(const gtsam::DiscreteConditional& c); - + void print(string s = "DecisionTreeFactor\n", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::DecisionTreeFactor& other, double tol = 1e-9) const; size_t cardinality(gtsam::Key j) const; - + double operator()(const gtsam::DiscreteValues& values) const; gtsam::DecisionTreeFactor operator*(const gtsam::DecisionTreeFactor& f) const; size_t cardinality(gtsam::Key j) const; diff --git a/python/gtsam/tests/test_DecisionTreeFactor.py b/python/gtsam/tests/test_DecisionTreeFactor.py index 12308bb3c..a78d9c94a 100644 --- a/python/gtsam/tests/test_DecisionTreeFactor.py +++ b/python/gtsam/tests/test_DecisionTreeFactor.py @@ -13,9 +13,10 @@ Author: Frank Dellaert import unittest +from gtsam.utils.test_case import GtsamTestCase + from gtsam import (DecisionTreeFactor, DiscreteDistribution, DiscreteValues, Ordering) -from gtsam.utils.test_case import GtsamTestCase class TestDecisionTreeFactor(GtsamTestCase): diff --git a/python/gtsam/tests/test_DiscreteBayesTree.py b/python/gtsam/tests/test_DiscreteBayesTree.py index 2a9b6ea09..e08491fab 100644 --- a/python/gtsam/tests/test_DiscreteBayesTree.py +++ b/python/gtsam/tests/test_DiscreteBayesTree.py @@ -19,8 +19,8 @@ from gtsam.utils.test_case import GtsamTestCase import gtsam from gtsam import (DiscreteBayesNet, DiscreteBayesTreeClique, - DiscreteConditional, DiscreteFactorGraph, - DiscreteValues, Ordering) + DiscreteConditional, DiscreteFactorGraph, DiscreteValues, + Ordering) class TestDiscreteBayesNet(GtsamTestCase): diff --git a/python/gtsam/tests/test_DiscreteConditional.py b/python/gtsam/tests/test_DiscreteConditional.py index 241a5f0be..6c9eb9aec 100644 --- a/python/gtsam/tests/test_DiscreteConditional.py +++ b/python/gtsam/tests/test_DiscreteConditional.py @@ -13,9 +13,10 @@ Author: Varun Agrawal import unittest -from gtsam import DecisionTreeFactor, DiscreteConditional, DiscreteKeys from gtsam.utils.test_case import GtsamTestCase +from gtsam import DecisionTreeFactor, DiscreteConditional, DiscreteKeys + # Some DiscreteKeys for binary variables: A = 0, 2 B = 1, 2 diff --git a/python/gtsam/tests/test_DiscreteFactorGraph.py b/python/gtsam/tests/test_DiscreteFactorGraph.py index d725ceac8..3053087b4 100644 --- a/python/gtsam/tests/test_DiscreteFactorGraph.py +++ b/python/gtsam/tests/test_DiscreteFactorGraph.py @@ -14,9 +14,12 @@ Author: Frank Dellaert import unittest import numpy as np -from gtsam import DecisionTreeFactor, DiscreteConditional, DiscreteFactorGraph, DiscreteKeys, DiscreteValues, Ordering, Symbol from gtsam.utils.test_case import GtsamTestCase +from gtsam import (DecisionTreeFactor, DiscreteConditional, + DiscreteFactorGraph, DiscreteKeys, DiscreteValues, Ordering, + Symbol) + OrderingType = Ordering.OrderingType From 80b1fe569a39b77986d38ccd127c99b0fdc5f395 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 10 Dec 2024 10:41:36 -0500 Subject: [PATCH 12/25] use product method since it has a nullptr check --- gtsam/discrete/DiscreteFactorGraph.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index 4ededbb8b..523b99201 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -66,8 +66,9 @@ namespace gtsam { /* ************************************************************************* */ DecisionTreeFactor DiscreteFactorGraph::product() const { DecisionTreeFactor result; - for(const sharedFactor& factor: *this) + for (const sharedFactor& factor : *this) { if (factor) result = (*factor) * result; + } return result; } @@ -75,8 +76,9 @@ namespace gtsam { double DiscreteFactorGraph::operator()( const DiscreteValues &values) const { double product = 1.0; - for( const sharedFactor& factor: factors_ ) + for (const sharedFactor& factor : factors_) { product *= (*factor)(values); + } return product; } @@ -117,8 +119,7 @@ namespace gtsam { const Ordering& frontalKeys) { // PRODUCT: multiply all factors gttic(product); - DecisionTreeFactor product; - for (auto&& factor : factors) product = (*factor) * product; + DecisionTreeFactor product = factors.product(); gttoc(product); // Max over all the potentials by pretending all keys are frontal: @@ -206,8 +207,7 @@ namespace gtsam { const Ordering& frontalKeys) { // PRODUCT: multiply all factors gttic(product); - DecisionTreeFactor product; - for (auto&& factor : factors) product = (*factor) * product; + DecisionTreeFactor product = factors.product(); gttoc(product); // Max over all the potentials by pretending all keys are frontal: From 92e0a55e7817d7507ebc098df85e1e8d7cbfca5f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 10 Dec 2024 10:41:57 -0500 Subject: [PATCH 13/25] generalize discreteKeys method --- gtsam/discrete/DiscreteFactorGraph.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index 523b99201..96fdfc338 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -54,7 +54,7 @@ namespace gtsam { DiscreteKeys DiscreteFactorGraph::discreteKeys() const { DiscreteKeys result; for (auto&& factor : *this) { - if (auto p = std::dynamic_pointer_cast(factor)) { + if (auto p = std::dynamic_pointer_cast(factor)) { DiscreteKeys factor_keys = p->discreteKeys(); result.insert(result.end(), factor_keys.begin(), factor_keys.end()); } From dea9c7f7655576d7629903e38fc357e81898ec82 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 10 Dec 2024 10:49:08 -0500 Subject: [PATCH 14/25] common function for product and normalization --- gtsam/discrete/DiscreteFactorGraph.cpp | 32 ++++++++++++++------------ 1 file changed, 17 insertions(+), 15 deletions(-) diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index 96fdfc338..436d784f7 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -112,11 +112,14 @@ namespace gtsam { // } // } - /* ************************************************************************ */ - // Alternate eliminate function for MPE - std::pair // - EliminateForMPE(const DiscreteFactorGraph& factors, - const Ordering& frontalKeys) { + /** + * @brief Multiply all the `factors` and normalize the + * product to prevent underflow. + * + * @param factors The factors to multiply as a DiscreteFactorGraph. + * @return DecisionTreeFactor + */ + static DecisionTreeFactor ProductAndNormalize(const DiscreteFactorGraph& factors) { // PRODUCT: multiply all factors gttic(product); DecisionTreeFactor product = factors.product(); @@ -127,6 +130,14 @@ namespace gtsam { // Normalize the product factor to prevent underflow. product = product / (*normalization); + } + + /* ************************************************************************ */ + // Alternate eliminate function for MPE + std::pair // + EliminateForMPE(const DiscreteFactorGraph& factors, + const Ordering& frontalKeys) { + DecisionTreeFactor product = ProductAndNormalize(factors); // max out frontals, this is the factor on the separator gttic(max); @@ -205,16 +216,7 @@ namespace gtsam { std::pair // EliminateDiscrete(const DiscreteFactorGraph& factors, const Ordering& frontalKeys) { - // PRODUCT: multiply all factors - gttic(product); - DecisionTreeFactor product = factors.product(); - gttoc(product); - - // Max over all the potentials by pretending all keys are frontal: - auto normalization = product.max(product.size()); - - // Normalize the product factor to prevent underflow. - product = product / (*normalization); + DecisionTreeFactor product = ProductAndNormalize(factors); // sum out frontals, this is the factor on the separator gttic(sum); From 590293bb92f53291a0a77837dfc51314e34c641a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 10 Dec 2024 10:52:15 -0500 Subject: [PATCH 15/25] return tau factor as DiscreteFactor for discrete elimination --- gtsam/discrete/DiscreteFactorGraph.cpp | 5 +++-- gtsam/discrete/DiscreteFactorGraph.h | 5 +++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index 436d784f7..508a5f0f8 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -14,6 +14,7 @@ * @date Feb 14, 2011 * @author Duy-Nguyen Ta * @author Frank Dellaert + * @author Varun Agrawal */ #include @@ -134,7 +135,7 @@ namespace gtsam { /* ************************************************************************ */ // Alternate eliminate function for MPE - std::pair // + std::pair // EliminateForMPE(const DiscreteFactorGraph& factors, const Ordering& frontalKeys) { DecisionTreeFactor product = ProductAndNormalize(factors); @@ -213,7 +214,7 @@ namespace gtsam { } /* ************************************************************************ */ - std::pair // + std::pair // EliminateDiscrete(const DiscreteFactorGraph& factors, const Ordering& frontalKeys) { DecisionTreeFactor product = ProductAndNormalize(factors); diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index d0dc282b4..c57d2258c 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -14,6 +14,7 @@ * @date Feb 14, 2011 * @author Duy-Nguyen Ta * @author Frank Dellaert + * @author Varun Agrawal */ #pragma once @@ -48,7 +49,7 @@ class DiscreteJunctionTree; * @ingroup discrete */ GTSAM_EXPORT -std::pair +std::pair EliminateDiscrete(const DiscreteFactorGraph& factors, const Ordering& frontalKeys); @@ -61,7 +62,7 @@ EliminateDiscrete(const DiscreteFactorGraph& factors, * @ingroup discrete */ GTSAM_EXPORT -std::pair +std::pair EliminateForMPE(const DiscreteFactorGraph& factors, const Ordering& frontalKeys); From 162f61061cbbe02298250c759ff84387dd065039 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 10 Dec 2024 10:52:36 -0500 Subject: [PATCH 16/25] use BaseFactor methods to reduce code in DiscreteConditional --- gtsam/discrete/DiscreteConditional.h | 10 +++------- gtsam/discrete/DiscreteDistribution.h | 2 +- 2 files changed, 4 insertions(+), 8 deletions(-) diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index 858623301..8cba6dbe7 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -168,13 +168,9 @@ class GTSAM_EXPORT DiscreteConditional static_cast(this)->print(s, formatter); } - /// Evaluate, just look up in AlgebraicDecisionTree - virtual double evaluate(const Assignment& values) const override { - return ADT::operator()(values); - } - - using DecisionTreeFactor::error; ///< DiscreteValues version - using DiscreteFactor::operator(); ///< DiscreteValues version + using BaseFactor::error; ///< DiscreteValues version + using BaseFactor::evaluate; ///< DiscreteValues version + using BaseFactor::operator(); ///< DiscreteValues version /** * @brief restrict to given *parent* values. diff --git a/gtsam/discrete/DiscreteDistribution.h b/gtsam/discrete/DiscreteDistribution.h index 4b690da15..09ea50332 100644 --- a/gtsam/discrete/DiscreteDistribution.h +++ b/gtsam/discrete/DiscreteDistribution.h @@ -40,7 +40,7 @@ class GTSAM_EXPORT DiscreteDistribution : public DiscreteConditional { /// Default constructor needed for serialization. DiscreteDistribution() {} - /// Constructor from factor. + /// Constructor from DecisionTreeFactor. explicit DiscreteDistribution(const DecisionTreeFactor& f) : Base(f.size(), f) {} From 372b133366626d03d5319843fad08e77909156db Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 10 Dec 2024 11:05:50 -0500 Subject: [PATCH 17/25] formatting --- gtsam/discrete/DiscreteFactorGraph.cpp | 27 +++++++++++++------------- 1 file changed, 13 insertions(+), 14 deletions(-) diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index 508a5f0f8..ebba93382 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -36,13 +36,12 @@ namespace gtsam { template class FactorGraph; template class EliminateableFactorGraph; - /* ************************************************************************* */ - bool DiscreteFactorGraph::equals(const This& fg, double tol) const - { + /* ************************************************************************ */ + bool DiscreteFactorGraph::equals(const This& fg, double tol) const { return Base::equals(fg, tol); } - /* ************************************************************************* */ + /* ************************************************************************ */ KeySet DiscreteFactorGraph::keys() const { KeySet keys; for (const sharedFactor& factor : *this) { @@ -51,7 +50,7 @@ namespace gtsam { return keys; } - /* ************************************************************************* */ + /* ************************************************************************ */ DiscreteKeys DiscreteFactorGraph::discreteKeys() const { DiscreteKeys result; for (auto&& factor : *this) { @@ -64,7 +63,7 @@ namespace gtsam { return result; } - /* ************************************************************************* */ + /* ************************************************************************ */ DecisionTreeFactor DiscreteFactorGraph::product() const { DecisionTreeFactor result; for (const sharedFactor& factor : *this) { @@ -73,9 +72,8 @@ namespace gtsam { return result; } - /* ************************************************************************* */ - double DiscreteFactorGraph::operator()( - const DiscreteValues &values) const { + /* ************************************************************************ */ + double DiscreteFactorGraph::operator()(const DiscreteValues& values) const { double product = 1.0; for (const sharedFactor& factor : factors_) { product *= (*factor)(values); @@ -83,9 +81,9 @@ namespace gtsam { return product; } - /* ************************************************************************* */ + /* ************************************************************************ */ void DiscreteFactorGraph::print(const string& s, - const KeyFormatter& formatter) const { + const KeyFormatter& formatter) const { std::cout << s << std::endl; std::cout << "size: " << size() << std::endl; for (size_t i = 0; i < factors_.size(); i++) { @@ -120,7 +118,8 @@ namespace gtsam { * @param factors The factors to multiply as a DiscreteFactorGraph. * @return DecisionTreeFactor */ - static DecisionTreeFactor ProductAndNormalize(const DiscreteFactorGraph& factors) { + static DecisionTreeFactor ProductAndNormalize( + const DiscreteFactorGraph& factors) { // PRODUCT: multiply all factors gttic(product); DecisionTreeFactor product = factors.product(); @@ -155,8 +154,8 @@ namespace gtsam { // Make lookup with product gttic(lookup); size_t nrFrontals = frontalKeys.size(); - auto lookup = std::make_shared(nrFrontals, - orderedKeys, product); + auto lookup = + std::make_shared(nrFrontals, orderedKeys, product); gttoc(lookup); return {std::dynamic_pointer_cast(lookup), max}; From 62008fc99543c8c87e6a38bea11ff1592b1aac16 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 10 Dec 2024 11:10:25 -0500 Subject: [PATCH 18/25] add return --- gtsam/discrete/DiscreteFactorGraph.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index ebba93382..ec6dac2fc 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -130,6 +130,8 @@ namespace gtsam { // Normalize the product factor to prevent underflow. product = product / (*normalization); + + return product; } /* ************************************************************************ */ From 2fd60a47a231b6cbebc6b916d213e64d379b39fc Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 10 Dec 2024 11:54:05 -0500 Subject: [PATCH 19/25] fix test --- gtsam/discrete/tests/testDiscreteFactorGraph.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp index 341eb63e3..0d71c12ba 100644 --- a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp +++ b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp @@ -113,7 +113,8 @@ TEST(DiscreteFactorGraph, test) { const Ordering frontalKeys{0}; const auto [conditional, newFactorPtr] = EliminateDiscrete(graph, frontalKeys); - DecisionTreeFactor newFactor = *newFactorPtr; + DecisionTreeFactor newFactor = + *std::dynamic_pointer_cast(newFactorPtr); // Normalize newFactor by max for comparison with expected auto normalization = newFactor.max(newFactor.size()); From 588533751b07d69a1940241ba6a144eb30ae4938 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 10 Dec 2024 14:10:33 -0500 Subject: [PATCH 20/25] add another pointer check --- gtsam/discrete/DiscreteFactorGraph.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index ec6dac2fc..d0bf21047 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -76,7 +76,7 @@ namespace gtsam { double DiscreteFactorGraph::operator()(const DiscreteValues& values) const { double product = 1.0; for (const sharedFactor& factor : factors_) { - product *= (*factor)(values); + if (factor) product *= (*factor)(values); } return product; } From 8ce55a221001d931a41cccfd013b9adc95dad5d9 Mon Sep 17 00:00:00 2001 From: Jade Turner Date: Thu, 12 Dec 2024 00:16:03 +0800 Subject: [PATCH 21/25] document flags Signed-off-by: Jade Turner --- INSTALL.md | 20 +++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/INSTALL.md b/INSTALL.md index 10bee196c..51f00fd3b 100644 --- a/INSTALL.md +++ b/INSTALL.md @@ -27,7 +27,7 @@ $ make install downloaded from https://www.threadingbuildingblocks.org/ - GTSAM may be configured to use MKL by toggling `GTSAM_WITH_EIGEN_MKL` and `GTSAM_WITH_EIGEN_MKL_OPENMP` to `ON`; however, best performance is usually - achieved with MKL disabled. We therefore advise you to benchmark your problem + achieved with MKL disabled. We therefore advise you to benchmark your problem before using MKL. Tested compilers: @@ -128,12 +128,12 @@ We support several build configurations for GTSAM (case insensitive) #### CMAKE_INSTALL_PREFIX -The install folder. The default is typically `/usr/local/`. +The install folder. The default is typically `/usr/local/`. To configure to install to your home directory, you could execute: ```cmake -DCMAKE_INSTALL_PREFIX:PATH=$HOME ..``` -#### GTSAM_TOOLBOX_INSTALL_PATH +#### GTSAM_TOOLBOX_INSTALL_PATH The Matlab toolbox will be installed in a subdirectory of this folder, called 'gtsam'. @@ -142,7 +142,7 @@ of this folder, called 'gtsam'. #### GTSAM_BUILD_CONVENIENCE_LIBRARIES -This is a build option to allow for tests in subfolders to be linked against convenience libraries rather than the full libgtsam. +This is a build option to allow for tests in subfolders to be linked against convenience libraries rather than the full libgtsam. Set with the command line as follows: ```cmake -DGTSAM_BUILD_CONVENIENCE_LIBRARIES:OPTION=ON ..``` @@ -159,6 +159,16 @@ Set with the command line as follows: ON: When enabled, libgtsam_unstable will be built and installed with the same options as libgtsam. In addition, if tests are enabled, the unit tests will be built as well. The Matlab toolbox will also be generated if the matlab toolbox is enabled, installing into a folder called `gtsam_unstable`. OFF (Default) If disabled, no `gtsam_unstable` code will be included in build or install. +## Convenience Options: + +#### GTSAM_BUILD_EXAMPLES_ALWAYS + +Whether or not to force building examples, can be true or false. + +#### GTSAM_BUILD_TESTS + +Whether or not to build tests, can be true or false. + ## Check `make check` will build and run all of the tests. Note that the tests will only be @@ -179,7 +189,7 @@ Here are some tips to get the best possible performance out of GTSAM. 1. Build in `Release` mode. GTSAM will run up to 10x faster compared to `Debug` mode. 2. Enable TBB. On modern processors with multiple cores, this can easily speed up - optimization by 30-50%. Please note that this may not be true for very small + optimization by 30-50%. Please note that this may not be true for very small problems where the overhead of dispatching work to multiple threads outweighs the benefit. We recommend that you benchmark your problem with/without TBB. 3. Use `GTSAM_BUILD_WITH_MARCH_NATIVE`. A performance gain of From ed098eaec64d185fbf91923b89e9442383b18979 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 4 Dec 2024 11:43:51 -0500 Subject: [PATCH 22/25] Cleanup some includes and fix cassert --- examples/CombinedImuFactorsExample.cpp | 1 + examples/ImuFactorsExample.cpp | 1 + examples/elaboratePoint2KalmanFilter.cpp | 2 ++ gtsam/base/Matrix.cpp | 1 + gtsam/base/Vector.cpp | 1 + gtsam/base/VerticalBlockMatrix.h | 2 ++ gtsam/base/cholesky.cpp | 1 + gtsam/base/tests/testNumericalDerivative.cpp | 2 ++ gtsam/basis/Chebyshev2.h | 2 ++ gtsam/discrete/DiscreteConditional.cpp | 1 + gtsam/discrete/DiscreteLookupDAG.cpp | 1 + gtsam/geometry/Cal3f.cpp | 1 + gtsam/geometry/CameraSet.h | 1 + gtsam/geometry/Pose2.cpp | 1 + gtsam/geometry/Pose2.h | 1 + gtsam/geometry/Rot3.cpp | 1 + gtsam/geometry/Rot3.h | 1 + gtsam/geometry/SOn.h | 1 + gtsam/geometry/triangulation.h | 3 ++- gtsam/hybrid/HybridJunctionTree.cpp | 1 + gtsam/inference/BayesTree-inst.h | 2 +- gtsam/inference/ClusterTree-inst.h | 1 + gtsam/inference/EliminationTree-inst.h | 1 + gtsam/inference/JunctionTree-inst.h | 2 ++ gtsam/inference/MetisIndex.h | 1 + gtsam/inference/Ordering.cpp | 1 + gtsam/linear/HessianFactor.cpp | 3 ++- gtsam/linear/HessianFactor.h | 1 + gtsam/linear/JacobianFactor.cpp | 1 + gtsam/linear/KalmanFilter.cpp | 3 +++ gtsam/linear/NoiseModel.cpp | 1 + gtsam/linear/Sampler.cpp | 3 +++ gtsam/linear/SubgraphPreconditioner.cpp | 1 + gtsam/navigation/ImuFactor.cpp | 1 + gtsam/navigation/PreintegrationBase.cpp | 4 +++- gtsam/nonlinear/DoglegOptimizerImpl.cpp | 1 + gtsam/nonlinear/DoglegOptimizerImpl.h | 1 + gtsam/nonlinear/Expression-inl.h | 1 + gtsam/nonlinear/ExtendedKalmanFilter-inl.h | 2 ++ gtsam/nonlinear/ISAM2-impl.cpp | 1 + gtsam/nonlinear/ISAM2-impl.h | 1 + gtsam/nonlinear/ISAM2.cpp | 1 + gtsam/nonlinear/ISAM2Clique.cpp | 1 + gtsam/nonlinear/NonlinearFactor.cpp | 2 ++ gtsam/nonlinear/Values.cpp | 1 + gtsam/nonlinear/internal/ExpressionNode.h | 1 + gtsam/sfm/ShonanAveraging.cpp | 1 + gtsam/slam/EssentialMatrixFactor.h | 1 + gtsam/slam/SmartFactorBase.h | 1 + gtsam_unstable/discrete/Scheduler.cpp | 1 + gtsam_unstable/dynamics/DynamicsPriors.h | 2 ++ gtsam_unstable/dynamics/FullIMUFactor.h | 2 ++ gtsam_unstable/dynamics/IMUFactor.h | 2 ++ gtsam_unstable/dynamics/PoseRTV.cpp | 2 ++ gtsam_unstable/dynamics/VelocityConstraint.h | 2 ++ gtsam_unstable/geometry/BearingS2.cpp | 2 ++ gtsam_unstable/geometry/Pose3Upright.cpp | 5 +++-- gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp | 2 ++ gtsam_unstable/nonlinear/LinearizedFactor.cpp | 1 + gtsam_unstable/partition/FindSeparator-inl.h | 1 + gtsam_unstable/slam/PartialPriorFactor.h | 2 ++ gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp | 2 ++ gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp | 2 ++ tests/testGaussianISAM2.cpp | 1 + 64 files changed, 91 insertions(+), 6 deletions(-) diff --git a/examples/CombinedImuFactorsExample.cpp b/examples/CombinedImuFactorsExample.cpp index 6ce4f76fa..436b6e283 100644 --- a/examples/CombinedImuFactorsExample.cpp +++ b/examples/CombinedImuFactorsExample.cpp @@ -48,6 +48,7 @@ #include #include #include +#include using namespace gtsam; using namespace std; diff --git a/examples/ImuFactorsExample.cpp b/examples/ImuFactorsExample.cpp index 342f1f220..7cbc8989f 100644 --- a/examples/ImuFactorsExample.cpp +++ b/examples/ImuFactorsExample.cpp @@ -49,6 +49,7 @@ #include #include +#include #include #include diff --git a/examples/elaboratePoint2KalmanFilter.cpp b/examples/elaboratePoint2KalmanFilter.cpp index a2ad8cf0b..bc4e6e340 100644 --- a/examples/elaboratePoint2KalmanFilter.cpp +++ b/examples/elaboratePoint2KalmanFilter.cpp @@ -29,6 +29,8 @@ #include #include +#include + using namespace std; using namespace gtsam; diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 247c53cce..1e04bc58a 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -24,6 +24,7 @@ #include #include +#include #include #include #include diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index ce7aeba7b..dbd48dfc8 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include diff --git a/gtsam/base/VerticalBlockMatrix.h b/gtsam/base/VerticalBlockMatrix.h index 777441300..c8fef76e9 100644 --- a/gtsam/base/VerticalBlockMatrix.h +++ b/gtsam/base/VerticalBlockMatrix.h @@ -21,6 +21,8 @@ #include #include +#include + namespace gtsam { // Forward declarations diff --git a/gtsam/base/cholesky.cpp b/gtsam/base/cholesky.cpp index 71cef2524..9fee627fe 100644 --- a/gtsam/base/cholesky.cpp +++ b/gtsam/base/cholesky.cpp @@ -21,6 +21,7 @@ #include #include +#include using namespace std; diff --git a/gtsam/base/tests/testNumericalDerivative.cpp b/gtsam/base/tests/testNumericalDerivative.cpp index e8838a476..b14f699a3 100644 --- a/gtsam/base/tests/testNumericalDerivative.cpp +++ b/gtsam/base/tests/testNumericalDerivative.cpp @@ -18,6 +18,8 @@ #include #include +#include + using namespace std; using namespace gtsam; diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h index 849a51104..1374faeef 100644 --- a/gtsam/basis/Chebyshev2.h +++ b/gtsam/basis/Chebyshev2.h @@ -36,6 +36,8 @@ #include #include +#include + namespace gtsam { /** diff --git a/gtsam/discrete/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index c44905d79..eeb5dca3f 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -30,6 +30,7 @@ #include #include #include +#include using namespace std; using std::pair; diff --git a/gtsam/discrete/DiscreteLookupDAG.cpp b/gtsam/discrete/DiscreteLookupDAG.cpp index 4d02e007b..d1108e7b7 100644 --- a/gtsam/discrete/DiscreteLookupDAG.cpp +++ b/gtsam/discrete/DiscreteLookupDAG.cpp @@ -23,6 +23,7 @@ #include #include #include +#include using std::pair; using std::vector; diff --git a/gtsam/geometry/Cal3f.cpp b/gtsam/geometry/Cal3f.cpp index 319155dc9..b8e365eed 100644 --- a/gtsam/geometry/Cal3f.cpp +++ b/gtsam/geometry/Cal3f.cpp @@ -28,6 +28,7 @@ #include #include +#include namespace gtsam { diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 26d4952c8..e3c16f066 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -26,6 +26,7 @@ #include #include +#include namespace gtsam { diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 05678dc27..640481406 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index aad81493f..d9cad37f3 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -28,6 +28,7 @@ #include #include +#include namespace gtsam { diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index a936bd02a..10ba5894e 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -23,6 +23,7 @@ #include #include +#include #include using namespace std; diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 7e05ee4da..398e8d473 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -30,6 +30,7 @@ #include // Get GTSAM_USE_QUATERNIONS macro #include +#include // You can override the default coordinate mode using this flag #ifndef ROT3_DEFAULT_COORDINATES_MODE diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index 7e1762b3f..75ff872a8 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -33,6 +33,7 @@ #include #include #include +#include namespace gtsam { diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index d82da3459..0fa9d074e 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -20,7 +20,7 @@ #pragma once -#include "gtsam/geometry/Point3.h" +#include #include #include #include @@ -35,6 +35,7 @@ #include #include +#include namespace gtsam { diff --git a/gtsam/hybrid/HybridJunctionTree.cpp b/gtsam/hybrid/HybridJunctionTree.cpp index 22d3c7dd2..b9bdcbed2 100644 --- a/gtsam/hybrid/HybridJunctionTree.cpp +++ b/gtsam/hybrid/HybridJunctionTree.cpp @@ -22,6 +22,7 @@ #include #include +#include namespace gtsam { diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index 95d475a02..fdfa94d2b 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -27,7 +27,7 @@ #include #include - +#include namespace gtsam { /* ************************************************************************* */ diff --git a/gtsam/inference/ClusterTree-inst.h b/gtsam/inference/ClusterTree-inst.h index 318624608..2a5f0b455 100644 --- a/gtsam/inference/ClusterTree-inst.h +++ b/gtsam/inference/ClusterTree-inst.h @@ -19,6 +19,7 @@ #include #endif #include +#include namespace gtsam { diff --git a/gtsam/inference/EliminationTree-inst.h b/gtsam/inference/EliminationTree-inst.h index e9acc12e1..16449d0f4 100644 --- a/gtsam/inference/EliminationTree-inst.h +++ b/gtsam/inference/EliminationTree-inst.h @@ -19,6 +19,7 @@ #include #include +#include #include #include diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h index 0f79c2a9a..6a8d6918a 100644 --- a/gtsam/inference/JunctionTree-inst.h +++ b/gtsam/inference/JunctionTree-inst.h @@ -25,6 +25,8 @@ #include #include +#include + namespace gtsam { template diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h index abdf11c5f..cc058dfbd 100644 --- a/gtsam/inference/MetisIndex.h +++ b/gtsam/inference/MetisIndex.h @@ -25,6 +25,7 @@ #include #include #include +#include namespace gtsam { /** diff --git a/gtsam/inference/Ordering.cpp b/gtsam/inference/Ordering.cpp index cb2ca752d..95eb17a1c 100644 --- a/gtsam/inference/Ordering.cpp +++ b/gtsam/inference/Ordering.cpp @@ -18,6 +18,7 @@ #include #include +#include #include #include diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 3c3050f90..4e8ef804c 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -25,13 +25,14 @@ #include #include #include +#include #include #include #include #include +#include #include -#include "gtsam/base/Vector.h" using namespace std; diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 8cbabcd5d..53b6d0e8b 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -23,6 +23,7 @@ #include #include +#include namespace gtsam { diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 3cfb5ce7b..51d513e33 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -32,6 +32,7 @@ #include #include +#include #include #include diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp index ded6bc5e3..6291c90b6 100644 --- a/gtsam/linear/KalmanFilter.cpp +++ b/gtsam/linear/KalmanFilter.cpp @@ -26,6 +26,9 @@ #include #include +#ifndef NDEBUG +#include +#endif using namespace std; diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 884c87270..39666b46b 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include #include diff --git a/gtsam/linear/Sampler.cpp b/gtsam/linear/Sampler.cpp index 20d4c955b..68f21d723 100644 --- a/gtsam/linear/Sampler.cpp +++ b/gtsam/linear/Sampler.cpp @@ -17,6 +17,9 @@ */ #include + +#include + namespace gtsam { /* ************************************************************************* */ diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index 96f4847b5..53ea94d6e 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -25,6 +25,7 @@ #include #include +#include using std::cout; using std::endl; diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 88e45d318..5697d54d4 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -23,6 +23,7 @@ /* External or standard includes */ #include +#include namespace gtsam { diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 7f998987b..dd86b829c 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -20,9 +20,11 @@ * @author Varun Agrawal **/ -#include "PreintegrationBase.h" +#include #include +#include + using namespace std; namespace gtsam { diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.cpp b/gtsam/nonlinear/DoglegOptimizerImpl.cpp index 4f1c6fb54..e82a513ca 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.cpp +++ b/gtsam/nonlinear/DoglegOptimizerImpl.cpp @@ -16,6 +16,7 @@ */ #include +#include #include using namespace std; diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h index 8ce9b361e..1038dd522 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.h +++ b/gtsam/nonlinear/DoglegOptimizerImpl.h @@ -17,6 +17,7 @@ #pragma once #include +#include #include #include diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 71ef1d840..b09bcb3a5 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -26,6 +26,7 @@ #include #include #include +#include namespace gtsam { diff --git a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h index 5bf643c12..072f40b58 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h @@ -23,6 +23,8 @@ #include #include +#include + namespace gtsam { /* ************************************************************************* */ diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 20874e2dc..d0b27f3f3 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -25,6 +25,7 @@ #include #include #include +#include using namespace std; diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index e9a9696eb..bb0210237 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -33,6 +33,7 @@ #include #include #include +#include namespace gtsam { diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 47857a2a2..0eb2deef5 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -29,6 +29,7 @@ #include #include #include +#include using namespace std; diff --git a/gtsam/nonlinear/ISAM2Clique.cpp b/gtsam/nonlinear/ISAM2Clique.cpp index d4b050f84..64ab7c66e 100644 --- a/gtsam/nonlinear/ISAM2Clique.cpp +++ b/gtsam/nonlinear/ISAM2Clique.cpp @@ -22,6 +22,7 @@ #include #include +#include using namespace std; diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp index 7312bf6d9..5eabfac91 100644 --- a/gtsam/nonlinear/NonlinearFactor.cpp +++ b/gtsam/nonlinear/NonlinearFactor.cpp @@ -19,6 +19,8 @@ #include #include +#include + namespace gtsam { /* ************************************************************************* */ diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index 6af9f2b6a..1caaa9db1 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -28,6 +28,7 @@ #include #include #include +#include using namespace std; diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h index de0c9721e..25587f511 100644 --- a/gtsam/nonlinear/internal/ExpressionNode.h +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -26,6 +26,7 @@ #include // operator typeid #include #include +#include class ExpressionFactorBinaryTest; // Forward declare for testing diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 830f1c719..6c9f91e90 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -37,6 +37,7 @@ #include #include #include +#include namespace gtsam { diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 5fdf138cc..3c2f105d4 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -25,6 +25,7 @@ #include #include +#include namespace gtsam { diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 5d6ec4445..b8e789aaa 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -34,6 +34,7 @@ #include #endif #include +#include namespace gtsam { diff --git a/gtsam_unstable/discrete/Scheduler.cpp b/gtsam_unstable/discrete/Scheduler.cpp index 0de4d32c4..53be25d55 100644 --- a/gtsam_unstable/discrete/Scheduler.cpp +++ b/gtsam_unstable/discrete/Scheduler.cpp @@ -14,6 +14,7 @@ #include #include #include +#include namespace gtsam { diff --git a/gtsam_unstable/dynamics/DynamicsPriors.h b/gtsam_unstable/dynamics/DynamicsPriors.h index d4ebcba19..2dc0d552b 100644 --- a/gtsam_unstable/dynamics/DynamicsPriors.h +++ b/gtsam_unstable/dynamics/DynamicsPriors.h @@ -12,6 +12,8 @@ #include #include +#include + namespace gtsam { // Indices of relevant variables in the PoseRTV tangent vector: diff --git a/gtsam_unstable/dynamics/FullIMUFactor.h b/gtsam_unstable/dynamics/FullIMUFactor.h index 02da899b7..eea019cd2 100644 --- a/gtsam_unstable/dynamics/FullIMUFactor.h +++ b/gtsam_unstable/dynamics/FullIMUFactor.h @@ -10,6 +10,8 @@ #include #include +#include + namespace gtsam { /** diff --git a/gtsam_unstable/dynamics/IMUFactor.h b/gtsam_unstable/dynamics/IMUFactor.h index ee09600e5..241b6b497 100644 --- a/gtsam_unstable/dynamics/IMUFactor.h +++ b/gtsam_unstable/dynamics/IMUFactor.h @@ -10,6 +10,8 @@ #include #include +#include + namespace gtsam { /** diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index cf21c315b..9457c99e8 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -7,6 +7,8 @@ #include #include +#include + namespace gtsam { using namespace std; diff --git a/gtsam_unstable/dynamics/VelocityConstraint.h b/gtsam_unstable/dynamics/VelocityConstraint.h index 2bb70e1b5..dbdd27259 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint.h +++ b/gtsam_unstable/dynamics/VelocityConstraint.h @@ -10,6 +10,8 @@ #include #include +#include + namespace gtsam { namespace dynamics { diff --git a/gtsam_unstable/geometry/BearingS2.cpp b/gtsam_unstable/geometry/BearingS2.cpp index 9dfed612c..23afd158b 100644 --- a/gtsam_unstable/geometry/BearingS2.cpp +++ b/gtsam_unstable/geometry/BearingS2.cpp @@ -9,6 +9,8 @@ #include +#include + namespace gtsam { using namespace std; diff --git a/gtsam_unstable/geometry/Pose3Upright.cpp b/gtsam_unstable/geometry/Pose3Upright.cpp index 9cd9f78f5..c5ef0d70f 100644 --- a/gtsam_unstable/geometry/Pose3Upright.cpp +++ b/gtsam_unstable/geometry/Pose3Upright.cpp @@ -6,8 +6,9 @@ */ #include -#include "gtsam/base/OptionalJacobian.h" -#include "gtsam/base/Vector.h" +#include +#include +#include #include diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp index 36cb6165f..8dfb15aa2 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp @@ -21,6 +21,8 @@ #include #include +#include + namespace gtsam { /* ************************************************************************* */ diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.cpp b/gtsam_unstable/nonlinear/LinearizedFactor.cpp index 2d404005d..375c49341 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.cpp +++ b/gtsam_unstable/nonlinear/LinearizedFactor.cpp @@ -17,6 +17,7 @@ #include #include +#include namespace gtsam { diff --git a/gtsam_unstable/partition/FindSeparator-inl.h b/gtsam_unstable/partition/FindSeparator-inl.h index f4718f7a9..21ed91604 100644 --- a/gtsam_unstable/partition/FindSeparator-inl.h +++ b/gtsam_unstable/partition/FindSeparator-inl.h @@ -14,6 +14,7 @@ #include #include #include +#include #include #include diff --git a/gtsam_unstable/slam/PartialPriorFactor.h b/gtsam_unstable/slam/PartialPriorFactor.h index 7722e5d82..0a735a2e1 100644 --- a/gtsam_unstable/slam/PartialPriorFactor.h +++ b/gtsam_unstable/slam/PartialPriorFactor.h @@ -20,6 +20,8 @@ #include #include +#include + namespace gtsam { /** diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp index c4a473154..6d78bdb7a 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp +++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp @@ -18,6 +18,8 @@ #include +#include + namespace gtsam { SmartStereoProjectionFactorPP::SmartStereoProjectionFactorPP( diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp index 57913385a..52129daa8 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp @@ -21,6 +21,8 @@ #include +#include + namespace gtsam { SmartStereoProjectionPoseFactor::SmartStereoProjectionPoseFactor( diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 721ed51c0..3b507d806 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -25,6 +25,7 @@ #include +#include using namespace std; using namespace gtsam; From 2d0672059cc26ec3dd9ea58ba462f2b087eca25c Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 10 Dec 2024 13:41:19 -0500 Subject: [PATCH 23/25] Update some header includes --- gtsam/basis/Chebyshev2.cpp | 8 ++++++++ gtsam/basis/Chebyshev2.h | 10 +--------- gtsam/geometry/Pose2.h | 11 +++++++---- gtsam/geometry/Rot3.h | 7 +++++-- gtsam/geometry/triangulation.h | 7 +++++-- gtsam/inference/MetisIndex.h | 7 +++++-- gtsam/linear/HessianFactor.h | 18 ++++++++++++------ gtsam/nonlinear/ExpressionFactor.h | 4 ++-- gtsam/slam/EssentialMatrixFactor.h | 5 +++-- gtsam/slam/SmartFactorBase.h | 7 +++++-- 10 files changed, 53 insertions(+), 31 deletions(-) diff --git a/gtsam/basis/Chebyshev2.cpp b/gtsam/basis/Chebyshev2.cpp index 63fca64cc..71c3db7f0 100644 --- a/gtsam/basis/Chebyshev2.cpp +++ b/gtsam/basis/Chebyshev2.cpp @@ -20,6 +20,14 @@ namespace gtsam { +double Chebyshev2::Point(size_t N, int j, double a, double b) { + assert(j >= 0 && size_t(j) < N); + const double dtheta = M_PI / (N > 1 ? (N - 1) : 1); + // We add -PI so that we get values ordered from -1 to +1 + // sin(-M_PI_2 + dtheta*j); also works + return a + (b - a) * (1. + cos(-M_PI + dtheta * j)) / 2; +} + Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) { // Allocate space for weights Weights weights(N); diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h index 1374faeef..207f4b617 100644 --- a/gtsam/basis/Chebyshev2.h +++ b/gtsam/basis/Chebyshev2.h @@ -36,8 +36,6 @@ #include #include -#include - namespace gtsam { /** @@ -63,13 +61,7 @@ class GTSAM_EXPORT Chebyshev2 : public Basis { * @param b Upper bound of interval (default: 1) * @return double */ - static double Point(size_t N, int j, double a = -1, double b = 1) { - assert(j >= 0 && size_t(j) < N); - const double dtheta = M_PI / (N > 1 ? (N - 1) : 1); - // We add -PI so that we get values ordered from -1 to +1 - // sin(-M_PI_2 + dtheta*j); also works - return a + (b - a) * (1. + cos(-M_PI + dtheta * j)) / 2; - } + static double Point(size_t N, int j, double a = -1, double b = 1); /// All Chebyshev points static Vector Points(size_t N) { diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index d9cad37f3..ff9a6399e 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -28,7 +28,6 @@ #include #include -#include namespace gtsam { @@ -82,9 +81,13 @@ public: Pose2(const Rot2& r, const Point2& t) : r_(r), t_(t) {} /** Constructor from 3*3 matrix */ - Pose2(const Matrix &T) : - r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) { - assert(T.rows() == 3 && T.cols() == 3); + Pose2(const Matrix &T) + : r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) { +#ifndef NDEBUG + if (T.rows() != 3 || T.cols() != 3) { + throw; + } +#endif } /// @} diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 398e8d473..d1e330438 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -30,7 +30,6 @@ #include // Get GTSAM_USE_QUATERNIONS macro #include -#include // You can override the default coordinate mode using this flag #ifndef ROT3_DEFAULT_COORDINATES_MODE @@ -160,7 +159,11 @@ class GTSAM_EXPORT Rot3 : public LieGroup { /// Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis. inline static Rot3 RzRyRx(const Vector& xyz, OptionalJacobian<3, 3> H = {}) { - assert(xyz.size() == 3); +#ifndef NDEBUG + if (xyz.size() != 3) { + throw; + } +#endif Rot3 out; if (H) { Vector3 Hx, Hy, Hz; diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 0fa9d074e..9087f01c5 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -35,7 +35,6 @@ #include #include -#include namespace gtsam { @@ -318,7 +317,11 @@ typename CAMERA::MeasurementVector undistortMeasurements( const CameraSet& cameras, const typename CAMERA::MeasurementVector& measurements) { const size_t nrMeasurements = measurements.size(); - assert(nrMeasurements == cameras.size()); +#ifndef NDEBUG + if (nrMeasurements != cameras.size()) { + throw; + } +#endif typename CAMERA::MeasurementVector undistortedMeasurements(nrMeasurements); for (size_t ii = 0; ii < nrMeasurements; ++ii) { // Calibrate with cal and uncalibrate with pinhole version of cal so that diff --git a/gtsam/inference/MetisIndex.h b/gtsam/inference/MetisIndex.h index cc058dfbd..e051b9ae9 100644 --- a/gtsam/inference/MetisIndex.h +++ b/gtsam/inference/MetisIndex.h @@ -25,7 +25,6 @@ #include #include #include -#include namespace gtsam { /** @@ -93,7 +92,11 @@ public: return nKeys_; } Key intToKey(int32_t value) const { - assert(value >= 0); +#ifndef NDEBUG + if (value < 0) { + throw; + } +#endif return intKeyBMap_.right.find(value)->second; } diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 53b6d0e8b..56b3d6537 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -23,8 +23,6 @@ #include #include -#include - namespace gtsam { // Forward declarations @@ -242,14 +240,18 @@ namespace gtsam { * use, for example, begin() + 2 to get the 3rd variable in this factor. * @return The linear term \f$ g \f$ */ SymmetricBlockMatrix::constBlock linearTerm(const_iterator j) const { - assert(!empty()); +#ifndef NDEBUG + if(empty()) throw; +#endif return info_.aboveDiagonalBlock(j - begin(), size()); } /** Return the complete linear term \f$ g \f$ as described above. * @return The linear term \f$ g \f$ */ SymmetricBlockMatrix::constBlock linearTerm() const { - assert(!empty()); +#ifndef NDEBUG + if(empty()) throw; +#endif // get the last column (except the bottom right block) return info_.aboveDiagonalRange(0, size(), size(), size() + 1); } @@ -257,7 +259,9 @@ namespace gtsam { /** Return the complete linear term \f$ g \f$ as described above. * @return The linear term \f$ g \f$ */ SymmetricBlockMatrix::Block linearTerm() { - assert(!empty()); +#ifndef NDEBUG + if(empty()) throw; +#endif return info_.aboveDiagonalRange(0, size(), size(), size() + 1); } @@ -326,7 +330,9 @@ namespace gtsam { * @param other the HessianFactor to be updated */ void updateHessian(HessianFactor* other) const { - assert(other); +#ifndef NDEBUG + if(!other) throw; +#endif updateHessian(other->keys_, &other->info_); } diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index b3e34d079..63b2469a9 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -43,7 +43,7 @@ namespace gtsam { * */ template -class ExpressionFactor : public NoiseModelFactor { +class GTSAM_EXPORT ExpressionFactor : public NoiseModelFactor { GTSAM_CONCEPT_ASSERT(IsTestable); protected: @@ -246,7 +246,7 @@ struct traits > : public Testable > {}; * */ template -class ExpressionFactorN : public ExpressionFactor { +class GTSAM_EXPORT ExpressionFactorN : public ExpressionFactor { public: static const std::size_t NARY_EXPRESSION_SIZE = sizeof...(Args); using ArrayNKeys = std::array; diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 3c2f105d4..601f8e94e 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -25,7 +25,6 @@ #include #include -#include namespace gtsam { @@ -71,7 +70,9 @@ class EssentialMatrixFactor : public NoiseModelFactorN { const SharedNoiseModel& model, std::shared_ptr K) : Base(model, key) { - assert(K); +#ifndef NDEBUG + if (K->empty()) throw; +#endif vA_ = EssentialMatrix::Homogeneous(K->calibrate(pA)); vB_ = EssentialMatrix::Homogeneous(K->calibrate(pB)); } diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index b8e789aaa..d3e879c33 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -34,7 +34,6 @@ #include #endif #include -#include namespace gtsam { @@ -136,7 +135,11 @@ protected: /// Add a bunch of measurements, together with the camera keys. void add(const ZVector& measurements, const KeyVector& cameraKeys) { - assert(measurements.size() == cameraKeys.size()); +#ifndef NDEBUG + if (measurements.size() != cameraKeys.size()) { + throw std::runtime_error("Number of measurements and camera keys do not match"); + } +#endif for (size_t i = 0; i < measurements.size(); i++) { this->add(measurements[i], cameraKeys[i]); } From a1ba3d77c8dcc896223bcbe4f08b7c3f207e3c3c Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 10 Dec 2024 17:25:12 -0500 Subject: [PATCH 24/25] Fix essential matrix factor --- gtsam/slam/EssentialMatrixFactor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 601f8e94e..5d38cb234 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -71,7 +71,7 @@ class EssentialMatrixFactor : public NoiseModelFactorN { std::shared_ptr K) : Base(model, key) { #ifndef NDEBUG - if (K->empty()) throw; + if (!K) throw; #endif vA_ = EssentialMatrix::Homogeneous(K->calibrate(pA)); vB_ = EssentialMatrix::Homogeneous(K->calibrate(pB)); From b76c6d8250270f7359884f0db98ce937f02ae5a3 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 11 Dec 2024 12:59:13 -0500 Subject: [PATCH 25/25] Fix ExpressionFactor wrong GTSAM_EXPORT --- gtsam/nonlinear/ExpressionFactor.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index 63b2469a9..b3e34d079 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -43,7 +43,7 @@ namespace gtsam { * */ template -class GTSAM_EXPORT ExpressionFactor : public NoiseModelFactor { +class ExpressionFactor : public NoiseModelFactor { GTSAM_CONCEPT_ASSERT(IsTestable); protected: @@ -246,7 +246,7 @@ struct traits > : public Testable > {}; * */ template -class GTSAM_EXPORT ExpressionFactorN : public ExpressionFactor { +class ExpressionFactorN : public ExpressionFactor { public: static const std::size_t NARY_EXPRESSION_SIZE = sizeof...(Args); using ArrayNKeys = std::array;