Merge branch 'develop' into feature/kf_docs

release/4.3a0
Frank Dellaert 2024-12-11 14:30:08 -05:00 committed by GitHub
commit 1849c7b424
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
92 changed files with 314 additions and 179 deletions

View File

@ -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; \

View File

@ -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

View File

@ -48,6 +48,7 @@
#include <cstring>
#include <fstream>
#include <iostream>
#include <cassert>
using namespace gtsam;
using namespace std;

View File

@ -49,6 +49,7 @@
#include <gtsam/slam/dataset.h>
#include <cstring>
#include <cassert>
#include <fstream>
#include <iostream>

View File

@ -29,6 +29,8 @@
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/geometry/Point2.h>
#include <cassert>
using namespace std;
using namespace gtsam;

View File

@ -24,6 +24,7 @@
#include <Eigen/LU>
#include <cstdarg>
#include <cassert>
#include <cstring>
#include <iomanip>
#include <list>

View File

@ -24,6 +24,7 @@
#include <iostream>
#include <fstream>
#include <sstream>
#include <cassert>
#include <iomanip>
#include <cmath>
#include <cstdio>

View File

@ -21,6 +21,8 @@
#include <gtsam/base/MatrixSerialization.h>
#include <gtsam/base/FastVector.h>
#include <cassert>
namespace gtsam {
// Forward declarations

View File

@ -21,6 +21,7 @@
#include <gtsam/base/timing.h>
#include <cmath>
#include <cassert>
using namespace std;

View File

@ -18,6 +18,8 @@
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <cassert>
using namespace std;
using namespace gtsam;

View File

@ -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);

View File

@ -61,13 +61,7 @@ class GTSAM_EXPORT Chebyshev2 : public Basis<Chebyshev2> {
* @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) {

View File

@ -20,12 +20,12 @@
#include <gtsam/base/Testable.h>
#include <gtsam/discrete/DecisionTree-inl.h>
#include <gtsam/discrete/Ring.h>
#include <algorithm>
#include <iomanip>
#include <limits>
#include <map>
#include <string>
#include <iomanip>
#include <vector>
namespace gtsam {
@ -55,26 +55,6 @@ namespace gtsam {
public:
using Base = DecisionTree<L, double>;
/** 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

View File

@ -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<Key, size_t> 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<std::pair<DiscreteValues, double>> result;
for (const auto& assignment : assignments) {
result.emplace_back(assignment, operator()(assignment));
result.emplace_back(assignment, evaluate(assignment));
}
return result;
}

View File

@ -21,11 +21,12 @@
#include <gtsam/discrete/AlgebraicDecisionTree.h>
#include <gtsam/discrete/DiscreteFactor.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/discrete/Ring.h>
#include <gtsam/inference/Ordering.h>
#include <algorithm>
#include <memory>
#include <map>
#include <memory>
#include <stdexcept>
#include <string>
#include <utility>
@ -50,6 +51,11 @@ namespace gtsam {
typedef std::shared_ptr<DecisionTreeFactor> shared_ptr;
typedef AlgebraicDecisionTree<Key> 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<Key>& values) const {
virtual double evaluate(const Assignment<Key>& 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<std::pair<DiscreteValues, double>> enumerate() const;

View File

@ -19,6 +19,7 @@
#include <gtsam/base/Testable.h>
#include <gtsam/base/debug.h>
#include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/discrete/Ring.h>
#include <gtsam/discrete/Signature.h>
#include <gtsam/hybrid/HybridValues.h>
@ -29,6 +30,7 @@
#include <string>
#include <utility>
#include <vector>
#include <cassert>
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);
}

View File

@ -168,13 +168,9 @@ class GTSAM_EXPORT DiscreteConditional
static_cast<const BaseConditional*>(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.

View File

@ -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) {}

View File

@ -46,6 +46,11 @@ class GTSAM_EXPORT DiscreteFactor : public Factor {
using Values = DiscreteValues; ///< backwards compatibility
using Unary = std::function<double(const double&)>;
using UnaryAssignment =
std::function<double(const Assignment<Key>&, const double&)>;
using Binary = std::function<double(const double, const double)>;
protected:
/// Map of Keys and their cardinalities.
std::map<Key, size_t> 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<Key> as it is the base class of DiscreteValues.
*
* @param values Discrete assignment.
* @return double
*/
virtual double evaluate(const Assignment<Key>& 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;

View File

@ -14,6 +14,7 @@
* @date Feb 14, 2011
* @author Duy-Nguyen Ta
* @author Frank Dellaert
* @author Varun Agrawal
*/
#include <gtsam/discrete/DiscreteBayesTree.h>
@ -35,13 +36,12 @@ namespace gtsam {
template class FactorGraph<DiscreteFactor>;
template class EliminateableFactorGraph<DiscreteFactorGraph>;
/* ************************************************************************* */
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<DecisionTreeFactor>(factor)) {
if (auto p = std::dynamic_pointer_cast<DiscreteFactor>(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<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr> //
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<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr> //
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<DiscreteLookupTable>(nrFrontals,
orderedKeys, product);
auto lookup =
std::make_shared<DiscreteLookupTable>(nrFrontals, orderedKeys, product);
gttoc(lookup);
return {std::dynamic_pointer_cast<DiscreteConditional>(lookup), max};
@ -201,20 +215,10 @@ namespace gtsam {
}
/* ************************************************************************ */
std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr> //
std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr> //
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);

View File

@ -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<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr>
std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr>
EliminateDiscrete(const DiscreteFactorGraph& factors,
const Ordering& frontalKeys);
@ -61,7 +62,7 @@ EliminateDiscrete(const DiscreteFactorGraph& factors,
* @ingroup discrete
*/
GTSAM_EXPORT
std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr>
std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr>
EliminateForMPE(const DiscreteFactorGraph& factors,
const Ordering& frontalKeys);

View File

@ -23,6 +23,7 @@
#include <iterator>
#include <string>
#include <utility>
#include <cassert>
using std::pair;
using std::vector;

37
gtsam/discrete/Ring.h Normal file
View File

@ -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 <algorithm>
/** 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; }
};

View File

@ -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<Key>& 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;
}

View File

@ -19,6 +19,7 @@
#include <gtsam/discrete/DiscreteFactor.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/discrete/Ring.h>
#include <gtsam/inference/Ordering.h>
#include <Eigen/Sparse>
@ -93,27 +94,8 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor {
typedef std::shared_ptr<TableFactor> shared_ptr;
typedef Eigen::SparseVector<double>::InnerIterator SparseIt;
typedef std::vector<std::pair<DiscreteValues, double>> AssignValList;
using Unary = std::function<double(const double&)>;
using UnaryAssignment =
std::function<double(const Assignment<Key>&, const double&)>;
using Binary = std::function<double(const double, const double)>;
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<Key>& values) const override;
/// Calculate error for DiscreteValues `x`, is -log(probability).
double error(const DiscreteValues& values) const override;

View File

@ -61,14 +61,14 @@ virtual class DecisionTreeFactor : gtsam::DiscreteFactor {
DecisionTreeFactor(const std::vector<gtsam::DiscreteKey>& 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;

View File

@ -23,6 +23,7 @@
#include <gtsam/base/Testable.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <gtsam/discrete/DecisionTree-inl.h>
#include <gtsam/discrete/Ring.h>
#include <gtsam/discrete/Signature.h>
#include <gtsam/inference/Symbol.h>
@ -124,14 +125,6 @@ struct traits<DT> : public Testable<DT> {};
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) {

View File

@ -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<DecisionTreeFactor>(newFactorPtr);
// Normalize newFactor by max for comparison with expected
auto normalization = newFactor.max(newFactor.size());

View File

@ -28,6 +28,7 @@
#include <gtsam/geometry/Cal3f.h>
#include <iostream>
#include <cassert>
namespace gtsam {

View File

@ -26,6 +26,7 @@
#include <gtsam/inference/Key.h>
#include <vector>
#include <cassert>
namespace gtsam {

View File

@ -20,6 +20,7 @@
#include <gtsam/base/concepts.h>
#include <cmath>
#include <cassert>
#include <iostream>
#include <iomanip>

View File

@ -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
}
/// @}

View File

@ -23,6 +23,7 @@
#include <gtsam/geometry/SO3.h>
#include <cmath>
#include <cassert>
#include <random>
using namespace std;

View File

@ -159,7 +159,11 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
/// 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;

View File

@ -33,6 +33,7 @@
#include <type_traits>
#include <vector>
#include <random>
#include <cassert>
namespace gtsam {

View File

@ -20,7 +20,7 @@
#pragma once
#include "gtsam/geometry/Point3.h"
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/geometry/Cal3Unified.h>
@ -317,7 +317,11 @@ typename CAMERA::MeasurementVector undistortMeasurements(
const CameraSet<CAMERA>& 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

View File

@ -22,6 +22,7 @@
#include <gtsam/inference/Key.h>
#include <unordered_map>
#include <cassert>
namespace gtsam {

View File

@ -27,7 +27,7 @@
#include <fstream>
#include <queue>
#include <cassert>
namespace gtsam {
/* ************************************************************************* */

View File

@ -19,6 +19,7 @@
#include <mutex>
#endif
#include <queue>
#include <cassert>
namespace gtsam {

View File

@ -19,6 +19,7 @@
#include <stack>
#include <queue>
#include <cassert>
#include <gtsam/base/timing.h>
#include <gtsam/base/treeTraversal-inst.h>

View File

@ -25,6 +25,8 @@
#include <gtsam/symbolic/SymbolicConditional.h>
#include <gtsam/symbolic/SymbolicFactor-inst.h>
#include <cassert>
namespace gtsam {
template<class BAYESTREE, class GRAPH, class ETREE_NODE>

View File

@ -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;
}

View File

@ -18,6 +18,7 @@
#include <vector>
#include <limits>
#include <cassert>
#include <gtsam/inference/Ordering.h>
#include <gtsam/3rdparty/CCOLAMD/Include/ccolamd.h>

View File

@ -25,13 +25,14 @@
#include <gtsam/base/cholesky.h>
#include <gtsam/base/debug.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/ThreadsafeException.h>
#include <gtsam/base/timing.h>
#include <sstream>
#include <cassert>
#include <limits>
#include "gtsam/base/Vector.h"
using namespace std;

View File

@ -23,7 +23,6 @@
#include <gtsam/base/SymmetricBlockMatrix.h>
#include <gtsam/base/FastVector.h>
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_);
}

View File

@ -32,6 +32,7 @@
#include <gtsam/base/cholesky.h>
#include <cmath>
#include <cassert>
#include <sstream>
#include <stdexcept>

View File

@ -20,12 +20,16 @@
* @author Frank Dellaert
*/
#include <gtsam/linear/KalmanFilter.h>
#include <gtsam/base/Testable.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/KalmanFilter.h>
#include "gtsam/base/Matrix.h"
#ifndef NDEBUG
#include <cassert>
#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

View File

@ -20,6 +20,7 @@
#include <gtsam/base/timing.h>
#include <cmath>
#include <cassert>
#include <iostream>
#include <limits>
#include <stdexcept>

View File

@ -17,6 +17,9 @@
*/
#include <gtsam/linear/Sampler.h>
#include <cassert>
namespace gtsam {
/* ************************************************************************* */

View File

@ -25,6 +25,7 @@
#include <gtsam/base/Vector.h>
#include <stdexcept>
#include <cassert>
using std::cout;
using std::endl;

View File

@ -23,6 +23,7 @@
/* External or standard includes */
#include <ostream>
#include <cassert>
namespace gtsam {

View File

@ -20,9 +20,11 @@
* @author Varun Agrawal
**/
#include "PreintegrationBase.h"
#include <gtsam/navigation/PreintegrationBase.h>
#include <gtsam/base/numericalDerivative.h>
#include <cassert>
using namespace std;
namespace gtsam {

View File

@ -16,6 +16,7 @@
*/
#include <cmath>
#include <cassert>
#include <gtsam/nonlinear/DoglegOptimizerImpl.h>
using namespace std;

View File

@ -17,6 +17,7 @@
#pragma once
#include <iomanip>
#include <cassert>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/Ordering.h>

View File

@ -26,6 +26,7 @@
#include <set>
#include <string>
#include <vector>
#include <cassert>
namespace gtsam {

View File

@ -23,6 +23,8 @@
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <cassert>
namespace gtsam {
/* ************************************************************************* */

View File

@ -25,6 +25,7 @@
#include <functional>
#include <limits>
#include <string>
#include <cassert>
using namespace std;

View File

@ -33,6 +33,7 @@
#include <string>
#include <utility>
#include <variant>
#include <cassert>
namespace gtsam {

View File

@ -29,6 +29,7 @@
#include <map>
#include <utility>
#include <variant>
#include <cassert>
using namespace std;

View File

@ -22,6 +22,7 @@
#include <stack>
#include <utility>
#include <cassert>
using namespace std;

View File

@ -19,6 +19,8 @@
#include <gtsam/hybrid/HybridValues.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <cassert>
namespace gtsam {
/* ************************************************************************* */

View File

@ -28,6 +28,7 @@
#include <list>
#include <memory>
#include <sstream>
#include <cassert>
using namespace std;

View File

@ -26,6 +26,7 @@
#include <typeinfo> // operator typeid
#include <ostream>
#include <map>
#include <cassert>
class ExpressionFactorBinaryTest;
// Forward declare for testing

View File

@ -37,6 +37,7 @@
#include <random>
#include <set>
#include <vector>
#include <cassert>
namespace gtsam {

View File

@ -70,7 +70,9 @@ class EssentialMatrixFactor : public NoiseModelFactorN<EssentialMatrix> {
const SharedNoiseModel& model,
std::shared_ptr<CALIBRATION> K)
: Base(model, key) {
assert(K);
#ifndef NDEBUG
if (!K) throw;
#endif
vA_ = EssentialMatrix::Homogeneous(K->calibrate(pA));
vB_ = EssentialMatrix::Homogeneous(K->calibrate(pB));
}

View File

@ -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]);
}

View File

@ -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<Key>& values) const {
std::set<size_t> taken; // record values taken by keys
for (Key dkey : keys_) {
size_t value = values.at(dkey); // get the value for that key

View File

@ -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<Key>& values) const override;
/// Convert into a decisiontree, can be *very* expensive !
DecisionTreeFactor toDecisionTreeFactor() const override;

View File

@ -47,7 +47,7 @@ class BinaryAllDiff : public Constraint {
}
/// Calculate value
double operator()(const DiscreteValues& values) const override {
double evaluate(const Assignment<Key>& values) const override {
return (double)(values.at(keys_[0]) != values.at(keys_[1]));
}

View File

@ -30,7 +30,7 @@ string Domain::base1Str() const {
}
/* ************************************************************************* */
double Domain::operator()(const DiscreteValues& values) const {
double Domain::evaluate(const Assignment<Key>& values) const {
return contains(values.at(key()));
}

View File

@ -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<Key>& values) const override;
/// Convert into a decisiontree
DecisionTreeFactor toDecisionTreeFactor() const override;

View File

@ -14,6 +14,7 @@
#include <cmath>
#include <fstream>
#include <iomanip>
#include <cassert>
namespace gtsam {

View File

@ -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<Key>& values) const {
return (double)(values.at(keys_[0]) == value_);
}

View File

@ -55,7 +55,7 @@ class GTSAM_UNSTABLE_EXPORT SingleValue : public Constraint {
}
/// Calculate value
double operator()(const DiscreteValues& values) const override;
double evaluate(const Assignment<Key>& values) const override;
/// Convert into a decisiontree
DecisionTreeFactor toDecisionTreeFactor() const override;

View File

@ -12,6 +12,8 @@
#include <gtsam_unstable/dynamics/PoseRTV.h>
#include <gtsam_unstable/slam/PartialPriorFactor.h>
#include <cassert>
namespace gtsam {
// Indices of relevant variables in the PoseRTV tangent vector:

View File

@ -10,6 +10,8 @@
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam_unstable/dynamics/PoseRTV.h>
#include <cassert>
namespace gtsam {
/**

View File

@ -10,6 +10,8 @@
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam_unstable/dynamics/PoseRTV.h>
#include <cassert>
namespace gtsam {
/**

View File

@ -7,6 +7,8 @@
#include <gtsam/geometry/Pose2.h>
#include <gtsam/base/Vector.h>
#include <cassert>
namespace gtsam {
using namespace std;

View File

@ -10,6 +10,8 @@
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam_unstable/dynamics/PoseRTV.h>
#include <cassert>
namespace gtsam {
namespace dynamics {

View File

@ -9,6 +9,8 @@
#include <gtsam_unstable/geometry/BearingS2.h>
#include <cassert>
namespace gtsam {
using namespace std;

View File

@ -6,8 +6,9 @@
*/
#include <iostream>
#include "gtsam/base/OptionalJacobian.h"
#include "gtsam/base/Vector.h"
#include <cassert>
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/base/Vector.h>
#include <gtsam_unstable/geometry/Pose3Upright.h>

View File

@ -21,6 +21,8 @@
#include <gtsam/base/timing.h>
#include <gtsam/base/debug.h>
#include <cassert>
namespace gtsam {
/* ************************************************************************* */

View File

@ -17,6 +17,7 @@
#include <gtsam_unstable/nonlinear/LinearizedFactor.h>
#include <iostream>
#include <cassert>
namespace gtsam {

View File

@ -14,6 +14,7 @@
#include <iostream>
#include <vector>
#include <optional>
#include <cassert>
#include <boost/shared_array.hpp>
#include <gtsam/base/timing.h>

View File

@ -20,6 +20,8 @@
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/Lie.h>
#include <cassert>
namespace gtsam {
/**

View File

@ -18,6 +18,8 @@
#include <gtsam_unstable/slam/SmartStereoProjectionFactorPP.h>
#include <cassert>
namespace gtsam {
SmartStereoProjectionFactorPP::SmartStereoProjectionFactorPP(

View File

@ -21,6 +21,8 @@
#include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h>
#include <cassert>
namespace gtsam {
SmartStereoProjectionPoseFactor::SmartStereoProjectionPoseFactor(

View File

@ -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):

View File

@ -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):

View File

@ -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

View File

@ -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

View File

@ -25,6 +25,7 @@
#include <CppUnitLite/TestHarness.h>
#include <cassert>
using namespace std;
using namespace gtsam;