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; \ 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 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.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 849a51104..207f4b617 100644 --- a/gtsam/basis/Chebyshev2.h +++ b/gtsam/basis/Chebyshev2.h @@ -61,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/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/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index 9ec3b0ac5..f68d2ae00 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 " @@ -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 a8ab2644f..e1eb2a453 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 @@ -50,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 /// @{ @@ -129,23 +135,21 @@ 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 { + virtual double evaluate(const Assignment& values) const override { return ADT::operator()(values); } - /// Evaluate probability distribution, sugar. - double operator()(const DiscreteValues& 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; /// 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 +164,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); } /// @} @@ -186,21 +190,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" @@ -208,7 +212,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" @@ -216,7 +220,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/DiscreteConditional.cpp b/gtsam/discrete/DiscreteConditional.cpp index 5ab0c59ec..eeb5dca3f 100644 --- a/gtsam/discrete/DiscreteConditional.cpp +++ b/gtsam/discrete/DiscreteConditional.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -29,6 +30,7 @@ #include #include #include +#include using namespace std; using std::pair; @@ -104,7 +106,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/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index f59e29285..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 - double evaluate(const DiscreteValues& values) const { - return ADT::operator()(values); - } - - using DecisionTreeFactor::error; ///< DiscreteValues version - using DecisionTreeFactor::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) {} diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index 19af5bd13..fa1179c39 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_; @@ -92,8 +97,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 + */ + virtual double evaluate(const Assignment& values) const = 0; + /// Find value for given assignment of values to variables - virtual double operator()(const DiscreteValues&) 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/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index 4ededbb8b..d0bf21047 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 @@ -35,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) { @@ -50,11 +50,11 @@ namespace gtsam { return keys; } - /* ************************************************************************* */ + /* ************************************************************************ */ 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()); } @@ -63,26 +63,27 @@ namespace gtsam { return result; } - /* ************************************************************************* */ + /* ************************************************************************ */ DecisionTreeFactor DiscreteFactorGraph::product() const { DecisionTreeFactor result; - for(const sharedFactor& factor: *this) + for (const sharedFactor& factor : *this) { if (factor) result = (*factor) * result; + } 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); + for (const sharedFactor& factor : factors_) { + if (factor) product *= (*factor)(values); + } 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++) { @@ -110,15 +111,18 @@ 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; - for (auto&& factor : factors) product = (*factor) * product; + DecisionTreeFactor product = factors.product(); gttoc(product); // Max over all the potentials by pretending all keys are frontal: @@ -127,6 +131,16 @@ namespace gtsam { // Normalize the product factor to prevent underflow. product = product / (*normalization); + return product; + } + + /* ************************************************************************ */ + // 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); DecisionTreeFactor::shared_ptr max = product.max(frontalKeys); @@ -142,8 +156,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}; @@ -201,20 +215,10 @@ namespace gtsam { } /* ************************************************************************ */ - std::pair // + std::pair // EliminateDiscrete(const DiscreteFactorGraph& factors, const Ordering& frontalKeys) { - // PRODUCT: multiply all factors - gttic(product); - DecisionTreeFactor product; - for (auto&& factor : factors) product = (*factor) * 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); 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); 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/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.cpp b/gtsam/discrete/TableFactor.cpp index f4e023a4d..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 DiscreteValues& 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) { @@ -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 f0ecd66a3..07564892f 100644 --- a/gtsam/discrete/TableFactor.h +++ b/gtsam/discrete/TableFactor.h @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -93,27 +94,8 @@ 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: - /** 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 /// @{ @@ -169,14 +151,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 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/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/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) { 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()); 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..ff9a6399e 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -81,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.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..d1e330438 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -159,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/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..9087f01c5 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 @@ -317,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/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..e051b9ae9 100644 --- a/gtsam/inference/MetisIndex.h +++ b/gtsam/inference/MetisIndex.h @@ -92,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/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..56b3d6537 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -23,7 +23,6 @@ #include #include - namespace gtsam { // Forward declarations @@ -241,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); } @@ -256,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); } @@ -325,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/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 7c6e78527..ebc4d583e 100644 --- a/gtsam/linear/KalmanFilter.cpp +++ b/gtsam/linear/KalmanFilter.cpp @@ -20,12 +20,16 @@ * @author Frank Dellaert */ + +#include + #include #include #include -#include -#include "gtsam/base/Matrix.h" +#ifndef NDEBUG +#include +#endif // In the code below we often get a covariance matrix Q, and we need to create a // JacobianFactor with cost 0.5 * |Ax - b|^T Q^{-1} |Ax - b|. Factorizing Q as 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..5d38cb234 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -70,7 +70,9 @@ class EssentialMatrixFactor : public NoiseModelFactorN { const SharedNoiseModel& model, std::shared_ptr K) : Base(model, key) { - assert(K); +#ifndef NDEBUG + if (!K) 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 5d6ec4445..d3e879c33 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -135,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]); } diff --git a/gtsam_unstable/discrete/AllDiff.cpp b/gtsam_unstable/discrete/AllDiff.cpp index 2bd9e6dfd..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 DiscreteValues& 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 d7a63eae0..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 DiscreteValues& 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 18b335092..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 DiscreteValues& 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 bbbc87667..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 DiscreteValues& 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 7f7b717c2..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 DiscreteValues& values) const override; + double evaluate(const Assignment& values) const override; /// Convert into a decisiontree DecisionTreeFactor toDecisionTreeFactor() const override; 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/discrete/SingleValue.cpp b/gtsam_unstable/discrete/SingleValue.cpp index 6b78f38f5..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 DiscreteValues& 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 3f7f22d6a..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 DiscreteValues& values) const override; + double evaluate(const Assignment& values) const override; /// Convert into a decisiontree DecisionTreeFactor toDecisionTreeFactor() const override; 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/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 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;